rtlwifi: rtl8192ce: rtl8192common: Update for latest version of Realtek drivers

Realtek released new drivers on 06/28/2014. These changes implement all their
changes into the kernel version of the driver. In addition, these modifications
are part of the process of unifying the Realtek and kernel code bases.

Signed-off-by: Larry Finger <Larry.Finger@lwfinger.net>
Signed-off-by: John W. Linville <linville@tuxdriver.com>

1 files changed, 204 insertions, 217 deletions

diff --git a/drivers/net/wireless/rtlwifi/rtl8192c/fw_common.c b/drivers/net/wireless/rtlwifi/rtl8192c/fw_common.c
index 04a4162..6a57e6d 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192c/fw_common.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192c/fw_common.c
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -71,66 +67,31 @@ static void _rtl92c_enable_fw_download(struct ieee80211_hw *hw, bool enable)
 	}
 }
 
-static void rtl_block_fw_writeN(struct ieee80211_hw *hw, const u8 *buffer,
-				u32 size)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u32 blockSize = REALTEK_USB_VENQT_MAX_BUF_SIZE - 20;
-	u8 *bufferPtr = (u8 *) buffer;
-	u32 i, offset, blockCount, remainSize;
-
-	blockCount = size / blockSize;
-	remainSize = size % blockSize;
-
-	for (i = 0; i < blockCount; i++) {
-		offset = i * blockSize;
-		rtlpriv->io.writeN_sync(rtlpriv,
-					(FW_8192C_START_ADDRESS + offset),
-					(void *)(bufferPtr + offset),
-					blockSize);
-	}
-
-	if (remainSize) {
-		offset = blockCount * blockSize;
-		rtlpriv->io.writeN_sync(rtlpriv,
-					(FW_8192C_START_ADDRESS + offset),
-					(void *)(bufferPtr + offset),
-					remainSize);
-	}
-}
-
 static void _rtl92c_fw_block_write(struct ieee80211_hw *hw,
 				   const u8 *buffer, u32 size)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u32 blockSize = sizeof(u32);
-	u8 *bufferPtr = (u8 *) buffer;
-	u32 *pu4BytePtr = (u32 *) buffer;
-	u32 i, offset, blockCount, remainSize;
-	u32 data;
-
-	if (rtlpriv->io.writeN_sync) {
-		rtl_block_fw_writeN(hw, buffer, size);
-		return;
-	}
-	blockCount = size / blockSize;
-	remainSize = size % blockSize;
-	if (remainSize) {
-		/* the last word is < 4 bytes - pad it with zeros */
-		for (i = 0; i < 4 - remainSize; i++)
-			*(bufferPtr + size + i) = 0;
-		blockCount++;
-	}
+	u32 blocksize = sizeof(u32);
+	u8 *bufferptr = (u8 *)buffer;
+	u32 *pu4byteptr = (u32 *)buffer;
+	u32 i, offset, blockcount, remainsize;
 
-	for (i = 0; i < blockCount; i++) {
-		offset = i * blockSize;
-		/* for big-endian platforms, the firmware data need to be byte
-		 * swapped as it was read as a byte string and will be written
-		 * as 32-bit dwords and byte swapped when written
-		 */
-		data = le32_to_cpu(*(__le32 *)(pu4BytePtr + i));
+	blockcount = size / blocksize;
+	remainsize = size % blocksize;
+
+	for (i = 0; i < blockcount; i++) {
+		offset = i * blocksize;
 		rtl_write_dword(rtlpriv, (FW_8192C_START_ADDRESS + offset),
-				data);
+				*(pu4byteptr + i));
+	}
+
+	if (remainsize) {
+		offset = blockcount * blocksize;
+		bufferptr += offset;
+		for (i = 0; i < remainsize; i++) {
+			rtl_write_byte(rtlpriv, (FW_8192C_START_ADDRESS +
+						 offset + i), *(bufferptr + i));
+		}
 	}
 }
 
@@ -168,19 +129,20 @@ static void _rtl92c_write_fw(struct ieee80211_hw *hw,
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	u8 *bufferPtr = buffer;
-
-	RT_TRACE(rtlpriv, COMP_FW, DBG_TRACE, "FW size is %d bytes\n", size);
+	bool is_version_b;
+	u8 *bufferptr = (u8 *)buffer;
 
-	if (IS_CHIP_VER_B(version)) {
-		u32 pageNums, remainSize;
+	RT_TRACE(rtlpriv, COMP_FW, DBG_TRACE, "FW size is %d bytes,\n", size);
+	is_version_b = IS_NORMAL_CHIP(version);
+	if (is_version_b) {
+		u32 pageNums, remainsize;
 		u32 page, offset;
 
-		if (IS_HARDWARE_TYPE_8192CE(rtlhal))
-			_rtl92c_fill_dummy(bufferPtr, &size);
+		if (rtlhal->hw_type == HARDWARE_TYPE_RTL8192CE)
+			_rtl92c_fill_dummy(bufferptr, &size);
 
 		pageNums = size / FW_8192C_PAGE_SIZE;
-		remainSize = size % FW_8192C_PAGE_SIZE;
+		remainsize = size % FW_8192C_PAGE_SIZE;
 
 		if (pageNums > 4) {
 			RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
@@ -189,15 +151,15 @@ static void _rtl92c_write_fw(struct ieee80211_hw *hw,
 
 		for (page = 0; page < pageNums; page++) {
 			offset = page * FW_8192C_PAGE_SIZE;
-			_rtl92c_fw_page_write(hw, page, (bufferPtr + offset),
+			_rtl92c_fw_page_write(hw, page, (bufferptr + offset),
 					      FW_8192C_PAGE_SIZE);
 		}
 
-		if (remainSize) {
+		if (remainsize) {
 			offset = pageNums * FW_8192C_PAGE_SIZE;
 			page = pageNums;
-			_rtl92c_fw_page_write(hw, page, (bufferPtr + offset),
-					      remainSize);
+			_rtl92c_fw_page_write(hw, page, (bufferptr + offset),
+					      remainsize);
 		}
 	} else {
 		_rtl92c_fw_block_write(hw, buffer, size);
@@ -207,6 +169,7 @@ static void _rtl92c_write_fw(struct ieee80211_hw *hw,
 static int _rtl92c_fw_free_to_go(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	int err = -EIO;
 	u32 counter = 0;
 	u32 value32;
 
@@ -217,12 +180,13 @@ static int _rtl92c_fw_free_to_go(struct ieee80211_hw *hw)
 
 	if (counter >= FW_8192C_POLLING_TIMEOUT_COUNT) {
 		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
-			 "chksum report faill ! REG_MCUFWDL:0x%08x\n", value32);
-		return -EIO;
+			 "chksum report faill ! REG_MCUFWDL:0x%08x .\n",
+			  value32);
+		goto exit;
 	}
 
 	RT_TRACE(rtlpriv, COMP_FW, DBG_TRACE,
-		 "Checksum report OK ! REG_MCUFWDL:0x%08x\n", value32);
+		 "Checksum report OK ! REG_MCUFWDL:0x%08x .\n", value32);
 
 	value32 = rtl_read_dword(rtlpriv, REG_MCUFWDL);
 	value32 |= MCUFWDL_RDY;
@@ -235,9 +199,10 @@ static int _rtl92c_fw_free_to_go(struct ieee80211_hw *hw)
 		value32 = rtl_read_dword(rtlpriv, REG_MCUFWDL);
 		if (value32 & WINTINI_RDY) {
 			RT_TRACE(rtlpriv, COMP_FW, DBG_TRACE,
-				 "Polling FW ready success!! REG_MCUFWDL:0x%08x\n",
-				 value32);
-			return 0;
+				 "Polling FW ready success!! REG_MCUFWDL:0x%08x .\n",
+					value32);
+			err = 0;
+			goto exit;
 		}
 
 		mdelay(FW_8192C_POLLING_DELAY);
@@ -245,8 +210,10 @@ static int _rtl92c_fw_free_to_go(struct ieee80211_hw *hw)
 	} while (counter++ < FW_8192C_POLLING_TIMEOUT_COUNT);
 
 	RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
-		 "Polling FW ready fail!! REG_MCUFWDL:0x%08x\n", value32);
-	return -EIO;
+		 "Polling FW ready fail!! REG_MCUFWDL:0x%08x .\n", value32);
+
+exit:
+	return err;
 }
 
 int rtl92c_download_fw(struct ieee80211_hw *hw)
@@ -256,21 +223,21 @@ int rtl92c_download_fw(struct ieee80211_hw *hw)
 	struct rtl92c_firmware_header *pfwheader;
 	u8 *pfwdata;
 	u32 fwsize;
+	int err;
 	enum version_8192c version = rtlhal->version;
 
-	if (rtlpriv->max_fw_size == 0 || !rtlhal->pfirmware)
+	if (!rtlhal->pfirmware)
 		return 1;
 
 	pfwheader = (struct rtl92c_firmware_header *)rtlhal->pfirmware;
-	pfwdata = rtlhal->pfirmware;
+	pfwdata = (u8 *)rtlhal->pfirmware;
 	fwsize = rtlhal->fwsize;
 
 	if (IS_FW_HEADER_EXIST(pfwheader)) {
 		RT_TRACE(rtlpriv, COMP_FW, DBG_DMESG,
 			 "Firmware Version(%d), Signature(%#x),Size(%d)\n",
-			 le16_to_cpu(pfwheader->version),
-			 le16_to_cpu(pfwheader->signature),
-			 (uint)sizeof(struct rtl92c_firmware_header));
+			  pfwheader->version, pfwheader->signature,
+			  (int)sizeof(struct rtl92c_firmware_header));
 
 		pfwdata = pfwdata + sizeof(struct rtl92c_firmware_header);
 		fwsize = fwsize - sizeof(struct rtl92c_firmware_header);
@@ -280,7 +247,8 @@ int rtl92c_download_fw(struct ieee80211_hw *hw)
 	_rtl92c_write_fw(hw, version, pfwdata, fwsize);
 	_rtl92c_enable_fw_download(hw, false);
 
-	if (_rtl92c_fw_free_to_go(hw)) {
+	err = _rtl92c_fw_free_to_go(hw);
+	if (err) {
 		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
 			 "Firmware is not ready to run!\n");
 	} else {
@@ -307,7 +275,7 @@ static bool _rtl92c_check_fw_read_last_h2c(struct ieee80211_hw *hw, u8 boxnum)
 }
 
 static void _rtl92c_fill_h2c_command(struct ieee80211_hw *hw,
-			      u8 element_id, u32 cmd_len, u8 *p_cmdbuffer)
+			      u8 element_id, u32 cmd_len, u8 *cmdbuffer)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
@@ -315,7 +283,8 @@ static void _rtl92c_fill_h2c_command(struct ieee80211_hw *hw,
 	u16 box_reg = 0, box_extreg = 0;
 	u8 u1b_tmp;
 	bool isfw_read = false;
-	bool bwrite_success = false;
+	u8 buf_index = 0;
+	bool bwrite_sucess = false;
 	u8 wait_h2c_limmit = 100;
 	u8 wait_writeh2c_limmit = 100;
 	u8 boxcontent[4], boxextcontent[2];
@@ -329,16 +298,15 @@ static void _rtl92c_fill_h2c_command(struct ieee80211_hw *hw,
 		spin_lock_irqsave(&rtlpriv->locks.h2c_lock, flag);
 		if (rtlhal->h2c_setinprogress) {
 			RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
-				 "H2C set in progress! Wait to set..element_id(%d)\n",
+				 "H2C set in progress! Wait to set..element_id(%d).\n",
 				 element_id);
-
 			while (rtlhal->h2c_setinprogress) {
 				spin_unlock_irqrestore(&rtlpriv->locks.h2c_lock,
 						       flag);
 				h2c_waitcounter++;
 				RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
 					 "Wait 100 us (%d times)...\n",
-					 h2c_waitcounter);
+					  h2c_waitcounter);
 				udelay(100);
 
 				if (h2c_waitcounter > 1000)
@@ -354,7 +322,7 @@ static void _rtl92c_fill_h2c_command(struct ieee80211_hw *hw,
 		}
 	}
 
-	while (!bwrite_success) {
+	while (!bwrite_sucess) {
 		wait_writeh2c_limmit--;
 		if (wait_writeh2c_limmit == 0) {
 			RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
@@ -381,14 +349,13 @@ static void _rtl92c_fill_h2c_command(struct ieee80211_hw *hw,
 			box_extreg = REG_HMEBOX_EXT_3;
 			break;
 		default:
-			RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
-				 "switch case not processed\n");
+			RT_TRACE(rtlpriv, COMP_ERR, DBG_LOUD,
+				 "switch case not process\n");
 			break;
 		}
 
 		isfw_read = _rtl92c_check_fw_read_last_h2c(hw, boxnum);
 		while (!isfw_read) {
-
 			wait_h2c_limmit--;
 			if (wait_h2c_limmit == 0) {
 				RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
@@ -408,7 +375,7 @@ static void _rtl92c_fill_h2c_command(struct ieee80211_hw *hw,
 
 		if (!isfw_read) {
 			RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
-				 "Write H2C register BOX[%d] fail!!!!! Fw do not read\n",
+				 "Write H2C register BOX[%d] fail!!!!! Fw do not read.\n",
 				 boxnum);
 			break;
 		}
@@ -418,13 +385,13 @@ static void _rtl92c_fill_h2c_command(struct ieee80211_hw *hw,
 		boxcontent[0] = element_id;
 		RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
 			 "Write element_id box_reg(%4x) = %2x\n",
-			 box_reg, element_id);
+			  box_reg, element_id);
 
 		switch (cmd_len) {
 		case 1:
 			boxcontent[0] &= ~(BIT(7));
-			memcpy((u8 *) (boxcontent) + 1,
-			       p_cmdbuffer, 1);
+			memcpy((u8 *)(boxcontent) + 1,
+			       cmdbuffer + buf_index, 1);
 
 			for (idx = 0; idx < 4; idx++) {
 				rtl_write_byte(rtlpriv, box_reg + idx,
@@ -433,8 +400,8 @@ static void _rtl92c_fill_h2c_command(struct ieee80211_hw *hw,
 			break;
 		case 2:
 			boxcontent[0] &= ~(BIT(7));
-			memcpy((u8 *) (boxcontent) + 1,
-			       p_cmdbuffer, 2);
+			memcpy((u8 *)(boxcontent) + 1,
+			       cmdbuffer + buf_index, 2);
 
 			for (idx = 0; idx < 4; idx++) {
 				rtl_write_byte(rtlpriv, box_reg + idx,
@@ -443,8 +410,8 @@ static void _rtl92c_fill_h2c_command(struct ieee80211_hw *hw,
 			break;
 		case 3:
 			boxcontent[0] &= ~(BIT(7));
-			memcpy((u8 *) (boxcontent) + 1,
-			       p_cmdbuffer, 3);
+			memcpy((u8 *)(boxcontent) + 1,
+			       cmdbuffer + buf_index, 3);
 
 			for (idx = 0; idx < 4; idx++) {
 				rtl_write_byte(rtlpriv, box_reg + idx,
@@ -453,10 +420,10 @@ static void _rtl92c_fill_h2c_command(struct ieee80211_hw *hw,
 			break;
 		case 4:
 			boxcontent[0] |= (BIT(7));
-			memcpy((u8 *) (boxextcontent),
-			       p_cmdbuffer, 2);
-			memcpy((u8 *) (boxcontent) + 1,
-			       p_cmdbuffer + 2, 2);
+			memcpy((u8 *)(boxextcontent),
+			       cmdbuffer + buf_index, 2);
+			memcpy((u8 *)(boxcontent) + 1,
+			       cmdbuffer + buf_index + 2, 2);
 
 			for (idx = 0; idx < 2; idx++) {
 				rtl_write_byte(rtlpriv, box_extreg + idx,
@@ -470,10 +437,10 @@ static void _rtl92c_fill_h2c_command(struct ieee80211_hw *hw,
 			break;
 		case 5:
 			boxcontent[0] |= (BIT(7));
-			memcpy((u8 *) (boxextcontent),
-			       p_cmdbuffer, 2);
-			memcpy((u8 *) (boxcontent) + 1,
-			       p_cmdbuffer + 2, 3);
+			memcpy((u8 *)(boxextcontent),
+			       cmdbuffer + buf_index, 2);
+			memcpy((u8 *)(boxcontent) + 1,
+			       cmdbuffer + buf_index + 2, 3);
 
 			for (idx = 0; idx < 2; idx++) {
 				rtl_write_byte(rtlpriv, box_extreg + idx,
@@ -486,12 +453,12 @@ static void _rtl92c_fill_h2c_command(struct ieee80211_hw *hw,
 			}
 			break;
 		default:
-			RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
-				 "switch case not processed\n");
+			RT_TRACE(rtlpriv, COMP_ERR, DBG_LOUD,
+				 "switch case not process\n");
 			break;
 		}
 
-		bwrite_success = true;
+		bwrite_sucess = true;
 
 		rtlhal->last_hmeboxnum = boxnum + 1;
 		if (rtlhal->last_hmeboxnum == 4)
@@ -499,7 +466,7 @@ static void _rtl92c_fill_h2c_command(struct ieee80211_hw *hw,
 
 		RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
 			 "pHalData->last_hmeboxnum  = %d\n",
-			 rtlhal->last_hmeboxnum);
+			  rtlhal->last_hmeboxnum);
 	}
 
 	spin_lock_irqsave(&rtlpriv->locks.h2c_lock, flag);
@@ -510,12 +477,19 @@ static void _rtl92c_fill_h2c_command(struct ieee80211_hw *hw,
 }
 
 void rtl92c_fill_h2c_cmd(struct ieee80211_hw *hw,
-			 u8 element_id, u32 cmd_len, u8 *p_cmdbuffer)
+			 u8 element_id, u32 cmd_len, u8 *cmdbuffer)
 {
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
 	u32 tmp_cmdbuf[2];
 
+	if (!rtlhal->fw_ready) {
+		RT_ASSERT(false,
+			  "return H2C cmd because of Fw download fail!!!\n");
+		return;
+	}
+
 	memset(tmp_cmdbuf, 0, 8);
-	memcpy(tmp_cmdbuf, p_cmdbuffer, cmd_len);
+	memcpy(tmp_cmdbuf, cmdbuffer, cmd_len);
 	_rtl92c_fill_h2c_command(hw, element_id, cmd_len, (u8 *)&tmp_cmdbuf);
 
 	return;
@@ -534,7 +508,7 @@ void rtl92c_firmware_selfreset(struct ieee80211_hw *hw)
 	while (u1b_tmp & BIT(2)) {
 		delay--;
 		if (delay == 0) {
-			RT_ASSERT(false, "8051 reset fail\n");
+			RT_ASSERT(false, "8051 reset fail.\n");
 			break;
 		}
 		udelay(50);
@@ -546,23 +520,21 @@ EXPORT_SYMBOL(rtl92c_firmware_selfreset);
 void rtl92c_set_fw_pwrmode_cmd(struct ieee80211_hw *hw, u8 mode)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u8 u1_h2c_set_pwrmode[3] = {0};
+	u8 u1_h2c_set_pwrmode[3] = { 0 };
 	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
 
 	RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD, "FW LPS mode = %d\n", mode);
 
 	SET_H2CCMD_PWRMODE_PARM_MODE(u1_h2c_set_pwrmode, mode);
 	SET_H2CCMD_PWRMODE_PARM_SMART_PS(u1_h2c_set_pwrmode,
-					 (rtlpriv->mac80211.p2p) ?
-					 ppsc->smart_ps : 1);
+		(rtlpriv->mac80211.p2p) ? ppsc->smart_ps : 1);
 	SET_H2CCMD_PWRMODE_PARM_BCN_PASS_TIME(u1_h2c_set_pwrmode,
 					      ppsc->reg_max_lps_awakeintvl);
 
 	RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_DMESG,
-		      "rtl92c_set_fw_rsvdpagepkt(): u1_h2c_set_pwrmode",
+		      "rtl92c_set_fw_rsvdpagepkt(): u1_h2c_set_pwrmode\n",
 		      u1_h2c_set_pwrmode, 3);
 	rtl92c_fill_h2c_cmd(hw, H2C_SETPWRMODE, 3, u1_h2c_set_pwrmode);
-
 }
 EXPORT_SYMBOL(rtl92c_set_fw_pwrmode_cmd);
 
@@ -573,19 +545,22 @@ static bool _rtl92c_cmd_send_packet(struct ieee80211_hw *hw,
 	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
 	struct rtl8192_tx_ring *ring;
 	struct rtl_tx_desc *pdesc;
+	u8 own;
 	unsigned long flags;
 	struct sk_buff *pskb = NULL;
 
 	ring = &rtlpci->tx_ring[BEACON_QUEUE];
 
 	pskb = __skb_dequeue(&ring->queue);
-	kfree_skb(pskb);
+	if (pskb)
+		kfree_skb(pskb);
 
 	spin_lock_irqsave(&rtlpriv->locks.irq_th_lock, flags);
 
 	pdesc = &ring->desc[0];
+	own = (u8)rtlpriv->cfg->ops->get_desc((u8 *)pdesc, true, HW_DESC_OWN);
 
-	rtlpriv->cfg->ops->fill_tx_cmddesc(hw, (u8 *) pdesc, 1, 1, skb);
+	rtlpriv->cfg->ops->fill_tx_cmddesc(hw, (u8 *)pdesc, 1, 1, skb);
 
 	__skb_queue_tail(&ring->queue, skb);
 
@@ -713,7 +688,7 @@ static u8 reserved_page_packet[TOTAL_RESERVED_PKT_LEN] = {
 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 };
 
-void rtl92c_set_fw_rsvdpagepkt(struct ieee80211_hw *hw, bool dl_finished)
+void rtl92c_set_fw_rsvdpagepkt(struct ieee80211_hw *hw, bool b_dl_finished)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
@@ -721,13 +696,13 @@ void rtl92c_set_fw_rsvdpagepkt(struct ieee80211_hw *hw, bool dl_finished)
 
 	u32 totalpacketlen;
 	bool rtstatus;
-	u8 u1RsvdPageLoc[3] = {0};
-	bool dlok = false;
+	u8 u1rsvdpageloc[3] = { 0 };
+	bool b_dlok = false;
 
 	u8 *beacon;
-	u8 *pspoll;
+	u8 *p_pspoll;
 	u8 *nullfunc;
-	u8 *probersp;
+	u8 *p_probersp;
 	/*---------------------------------------------------------
 				(1) beacon
 	---------------------------------------------------------*/
@@ -738,12 +713,12 @@ void rtl92c_set_fw_rsvdpagepkt(struct ieee80211_hw *hw, bool dl_finished)
 	/*-------------------------------------------------------
 				(2) ps-poll
 	--------------------------------------------------------*/
-	pspoll = &reserved_page_packet[PSPOLL_PG * 128];
-	SET_80211_PS_POLL_AID(pspoll, (mac->assoc_id | 0xc000));
-	SET_80211_PS_POLL_BSSID(pspoll, mac->bssid);
-	SET_80211_PS_POLL_TA(pspoll, mac->mac_addr);
+	p_pspoll = &reserved_page_packet[PSPOLL_PG * 128];
+	SET_80211_PS_POLL_AID(p_pspoll, (mac->assoc_id | 0xc000));
+	SET_80211_PS_POLL_BSSID(p_pspoll, mac->bssid);
+	SET_80211_PS_POLL_TA(p_pspoll, mac->mac_addr);
 
-	SET_H2CCMD_RSVDPAGE_LOC_PSPOLL(u1RsvdPageLoc, PSPOLL_PG);
+	SET_H2CCMD_RSVDPAGE_LOC_PSPOLL(u1rsvdpageloc, PSPOLL_PG);
 
 	/*--------------------------------------------------------
 				(3) null data
@@ -753,57 +728,54 @@ void rtl92c_set_fw_rsvdpagepkt(struct ieee80211_hw *hw, bool dl_finished)
 	SET_80211_HDR_ADDRESS2(nullfunc, mac->mac_addr);
 	SET_80211_HDR_ADDRESS3(nullfunc, mac->bssid);
 
-	SET_H2CCMD_RSVDPAGE_LOC_NULL_DATA(u1RsvdPageLoc, NULL_PG);
+	SET_H2CCMD_RSVDPAGE_LOC_NULL_DATA(u1rsvdpageloc, NULL_PG);
 
 	/*---------------------------------------------------------
 				(4) probe response
 	----------------------------------------------------------*/
-	probersp = &reserved_page_packet[PROBERSP_PG * 128];
-	SET_80211_HDR_ADDRESS1(probersp, mac->bssid);
-	SET_80211_HDR_ADDRESS2(probersp, mac->mac_addr);
-	SET_80211_HDR_ADDRESS3(probersp, mac->bssid);
+	p_probersp = &reserved_page_packet[PROBERSP_PG * 128];
+	SET_80211_HDR_ADDRESS1(p_probersp, mac->bssid);
+	SET_80211_HDR_ADDRESS2(p_probersp, mac->mac_addr);
+	SET_80211_HDR_ADDRESS3(p_probersp, mac->bssid);
 
-	SET_H2CCMD_RSVDPAGE_LOC_PROBE_RSP(u1RsvdPageLoc, PROBERSP_PG);
+	SET_H2CCMD_RSVDPAGE_LOC_PROBE_RSP(u1rsvdpageloc, PROBERSP_PG);
 
 	totalpacketlen = TOTAL_RESERVED_PKT_LEN;
 
 	RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_LOUD,
-		      "rtl92c_set_fw_rsvdpagepkt(): HW_VAR_SET_TX_CMD: ALL",
+		      "rtl92c_set_fw_rsvdpagepkt(): HW_VAR_SET_TX_CMD: ALL\n",
 		      &reserved_page_packet[0], totalpacketlen);
 	RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_DMESG,
-		      "rtl92c_set_fw_rsvdpagepkt(): HW_VAR_SET_TX_CMD: ALL",
-		      u1RsvdPageLoc, 3);
+		      "rtl92c_set_fw_rsvdpagepkt(): HW_VAR_SET_TX_CMD: ALL\n",
+		      u1rsvdpageloc, 3);
 
 
 	skb = dev_alloc_skb(totalpacketlen);
-	if (!skb)
-		return;
-	kmemleak_not_leak(skb);
-
-	memcpy((u8 *) skb_put(skb, totalpacketlen),
+	memcpy((u8 *)skb_put(skb, totalpacketlen),
 	       &reserved_page_packet, totalpacketlen);
 
 	rtstatus = _rtl92c_cmd_send_packet(hw, skb);
 
 	if (rtstatus)
-		dlok = true;
+		b_dlok = true;
 
-	if (dlok) {
+	if (b_dlok) {
 		RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD,
-			 "Set RSVD page location to Fw\n");
+			 "Set RSVD page location to Fw.\n");
 		RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_DMESG,
-			      "H2C_RSVDPAGE", u1RsvdPageLoc, 3);
+				"H2C_RSVDPAGE:\n",
+				u1rsvdpageloc, 3);
 		rtl92c_fill_h2c_cmd(hw, H2C_RSVDPAGE,
-				    sizeof(u1RsvdPageLoc), u1RsvdPageLoc);
+				    sizeof(u1rsvdpageloc), u1rsvdpageloc);
 	} else
 		RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
-			 "Set RSVD page location to Fw FAIL!!!!!!\n");
+			 "Set RSVD page location to Fw FAIL!!!!!!.\n");
 }
 EXPORT_SYMBOL(rtl92c_set_fw_rsvdpagepkt);
 
 void rtl92c_set_fw_joinbss_report_cmd(struct ieee80211_hw *hw, u8 mstatus)
 {
-	u8 u1_joinbssrpt_parm[1] = {0};
+	u8 u1_joinbssrpt_parm[1] = { 0 };
 
 	SET_H2CCMD_JOINBSSRPT_PARM_OPMODE(u1_joinbssrpt_parm, mstatus);
 
@@ -813,11 +785,51 @@ EXPORT_SYMBOL(rtl92c_set_fw_joinbss_report_cmd);
 
 static void rtl92c_set_p2p_ctw_period_cmd(struct ieee80211_hw *hw, u8 ctwindow)
 {
-	u8 u1_ctwindow_period[1] = {ctwindow};
+	u8 u1_ctwindow_period[1] = { ctwindow};
 
 	rtl92c_fill_h2c_cmd(hw, H2C_P2P_PS_CTW_CMD, 1, u1_ctwindow_period);
 }
 
+/* refactored routine */
+static void set_noa_data(struct rtl_priv *rtlpriv,
+			 struct rtl_p2p_ps_info *p2pinfo,
+			 struct p2p_ps_offload_t *p2p_ps_offload)
+{
+	int i;
+	u32	start_time, tsf_low;
+
+	/* hw only support 2 set of NoA */
+	for (i = 0 ; i < p2pinfo->noa_num ; i++) {
+		/* To control the reg setting for which NOA*/
+		rtl_write_byte(rtlpriv, 0x5cf, (i << 4));
+		if (i == 0)
+			p2p_ps_offload->noa0_en = 1;
+		else
+			p2p_ps_offload->noa1_en = 1;
+
+		/* config P2P NoA Descriptor Register */
+		rtl_write_dword(rtlpriv, 0x5E0,
+				p2pinfo->noa_duration[i]);
+		rtl_write_dword(rtlpriv, 0x5E4,
+				p2pinfo->noa_interval[i]);
+
+		/*Get Current TSF value */
+		tsf_low = rtl_read_dword(rtlpriv, REG_TSFTR);
+
+		start_time = p2pinfo->noa_start_time[i];
+		if (p2pinfo->noa_count_type[i] != 1) {
+			while (start_time <= (tsf_low+(50*1024))) {
+				start_time += p2pinfo->noa_interval[i];
+				if (p2pinfo->noa_count_type[i] != 255)
+					p2pinfo->noa_count_type[i]--;
+			}
+		}
+		rtl_write_dword(rtlpriv, 0x5E8, start_time);
+		rtl_write_dword(rtlpriv, 0x5EC,
+				p2pinfo->noa_count_type[i]);
+	}
+}
+
 void rtl92c_set_p2p_ps_offload_cmd(struct ieee80211_hw *hw, u8 p2p_ps_state)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
@@ -825,83 +837,58 @@ void rtl92c_set_p2p_ps_offload_cmd(struct ieee80211_hw *hw, u8 p2p_ps_state)
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
 	struct rtl_p2p_ps_info *p2pinfo = &(rtlps->p2p_ps_info);
 	struct p2p_ps_offload_t *p2p_ps_offload = &rtlhal->p2p_ps_offload;
-	u8	i;
 	u16	ctwindow;
-	u32	start_time, tsf_low;
 
 	switch (p2p_ps_state) {
 	case P2P_PS_DISABLE:
-		RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "P2P_PS_DISABLE\n");
-		memset(p2p_ps_offload, 0, sizeof(struct p2p_ps_offload_t));
-		break;
+			RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD,
+				 "P2P_PS_DISABLE\n");
+			memset(p2p_ps_offload, 0, sizeof(*p2p_ps_offload));
+			break;
 	case P2P_PS_ENABLE:
-		RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "P2P_PS_ENABLE\n");
-		/* update CTWindow value. */
-		if (p2pinfo->ctwindow > 0) {
-			p2p_ps_offload->ctwindow_en = 1;
-			ctwindow = p2pinfo->ctwindow;
-			rtl92c_set_p2p_ctw_period_cmd(hw, ctwindow);
-		}
-		/* hw only support 2 set of NoA */
-		for (i = 0; i < p2pinfo->noa_num; i++) {
-			/* To control the register setting for which NOA*/
-			rtl_write_byte(rtlpriv, 0x5cf, (i << 4));
-			if (i == 0)
-				p2p_ps_offload->noa0_en = 1;
-			else
-				p2p_ps_offload->noa1_en = 1;
-
-			/* config P2P NoA Descriptor Register */
-			rtl_write_dword(rtlpriv, 0x5E0,
-					p2pinfo->noa_duration[i]);
-			rtl_write_dword(rtlpriv, 0x5E4,
-					p2pinfo->noa_interval[i]);
-
-			/*Get Current TSF value */
-			tsf_low = rtl_read_dword(rtlpriv, REG_TSFTR);
-
-			start_time = p2pinfo->noa_start_time[i];
-			if (p2pinfo->noa_count_type[i] != 1) {
-				while (start_time <= (tsf_low+(50*1024))) {
-					start_time += p2pinfo->noa_interval[i];
-					if (p2pinfo->noa_count_type[i] != 255)
-						p2pinfo->noa_count_type[i]--;
-				}
+			RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD,
+				 "P2P_PS_ENABLE\n");
+			/* update CTWindow value. */
+			if (p2pinfo->ctwindow > 0) {
+				p2p_ps_offload->ctwindow_en = 1;
+				ctwindow = p2pinfo->ctwindow;
+				rtl92c_set_p2p_ctw_period_cmd(hw, ctwindow);
 			}
-			rtl_write_dword(rtlpriv, 0x5E8, start_time);
-			rtl_write_dword(rtlpriv, 0x5EC,
-					p2pinfo->noa_count_type[i]);
-		}
+			/* call refactored routine */
+			set_noa_data(rtlpriv, p2pinfo, p2p_ps_offload);
 
-		if ((p2pinfo->opp_ps == 1) || (p2pinfo->noa_num > 0)) {
-			/* rst p2p circuit */
-			rtl_write_byte(rtlpriv, REG_DUAL_TSF_RST, BIT(4));
+			if ((p2pinfo->opp_ps == 1) || (p2pinfo->noa_num > 0)) {
+				/* rst p2p circuit */
+				rtl_write_byte(rtlpriv, REG_DUAL_TSF_RST,
+					       BIT(4));
 
-			p2p_ps_offload->offload_en = 1;
+				p2p_ps_offload->offload_en = 1;
 
-			if (P2P_ROLE_GO == rtlpriv->mac80211.p2p) {
-				p2p_ps_offload->role = 1;
-				p2p_ps_offload->allstasleep = 0;
-			} else {
-				p2p_ps_offload->role = 0;
-			}
+				if (P2P_ROLE_GO == rtlpriv->mac80211.p2p) {
+					p2p_ps_offload->role = 1;
+					p2p_ps_offload->allstasleep = 0;
+				} else {
+					p2p_ps_offload->role = 0;
+				}
 
-			p2p_ps_offload->discovery = 0;
-		}
-		break;
+				p2p_ps_offload->discovery = 0;
+			}
+			break;
 	case P2P_PS_SCAN:
-		RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "P2P_PS_SCAN\n");
-		p2p_ps_offload->discovery = 1;
-		break;
+			RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "P2P_PS_SCAN\n");
+			p2p_ps_offload->discovery = 1;
+			break;
 	case P2P_PS_SCAN_DONE:
-		RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "P2P_PS_SCAN_DONE\n");
-		p2p_ps_offload->discovery = 0;
-		p2pinfo->p2p_ps_state = P2P_PS_ENABLE;
-		break;
+			RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD,
+				 "P2P_PS_SCAN_DONE\n");
+			p2p_ps_offload->discovery = 0;
+			p2pinfo->p2p_ps_state = P2P_PS_ENABLE;
+			break;
 	default:
-		break;
+			break;
 	}
 
 	rtl92c_fill_h2c_cmd(hw, H2C_P2P_PS_OFFLOAD, 1, (u8 *)p2p_ps_offload);
+
 }
 EXPORT_SYMBOL_GPL(rtl92c_set_p2p_ps_offload_cmd);