rtl8723ds: Initial commit of files

This repository contains the Realtek driver V5.1.1.5_20523.20161209_BTCOEX20161208-1212.

At inclusion, the only changes from the Realtek version were to fix any compile
warnings or errors. With these changes, the driver builds on kernels through
4.11.

Signed-off-by: Larry Finger <Larry.Finger@lwfinger.net>

core/rtw_eeprom.c

@@ -0,0 +1,374 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * 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
+ *
+ *
+ ******************************************************************************/
+#define _RTW_EEPROM_C_
+
+#include <drv_conf.h>
+#include <osdep_service.h>
+#include <drv_types.h>
+
+void up_clk(_adapter	*padapter,	 u16 *x)
+{
+	*x = *x | _EESK;
+	rtw_write8(padapter, EE_9346CR, (u8)*x);
+	rtw_udelay_os(CLOCK_RATE);
+
+
+}
+
+void down_clk(_adapter	*padapter, u16 *x)
+{
+	*x = *x & ~_EESK;
+	rtw_write8(padapter, EE_9346CR, (u8)*x);
+	rtw_udelay_os(CLOCK_RATE);
+}
+
+void shift_out_bits(_adapter *padapter, u16 data, u16 count)
+{
+	u16 x, mask;
+
+	if (rtw_is_surprise_removed(padapter)) {
+		goto out;
+	}
+	mask = 0x01 << (count - 1);
+	x = rtw_read8(padapter, EE_9346CR);
+
+	x &= ~(_EEDO | _EEDI);
+
+	do {
+		x &= ~_EEDI;
+		if (data & mask)
+			x |= _EEDI;
+		if (rtw_is_surprise_removed(padapter)) {
+			goto out;
+		}
+		rtw_write8(padapter, EE_9346CR, (u8)x);
+		rtw_udelay_os(CLOCK_RATE);
+		up_clk(padapter, &x);
+		down_clk(padapter, &x);
+		mask = mask >> 1;
+	} while (mask);
+	if (rtw_is_surprise_removed(padapter)) {
+		goto out;
+	}
+	x &= ~_EEDI;
+	rtw_write8(padapter, EE_9346CR, (u8)x);
+out:
+	return;
+}
+
+u16 shift_in_bits(_adapter *padapter)
+{
+	u16 x, d = 0, i;
+	if (rtw_is_surprise_removed(padapter)) {
+		goto out;
+	}
+	x = rtw_read8(padapter, EE_9346CR);
+
+	x &= ~(_EEDO | _EEDI);
+	d = 0;
+
+	for (i = 0; i < 16; i++) {
+		d = d << 1;
+		up_clk(padapter, &x);
+		if (rtw_is_surprise_removed(padapter)) {
+			goto out;
+		}
+		x = rtw_read8(padapter, EE_9346CR);
+
+		x &= ~(_EEDI);
+		if (x & _EEDO)
+			d |= 1;
+
+		down_clk(padapter, &x);
+	}
+out:
+
+	return d;
+}
+
+void standby(_adapter	*padapter)
+{
+	u8   x;
+	x = rtw_read8(padapter, EE_9346CR);
+
+	x &= ~(_EECS | _EESK);
+	rtw_write8(padapter, EE_9346CR, x);
+
+	rtw_udelay_os(CLOCK_RATE);
+	x |= _EECS;
+	rtw_write8(padapter, EE_9346CR, x);
+	rtw_udelay_os(CLOCK_RATE);
+}
+
+u16 wait_eeprom_cmd_done(_adapter *padapter)
+{
+	u8	x;
+	u16	i, res = _FALSE;
+	standby(padapter);
+	for (i = 0; i < 200; i++) {
+		x = rtw_read8(padapter, EE_9346CR);
+		if (x & _EEDO) {
+			res = _TRUE;
+			goto exit;
+		}
+		rtw_udelay_os(CLOCK_RATE);
+	}
+exit:
+	return res;
+}
+
+void eeprom_clean(_adapter *padapter)
+{
+	u16 x;
+	if (rtw_is_surprise_removed(padapter)) {
+		goto out;
+	}
+	x = rtw_read8(padapter, EE_9346CR);
+	if (rtw_is_surprise_removed(padapter)) {
+		goto out;
+	}
+	x &= ~(_EECS | _EEDI);
+	rtw_write8(padapter, EE_9346CR, (u8)x);
+	if (rtw_is_surprise_removed(padapter)) {
+		goto out;
+	}
+	up_clk(padapter, &x);
+	if (rtw_is_surprise_removed(padapter)) {
+		goto out;
+	}
+	down_clk(padapter, &x);
+out:
+	return;
+}
+
+void eeprom_write16(_adapter *padapter, u16 reg, u16 data)
+{
+	u8 x;
+#ifdef CONFIG_RTL8712
+	u8	tmp8_ori, tmp8_new, tmp8_clk_ori, tmp8_clk_new;
+	tmp8_ori = rtw_read8(padapter, 0x102502f1);
+	tmp8_new = tmp8_ori & 0xf7;
+	if (tmp8_ori != tmp8_new) {
+		rtw_write8(padapter, 0x102502f1, tmp8_new);
+	}
+	tmp8_clk_ori = rtw_read8(padapter, 0x10250003);
+	tmp8_clk_new = tmp8_clk_ori | 0x20;
+	if (tmp8_clk_new != tmp8_clk_ori) {
+		rtw_write8(padapter, 0x10250003, tmp8_clk_new);
+	}
+#endif
+
+	x = rtw_read8(padapter, EE_9346CR);
+
+	x &= ~(_EEDI | _EEDO | _EESK | _EEM0);
+	x |= _EEM1 | _EECS;
+	rtw_write8(padapter, EE_9346CR, x);
+
+	shift_out_bits(padapter, EEPROM_EWEN_OPCODE, 5);
+
+	if (padapter->EepromAddressSize == 8)	/* CF+ and SDIO */
+		shift_out_bits(padapter, 0, 6);
+	else									/* USB */
+		shift_out_bits(padapter, 0, 4);
+
+	standby(padapter);
+
+	/* Commented out by rcnjko, 2004.0
+	 * 	  Erase this particular word.  Write the erase opcode and register
+	 *    number in that order. The opcode is 3bits in length; reg is 6 bits long. */
+/*	shift_out_bits(Adapter, EEPROM_ERASE_OPCODE, 3);
+ *	shift_out_bits(Adapter, reg, Adapter->EepromAddressSize);
+ *
+ *	if (wait_eeprom_cmd_done(Adapter ) == FALSE)
+ *	{
+ *		return;
+ *	} */
+
+
+	standby(padapter);
+
+	/* write the new word to the EEPROM */
+
+	/* send the write opcode the EEPORM */
+	shift_out_bits(padapter, EEPROM_WRITE_OPCODE, 3);
+
+	/* select which word in the EEPROM that we are writing to. */
+	shift_out_bits(padapter, reg, padapter->EepromAddressSize);
+
+	/* write the data to the selected EEPROM word. */
+	shift_out_bits(padapter, data, 16);
+
+	if (wait_eeprom_cmd_done(padapter) == _FALSE)
+
+		goto exit;
+
+	standby(padapter);
+
+	shift_out_bits(padapter, EEPROM_EWDS_OPCODE, 5);
+	shift_out_bits(padapter, reg, 4);
+
+	eeprom_clean(padapter);
+exit:
+#ifdef CONFIG_RTL8712
+	if (tmp8_clk_new != tmp8_clk_ori)
+		rtw_write8(padapter, 0x10250003, tmp8_clk_ori);
+	if (tmp8_new != tmp8_ori)
+		rtw_write8(padapter, 0x102502f1, tmp8_ori);
+
+#endif
+	return;
+}
+
+u16 eeprom_read16(_adapter *padapter, u16 reg)  /* ReadEEprom */
+{
+
+	u16 x;
+	u16 data = 0;
+#ifdef CONFIG_RTL8712
+	u8	tmp8_ori, tmp8_new, tmp8_clk_ori, tmp8_clk_new;
+	tmp8_ori = rtw_read8(padapter, 0x102502f1);
+	tmp8_new = tmp8_ori & 0xf7;
+	if (tmp8_ori != tmp8_new) {
+		rtw_write8(padapter, 0x102502f1, tmp8_new);
+	}
+	tmp8_clk_ori = rtw_read8(padapter, 0x10250003);
+	tmp8_clk_new = tmp8_clk_ori | 0x20;
+	if (tmp8_clk_new != tmp8_clk_ori) {
+		rtw_write8(padapter, 0x10250003, tmp8_clk_new);
+	}
+#endif
+
+	if (rtw_is_surprise_removed(padapter)) {
+		goto out;
+	}
+	/* select EEPROM, reset bits, set _EECS */
+	x = rtw_read8(padapter, EE_9346CR);
+
+	if (rtw_is_surprise_removed(padapter)) {
+		goto out;
+	}
+
+	x &= ~(_EEDI | _EEDO | _EESK | _EEM0);
+	x |= _EEM1 | _EECS;
+	rtw_write8(padapter, EE_9346CR, (unsigned char)x);
+
+	/* write the read opcode and register number in that order */
+	/* The opcode is 3bits in length, reg is 6 bits long */
+	shift_out_bits(padapter, EEPROM_READ_OPCODE, 3);
+	shift_out_bits(padapter, reg, padapter->EepromAddressSize);
+
+	/* Now read the data (16 bits) in from the selected EEPROM word */
+	data = shift_in_bits(padapter);
+
+	eeprom_clean(padapter);
+out:
+#ifdef CONFIG_RTL8712
+	if (tmp8_clk_new != tmp8_clk_ori)
+		rtw_write8(padapter, 0x10250003, tmp8_clk_ori);
+	if (tmp8_new != tmp8_ori)
+		rtw_write8(padapter, 0x102502f1, tmp8_ori);
+
+#endif
+	return data;
+
+
+}
+
+
+
+
+/* From even offset */
+void eeprom_read_sz(_adapter *padapter, u16 reg, u8 *data, u32 sz)
+{
+
+	u16 x, data16;
+	u32 i;
+	if (rtw_is_surprise_removed(padapter)) {
+		goto out;
+	}
+	/* select EEPROM, reset bits, set _EECS */
+	x = rtw_read8(padapter, EE_9346CR);
+
+	if (rtw_is_surprise_removed(padapter)) {
+		goto out;
+	}
+
+	x &= ~(_EEDI | _EEDO | _EESK | _EEM0);
+	x |= _EEM1 | _EECS;
+	rtw_write8(padapter, EE_9346CR, (unsigned char)x);
+
+	/* write the read opcode and register number in that order */
+	/* The opcode is 3bits in length, reg is 6 bits long */
+	shift_out_bits(padapter, EEPROM_READ_OPCODE, 3);
+	shift_out_bits(padapter, reg, padapter->EepromAddressSize);
+
+
+	for (i = 0; i < sz; i += 2) {
+		data16 = shift_in_bits(padapter);
+		data[i] = data16 & 0xff;
+		data[i + 1] = data16 >> 8;
+	}
+
+	eeprom_clean(padapter);
+out:
+	return;
+}
+
+
+/* addr_off : address offset of the entry in eeprom (not the tuple number of eeprom (reg); that is addr_off !=reg) */
+u8 eeprom_read(_adapter *padapter, u32 addr_off, u8 sz, u8 *rbuf)
+{
+	u8 quotient, remainder, addr_2align_odd;
+	u16 reg, stmp , i = 0, idx = 0;
+	reg = (u16)(addr_off >> 1);
+	addr_2align_odd = (u8)(addr_off & 0x1);
+
+	if (addr_2align_odd) { /* read that start at high part: e.g  1,3,5,7,9,... */
+		stmp = eeprom_read16(padapter, reg);
+		rbuf[idx++] = (u8)((stmp >> 8) & 0xff); /* return hogh-part of the short */
+		reg++;
+		sz--;
+	}
+
+	quotient = sz >> 1;
+	remainder = sz & 0x1;
+
+	for (i = 0 ; i < quotient; i++) {
+		stmp = eeprom_read16(padapter, reg + i);
+		rbuf[idx++] = (u8)(stmp & 0xff);
+		rbuf[idx++] = (u8)((stmp >> 8) & 0xff);
+	}
+
+	reg = reg + i;
+	if (remainder) { /* end of read at lower part of short : 0,2,4,6,... */
+		stmp = eeprom_read16(padapter, reg);
+		rbuf[idx] = (u8)(stmp & 0xff);
+	}
+	return _TRUE;
+}
+
+
+
+VOID read_eeprom_content(_adapter	*padapter)
+{
+
+
+
+}

core/rtw_io.c

@@ -0,0 +1,701 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * 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 purpose of rtw_io.c
+
+a. provides the API
+
+b. provides the protocol engine
+
+c. provides the software interface between caller and the hardware interface
+
+
+Compiler Flag Option:
+
+1. CONFIG_SDIO_HCI:
+    a. USE_SYNC_IRP:  Only sync operations are provided.
+    b. USE_ASYNC_IRP:Both sync/async operations are provided.
+
+2. CONFIG_USB_HCI:
+   a. USE_ASYNC_IRP: Both sync/async operations are provided.
+
+3. CONFIG_CFIO_HCI:
+   b. USE_SYNC_IRP: Only sync operations are provided.
+
+
+Only sync read/rtw_write_mem operations are provided.
+
+jackson@realtek.com.tw
+
+*/
+
+#define _RTW_IO_C_
+
+#include <drv_types.h>
+#include <hal_data.h>
+
+#if defined(PLATFORM_LINUX) && defined (PLATFORM_WINDOWS)
+	#error "Shall be Linux or Windows, but not both!\n"
+#endif
+
+#ifdef CONFIG_SDIO_HCI
+	#define rtw_le16_to_cpu(val)		val
+	#define rtw_le32_to_cpu(val)		val
+	#define rtw_cpu_to_le16(val)		val
+	#define rtw_cpu_to_le32(val)		val
+#else
+	#define rtw_le16_to_cpu(val)		le16_to_cpu(val)
+	#define rtw_le32_to_cpu(val)		le32_to_cpu(val)
+	#define rtw_cpu_to_le16(val)		cpu_to_le16(val)
+	#define rtw_cpu_to_le32(val)		cpu_to_le32(val)
+#endif
+
+
+u8 _rtw_read8(_adapter *adapter, u32 addr)
+{
+	u8 r_val;
+	/* struct	io_queue  	*pio_queue = (struct io_queue *)adapter->pio_queue; */
+	struct io_priv *pio_priv = &adapter->iopriv;
+	struct	intf_hdl		*pintfhdl = &(pio_priv->intf);
+	u8(*_read8)(struct intf_hdl *pintfhdl, u32 addr);
+	_read8 = pintfhdl->io_ops._read8;
+
+	r_val = _read8(pintfhdl, addr);
+	return r_val;
+}
+
+u16 _rtw_read16(_adapter *adapter, u32 addr)
+{
+	u16 r_val;
+	/* struct	io_queue  	*pio_queue = (struct io_queue *)adapter->pio_queue; */
+	struct io_priv *pio_priv = &adapter->iopriv;
+	struct	intf_hdl		*pintfhdl = &(pio_priv->intf);
+	u16(*_read16)(struct intf_hdl *pintfhdl, u32 addr);
+	_read16 = pintfhdl->io_ops._read16;
+
+	r_val = _read16(pintfhdl, addr);
+	return rtw_le16_to_cpu(r_val);
+}
+
+u32 _rtw_read32(_adapter *adapter, u32 addr)
+{
+	u32 r_val;
+	/* struct	io_queue  	*pio_queue = (struct io_queue *)adapter->pio_queue; */
+	struct io_priv *pio_priv = &adapter->iopriv;
+	struct	intf_hdl		*pintfhdl = &(pio_priv->intf);
+	u32(*_read32)(struct intf_hdl *pintfhdl, u32 addr);
+	_read32 = pintfhdl->io_ops._read32;
+
+	r_val = _read32(pintfhdl, addr);
+	return rtw_le32_to_cpu(r_val);
+
+}
+
+int _rtw_write8(_adapter *adapter, u32 addr, u8 val)
+{
+	/* struct	io_queue  	*pio_queue = (struct io_queue *)adapter->pio_queue; */
+	struct io_priv *pio_priv = &adapter->iopriv;
+	struct	intf_hdl		*pintfhdl = &(pio_priv->intf);
+	int (*_write8)(struct intf_hdl *pintfhdl, u32 addr, u8 val);
+	int ret;
+	_write8 = pintfhdl->io_ops._write8;
+
+	ret = _write8(pintfhdl, addr, val);
+
+	return RTW_STATUS_CODE(ret);
+}
+int _rtw_write16(_adapter *adapter, u32 addr, u16 val)
+{
+	/* struct	io_queue  	*pio_queue = (struct io_queue *)adapter->pio_queue; */
+	struct io_priv *pio_priv = &adapter->iopriv;
+	struct	intf_hdl		*pintfhdl = &(pio_priv->intf);
+	int (*_write16)(struct intf_hdl *pintfhdl, u32 addr, u16 val);
+	int ret;
+	_write16 = pintfhdl->io_ops._write16;
+
+	val = rtw_cpu_to_le16(val);
+	ret = _write16(pintfhdl, addr, val);
+
+	return RTW_STATUS_CODE(ret);
+}
+int _rtw_write32(_adapter *adapter, u32 addr, u32 val)
+{
+	/* struct	io_queue  	*pio_queue = (struct io_queue *)adapter->pio_queue; */
+	struct io_priv *pio_priv = &adapter->iopriv;
+	struct	intf_hdl		*pintfhdl = &(pio_priv->intf);
+	int (*_write32)(struct intf_hdl *pintfhdl, u32 addr, u32 val);
+	int ret;
+	_write32 = pintfhdl->io_ops._write32;
+
+	val = rtw_cpu_to_le32(val);
+	ret = _write32(pintfhdl, addr, val);
+
+	return RTW_STATUS_CODE(ret);
+}
+
+int _rtw_writeN(_adapter *adapter, u32 addr , u32 length , u8 *pdata)
+{
+	/* struct	io_queue  	*pio_queue = (struct io_queue *)adapter->pio_queue; */
+	struct io_priv *pio_priv = &adapter->iopriv;
+	struct	intf_hdl	*pintfhdl = (struct intf_hdl *)(&(pio_priv->intf));
+	int (*_writeN)(struct intf_hdl *pintfhdl, u32 addr, u32 length, u8 *pdata);
+	int ret;
+	_writeN = pintfhdl->io_ops._writeN;
+
+	ret = _writeN(pintfhdl, addr, length, pdata);
+
+	return RTW_STATUS_CODE(ret);
+}
+
+#ifdef CONFIG_SDIO_HCI
+u8 _rtw_sd_f0_read8(_adapter *adapter, u32 addr)
+{
+	u8 r_val = 0x00;
+	struct io_priv *pio_priv = &adapter->iopriv;
+	struct intf_hdl *pintfhdl = &(pio_priv->intf);
+	u8(*_sd_f0_read8)(struct intf_hdl *pintfhdl, u32 addr);
+
+	_sd_f0_read8 = pintfhdl->io_ops._sd_f0_read8;
+
+	if (_sd_f0_read8)
+		r_val = _sd_f0_read8(pintfhdl, addr);
+	else
+		RTW_WARN(FUNC_ADPT_FMT" _sd_f0_read8 callback is NULL\n", FUNC_ADPT_ARG(adapter));
+
+	return r_val;
+}
+
+#ifdef CONFIG_SDIO_INDIRECT_ACCESS
+u8 _rtw_sd_iread8(_adapter *adapter, u32 addr)
+{
+	u8 r_val = 0x00;
+	struct io_priv *pio_priv = &adapter->iopriv;
+	struct intf_hdl *pintfhdl = &(pio_priv->intf);
+	u8(*_sd_iread8)(struct intf_hdl *pintfhdl, u32 addr);
+
+	_sd_iread8 = pintfhdl->io_ops._sd_iread8;
+
+	if (_sd_iread8)
+		r_val = _sd_iread8(pintfhdl, addr);
+	else
+		RTW_ERR(FUNC_ADPT_FMT" _sd_iread8 callback is NULL\n", FUNC_ADPT_ARG(adapter));
+
+	return r_val;
+}
+
+u16 _rtw_sd_iread16(_adapter *adapter, u32 addr)
+{
+	u16 r_val = 0x00;
+	struct io_priv *pio_priv = &adapter->iopriv;
+	struct intf_hdl *pintfhdl = &(pio_priv->intf);
+	u16(*_sd_iread16)(struct intf_hdl *pintfhdl, u32 addr);
+
+	_sd_iread16 = pintfhdl->io_ops._sd_iread16;
+
+	if (_sd_iread16)
+		r_val = _sd_iread16(pintfhdl, addr);
+	else
+		RTW_ERR(FUNC_ADPT_FMT" _sd_iread16 callback is NULL\n", FUNC_ADPT_ARG(adapter));
+
+	return r_val;
+}
+
+u32 _rtw_sd_iread32(_adapter *adapter, u32 addr)
+{
+	u32 r_val = 0x00;
+	struct io_priv *pio_priv = &adapter->iopriv;
+	struct intf_hdl *pintfhdl = &(pio_priv->intf);
+	u32(*_sd_iread32)(struct intf_hdl *pintfhdl, u32 addr);
+
+	_sd_iread32 = pintfhdl->io_ops._sd_iread32;
+
+	if (_sd_iread32)
+		r_val = _sd_iread32(pintfhdl, addr);
+	else
+		RTW_ERR(FUNC_ADPT_FMT" _sd_iread32 callback is NULL\n", FUNC_ADPT_ARG(adapter));
+
+	return r_val;
+}
+
+int _rtw_sd_iwrite8(_adapter *adapter, u32 addr, u8 val)
+{
+	struct io_priv *pio_priv = &adapter->iopriv;
+	struct intf_hdl *pintfhdl = &(pio_priv->intf);
+	int (*_sd_iwrite8)(struct intf_hdl *pintfhdl, u32 addr, u8 val);
+	int ret = -1;
+
+	_sd_iwrite8 = pintfhdl->io_ops._sd_iwrite8;
+
+	if (_sd_iwrite8)
+		ret = _sd_iwrite8(pintfhdl, addr, val);
+	else
+		RTW_ERR(FUNC_ADPT_FMT" _sd_iwrite8 callback is NULL\n", FUNC_ADPT_ARG(adapter));
+
+	return RTW_STATUS_CODE(ret);
+}
+
+int _rtw_sd_iwrite16(_adapter *adapter, u32 addr, u16 val)
+{
+	struct io_priv *pio_priv = &adapter->iopriv;
+	struct intf_hdl *pintfhdl = &(pio_priv->intf);
+	int (*_sd_iwrite16)(struct intf_hdl *pintfhdl, u32 addr, u16 val);
+	int ret = -1;
+
+	_sd_iwrite16 = pintfhdl->io_ops._sd_iwrite16;
+
+	if (_sd_iwrite16)
+		ret = _sd_iwrite16(pintfhdl, addr, val);
+	else
+		RTW_ERR(FUNC_ADPT_FMT" _sd_iwrite16 callback is NULL\n", FUNC_ADPT_ARG(adapter));
+
+	return RTW_STATUS_CODE(ret);
+}
+int _rtw_sd_iwrite32(_adapter *adapter, u32 addr, u32 val)
+{
+	struct io_priv *pio_priv = &adapter->iopriv;
+	struct intf_hdl *pintfhdl = &(pio_priv->intf);
+	int (*_sd_iwrite32)(struct intf_hdl *pintfhdl, u32 addr, u32 val);
+	int ret = -1;
+
+	_sd_iwrite32 = pintfhdl->io_ops._sd_iwrite32;
+
+	if (_sd_iwrite32)
+		ret = _sd_iwrite32(pintfhdl, addr, val);
+	else
+		RTW_ERR(FUNC_ADPT_FMT" _sd_iwrite32 callback is NULL\n", FUNC_ADPT_ARG(adapter));
+
+	return RTW_STATUS_CODE(ret);
+}
+
+#endif /* CONFIG_SDIO_INDIRECT_ACCESS */
+
+#endif /* CONFIG_SDIO_HCI */
+
+int _rtw_write8_async(_adapter *adapter, u32 addr, u8 val)
+{
+	/* struct	io_queue  	*pio_queue = (struct io_queue *)adapter->pio_queue; */
+	struct io_priv *pio_priv = &adapter->iopriv;
+	struct	intf_hdl		*pintfhdl = &(pio_priv->intf);
+	int (*_write8_async)(struct intf_hdl *pintfhdl, u32 addr, u8 val);
+	int ret;
+	_write8_async = pintfhdl->io_ops._write8_async;
+
+	ret = _write8_async(pintfhdl, addr, val);
+
+	return RTW_STATUS_CODE(ret);
+}
+int _rtw_write16_async(_adapter *adapter, u32 addr, u16 val)
+{
+	/* struct	io_queue  	*pio_queue = (struct io_queue *)adapter->pio_queue; */
+	struct io_priv *pio_priv = &adapter->iopriv;
+	struct	intf_hdl		*pintfhdl = &(pio_priv->intf);
+	int (*_write16_async)(struct intf_hdl *pintfhdl, u32 addr, u16 val);
+	int ret;
+	_write16_async = pintfhdl->io_ops._write16_async;
+	val = rtw_cpu_to_le16(val);
+	ret = _write16_async(pintfhdl, addr, val);
+
+	return RTW_STATUS_CODE(ret);
+}
+int _rtw_write32_async(_adapter *adapter, u32 addr, u32 val)
+{
+	/* struct	io_queue  	*pio_queue = (struct io_queue *)adapter->pio_queue; */
+	struct io_priv *pio_priv = &adapter->iopriv;
+	struct	intf_hdl		*pintfhdl = &(pio_priv->intf);
+	int (*_write32_async)(struct intf_hdl *pintfhdl, u32 addr, u32 val);
+	int ret;
+	_write32_async = pintfhdl->io_ops._write32_async;
+	val = rtw_cpu_to_le32(val);
+	ret = _write32_async(pintfhdl, addr, val);
+
+	return RTW_STATUS_CODE(ret);
+}
+
+void _rtw_read_mem(_adapter *adapter, u32 addr, u32 cnt, u8 *pmem)
+{
+	void (*_read_mem)(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, u8 *pmem);
+	/* struct	io_queue  	*pio_queue = (struct io_queue *)adapter->pio_queue; */
+	struct io_priv *pio_priv = &adapter->iopriv;
+	struct	intf_hdl		*pintfhdl = &(pio_priv->intf);
+
+
+	if (RTW_CANNOT_RUN(adapter)) {
+		return;
+	}
+
+	_read_mem = pintfhdl->io_ops._read_mem;
+
+	_read_mem(pintfhdl, addr, cnt, pmem);
+
+
+}
+
+void _rtw_write_mem(_adapter *adapter, u32 addr, u32 cnt, u8 *pmem)
+{
+	void (*_write_mem)(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, u8 *pmem);
+	/* struct	io_queue  	*pio_queue = (struct io_queue *)adapter->pio_queue; */
+	struct io_priv *pio_priv = &adapter->iopriv;
+	struct	intf_hdl		*pintfhdl = &(pio_priv->intf);
+
+
+	_write_mem = pintfhdl->io_ops._write_mem;
+
+	_write_mem(pintfhdl, addr, cnt, pmem);
+
+
+}
+
+void _rtw_read_port(_adapter *adapter, u32 addr, u32 cnt, u8 *pmem)
+{
+	u32(*_read_port)(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, u8 *pmem);
+	/* struct	io_queue  	*pio_queue = (struct io_queue *)adapter->pio_queue; */
+	struct io_priv *pio_priv = &adapter->iopriv;
+	struct	intf_hdl		*pintfhdl = &(pio_priv->intf);
+
+
+	if (RTW_CANNOT_RUN(adapter)) {
+		return;
+	}
+
+	_read_port = pintfhdl->io_ops._read_port;
+
+	_read_port(pintfhdl, addr, cnt, pmem);
+
+
+}
+
+void _rtw_read_port_cancel(_adapter *adapter)
+{
+	void (*_read_port_cancel)(struct intf_hdl *pintfhdl);
+	struct io_priv *pio_priv = &adapter->iopriv;
+	struct intf_hdl *pintfhdl = &(pio_priv->intf);
+
+	_read_port_cancel = pintfhdl->io_ops._read_port_cancel;
+
+	RTW_DISABLE_FUNC(adapter, DF_RX_BIT);
+
+	if (_read_port_cancel)
+		_read_port_cancel(pintfhdl);
+}
+
+u32 _rtw_write_port(_adapter *adapter, u32 addr, u32 cnt, u8 *pmem)
+{
+	u32(*_write_port)(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, u8 *pmem);
+	/* struct	io_queue  	*pio_queue = (struct io_queue *)adapter->pio_queue; */
+	struct io_priv *pio_priv = &adapter->iopriv;
+	struct	intf_hdl		*pintfhdl = &(pio_priv->intf);
+	u32 ret = _SUCCESS;
+
+
+	_write_port = pintfhdl->io_ops._write_port;
+
+	ret = _write_port(pintfhdl, addr, cnt, pmem);
+
+
+	return ret;
+}
+
+u32 _rtw_write_port_and_wait(_adapter *adapter, u32 addr, u32 cnt, u8 *pmem, int timeout_ms)
+{
+	int ret = _SUCCESS;
+	struct xmit_buf *pxmitbuf = (struct xmit_buf *)pmem;
+	struct submit_ctx sctx;
+
+	rtw_sctx_init(&sctx, timeout_ms);
+	pxmitbuf->sctx = &sctx;
+
+	ret = _rtw_write_port(adapter, addr, cnt, pmem);
+
+	if (ret == _SUCCESS)
+		ret = rtw_sctx_wait(&sctx, __func__);
+
+	return ret;
+}
+
+void _rtw_write_port_cancel(_adapter *adapter)
+{
+	void (*_write_port_cancel)(struct intf_hdl *pintfhdl);
+	struct io_priv *pio_priv = &adapter->iopriv;
+	struct intf_hdl *pintfhdl = &(pio_priv->intf);
+
+	_write_port_cancel = pintfhdl->io_ops._write_port_cancel;
+
+	RTW_DISABLE_FUNC(adapter, DF_TX_BIT);
+
+	if (_write_port_cancel)
+		_write_port_cancel(pintfhdl);
+}
+int rtw_init_io_priv(_adapter *padapter, void (*set_intf_ops)(_adapter *padapter, struct _io_ops *pops))
+{
+	struct io_priv	*piopriv = &padapter->iopriv;
+	struct intf_hdl *pintf = &piopriv->intf;
+
+	if (set_intf_ops == NULL)
+		return _FAIL;
+
+	piopriv->padapter = padapter;
+	pintf->padapter = padapter;
+	pintf->pintf_dev = adapter_to_dvobj(padapter);
+
+	set_intf_ops(padapter, &pintf->io_ops);
+
+	return _SUCCESS;
+}
+
+/*
+* Increase and check if the continual_io_error of this @param dvobjprive is larger than MAX_CONTINUAL_IO_ERR
+* @return _TRUE:
+* @return _FALSE:
+*/
+int rtw_inc_and_chk_continual_io_error(struct dvobj_priv *dvobj)
+{
+	int ret = _FALSE;
+	int value;
+
+	value = ATOMIC_INC_RETURN(&dvobj->continual_io_error);
+	if (value > MAX_CONTINUAL_IO_ERR) {
+		RTW_INFO("[dvobj:%p][ERROR] continual_io_error:%d > %d\n", dvobj, value, MAX_CONTINUAL_IO_ERR);
+		ret = _TRUE;
+	} else {
+		/* RTW_INFO("[dvobj:%p] continual_io_error:%d\n", dvobj, value); */
+	}
+	return ret;
+}
+
+/*
+* Set the continual_io_error of this @param dvobjprive to 0
+*/
+void rtw_reset_continual_io_error(struct dvobj_priv *dvobj)
+{
+	ATOMIC_SET(&dvobj->continual_io_error, 0);
+}
+
+#ifdef DBG_IO
+
+u32 read_sniff_ranges[][2] = {
+	/* {0x520, 0x523}, */
+};
+
+u32 write_sniff_ranges[][2] = {
+	/* {0x520, 0x523}, */
+	/* {0x4c, 0x4c}, */
+};
+
+int read_sniff_num = sizeof(read_sniff_ranges) / sizeof(u32) / 2;
+int write_sniff_num = sizeof(write_sniff_ranges) / sizeof(u32) / 2;
+
+bool match_read_sniff_ranges(u32 addr, u16 len)
+{
+	int i;
+	for (i = 0; i < read_sniff_num; i++) {
+		if (addr + len > read_sniff_ranges[i][0] && addr <= read_sniff_ranges[i][1])
+			return _TRUE;
+	}
+
+	return _FALSE;
+}
+
+bool match_write_sniff_ranges(u32 addr, u16 len)
+{
+	int i;
+	for (i = 0; i < write_sniff_num; i++) {
+		if (addr + len > write_sniff_ranges[i][0] && addr <= write_sniff_ranges[i][1])
+			return _TRUE;
+	}
+
+	return _FALSE;
+}
+
+struct rf_sniff_ent {
+	u8 path;
+	u16 reg;
+	u32 mask;
+};
+
+struct rf_sniff_ent rf_read_sniff_ranges[] = {
+	/* example for all path addr 0x55 with all RF Reg mask */
+	/* {MAX_RF_PATH, 0x55, bRFRegOffsetMask}, */
+};
+
+struct rf_sniff_ent rf_write_sniff_ranges[] = {
+	/* example for all path addr 0x55 with all RF Reg mask */
+	/* {MAX_RF_PATH, 0x55, bRFRegOffsetMask}, */
+};
+
+int rf_read_sniff_num = sizeof(rf_read_sniff_ranges) / sizeof(struct rf_sniff_ent);
+int rf_write_sniff_num = sizeof(rf_write_sniff_ranges) / sizeof(struct rf_sniff_ent);
+
+bool match_rf_read_sniff_ranges(u8 path, u32 addr, u32 mask)
+{
+	int i;
+
+	for (i = 0; i < rf_read_sniff_num; i++) {
+		if (rf_read_sniff_ranges[i].path == MAX_RF_PATH || rf_read_sniff_ranges[i].path == path)
+			if (addr == rf_read_sniff_ranges[i].reg && (mask & rf_read_sniff_ranges[i].mask))
+				return _TRUE;
+	}
+
+	return _FALSE;
+}
+
+bool match_rf_write_sniff_ranges(u8 path, u32 addr, u32 mask)
+{
+	int i;
+
+	for (i = 0; i < rf_write_sniff_num; i++) {
+		if (rf_write_sniff_ranges[i].path == MAX_RF_PATH || rf_write_sniff_ranges[i].path == path)
+			if (addr == rf_write_sniff_ranges[i].reg && (mask & rf_write_sniff_ranges[i].mask))
+				return _TRUE;
+	}
+
+	return _FALSE;
+}
+
+u8 dbg_rtw_read8(_adapter *adapter, u32 addr, const char *caller, const int line)
+{
+	u8 val = _rtw_read8(adapter, addr);
+
+	if (match_read_sniff_ranges(addr, 1))
+		RTW_INFO("DBG_IO %s:%d rtw_read8(0x%04x) return 0x%02x\n", caller, line, addr, val);
+
+	return val;
+}
+
+u16 dbg_rtw_read16(_adapter *adapter, u32 addr, const char *caller, const int line)
+{
+	u16 val = _rtw_read16(adapter, addr);
+
+	if (match_read_sniff_ranges(addr, 2))
+		RTW_INFO("DBG_IO %s:%d rtw_read16(0x%04x) return 0x%04x\n", caller, line, addr, val);
+
+	return val;
+}
+
+u32 dbg_rtw_read32(_adapter *adapter, u32 addr, const char *caller, const int line)
+{
+	u32 val = _rtw_read32(adapter, addr);
+
+	if (match_read_sniff_ranges(addr, 4))
+		RTW_INFO("DBG_IO %s:%d rtw_read32(0x%04x) return 0x%08x\n", caller, line, addr, val);
+
+	return val;
+}
+
+int dbg_rtw_write8(_adapter *adapter, u32 addr, u8 val, const char *caller, const int line)
+{
+	if (match_write_sniff_ranges(addr, 1))
+		RTW_INFO("DBG_IO %s:%d rtw_write8(0x%04x, 0x%02x)\n", caller, line, addr, val);
+
+	return _rtw_write8(adapter, addr, val);
+}
+int dbg_rtw_write16(_adapter *adapter, u32 addr, u16 val, const char *caller, const int line)
+{
+	if (match_write_sniff_ranges(addr, 2))
+		RTW_INFO("DBG_IO %s:%d rtw_write16(0x%04x, 0x%04x)\n", caller, line, addr, val);
+
+	return _rtw_write16(adapter, addr, val);
+}
+int dbg_rtw_write32(_adapter *adapter, u32 addr, u32 val, const char *caller, const int line)
+{
+	if (match_write_sniff_ranges(addr, 4))
+		RTW_INFO("DBG_IO %s:%d rtw_write32(0x%04x, 0x%08x)\n", caller, line, addr, val);
+
+	return _rtw_write32(adapter, addr, val);
+}
+int dbg_rtw_writeN(_adapter *adapter, u32 addr , u32 length , u8 *data, const char *caller, const int line)
+{
+	if (match_write_sniff_ranges(addr, length))
+		RTW_INFO("DBG_IO %s:%d rtw_writeN(0x%04x, %u)\n", caller, line, addr, length);
+
+	return _rtw_writeN(adapter, addr, length, data);
+}
+
+#ifdef CONFIG_SDIO_HCI
+u8 dbg_rtw_sd_f0_read8(_adapter *adapter, u32 addr, const char *caller, const int line)
+{
+	u8 val = _rtw_sd_f0_read8(adapter, addr);
+
+#if 0
+	if (match_read_sniff_ranges(addr, 1))
+		RTW_INFO("DBG_IO %s:%d rtw_sd_f0_read8(0x%04x) return 0x%02x\n", caller, line, addr, val);
+#endif
+
+	return val;
+}
+
+#ifdef CONFIG_SDIO_INDIRECT_ACCESS
+u8 dbg_rtw_sd_iread8(_adapter *adapter, u32 addr, const char *caller, const int line)
+{
+	u8 val = rtw_sd_iread8(adapter, addr);
+
+	if (match_read_sniff_ranges(addr, 1))
+		RTW_INFO("DBG_IO %s:%d rtw_sd_iread8(0x%04x) return 0x%02x\n", caller, line, addr, val);
+
+	return val;
+}
+
+u16 dbg_rtw_sd_iread16(_adapter *adapter, u32 addr, const char *caller, const int line)
+{
+	u16 val = _rtw_sd_iread16(adapter, addr);
+
+	if (match_read_sniff_ranges(addr, 2))
+		RTW_INFO("DBG_IO %s:%d rtw_sd_iread16(0x%04x) return 0x%04x\n", caller, line, addr, val);
+
+	return val;
+}
+
+u32 dbg_rtw_sd_iread32(_adapter *adapter, u32 addr, const char *caller, const int line)
+{
+	u32 val = _rtw_sd_iread32(adapter, addr);
+
+	if (match_read_sniff_ranges(addr, 4))
+		RTW_INFO("DBG_IO %s:%d rtw_sd_iread32(0x%04x) return 0x%08x\n", caller, line, addr, val);
+
+	return val;
+}
+
+int dbg_rtw_sd_iwrite8(_adapter *adapter, u32 addr, u8 val, const char *caller, const int line)
+{
+	if (match_write_sniff_ranges(addr, 1))
+		RTW_INFO("DBG_IO %s:%d rtw_sd_iwrite8(0x%04x, 0x%02x)\n", caller, line, addr, val);
+
+	return _rtw_sd_iwrite8(adapter, addr, val);
+}
+int dbg_rtw_sd_iwrite16(_adapter *adapter, u32 addr, u16 val, const char *caller, const int line)
+{
+	if (match_write_sniff_ranges(addr, 2))
+		RTW_INFO("DBG_IO %s:%d rtw_sd_iwrite16(0x%04x, 0x%04x)\n", caller, line, addr, val);
+
+	return _rtw_sd_iwrite16(adapter, addr, val);
+}
+int dbg_rtw_sd_iwrite32(_adapter *adapter, u32 addr, u32 val, const char *caller, const int line)
+{
+	if (match_write_sniff_ranges(addr, 4))
+		RTW_INFO("DBG_IO %s:%d rtw_sd_iwrite32(0x%04x, 0x%08x)\n", caller, line, addr, val);
+
+	return _rtw_sd_iwrite32(adapter, addr, val);
+}
+
+#endif /* CONFIG_SDIO_INDIRECT_ACCESS */
+
+#endif /* CONFIG_SDIO_HCI */
+
+#endif

core/rtw_ioctl_query.c

@@ -0,0 +1,171 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * 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
+ *
+ *
+ ******************************************************************************/
+#define _RTW_IOCTL_QUERY_C_
+
+#include <drv_types.h>
+
+
+#ifdef PLATFORM_WINDOWS
+/*
+ * Added for WPA2-PSK, by Annie, 2005-09-20.
+ *   */
+u8
+query_802_11_capability(
+	_adapter		*Adapter,
+	u8			*pucBuf,
+	u32		*pulOutLen
+)
+{
+	static NDIS_802_11_AUTHENTICATION_ENCRYPTION szAuthEnc[] = {
+		{Ndis802_11AuthModeOpen, Ndis802_11EncryptionDisabled},
+		{Ndis802_11AuthModeOpen, Ndis802_11Encryption1Enabled},
+		{Ndis802_11AuthModeShared, Ndis802_11EncryptionDisabled},
+		{Ndis802_11AuthModeShared, Ndis802_11Encryption1Enabled},
+		{Ndis802_11AuthModeWPA, Ndis802_11Encryption2Enabled},
+		{Ndis802_11AuthModeWPA, Ndis802_11Encryption3Enabled},
+		{Ndis802_11AuthModeWPAPSK, Ndis802_11Encryption2Enabled},
+		{Ndis802_11AuthModeWPAPSK, Ndis802_11Encryption3Enabled},
+		{Ndis802_11AuthModeWPANone, Ndis802_11Encryption2Enabled},
+		{Ndis802_11AuthModeWPANone, Ndis802_11Encryption3Enabled},
+		{Ndis802_11AuthModeWPA2, Ndis802_11Encryption2Enabled},
+		{Ndis802_11AuthModeWPA2, Ndis802_11Encryption3Enabled},
+		{Ndis802_11AuthModeWPA2PSK, Ndis802_11Encryption2Enabled},
+		{Ndis802_11AuthModeWPA2PSK, Ndis802_11Encryption3Enabled}
+	};
+	static ULONG	ulNumOfPairSupported = sizeof(szAuthEnc) / sizeof(NDIS_802_11_AUTHENTICATION_ENCRYPTION);
+	NDIS_802_11_CAPABILITY *pCap = (NDIS_802_11_CAPABILITY *)pucBuf;
+	u8	*pucAuthEncryptionSupported = (u8 *) pCap->AuthenticationEncryptionSupported;
+
+
+	pCap->Length = sizeof(NDIS_802_11_CAPABILITY);
+	if (ulNumOfPairSupported > 1)
+		pCap->Length +=	(ulNumOfPairSupported - 1) * sizeof(NDIS_802_11_AUTHENTICATION_ENCRYPTION);
+
+	pCap->Version = 2;
+	pCap->NoOfPMKIDs = NUM_PMKID_CACHE;
+	pCap->NoOfAuthEncryptPairsSupported = ulNumOfPairSupported;
+
+	if (sizeof(szAuthEnc) <= 240)		/* 240 = 256 - 4*4	 */ { /* SecurityInfo.szCapability: only 256 bytes in size. */
+		_rtw_memcpy(pucAuthEncryptionSupported, (u8 *)szAuthEnc,  sizeof(szAuthEnc));
+		*pulOutLen = pCap->Length;
+		return _TRUE;
+	} else {
+		*pulOutLen = 0;
+		return _FALSE;
+	}
+}
+
+u8 query_802_11_association_information(_adapter *padapter, PNDIS_802_11_ASSOCIATION_INFORMATION	pAssocInfo)
+{
+	struct wlan_network *tgt_network;
+	struct	mlme_priv	*pmlmepriv = &(padapter->mlmepriv);
+	struct	security_priv  *psecuritypriv = &(padapter->securitypriv);
+	WLAN_BSSID_EX	*psecnetwork = (WLAN_BSSID_EX *)&pmlmepriv->cur_network.network;
+	u8	*pDest = (u8 *)pAssocInfo + sizeof(NDIS_802_11_ASSOCIATION_INFORMATION);
+	unsigned char i, *auth_ie, *supp_ie;
+
+	/* NdisZeroMemory(pAssocInfo, sizeof(NDIS_802_11_ASSOCIATION_INFORMATION)); */
+	_rtw_memset(pAssocInfo, 0, sizeof(NDIS_802_11_ASSOCIATION_INFORMATION));
+	/* pAssocInfo->Length = sizeof(NDIS_802_11_ASSOCIATION_INFORMATION); */
+
+	/* ------------------------------------------------------ */
+	/* Association Request related information */
+	/* ------------------------------------------------------ */
+	/* Req_1. AvailableRequestFixedIEs */
+	if (psecnetwork != NULL) {
+
+		pAssocInfo->AvailableRequestFixedIEs |= NDIS_802_11_AI_REQFI_CAPABILITIES | NDIS_802_11_AI_REQFI_CURRENTAPADDRESS;
+		pAssocInfo->RequestFixedIEs.Capabilities = (unsigned short) *&psecnetwork->IEs[10];
+		_rtw_memcpy(pAssocInfo->RequestFixedIEs.CurrentAPAddress,
+			    &psecnetwork->MacAddress, 6);
+
+		pAssocInfo->OffsetRequestIEs = sizeof(NDIS_802_11_ASSOCIATION_INFORMATION);
+
+		if (check_fwstate(pmlmepriv, _FW_UNDER_LINKING | _FW_LINKED) == _TRUE) {
+
+			if (psecuritypriv->ndisauthtype >= Ndis802_11AuthModeWPA2)
+				pDest[0] = 48;		/* RSN Information Element */
+			else
+				pDest[0] = 221;	/* WPA(SSN) Information Element */
+
+			supp_ie = &psecuritypriv->supplicant_ie[0];
+
+			i = 13;	/* 0~11 is fixed information element		 */
+			while ((i < supp_ie[0]) && (i < 256)) {
+				if ((unsigned char)supp_ie[i] == pDest[0]) {
+					_rtw_memcpy((u8 *)(pDest),
+						    &supp_ie[i],
+						    supp_ie[1 + i] + 2);
+
+					break;
+				}
+
+				i = i + supp_ie[i + 1] + 2;
+				if (supp_ie[1 + i] == 0)
+					i = i + 1;
+
+			}
+
+
+			pAssocInfo->RequestIELength += (2 + supp_ie[1 + i]); /* (2 + psecnetwork->IEs[1+i]+4); */
+
+		}
+
+
+
+	}
+
+
+	/* ------------------------------------------------------ */
+	/* Association Response related information */
+	/* ------------------------------------------------------ */
+
+	if (check_fwstate(pmlmepriv, _FW_LINKED) == _TRUE) {
+		tgt_network = &(pmlmepriv->cur_network);
+		if (tgt_network != NULL) {
+			pAssocInfo->AvailableResponseFixedIEs =
+				NDIS_802_11_AI_RESFI_CAPABILITIES
+				| NDIS_802_11_AI_RESFI_ASSOCIATIONID
+				;
+
+			pAssocInfo->ResponseFixedIEs.Capabilities = (unsigned short) *&tgt_network->network.IEs[10];
+			pAssocInfo->ResponseFixedIEs.StatusCode = 0;
+			pAssocInfo->ResponseFixedIEs.AssociationId = (unsigned short) tgt_network->aid;
+
+			pDest = (u8 *)pAssocInfo + sizeof(NDIS_802_11_ASSOCIATION_INFORMATION) + pAssocInfo->RequestIELength;
+			auth_ie = &psecuritypriv->authenticator_ie[0];
+
+
+			i = auth_ie[0] - 12;
+			if (i > 0) {
+				_rtw_memcpy((u8 *)&pDest[0], &auth_ie[1], i);
+				pAssocInfo->ResponseIELength = i;
+			}
+
+
+			pAssocInfo->OffsetResponseIEs = sizeof(NDIS_802_11_ASSOCIATION_INFORMATION) + pAssocInfo->RequestIELength;
+
+
+		}
+	}
+
+	return _TRUE;
+}
+#endif

core/rtw_ioctl_rtl.c

@@ -0,0 +1,904 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * 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
+ *
+ *
+ ******************************************************************************/
+#define  _RTW_IOCTL_RTL_C_
+
+#include <drv_types.h>
+
+#ifdef CONFIG_MP_INCLUDED
+	#include <rtw_mp_ioctl.h>
+#endif
+
+struct oid_obj_priv oid_rtl_seg_01_01[] = {
+	{1, &oid_null_function},										/* 0x80 */
+	{1, &oid_null_function},										/* 0x81 */
+	{1, &oid_null_function},										/* 0x82 */
+	{1, &oid_null_function},										/* 0x83 */ /* OID_RT_SET_SNIFFER_MODE */
+	{1, &oid_rt_get_signal_quality_hdl},							/* 0x84 */
+	{1, &oid_rt_get_small_packet_crc_hdl},						/* 0x85 */
+	{1, &oid_rt_get_middle_packet_crc_hdl},						/* 0x86 */
+	{1, &oid_rt_get_large_packet_crc_hdl},						/* 0x87 */
+	{1, &oid_rt_get_tx_retry_hdl},								/* 0x88 */
+	{1, &oid_rt_get_rx_retry_hdl},								/* 0x89 */
+	{1, &oid_rt_pro_set_fw_dig_state_hdl},						/* 0x8A */
+	{1, &oid_rt_pro_set_fw_ra_state_hdl}	,						/* 0x8B */
+	{1, &oid_null_function},										/* 0x8C */
+	{1, &oid_null_function},										/* 0x8D */
+	{1, &oid_null_function},										/* 0x8E */
+	{1, &oid_null_function},										/* 0x8F */
+	{1, &oid_rt_get_rx_total_packet_hdl},							/* 0x90 */
+	{1, &oid_rt_get_tx_beacon_ok_hdl},							/* 0x91 */
+	{1, &oid_rt_get_tx_beacon_err_hdl},							/* 0x92 */
+	{1, &oid_rt_get_rx_icv_err_hdl},								/* 0x93 */
+	{1, &oid_rt_set_encryption_algorithm_hdl},					/* 0x94 */
+	{1, &oid_null_function},										/* 0x95 */
+	{1, &oid_rt_get_preamble_mode_hdl},							/* 0x96 */
+	{1, &oid_null_function},										/* 0x97 */
+	{1, &oid_rt_get_ap_ip_hdl},									/* 0x98 */
+	{1, &oid_rt_get_channelplan_hdl},							/* 0x99	 */
+	{1, &oid_rt_set_preamble_mode_hdl},	 						/* 0x9A */
+	{1, &oid_rt_set_bcn_intvl_hdl},								/* 0x9B */
+	{1, &oid_null_function},										/* 0x9C */
+	{1, &oid_rt_dedicate_probe_hdl},								/* 0x9D */
+	{1, &oid_null_function},										/* 0x9E */
+	{1, &oid_null_function},										/* 0x9F */
+	{1, &oid_null_function},										/* 0xA0 */
+	{1, &oid_null_function},										/* 0xA1 */
+	{1, &oid_null_function},										/* 0xA2 */
+	{1, &oid_null_function},										/* 0xA3 */
+	{1, &oid_null_function},										/* 0xA4 */
+	{1, &oid_null_function},										/* 0xA5 */
+	{1, &oid_null_function},										/* 0xA6 */
+	{1, &oid_rt_get_total_tx_bytes_hdl},							/* 0xA7 */
+	{1, &oid_rt_get_total_rx_bytes_hdl},							/* 0xA8 */
+	{1, &oid_rt_current_tx_power_level_hdl},						/* 0xA9	 */
+	{1, &oid_rt_get_enc_key_mismatch_count_hdl},	 			/* 0xAA */
+	{1, &oid_rt_get_enc_key_match_count_hdl},					/* 0xAB */
+	{1, &oid_rt_get_channel_hdl},								/* 0xAC */
+	{1, &oid_rt_set_channelplan_hdl},								/* 0xAD */
+	{1, &oid_rt_get_hardware_radio_off_hdl},						/* 0xAE */
+	{1, &oid_null_function},										/* 0xAF */
+	{1, &oid_null_function},										/* 0xB0 */
+	{1, &oid_null_function},										/* 0xB1 */
+	{1, &oid_null_function},										/* 0xB2 */
+	{1, &oid_null_function},										/* 0xB3 */
+	{1, &oid_rt_get_key_mismatch_hdl},							/* 0xB4 */
+	{1, &oid_null_function},										/* 0xB5 */
+	{1, &oid_null_function},										/* 0xB6 */
+	{1, &oid_null_function},										/* 0xB7 */
+	{1, &oid_null_function},										/* 0xB8 */
+	{1, &oid_null_function},										/* 0xB9	 */
+	{1, &oid_null_function},	 									/* 0xBA */
+	{1, &oid_rt_supported_wireless_mode_hdl},					/* 0xBB */
+	{1, &oid_rt_get_channel_list_hdl},							/* 0xBC */
+	{1, &oid_rt_get_scan_in_progress_hdl},						/* 0xBD */
+	{1, &oid_null_function},										/* 0xBE */
+	{1, &oid_null_function},										/* 0xBF */
+	{1, &oid_null_function},										/* 0xC0 */
+	{1, &oid_rt_forced_data_rate_hdl},							/* 0xC1 */
+	{1, &oid_rt_wireless_mode_for_scan_list_hdl},					/* 0xC2 */
+	{1, &oid_rt_get_bss_wireless_mode_hdl},						/* 0xC3 */
+	{1, &oid_rt_scan_with_magic_packet_hdl},					/* 0xC4 */
+	{1, &oid_null_function},										/* 0xC5 */
+	{1, &oid_null_function},										/* 0xC6 */
+	{1, &oid_null_function},										/* 0xC7 */
+	{1, &oid_null_function},										/* 0xC8 */
+	{1, &oid_null_function},										/* 0xC9	 */
+	{1, &oid_null_function},	 									/* 0xCA */
+	{1, &oid_null_function},										/* 0xCB */
+	{1, &oid_null_function},										/* 0xCC */
+	{1, &oid_null_function},										/* 0xCD */
+	{1, &oid_null_function},										/* 0xCE */
+	{1, &oid_null_function},										/* 0xCF */
+
+};
+
+struct oid_obj_priv oid_rtl_seg_01_03[] = {
+	{1, &oid_rt_ap_get_associated_station_list_hdl},				/* 0x00 */
+	{1, &oid_null_function},										/* 0x01 */
+	{1, &oid_rt_ap_switch_into_ap_mode_hdl},					/* 0x02 */
+	{1, &oid_null_function},										/* 0x03 */
+	{1, &oid_rt_ap_supported_hdl},								/* 0x04 */
+	{1, &oid_rt_ap_set_passphrase_hdl},							/* 0x05 */
+
+};
+
+struct oid_obj_priv oid_rtl_seg_01_11[] = {
+	{1, &oid_null_function},					/* 0xC0	OID_RT_PRO_RX_FILTER	 */
+	{1, &oid_null_function},					/* 0xC1	OID_CE_USB_WRITE_REGISTRY */
+	{1, &oid_null_function},					/* 0xC2	OID_CE_USB_READ_REGISTRY */
+	{1, &oid_null_function},					/* 0xC3	OID_RT_PRO_SET_INITIAL_GAIN */
+	{1, &oid_null_function},					/* 0xC4	OID_RT_PRO_SET_BB_RF_STANDBY_MODE */
+	{1, &oid_null_function},					/* 0xC5	OID_RT_PRO_SET_BB_RF_SHUTDOWN_MODE */
+	{1, &oid_null_function},					/* 0xC6	OID_RT_PRO_SET_TX_CHARGE_PUMP */
+	{1, &oid_null_function},					/* 0xC7	OID_RT_PRO_SET_RX_CHARGE_PUMP */
+	{1, &oid_rt_pro_rf_write_registry_hdl},	/* 0xC8	 */
+	{1, &oid_rt_pro_rf_read_registry_hdl},	/* 0xC9	 */
+	{1, &oid_null_function}					/* 0xCA	OID_RT_PRO_QUERY_RF_TYPE */
+
+};
+
+struct oid_obj_priv oid_rtl_seg_03_00[] = {
+	{1, &oid_null_function},										/* 0x00 */
+	{1, &oid_rt_get_connect_state_hdl},							/* 0x01 */
+	{1, &oid_null_function},										/* 0x02 */
+	{1, &oid_null_function},										/* 0x03 */
+	{1, &oid_rt_set_default_key_id_hdl},							/* 0x04 */
+
+
+};
+
+
+/* **************  oid_rtl_seg_01_01 section start ************** */
+
+NDIS_STATUS oid_rt_pro_set_fw_dig_state_hdl(struct oid_par_priv *poid_par_priv)
+{
+	NDIS_STATUS		status = NDIS_STATUS_SUCCESS;
+#if 0
+	PADAPTER		Adapter = (PADAPTER)(poid_par_priv->adapter_context);
+	_irqL			oldirql;
+
+
+	if (poid_par_priv->type_of_oid != SET_OID) {
+		status = NDIS_STATUS_NOT_ACCEPTED;
+		return status;
+	}
+
+	_irqlevel_changed_(&oldirql, LOWER);
+	if (poid_par_priv->information_buf_len >= sizeof(struct setdig_parm)) {
+		/* DEBUG_ERR(("===> oid_rt_pro_set_fw_dig_state_hdl. type:0x%02x.\n",*((unsigned char*)poid_par_priv->information_buf )));	 */
+		if (!rtw_setfwdig_cmd(Adapter, *((unsigned char *)poid_par_priv->information_buf)))
+			status = NDIS_STATUS_NOT_ACCEPTED;
+
+	} else
+		status = NDIS_STATUS_NOT_ACCEPTED;
+	_irqlevel_changed_(&oldirql, RAISE);
+#endif
+	return status;
+}
+/* ----------------------------------------------------------------------------- */
+NDIS_STATUS oid_rt_pro_set_fw_ra_state_hdl(struct oid_par_priv *poid_par_priv)
+{
+
+	NDIS_STATUS		status = NDIS_STATUS_SUCCESS;
+#if 0
+	PADAPTER		Adapter = (PADAPTER)(poid_par_priv->adapter_context);
+	_irqL			oldirql;
+
+	if (poid_par_priv->type_of_oid != SET_OID) {
+		status = NDIS_STATUS_NOT_ACCEPTED;
+		return status;
+	}
+
+
+	_irqlevel_changed_(&oldirql, LOWER);
+
+	if (poid_par_priv->information_buf_len >= sizeof(struct setra_parm)) {
+		/* DEBUG_ERR(("===> oid_rt_pro_set_fw_ra_state_hdl. type:0x%02x.\n",*((unsigned char*)poid_par_priv->information_buf )));	 */
+		if (!rtw_setfwra_cmd(Adapter, *((unsigned char *)poid_par_priv->information_buf)))
+			status = NDIS_STATUS_NOT_ACCEPTED;
+
+	} else
+		status = NDIS_STATUS_NOT_ACCEPTED;
+	_irqlevel_changed_(&oldirql, RAISE);
+#endif
+	return status;
+}
+/* ----------------------------------------------------------------------------- */
+NDIS_STATUS oid_rt_get_signal_quality_hdl(struct oid_par_priv *poid_par_priv)
+{
+	NDIS_STATUS		status = NDIS_STATUS_SUCCESS;
+	PADAPTER		padapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+	/* DEBUG_ERR(("<**********************oid_rt_get_signal_quality_hdl\n")); */
+	if (poid_par_priv->type_of_oid != QUERY_OID) {
+		status = NDIS_STATUS_NOT_ACCEPTED;
+		return status;
+	}
+
+#if 0
+	if (pMgntInfo->mAssoc || pMgntInfo->mIbss) {
+		ulInfo = pAdapter->RxStats.SignalQuality;
+		*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+	} else {
+		ulInfo = 0xffffffff; /* It stands for -1 in 4-byte integer. */
+	}
+	break;
+#endif
+
+	return status;
+}
+
+/* ------------------------------------------------------------------------------ */
+
+NDIS_STATUS oid_rt_get_small_packet_crc_hdl(struct oid_par_priv *poid_par_priv)
+{
+	NDIS_STATUS		status = NDIS_STATUS_SUCCESS;
+	PADAPTER		padapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+	if (poid_par_priv->type_of_oid != QUERY_OID) {
+		status = NDIS_STATUS_NOT_ACCEPTED;
+		return status;
+	}
+
+	if (poid_par_priv->information_buf_len >=  sizeof(ULONG)) {
+		*(ULONG *)poid_par_priv->information_buf = padapter->recvpriv.rx_smallpacket_crcerr;
+		*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+	} else
+		status = NDIS_STATUS_INVALID_LENGTH;
+
+	return status;
+}
+/* ------------------------------------------------------------------------------ */
+NDIS_STATUS oid_rt_get_middle_packet_crc_hdl(struct oid_par_priv *poid_par_priv)
+{
+	NDIS_STATUS		status = NDIS_STATUS_SUCCESS;
+	PADAPTER		padapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+	if (poid_par_priv->type_of_oid != QUERY_OID) {
+		status = NDIS_STATUS_NOT_ACCEPTED;
+		return status;
+	}
+
+	if (poid_par_priv->information_buf_len >=  sizeof(ULONG)) {
+		*(ULONG *)poid_par_priv->information_buf = padapter->recvpriv.rx_middlepacket_crcerr;
+		*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+	} else
+		status = NDIS_STATUS_INVALID_LENGTH;
+
+
+	return status;
+}
+/* ------------------------------------------------------------------------------ */
+NDIS_STATUS oid_rt_get_large_packet_crc_hdl(struct oid_par_priv *poid_par_priv)
+{
+	NDIS_STATUS		status = NDIS_STATUS_SUCCESS;
+	PADAPTER		padapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+	if (poid_par_priv->type_of_oid != QUERY_OID) {
+		status = NDIS_STATUS_NOT_ACCEPTED;
+		return status;
+	}
+
+	if (poid_par_priv->information_buf_len >=  sizeof(ULONG)) {
+		*(ULONG *)poid_par_priv->information_buf = padapter->recvpriv.rx_largepacket_crcerr;
+		*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+	} else
+		status = NDIS_STATUS_INVALID_LENGTH;
+
+
+	return status;
+}
+
+/* ------------------------------------------------------------------------------ */
+NDIS_STATUS oid_rt_get_tx_retry_hdl(struct oid_par_priv *poid_par_priv)
+{
+	NDIS_STATUS		status = NDIS_STATUS_SUCCESS;
+	PADAPTER		padapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+	if (poid_par_priv->type_of_oid != QUERY_OID) {
+		status = NDIS_STATUS_NOT_ACCEPTED;
+		return status;
+	}
+
+	return status;
+}
+NDIS_STATUS oid_rt_get_rx_retry_hdl(struct oid_par_priv *poid_par_priv)
+{
+	NDIS_STATUS		status = NDIS_STATUS_SUCCESS;
+	PADAPTER		padapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+	if (poid_par_priv->type_of_oid != QUERY_OID) {
+		status = NDIS_STATUS_NOT_ACCEPTED;
+		return status;
+	}
+	*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+	return status;
+}
+/* ------------------------------------------------------------------------------ */
+NDIS_STATUS oid_rt_get_rx_total_packet_hdl(struct oid_par_priv *poid_par_priv)
+{
+	NDIS_STATUS		status = NDIS_STATUS_SUCCESS;
+	PADAPTER		padapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+	if (poid_par_priv->type_of_oid != QUERY_OID) {
+		status = NDIS_STATUS_NOT_ACCEPTED;
+		return status;
+	}
+	if (poid_par_priv->information_buf_len >=  sizeof(ULONG)) {
+		*(u64 *)poid_par_priv->information_buf = padapter->recvpriv.rx_pkts + padapter->recvpriv.rx_drop;
+		*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+	} else
+		status = NDIS_STATUS_INVALID_LENGTH;
+
+
+	return status;
+}
+/* ------------------------------------------------------------------------------ */
+NDIS_STATUS oid_rt_get_tx_beacon_ok_hdl(struct oid_par_priv *poid_par_priv)
+{
+	NDIS_STATUS		status = NDIS_STATUS_SUCCESS;
+	PADAPTER		padapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+	if (poid_par_priv->type_of_oid != QUERY_OID) {
+		status = NDIS_STATUS_NOT_ACCEPTED;
+		return status;
+	}
+
+	return status;
+}
+NDIS_STATUS oid_rt_get_tx_beacon_err_hdl(struct oid_par_priv *poid_par_priv)
+{
+	NDIS_STATUS		status = NDIS_STATUS_SUCCESS;
+	PADAPTER		padapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+	if (poid_par_priv->type_of_oid != QUERY_OID) {
+		status = NDIS_STATUS_NOT_ACCEPTED;
+		return status;
+	}
+
+	return status;
+}
+/* ------------------------------------------------------------------------------ */
+NDIS_STATUS oid_rt_get_rx_icv_err_hdl(struct oid_par_priv *poid_par_priv)
+{
+	NDIS_STATUS		status = NDIS_STATUS_SUCCESS;
+	PADAPTER		padapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+	if (poid_par_priv->type_of_oid != QUERY_OID) {
+		status = NDIS_STATUS_NOT_ACCEPTED;
+		return status;
+	}
+	if (poid_par_priv->information_buf_len >= sizeof(u32)) {
+		/* _rtw_memcpy(*(uint *)poid_par_priv->information_buf,padapter->recvpriv.rx_icv_err,sizeof(u32)); */
+		*(uint *)poid_par_priv->information_buf = padapter->recvpriv.rx_icv_err;
+		*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+	} else
+		status = NDIS_STATUS_INVALID_LENGTH ;
+
+
+	return status;
+}
+/* ------------------------------------------------------------------------------ */
+NDIS_STATUS oid_rt_set_encryption_algorithm_hdl(struct oid_par_priv *poid_par_priv)
+{
+	NDIS_STATUS		status = NDIS_STATUS_SUCCESS;
+	PADAPTER		padapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+	if (poid_par_priv->type_of_oid != SET_OID) {
+		status = NDIS_STATUS_NOT_ACCEPTED;
+		return status;
+	}
+
+	return status;
+}
+/* ------------------------------------------------------------------------------ */
+NDIS_STATUS oid_rt_get_preamble_mode_hdl(struct oid_par_priv *poid_par_priv)
+{
+	NDIS_STATUS		status = NDIS_STATUS_SUCCESS;
+	PADAPTER		padapter = (PADAPTER)(poid_par_priv->adapter_context);
+	ULONG			preamblemode = 0 ;
+
+	if (poid_par_priv->type_of_oid != QUERY_OID) {
+		status = NDIS_STATUS_NOT_ACCEPTED;
+		return status;
+	}
+	if (poid_par_priv->information_buf_len >= sizeof(ULONG)) {
+		if (padapter->registrypriv.preamble == PREAMBLE_LONG)
+			preamblemode = 0;
+		else if (padapter->registrypriv.preamble == PREAMBLE_AUTO)
+			preamblemode = 1;
+		else if (padapter->registrypriv.preamble == PREAMBLE_SHORT)
+			preamblemode = 2;
+
+
+		*(ULONG *)poid_par_priv->information_buf = preamblemode ;
+		*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+	} else
+		status = NDIS_STATUS_INVALID_LENGTH ;
+	return status;
+}
+/* ------------------------------------------------------------------------------ */
+NDIS_STATUS oid_rt_get_ap_ip_hdl(struct oid_par_priv *poid_par_priv)
+{
+	NDIS_STATUS		status = NDIS_STATUS_SUCCESS;
+	PADAPTER		padapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+	if (poid_par_priv->type_of_oid != QUERY_OID) {
+		status = NDIS_STATUS_NOT_ACCEPTED;
+		return status;
+	}
+
+	return status;
+}
+
+NDIS_STATUS oid_rt_get_channelplan_hdl(struct oid_par_priv *poid_par_priv)
+{
+	NDIS_STATUS		status = NDIS_STATUS_SUCCESS;
+	PADAPTER		padapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+	if (poid_par_priv->type_of_oid != QUERY_OID) {
+		status = NDIS_STATUS_NOT_ACCEPTED;
+		return status;
+	}
+	*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+	*(u16 *)poid_par_priv->information_buf = padapter->mlmepriv.ChannelPlan ;
+
+	return status;
+}
+NDIS_STATUS oid_rt_set_channelplan_hdl(struct oid_par_priv *poid_par_priv)
+{
+	NDIS_STATUS		status = NDIS_STATUS_SUCCESS;
+	PADAPTER		padapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+	if (poid_par_priv->type_of_oid != SET_OID) {
+		status = NDIS_STATUS_NOT_ACCEPTED;
+		return status;
+	}
+
+	padapter->mlmepriv.ChannelPlan  = *(u16 *)poid_par_priv->information_buf ;
+
+	return status;
+}
+/* ------------------------------------------------------------------------------ */
+NDIS_STATUS oid_rt_set_preamble_mode_hdl(struct oid_par_priv *poid_par_priv)
+{
+	NDIS_STATUS		status = NDIS_STATUS_SUCCESS;
+	PADAPTER		padapter = (PADAPTER)(poid_par_priv->adapter_context);
+	ULONG			preamblemode = 0;
+	if (poid_par_priv->type_of_oid != SET_OID) {
+		status = NDIS_STATUS_NOT_ACCEPTED;
+		return status;
+	}
+
+	if (poid_par_priv->information_buf_len >= sizeof(ULONG)) {
+		preamblemode = *(ULONG *)poid_par_priv->information_buf ;
+		if (preamblemode == 0)
+			padapter->registrypriv.preamble = PREAMBLE_LONG;
+		else if (preamblemode == 1)
+			padapter->registrypriv.preamble = PREAMBLE_AUTO;
+		else if (preamblemode == 2)
+			padapter->registrypriv.preamble = PREAMBLE_SHORT;
+
+		*(ULONG *)poid_par_priv->information_buf = preamblemode ;
+		*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+	} else
+		status = NDIS_STATUS_INVALID_LENGTH ;
+
+	return status;
+}
+/* ------------------------------------------------------------------------------ */
+NDIS_STATUS oid_rt_set_bcn_intvl_hdl(struct oid_par_priv *poid_par_priv)
+{
+	NDIS_STATUS		status = NDIS_STATUS_SUCCESS;
+	PADAPTER		padapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+	if (poid_par_priv->type_of_oid != SET_OID) {
+		status = NDIS_STATUS_NOT_ACCEPTED;
+		return status;
+	}
+
+	return status;
+}
+NDIS_STATUS oid_rt_dedicate_probe_hdl(struct oid_par_priv *poid_par_priv)
+{
+	NDIS_STATUS		status = NDIS_STATUS_SUCCESS;
+	PADAPTER		padapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+	return status;
+}
+/* ------------------------------------------------------------------------------ */
+NDIS_STATUS oid_rt_get_total_tx_bytes_hdl(struct oid_par_priv *poid_par_priv)
+{
+	NDIS_STATUS		status = NDIS_STATUS_SUCCESS;
+	PADAPTER		padapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+	if (poid_par_priv->type_of_oid != QUERY_OID) {
+		status = NDIS_STATUS_NOT_ACCEPTED;
+		return status;
+	}
+	if (poid_par_priv->information_buf_len >= sizeof(ULONG)) {
+		*(u64 *)poid_par_priv->information_buf = padapter->xmitpriv.tx_bytes;
+		*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+	} else
+		status = NDIS_STATUS_INVALID_LENGTH ;
+
+
+	return status;
+}
+/* ------------------------------------------------------------------------------ */
+NDIS_STATUS oid_rt_get_total_rx_bytes_hdl(struct oid_par_priv *poid_par_priv)
+{
+	NDIS_STATUS		status = NDIS_STATUS_SUCCESS;
+	PADAPTER		padapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+	if (poid_par_priv->type_of_oid != QUERY_OID) {
+		status = NDIS_STATUS_NOT_ACCEPTED;
+		return status;
+	}
+	if (poid_par_priv->information_buf_len >= sizeof(ULONG)) {
+		/* _rtw_memcpy(*(uint *)poid_par_priv->information_buf,padapter->recvpriv.rx_icv_err,sizeof(u32)); */
+		*(u64 *)poid_par_priv->information_buf = padapter->recvpriv.rx_bytes;
+		*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+	} else
+		status = NDIS_STATUS_INVALID_LENGTH ;
+	return status;
+}
+/* ------------------------------------------------------------------------------ */
+NDIS_STATUS oid_rt_current_tx_power_level_hdl(struct oid_par_priv *poid_par_priv)
+{
+	NDIS_STATUS		status = NDIS_STATUS_SUCCESS;
+	PADAPTER		padapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+	return status;
+}
+NDIS_STATUS oid_rt_get_enc_key_mismatch_count_hdl(struct oid_par_priv *poid_par_priv)
+{
+	NDIS_STATUS		status = NDIS_STATUS_SUCCESS;
+	PADAPTER		padapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+	if (poid_par_priv->type_of_oid != QUERY_OID) {
+		status = NDIS_STATUS_NOT_ACCEPTED;
+		return status;
+	}
+
+	return status;
+}
+NDIS_STATUS oid_rt_get_enc_key_match_count_hdl(struct oid_par_priv *poid_par_priv)
+{
+	NDIS_STATUS		status = NDIS_STATUS_SUCCESS;
+	PADAPTER		padapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+	if (poid_par_priv->type_of_oid != QUERY_OID) {
+		status = NDIS_STATUS_NOT_ACCEPTED;
+		return status;
+	}
+
+	return status;
+}
+NDIS_STATUS oid_rt_get_channel_hdl(struct oid_par_priv *poid_par_priv)
+{
+	NDIS_STATUS		status = NDIS_STATUS_SUCCESS;
+	PADAPTER		padapter = (PADAPTER)(poid_par_priv->adapter_context);
+	struct	mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+	NDIS_802_11_CONFIGURATION		*pnic_Config;
+
+	ULONG   channelnum;
+
+	if (poid_par_priv->type_of_oid != QUERY_OID) {
+		status = NDIS_STATUS_NOT_ACCEPTED;
+		return status;
+	}
+
+	if ((check_fwstate(pmlmepriv, _FW_LINKED) == _TRUE) ||
+	    (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) == _TRUE))
+		pnic_Config = &pmlmepriv->cur_network.network.Configuration;
+	else
+		pnic_Config = &padapter->registrypriv.dev_network.Configuration;
+
+	channelnum = pnic_Config->DSConfig;
+	*(ULONG *)poid_par_priv->information_buf = channelnum;
+
+	*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+
+
+
+
+	return status;
+}
+NDIS_STATUS oid_rt_get_hardware_radio_off_hdl(struct oid_par_priv *poid_par_priv)
+{
+	NDIS_STATUS		status = NDIS_STATUS_SUCCESS;
+	PADAPTER		padapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+	if (poid_par_priv->type_of_oid != QUERY_OID) {
+		status = NDIS_STATUS_NOT_ACCEPTED;
+		return status;
+	}
+
+	return status;
+}
+NDIS_STATUS oid_rt_get_key_mismatch_hdl(struct oid_par_priv *poid_par_priv)
+{
+	NDIS_STATUS		status = NDIS_STATUS_SUCCESS;
+	PADAPTER		padapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+	if (poid_par_priv->type_of_oid != QUERY_OID) {
+		status = NDIS_STATUS_NOT_ACCEPTED;
+		return status;
+	}
+
+	return status;
+}
+NDIS_STATUS oid_rt_supported_wireless_mode_hdl(struct oid_par_priv *poid_par_priv)
+{
+	NDIS_STATUS		status = NDIS_STATUS_SUCCESS;
+	PADAPTER		padapter = (PADAPTER)(poid_par_priv->adapter_context);
+	ULONG			ulInfo = 0 ;
+	/* DEBUG_ERR(("<**********************oid_rt_supported_wireless_mode_hdl\n"));	 */
+	if (poid_par_priv->type_of_oid != QUERY_OID) {
+		status = NDIS_STATUS_NOT_ACCEPTED;
+		return status;
+	}
+	if (poid_par_priv->information_buf_len >= sizeof(ULONG)) {
+		ulInfo |= 0x0100; /* WIRELESS_MODE_B */
+		ulInfo |= 0x0200; /* WIRELESS_MODE_G */
+		ulInfo |= 0x0400; /* WIRELESS_MODE_A */
+
+		*(ULONG *) poid_par_priv->information_buf = ulInfo;
+		/* DEBUG_ERR(("<===oid_rt_supported_wireless_mode %x\n",ulInfo));	 */
+		*poid_par_priv->bytes_rw = poid_par_priv->information_buf_len;
+	} else
+		status = NDIS_STATUS_INVALID_LENGTH;
+
+	return status;
+}
+NDIS_STATUS oid_rt_get_channel_list_hdl(struct oid_par_priv *poid_par_priv)
+{
+	NDIS_STATUS		status = NDIS_STATUS_SUCCESS;
+	PADAPTER		padapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+	if (poid_par_priv->type_of_oid != QUERY_OID) {
+		status = NDIS_STATUS_NOT_ACCEPTED;
+		return status;
+	}
+
+	return status;
+}
+NDIS_STATUS oid_rt_get_scan_in_progress_hdl(struct oid_par_priv *poid_par_priv)
+{
+	NDIS_STATUS		status = NDIS_STATUS_SUCCESS;
+	PADAPTER		padapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+	if (poid_par_priv->type_of_oid != QUERY_OID) {
+		status = NDIS_STATUS_NOT_ACCEPTED;
+		return status;
+	}
+
+	return status;
+}
+
+
+NDIS_STATUS oid_rt_forced_data_rate_hdl(struct oid_par_priv *poid_par_priv)
+{
+	NDIS_STATUS		status = NDIS_STATUS_SUCCESS;
+	PADAPTER		padapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+	return status;
+}
+NDIS_STATUS oid_rt_wireless_mode_for_scan_list_hdl(struct oid_par_priv *poid_par_priv)
+{
+	NDIS_STATUS		status = NDIS_STATUS_SUCCESS;
+	PADAPTER		padapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+	return status;
+}
+NDIS_STATUS oid_rt_get_bss_wireless_mode_hdl(struct oid_par_priv *poid_par_priv)
+{
+	NDIS_STATUS		status = NDIS_STATUS_SUCCESS;
+	PADAPTER		padapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+	if (poid_par_priv->type_of_oid != QUERY_OID) {
+		status = NDIS_STATUS_NOT_ACCEPTED;
+		return status;
+	}
+
+	return status;
+}
+
+NDIS_STATUS oid_rt_scan_with_magic_packet_hdl(struct oid_par_priv *poid_par_priv)
+{
+	NDIS_STATUS		status = NDIS_STATUS_SUCCESS;
+	PADAPTER		padapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+	return status;
+}
+/* **************  oid_rtl_seg_01_01 section end ************** */
+
+/* **************  oid_rtl_seg_01_03 section start ************** */
+NDIS_STATUS oid_rt_ap_get_associated_station_list_hdl(struct oid_par_priv *poid_par_priv)
+{
+	NDIS_STATUS		status = NDIS_STATUS_SUCCESS;
+	PADAPTER		padapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+	if (poid_par_priv->type_of_oid != QUERY_OID) {
+		status = NDIS_STATUS_NOT_ACCEPTED;
+		return status;
+	}
+
+	return status;
+}
+NDIS_STATUS oid_rt_ap_switch_into_ap_mode_hdl(struct oid_par_priv *poid_par_priv)
+{
+	NDIS_STATUS		status = NDIS_STATUS_SUCCESS;
+	PADAPTER		padapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+	return status;
+}
+NDIS_STATUS oid_rt_ap_supported_hdl(struct oid_par_priv *poid_par_priv)
+{
+	NDIS_STATUS		status = NDIS_STATUS_SUCCESS;
+	PADAPTER		padapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+	return status;
+}
+NDIS_STATUS oid_rt_ap_set_passphrase_hdl(struct oid_par_priv *poid_par_priv)
+{
+	NDIS_STATUS		status = NDIS_STATUS_SUCCESS;
+	PADAPTER		padapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+	if (poid_par_priv->type_of_oid != SET_OID) {
+		status = NDIS_STATUS_NOT_ACCEPTED;
+		return status;
+	}
+
+	return status;
+}
+
+/* **************  oid_rtl_seg_01_03 section end ************** */
+
+/* ****************  oid_rtl_seg_01_11   section start **************** */
+NDIS_STATUS oid_rt_pro_rf_write_registry_hdl(struct oid_par_priv *poid_par_priv)
+{
+	NDIS_STATUS		status = NDIS_STATUS_SUCCESS;
+	PADAPTER		Adapter = (PADAPTER)(poid_par_priv->adapter_context);
+	_irqL			oldirql;
+	/* DEBUG_ERR(("<**********************oid_rt_pro_rf_write_registry_hdl\n")); */
+	if (poid_par_priv->type_of_oid != SET_OID) { /* QUERY_OID */
+		status = NDIS_STATUS_NOT_ACCEPTED;
+		return status;
+	}
+
+	_irqlevel_changed_(&oldirql, LOWER);
+	if (poid_par_priv->information_buf_len == (sizeof(unsigned long) * 3)) {
+		/* RegOffsetValue	- The offset of RF register to write. */
+		/* RegDataWidth	- The data width of RF register to write. */
+		/* RegDataValue	- The value to write. */
+		/* RegOffsetValue = *((unsigned long*)InformationBuffer); */
+		/* RegDataWidth = *((unsigned long*)InformationBuffer+1);	   */
+		/* RegDataValue =  *((unsigned long*)InformationBuffer+2);	 */
+		if (!rtw_setrfreg_cmd(Adapter,
+			      *(unsigned char *)poid_par_priv->information_buf,
+			(unsigned long)(*((unsigned long *)poid_par_priv->information_buf + 2))))
+			status = NDIS_STATUS_NOT_ACCEPTED;
+
+	} else
+		status = NDIS_STATUS_INVALID_LENGTH;
+	_irqlevel_changed_(&oldirql, RAISE);
+
+	return status;
+}
+
+/* ------------------------------------------------------------------------------ */
+NDIS_STATUS oid_rt_pro_rf_read_registry_hdl(struct oid_par_priv *poid_par_priv)
+{
+	NDIS_STATUS		status = NDIS_STATUS_SUCCESS;
+#if 0
+	PADAPTER		Adapter = (PADAPTER)(poid_par_priv->adapter_context);
+	_irqL	oldirql;
+
+	/* DEBUG_ERR(("<**********************oid_rt_pro_rf_read_registry_hdl\n")); */
+	if (poid_par_priv->type_of_oid != SET_OID) { /* QUERY_OID */
+		status = NDIS_STATUS_NOT_ACCEPTED;
+		return status;
+	}
+
+	_irqlevel_changed_(&oldirql, LOWER);
+	if (poid_par_priv->information_buf_len == (sizeof(unsigned long) * 3)) {
+		if (Adapter->mppriv.act_in_progress == _TRUE)
+			status = NDIS_STATUS_NOT_ACCEPTED;
+		else {
+			/* init workparam */
+			Adapter->mppriv.act_in_progress = _TRUE;
+			Adapter->mppriv.workparam.bcompleted = _FALSE;
+			Adapter->mppriv.workparam.act_type = MPT_READ_RF;
+			Adapter->mppriv.workparam.io_offset = *(unsigned long *)poid_par_priv->information_buf;
+			Adapter->mppriv.workparam.io_value = 0xcccccccc;
+
+			/* RegOffsetValue	- The offset of RF register to read. */
+			/* RegDataWidth	- The data width of RF register to read. */
+			/* RegDataValue	- The value to read. */
+			/* RegOffsetValue = *((unsigned long*)InformationBuffer); */
+			/* RegDataWidth = *((unsigned long*)InformationBuffer+1);	   */
+			/* RegDataValue =  *((unsigned long*)InformationBuffer+2);	   	 	                   */
+			if (!rtw_getrfreg_cmd(Adapter,
+				*(unsigned char *)poid_par_priv->information_buf,
+				(unsigned char *)&Adapter->mppriv.workparam.io_value))
+				status = NDIS_STATUS_NOT_ACCEPTED;
+		}
+
+
+	} else
+		status = NDIS_STATUS_INVALID_LENGTH;
+	_irqlevel_changed_(&oldirql, RAISE);
+#endif
+	return status;
+}
+
+/* ****************  oid_rtl_seg_01_11   section end****************	 */
+
+
+/* **************  oid_rtl_seg_03_00 section start **************  */
+enum _CONNECT_STATE_ {
+	CHECKINGSTATUS,
+	ASSOCIATED,
+	ADHOCMODE,
+	NOTASSOCIATED
+};
+
+NDIS_STATUS oid_rt_get_connect_state_hdl(struct oid_par_priv *poid_par_priv)
+{
+	NDIS_STATUS		status = NDIS_STATUS_SUCCESS;
+	PADAPTER		padapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+	struct mlme_priv	*pmlmepriv = &(padapter->mlmepriv);
+
+	ULONG ulInfo;
+
+	if (poid_par_priv->type_of_oid != QUERY_OID) {
+		status = NDIS_STATUS_NOT_ACCEPTED;
+		return status;
+	}
+
+	/* nStatus==0	CheckingStatus */
+	/* nStatus==1	Associated */
+	/* nStatus==2	AdHocMode */
+	/* nStatus==3	NotAssociated */
+
+	if (check_fwstate(pmlmepriv, _FW_UNDER_LINKING) == _TRUE)
+		ulInfo = CHECKINGSTATUS;
+	else if (check_fwstate(pmlmepriv, _FW_LINKED) == _TRUE)
+		ulInfo = ASSOCIATED;
+	else if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) == _TRUE)
+		ulInfo = ADHOCMODE;
+	else
+		ulInfo = NOTASSOCIATED ;
+
+	*(ULONG *)poid_par_priv->information_buf = ulInfo;
+	*poid_par_priv->bytes_rw =  poid_par_priv->information_buf_len;
+
+#if 0
+	/* Rearrange the order to let the UI still shows connection when scan is in progress */
+	if (pMgntInfo->mAssoc)
+		ulInfo = 1;
+	else if (pMgntInfo->mIbss)
+		ulInfo = 2;
+	else if (pMgntInfo->bScanInProgress)
+		ulInfo = 0;
+	else
+		ulInfo = 3;
+	ulInfoLen = sizeof(ULONG);
+#endif
+
+	return status;
+}
+
+NDIS_STATUS oid_rt_set_default_key_id_hdl(struct oid_par_priv *poid_par_priv)
+{
+	NDIS_STATUS		status = NDIS_STATUS_SUCCESS;
+	PADAPTER		padapter = (PADAPTER)(poid_par_priv->adapter_context);
+
+	if (poid_par_priv->type_of_oid != SET_OID) {
+		status = NDIS_STATUS_NOT_ACCEPTED;
+		return status;
+	}
+
+	return status;
+}
+/* **************  oid_rtl_seg_03_00 section end **************  */

core/rtw_iol.c

@@ -0,0 +1,387 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * 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
+ *
+ *
+ ******************************************************************************/
+
+#include <drv_types.h>
+
+#ifdef CONFIG_IOL
+struct xmit_frame	*rtw_IOL_accquire_xmit_frame(ADAPTER *adapter)
+{
+	struct xmit_frame	*xmit_frame;
+	struct xmit_buf	*xmitbuf;
+	struct pkt_attrib	*pattrib;
+	struct xmit_priv	*pxmitpriv = &(adapter->xmitpriv);
+
+#if 1
+	xmit_frame = rtw_alloc_xmitframe(pxmitpriv);
+	if (xmit_frame == NULL) {
+		RTW_INFO("%s rtw_alloc_xmitframe return null\n", __FUNCTION__);
+		goto exit;
+	}
+
+	xmitbuf = rtw_alloc_xmitbuf(pxmitpriv);
+	if (xmitbuf == NULL) {
+		RTW_INFO("%s rtw_alloc_xmitbuf return null\n", __FUNCTION__);
+		rtw_free_xmitframe(pxmitpriv, xmit_frame);
+		xmit_frame = NULL;
+		goto exit;
+	}
+
+	xmit_frame->frame_tag = MGNT_FRAMETAG;
+	xmit_frame->pxmitbuf = xmitbuf;
+	xmit_frame->buf_addr = xmitbuf->pbuf;
+	xmitbuf->priv_data = xmit_frame;
+
+	pattrib = &xmit_frame->attrib;
+	update_mgntframe_attrib(adapter, pattrib);
+	pattrib->qsel = QSLT_BEACON;/* Beacon	 */
+	pattrib->subtype = WIFI_BEACON;
+	pattrib->pktlen = pattrib->last_txcmdsz = 0;
+
+#else
+	xmit_frame = alloc_mgtxmitframe(pxmitpriv);
+	if (xmit_frame == NULL)
+		RTW_INFO("%s alloc_mgtxmitframe return null\n", __FUNCTION__);
+	else {
+		pattrib = &xmit_frame->attrib;
+		update_mgntframe_attrib(adapter, pattrib);
+		pattrib->qsel = QSLT_BEACON;
+		pattrib->pktlen = pattrib->last_txcmdsz = 0;
+	}
+#endif
+
+exit:
+	return xmit_frame;
+}
+
+
+int rtw_IOL_append_cmds(struct xmit_frame *xmit_frame, u8 *IOL_cmds, u32 cmd_len)
+{
+	struct pkt_attrib	*pattrib = &xmit_frame->attrib;
+	u16 buf_offset;
+	u32 ori_len;
+
+	buf_offset = TXDESC_OFFSET;
+	ori_len = buf_offset + pattrib->pktlen;
+
+	/* check if the io_buf can accommodate new cmds */
+	if (ori_len + cmd_len + 8 > MAX_XMITBUF_SZ) {
+		RTW_INFO("%s %u is large than MAX_XMITBUF_SZ:%u, can't accommodate new cmds\n", __FUNCTION__
+			 , ori_len + cmd_len + 8, MAX_XMITBUF_SZ);
+		return _FAIL;
+	}
+
+	_rtw_memcpy(xmit_frame->buf_addr + buf_offset + pattrib->pktlen, IOL_cmds, cmd_len);
+	pattrib->pktlen += cmd_len;
+	pattrib->last_txcmdsz += cmd_len;
+
+	/* RTW_INFO("%s ori:%u + cmd_len:%u = %u\n", __FUNCTION__, ori_len, cmd_len, buf_offset+pattrib->pktlen); */
+
+	return _SUCCESS;
+}
+
+bool rtw_IOL_applied(ADAPTER *adapter)
+{
+	if (1 == adapter->registrypriv.fw_iol)
+		return _TRUE;
+
+#ifdef CONFIG_USB_HCI
+	if ((2 == adapter->registrypriv.fw_iol) && (IS_FULL_SPEED_USB(adapter)))
+		return _TRUE;
+#endif
+
+	return _FALSE;
+}
+
+int rtw_IOL_exec_cmds_sync(ADAPTER *adapter, struct xmit_frame *xmit_frame, u32 max_wating_ms, u32 bndy_cnt)
+{
+	return rtw_hal_iol_cmd(adapter, xmit_frame, max_wating_ms, bndy_cnt);
+}
+
+#ifdef CONFIG_IOL_NEW_GENERATION
+int rtw_IOL_append_LLT_cmd(struct xmit_frame *xmit_frame, u8 page_boundary)
+{
+	return _SUCCESS;
+}
+int _rtw_IOL_append_WB_cmd(struct xmit_frame *xmit_frame, u16 addr, u8 value, u8 mask)
+{
+	struct ioreg_cfg cmd = {8, IOREG_CMD_WB_REG, 0x0, 0x0, 0x0};
+
+	/* RTW_PUT_LE16((u8*)&cmd.address, addr);	 */
+	/* RTW_PUT_LE32((u8*)&cmd.value, (u32)value);	 */
+	cmd.address = cpu_to_le16(addr);
+	cmd.data = cpu_to_le32(value);
+
+	if (mask != 0xFF) {
+		cmd.length = 12;
+		/* RTW_PUT_LE32((u8*)&cmd.mask, (u32)mask);	 */
+		cmd.mask = cpu_to_le32(mask);
+	}
+
+	/* RTW_INFO("%s addr:0x%04x,value:0x%08x,mask:0x%08x\n", __FUNCTION__, addr,value,mask); */
+
+	return rtw_IOL_append_cmds(xmit_frame, (u8 *)&cmd, cmd.length);
+
+}
+int _rtw_IOL_append_WW_cmd(struct xmit_frame *xmit_frame, u16 addr, u16 value, u16 mask)
+{
+	struct ioreg_cfg cmd = {8, IOREG_CMD_WW_REG, 0x0, 0x0, 0x0};
+
+	/* RTW_PUT_LE16((u8*)&cmd.address, addr);	 */
+	/* RTW_PUT_LE32((u8*)&cmd.value, (u32)value);	 */
+	cmd.address = cpu_to_le16(addr);
+	cmd.data = cpu_to_le32(value);
+
+	if (mask != 0xFFFF) {
+		cmd.length = 12;
+		/* RTW_PUT_LE32((u8*)&cmd.mask, (u32)mask);	 */
+		cmd.mask =  cpu_to_le32(mask);
+	}
+
+	/* RTW_INFO("%s addr:0x%04x,value:0x%08x,mask:0x%08x\n", __FUNCTION__, addr,value,mask); */
+
+	return rtw_IOL_append_cmds(xmit_frame, (u8 *)&cmd, cmd.length);
+
+}
+int _rtw_IOL_append_WD_cmd(struct xmit_frame *xmit_frame, u16 addr, u32 value, u32 mask)
+{
+	struct ioreg_cfg cmd = {8, IOREG_CMD_WD_REG, 0x0, 0x0, 0x0};
+
+	/* RTW_PUT_LE16((u8*)&cmd.address, addr);	 */
+	/* RTW_PUT_LE32((u8*)&cmd.value, (u32)value);	 */
+	cmd.address = cpu_to_le16(addr);
+	cmd.data = cpu_to_le32(value);
+
+	if (mask != 0xFFFFFFFF) {
+		cmd.length = 12;
+		/* RTW_PUT_LE32((u8*)&cmd.mask, (u32)mask);	 */
+		cmd.mask =  cpu_to_le32(mask);
+	}
+
+	/* RTW_INFO("%s addr:0x%04x,value:0x%08x,mask:0x%08x\n", __FU2NCTION__, addr,value,mask); */
+
+	return rtw_IOL_append_cmds(xmit_frame, (u8 *)&cmd, cmd.length);
+
+}
+
+int _rtw_IOL_append_WRF_cmd(struct xmit_frame *xmit_frame, u8 rf_path, u16 addr, u32 value, u32 mask)
+{
+	struct ioreg_cfg cmd = {8, IOREG_CMD_W_RF, 0x0, 0x0, 0x0};
+
+	/* RTW_PUT_LE16((u8*)&cmd.address, addr);	 */
+	/* RTW_PUT_LE32((u8*)&cmd.value, (u32)value);	 */
+	cmd.address = (rf_path << 8) | ((addr) & 0xFF);
+	cmd.data = cpu_to_le32(value);
+
+	if (mask != 0x000FFFFF) {
+		cmd.length = 12;
+		/* RTW_PUT_LE32((u8*)&cmd.mask, (u32)mask);	 */
+		cmd.mask =  cpu_to_le32(mask);
+	}
+
+	/* RTW_INFO("%s rf_path:0x%02x addr:0x%04x,value:0x%08x,mask:0x%08x\n", __FU2NCTION__,rf_path, addr,value,mask); */
+
+	return rtw_IOL_append_cmds(xmit_frame, (u8 *)&cmd, cmd.length);
+
+}
+
+
+
+int rtw_IOL_append_DELAY_US_cmd(struct xmit_frame *xmit_frame, u16 us)
+{
+	struct ioreg_cfg cmd = {4, IOREG_CMD_DELAY_US, 0x0, 0x0, 0x0};
+	/* RTW_PUT_LE16((u8*)&cmd.address, us);	 */
+	cmd.address = cpu_to_le16(us);
+
+	/* RTW_INFO("%s %u\n", __FUNCTION__, us); */
+	return rtw_IOL_append_cmds(xmit_frame, (u8 *)&cmd, 4);
+}
+
+int rtw_IOL_append_DELAY_MS_cmd(struct xmit_frame *xmit_frame, u16 ms)
+{
+	struct ioreg_cfg cmd = {4, IOREG_CMD_DELAY_US, 0x0, 0x0, 0x0};
+
+	/* RTW_PUT_LE16((u8*)&cmd.address, ms);	 */
+	cmd.address = cpu_to_le16(ms);
+
+	/* RTW_INFO("%s %u\n", __FUNCTION__, ms); */
+	return rtw_IOL_append_cmds(xmit_frame, (u8 *)&cmd, 4);
+}
+int rtw_IOL_append_END_cmd(struct xmit_frame *xmit_frame)
+{
+	struct ioreg_cfg cmd = {4, IOREG_CMD_END, 0xFFFF, 0xFF, 0x0};
+	return rtw_IOL_append_cmds(xmit_frame, (u8 *)&cmd, 4);
+
+}
+
+u8 rtw_IOL_cmd_boundary_handle(struct xmit_frame *pxmit_frame)
+{
+	u8 is_cmd_bndy = _FALSE;
+	if (((pxmit_frame->attrib.pktlen + 32) % 256) + 8 >= 256) {
+		rtw_IOL_append_END_cmd(pxmit_frame);
+		pxmit_frame->attrib.pktlen = ((((pxmit_frame->attrib.pktlen + 32) / 256) + 1) * 256);
+
+		/* printk("==> %s, pktlen(%d)\n",__FUNCTION__,pxmit_frame->attrib.pktlen); */
+		pxmit_frame->attrib.last_txcmdsz = pxmit_frame->attrib.pktlen;
+		is_cmd_bndy = _TRUE;
+	}
+	return is_cmd_bndy;
+}
+
+void rtw_IOL_cmd_buf_dump(ADAPTER *Adapter, int buf_len, u8 *pbuf)
+{
+	int i;
+	int j = 1;
+
+	printk("###### %s ######\n", __FUNCTION__);
+	for (i = 0; i < buf_len; i++) {
+		printk("%02x-", *(pbuf + i));
+
+		if (j % 32 == 0)
+			printk("\n");
+		j++;
+	}
+	printk("\n");
+	printk("============= ioreg_cmd len = %d ===============\n", buf_len);
+}
+
+
+#else /* CONFIG_IOL_NEW_GENERATION */
+int rtw_IOL_append_LLT_cmd(struct xmit_frame *xmit_frame, u8 page_boundary)
+{
+	IOL_CMD cmd = {0x0, IOL_CMD_LLT, 0x0, 0x0};
+
+	RTW_PUT_BE32((u8 *)&cmd.value, (u32)page_boundary);
+
+	return rtw_IOL_append_cmds(xmit_frame, (u8 *)&cmd, 8);
+}
+
+int _rtw_IOL_append_WB_cmd(struct xmit_frame *xmit_frame, u16 addr, u8 value)
+{
+	IOL_CMD cmd = {0x0, IOL_CMD_WB_REG, 0x0, 0x0};
+
+	RTW_PUT_BE16((u8 *)&cmd.address, (u16)addr);
+	RTW_PUT_BE32((u8 *)&cmd.value, (u32)value);
+
+	return rtw_IOL_append_cmds(xmit_frame, (u8 *)&cmd, 8);
+}
+
+int _rtw_IOL_append_WW_cmd(struct xmit_frame *xmit_frame, u16 addr, u16 value)
+{
+	IOL_CMD cmd = {0x0, IOL_CMD_WW_REG, 0x0, 0x0};
+
+	RTW_PUT_BE16((u8 *)&cmd.address, (u16)addr);
+	RTW_PUT_BE32((u8 *)&cmd.value, (u32)value);
+
+	return rtw_IOL_append_cmds(xmit_frame, (u8 *)&cmd, 8);
+}
+
+int _rtw_IOL_append_WD_cmd(struct xmit_frame *xmit_frame, u16 addr, u32 value)
+{
+	IOL_CMD cmd = {0x0, IOL_CMD_WD_REG, 0x0, 0x0};
+	u8 *pos = (u8 *)&cmd;
+
+	RTW_PUT_BE16((u8 *)&cmd.address, (u16)addr);
+	RTW_PUT_BE32((u8 *)&cmd.value, (u32)value);
+
+	return rtw_IOL_append_cmds(xmit_frame, (u8 *)&cmd, 8);
+}
+
+#ifdef DBG_IO
+int dbg_rtw_IOL_append_WB_cmd(struct xmit_frame *xmit_frame, u16 addr, u8 value, const char *caller, const int line)
+{
+	if (match_write_sniff_ranges(addr, 1))
+		RTW_INFO("DBG_IO %s:%d IOL_WB(0x%04x, 0x%02x)\n", caller, line, addr, value);
+
+	return _rtw_IOL_append_WB_cmd(xmit_frame, addr, value);
+}
+
+int dbg_rtw_IOL_append_WW_cmd(struct xmit_frame *xmit_frame, u16 addr, u16 value, const char *caller, const int line)
+{
+	if (match_write_sniff_ranges(addr, 2))
+		RTW_INFO("DBG_IO %s:%d IOL_WW(0x%04x, 0x%04x)\n", caller, line, addr, value);
+
+	return _rtw_IOL_append_WW_cmd(xmit_frame, addr, value);
+}
+
+int dbg_rtw_IOL_append_WD_cmd(struct xmit_frame *xmit_frame, u16 addr, u32 value, const char *caller, const int line)
+{
+	if (match_write_sniff_ranges(addr, 4))
+		RTW_INFO("DBG_IO %s:%d IOL_WD(0x%04x, 0x%08x)\n", caller, line, addr, value);
+
+	return _rtw_IOL_append_WD_cmd(xmit_frame, addr, value);
+}
+#endif
+
+int rtw_IOL_append_DELAY_US_cmd(struct xmit_frame *xmit_frame, u16 us)
+{
+	IOL_CMD cmd = {0x0, IOL_CMD_DELAY_US, 0x0, 0x0};
+
+	RTW_PUT_BE32((u8 *)&cmd.value, (u32)us);
+
+	/* RTW_INFO("%s %u\n", __FUNCTION__, us); */
+
+	return rtw_IOL_append_cmds(xmit_frame, (u8 *)&cmd, 8);
+}
+
+int rtw_IOL_append_DELAY_MS_cmd(struct xmit_frame *xmit_frame, u16 ms)
+{
+	IOL_CMD cmd = {0x0, IOL_CMD_DELAY_MS, 0x0, 0x0};
+
+	RTW_PUT_BE32((u8 *)&cmd.value, (u32)ms);
+
+	/* RTW_INFO("%s %u\n", __FUNCTION__, ms); */
+
+	return rtw_IOL_append_cmds(xmit_frame, (u8 *)&cmd, 8);
+}
+
+int rtw_IOL_append_END_cmd(struct xmit_frame *xmit_frame)
+{
+	IOL_CMD end_cmd = {0x0, IOL_CMD_END, 0x0, 0x0};
+
+
+	return rtw_IOL_append_cmds(xmit_frame, (u8 *)&end_cmd, 8);
+
+}
+
+int rtw_IOL_exec_cmd_array_sync(PADAPTER adapter, u8 *IOL_cmds, u32 cmd_num, u32 max_wating_ms)
+{
+	struct xmit_frame	*xmit_frame;
+
+	xmit_frame = rtw_IOL_accquire_xmit_frame(adapter);
+	if (xmit_frame == NULL)
+		return _FAIL;
+
+	if (rtw_IOL_append_cmds(xmit_frame, IOL_cmds, cmd_num << 3) == _FAIL)
+		return _FAIL;
+
+	return rtw_IOL_exec_cmds_sync(adapter, xmit_frame, max_wating_ms, 0);
+}
+
+int rtw_IOL_exec_empty_cmds_sync(ADAPTER *adapter, u32 max_wating_ms)
+{
+	IOL_CMD end_cmd = {0x0, IOL_CMD_END, 0x0, 0x0};
+	return rtw_IOL_exec_cmd_array_sync(adapter, (u8 *)&end_cmd, 1, max_wating_ms);
+}
+#endif /* CONFIG_IOL_NEW_GENERATION */
+
+
+
+
+#endif /* CONFIG_IOL */

core/rtw_mem.c

@@ -0,0 +1,114 @@
+
+#include <drv_types.h>
+#include <rtw_mem.h>
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("Realtek Wireless Lan Driver");
+MODULE_AUTHOR("Realtek Semiconductor Corp.");
+MODULE_VERSION("DRIVERVERSION");
+
+struct sk_buff_head rtk_skb_mem_q;
+struct u8 *rtk_buf_mem[NR_RECVBUFF];
+
+struct u8	*rtw_get_buf_premem(int index)
+{
+	printk("%s, rtk_buf_mem index : %d\n", __func__, index);
+	return rtk_buf_mem[index];
+}
+
+u16 rtw_rtkm_get_buff_size(void)
+{
+	return MAX_RTKM_RECVBUF_SZ;
+}
+EXPORT_SYMBOL(rtw_rtkm_get_buff_size);
+
+u8 rtw_rtkm_get_nr_recv_skb(void)
+{
+	return MAX_RTKM_NR_PREALLOC_RECV_SKB;
+}
+EXPORT_SYMBOL(rtw_rtkm_get_nr_recv_skb);
+
+struct sk_buff *rtw_alloc_skb_premem(u16 in_size)
+{
+	struct sk_buff *skb = NULL;
+
+	if (in_size > MAX_RTKM_RECVBUF_SZ) {
+		pr_info("warning %s: driver buffer size(%d) > rtkm buffer size(%d)\n", __func__, in_size, MAX_RTKM_RECVBUF_SZ);
+		WARN_ON(1);
+		return skb;
+	}
+
+	skb = skb_dequeue(&rtk_skb_mem_q);
+
+	printk("%s, rtk_skb_mem_q len : %d\n", __func__, skb_queue_len(&rtk_skb_mem_q));
+
+	return skb;
+}
+EXPORT_SYMBOL(rtw_alloc_skb_premem);
+
+int rtw_free_skb_premem(struct sk_buff *pskb)
+{
+	if (!pskb)
+		return -1;
+
+	if (skb_queue_len(&rtk_skb_mem_q) >= MAX_RTKM_NR_PREALLOC_RECV_SKB)
+		return -1;
+
+	skb_queue_tail(&rtk_skb_mem_q, pskb);
+
+	printk("%s, rtk_skb_mem_q len : %d\n", __func__, skb_queue_len(&rtk_skb_mem_q));
+
+	return 0;
+}
+EXPORT_SYMBOL(rtw_free_skb_premem);
+
+static int __init rtw_mem_init(void)
+{
+	int i;
+	SIZE_PTR tmpaddr = 0;
+	SIZE_PTR alignment = 0;
+	struct sk_buff *pskb = NULL;
+
+	printk("%s\n", __func__);
+	pr_info("MAX_RTKM_NR_PREALLOC_RECV_SKB: %d\n", MAX_RTKM_NR_PREALLOC_RECV_SKB);
+	pr_info("MAX_RTKM_RECVBUF_SZ: %d\n", MAX_RTKM_RECVBUF_SZ);
+
+#ifdef CONFIG_USE_USB_BUFFER_ALLOC_RX
+	for (i = 0; i < NR_RECVBUFF; i++)
+		rtk_buf_mem[i] = usb_buffer_alloc(dev, size, (in_interrupt() ? GFP_ATOMIC : GFP_KERNEL), dma);
+#endif /* CONFIG_USE_USB_BUFFER_ALLOC_RX */
+
+	skb_queue_head_init(&rtk_skb_mem_q);
+
+	for (i = 0; i < MAX_RTKM_NR_PREALLOC_RECV_SKB; i++) {
+		pskb = __dev_alloc_skb(MAX_RTKM_RECVBUF_SZ + RECVBUFF_ALIGN_SZ, in_interrupt() ? GFP_ATOMIC : GFP_KERNEL);
+		if (pskb) {
+			tmpaddr = (SIZE_PTR)pskb->data;
+			alignment = tmpaddr & (RECVBUFF_ALIGN_SZ - 1);
+			skb_reserve(pskb, (RECVBUFF_ALIGN_SZ - alignment));
+
+			skb_queue_tail(&rtk_skb_mem_q, pskb);
+		} else
+			printk("%s, alloc skb memory fail!\n", __func__);
+
+		pskb = NULL;
+	}
+
+	printk("%s, rtk_skb_mem_q len : %d\n", __func__, skb_queue_len(&rtk_skb_mem_q));
+
+	return 0;
+
+}
+
+static void __exit rtw_mem_exit(void)
+{
+	if (skb_queue_len(&rtk_skb_mem_q))
+		printk("%s, rtk_skb_mem_q len : %d\n", __func__, skb_queue_len(&rtk_skb_mem_q));
+
+	skb_queue_purge(&rtk_skb_mem_q);
+
+	printk("%s\n", __func__);
+}
+
+module_init(rtw_mem_init);
+module_exit(rtw_mem_exit);

core/rtw_odm.c

@@ -0,0 +1,577 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2013 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * 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
+ *
+ *
+ ******************************************************************************/
+
+#include <rtw_odm.h>
+#include <hal_data.h>
+
+const char *odm_comp_str[] = {
+	/* BIT0 */"ODM_COMP_DIG",
+	/* BIT1 */"ODM_COMP_RA_MASK",
+	/* BIT2 */"ODM_COMP_DYNAMIC_TXPWR",
+	/* BIT3 */"ODM_COMP_FA_CNT",
+	/* BIT4 */"ODM_COMP_RSSI_MONITOR",
+	/* BIT5 */"ODM_COMP_SNIFFER",
+	/* BIT6 */"ODM_COMP_ANT_DIV",
+	/* BIT7 */"ODM_COMP_DFS",
+	/* BIT8 */"ODM_COMP_NOISY_DETECT",
+	/* BIT9 */"ODM_COMP_RATE_ADAPTIVE",
+	/* BIT10 */"ODM_COMP_PATH_DIV",
+	/* BIT11 */"ODM_COMP_CCX",
+	/* BIT12 */"ODM_COMP_DYNAMIC_PRICCA",
+	/* BIT13 */NULL,
+	/* BIT14 */"ODM_COMP_MP",
+	/* BIT15 */"ODM_COMP_CFO_TRACKING",
+	/* BIT16 */"ODM_COMP_ACS",
+	/* BIT17 */"PHYDM_COMP_ADAPTIVITY",
+	/* BIT18 */"PHYDM_COMP_RA_DBG",
+	/* BIT19 */"PHYDM_COMP_TXBF",
+	/* BIT20 */"ODM_COMP_EDCA_TURBO",
+	/* BIT21 */"ODM_COMP_EARLY_MODE",
+	/* BIT22 */"ODM_FW_DEBUG_TRACE",
+	/* BIT23 */NULL,
+	/* BIT24 */"ODM_COMP_TX_PWR_TRACK",
+	/* BIT25 */"ODM_COMP_RX_GAIN_TRACK",
+	/* BIT26 */"ODM_COMP_CALIBRATION",
+	/* BIT27 */NULL,
+	/* BIT28 */"ODM_PHY_CONFIG",
+	/* BIT29 */"ODM_COMP_INIT",
+	/* BIT30 */"ODM_COMP_COMMON",
+};
+
+#define RTW_ODM_COMP_MAX 31
+
+const char *odm_ability_str[] = {
+	/* BIT0 */"ODM_BB_DIG",
+	/* BIT1 */"ODM_BB_RA_MASK",
+	/* BIT2 */"ODM_BB_DYNAMIC_TXPWR",
+	/* BIT3 */"ODM_BB_FA_CNT",
+	/* BIT4 */"ODM_BB_RSSI_MONITOR",
+	/* BIT5 */"ODM_BB_CCK_PD",
+	/* BIT6 */"ODM_BB_ANT_DIV",
+	/* BIT7 */NULL,
+	/* BIT8 */"ODM_BB_PWR_TRAIN",
+	/* BIT9 */"ODM_BB_RATE_ADAPTIVE",
+	/* BIT10 */"ODM_BB_PATH_DIV",
+	/* BIT11 */NULL,
+	/* BIT12 */NULL,
+	/* BIT13 */"ODM_BB_ADAPTIVITY",
+	/* BIT14 */"ODM_BB_CFO_TRACKING",
+	/* BIT15 */"ODM_BB_NHM_CNT",
+	/* BIT16 */"ODM_BB_PRIMARY_CCA",
+	/* BIT17 */"ODM_BB_TXBF",
+	/* BIT18 */"ODM_BB_DYNAMIC_ARFR",
+	/* BIT19 */NULL,
+	/* BIT20 */"ODM_MAC_EDCA_TURBO",
+	/* BIT21 */"ODM_MAC_EARLY_MODE",
+	/* BIT22 */NULL,
+	/* BIT23 */NULL,
+	/* BIT24 */"ODM_RF_TX_PWR_TRACK",
+	/* BIT25 */"ODM_RF_RX_GAIN_TRACK",
+	/* BIT26 */"ODM_RF_CALIBRATION",
+};
+
+#define RTW_ODM_ABILITY_MAX 27
+
+const char *odm_dbg_level_str[] = {
+	NULL,
+	"ODM_DBG_OFF",
+	"ODM_DBG_SERIOUS",
+	"ODM_DBG_WARNING",
+	"ODM_DBG_LOUD",
+	"ODM_DBG_TRACE",
+};
+
+#define RTW_ODM_DBG_LEVEL_NUM 6
+
+void rtw_odm_dbg_comp_msg(void *sel, _adapter *adapter)
+{
+	HAL_DATA_TYPE *pHalData = GET_HAL_DATA(adapter);
+	DM_ODM_T *odm = &pHalData->odmpriv;
+	int cnt = 0;
+	u64 dbg_comp = 0;
+	int i;
+
+	rtw_hal_get_odm_var(adapter, HAL_ODM_DBG_FLAG, &dbg_comp, NULL);
+
+	RTW_PRINT_SEL(sel, "odm.DebugComponents = 0x%016llx\n", dbg_comp);
+	for (i = 0; i < RTW_ODM_COMP_MAX; i++) {
+		if (odm_comp_str[i])
+			RTW_PRINT_SEL(sel, "%cBIT%-2d %s\n",
+				(BIT0 & (dbg_comp >> i)) ? '+' : ' ', i, odm_comp_str[i]);
+	}
+}
+
+inline void rtw_odm_dbg_comp_set(_adapter *adapter, u64 comps)
+{
+	rtw_hal_set_odm_var(adapter, HAL_ODM_DBG_FLAG, &comps, _FALSE);
+}
+
+void rtw_odm_dbg_level_msg(void *sel, _adapter *adapter)
+{
+	HAL_DATA_TYPE *pHalData = GET_HAL_DATA(adapter);
+	DM_ODM_T *odm = &pHalData->odmpriv;
+	int cnt = 0;
+	u32 dbg_level = 0;
+	int i;
+
+	rtw_hal_get_odm_var(adapter, HAL_ODM_DBG_LEVEL, &dbg_level, NULL);
+	RTW_PRINT_SEL(sel, "odm.DebugLevel = %u\n", dbg_level);
+	for (i = 0; i < RTW_ODM_DBG_LEVEL_NUM; i++) {
+		if (odm_dbg_level_str[i])
+			RTW_PRINT_SEL(sel, "%u %s\n", i, odm_dbg_level_str[i]);
+	}
+}
+
+inline void rtw_odm_dbg_level_set(_adapter *adapter, u32 level)
+{
+	rtw_hal_set_odm_var(adapter, HAL_ODM_DBG_LEVEL, &level, _FALSE);
+}
+
+void rtw_odm_ability_msg(void *sel, _adapter *adapter)
+{
+	HAL_DATA_TYPE *pHalData = GET_HAL_DATA(adapter);
+	DM_ODM_T *odm = &pHalData->odmpriv;
+	int cnt = 0;
+	u32 ability = 0;
+	int i;
+
+	ability = rtw_phydm_ability_get(adapter);
+	RTW_PRINT_SEL(sel, "odm.SupportAbility = 0x%08x\n", ability);
+	for (i = 0; i < RTW_ODM_ABILITY_MAX; i++) {
+		if (odm_ability_str[i])
+			RTW_PRINT_SEL(sel, "%cBIT%-2d %s\n",
+				(BIT0 << i) & ability ? '+' : ' ', i, odm_ability_str[i]);
+	}
+}
+
+inline void rtw_odm_ability_set(_adapter *adapter, u32 ability)
+{
+	rtw_phydm_ability_set(adapter, ability);
+}
+
+/* set ODM_CMNINFO_IC_TYPE based on chip_type */
+void rtw_odm_init_ic_type(_adapter *adapter)
+{
+	HAL_DATA_TYPE *hal_data = GET_HAL_DATA(adapter);
+	DM_ODM_T *odm = &hal_data->odmpriv;
+	u4Byte ic_type = chip_type_to_odm_ic_type(rtw_get_chip_type(adapter));
+
+	rtw_warn_on(!ic_type);
+
+	ODM_CmnInfoInit(odm, ODM_CMNINFO_IC_TYPE, ic_type);
+}
+
+inline void rtw_odm_set_force_igi_lb(_adapter *adapter, u8 lb)
+{
+	HAL_DATA_TYPE *hal_data = GET_HAL_DATA(adapter);
+
+	hal_data->u1ForcedIgiLb = lb;
+}
+
+inline u8 rtw_odm_get_force_igi_lb(_adapter *adapter)
+{
+	HAL_DATA_TYPE *hal_data = GET_HAL_DATA(adapter);
+
+	return hal_data->u1ForcedIgiLb;
+}
+
+void rtw_odm_adaptivity_ver_msg(void *sel, _adapter *adapter)
+{
+	RTW_PRINT_SEL(sel, "ADAPTIVITY_VERSION "ADAPTIVITY_VERSION"\n");
+}
+
+#define RTW_ADAPTIVITY_EN_DISABLE 0
+#define RTW_ADAPTIVITY_EN_ENABLE 1
+
+void rtw_odm_adaptivity_en_msg(void *sel, _adapter *adapter)
+{
+	struct registry_priv *regsty = &adapter->registrypriv;
+	struct mlme_priv *mlme = &adapter->mlmepriv;
+	HAL_DATA_TYPE *hal_data = GET_HAL_DATA(adapter);
+	DM_ODM_T *odm = &hal_data->odmpriv;
+
+	RTW_PRINT_SEL(sel, "RTW_ADAPTIVITY_EN_");
+
+	if (regsty->adaptivity_en == RTW_ADAPTIVITY_EN_DISABLE)
+		_RTW_PRINT_SEL(sel, "DISABLE\n");
+	else if (regsty->adaptivity_en == RTW_ADAPTIVITY_EN_ENABLE)
+		_RTW_PRINT_SEL(sel, "ENABLE\n");
+	else
+		_RTW_PRINT_SEL(sel, "INVALID\n");
+}
+
+#define RTW_ADAPTIVITY_MODE_NORMAL 0
+#define RTW_ADAPTIVITY_MODE_CARRIER_SENSE 1
+
+void rtw_odm_adaptivity_mode_msg(void *sel, _adapter *adapter)
+{
+	struct registry_priv *regsty = &adapter->registrypriv;
+
+	RTW_PRINT_SEL(sel, "RTW_ADAPTIVITY_MODE_");
+
+	if (regsty->adaptivity_mode == RTW_ADAPTIVITY_MODE_NORMAL)
+		_RTW_PRINT_SEL(sel, "NORMAL\n");
+	else if (regsty->adaptivity_mode == RTW_ADAPTIVITY_MODE_CARRIER_SENSE)
+		_RTW_PRINT_SEL(sel, "CARRIER_SENSE\n");
+	else
+		_RTW_PRINT_SEL(sel, "INVALID\n");
+}
+
+#define RTW_ADAPTIVITY_DML_DISABLE 0
+#define RTW_ADAPTIVITY_DML_ENABLE 1
+
+void rtw_odm_adaptivity_dml_msg(void *sel, _adapter *adapter)
+{
+	struct registry_priv *regsty = &adapter->registrypriv;
+
+	RTW_PRINT_SEL(sel, "RTW_ADAPTIVITY_DML_");
+
+	if (regsty->adaptivity_dml == RTW_ADAPTIVITY_DML_DISABLE)
+		_RTW_PRINT_SEL(sel, "DISABLE\n");
+	else if (regsty->adaptivity_dml == RTW_ADAPTIVITY_DML_ENABLE)
+		_RTW_PRINT_SEL(sel, "ENABLE\n");
+	else
+		_RTW_PRINT_SEL(sel, "INVALID\n");
+}
+
+void rtw_odm_adaptivity_dc_backoff_msg(void *sel, _adapter *adapter)
+{
+	struct registry_priv *regsty = &adapter->registrypriv;
+
+	RTW_PRINT_SEL(sel, "RTW_ADAPTIVITY_DC_BACKOFF:%u\n", regsty->adaptivity_dc_backoff);
+}
+
+void rtw_odm_adaptivity_config_msg(void *sel, _adapter *adapter)
+{
+	rtw_odm_adaptivity_ver_msg(sel, adapter);
+	rtw_odm_adaptivity_en_msg(sel, adapter);
+	rtw_odm_adaptivity_mode_msg(sel, adapter);
+	rtw_odm_adaptivity_dml_msg(sel, adapter);
+	rtw_odm_adaptivity_dc_backoff_msg(sel, adapter);
+}
+
+bool rtw_odm_adaptivity_needed(_adapter *adapter)
+{
+	struct registry_priv *regsty = &adapter->registrypriv;
+	struct mlme_priv *mlme = &adapter->mlmepriv;
+	bool ret = _FALSE;
+
+	if (regsty->adaptivity_en == RTW_ADAPTIVITY_EN_ENABLE)
+		ret = _TRUE;
+
+	return ret;
+}
+
+void rtw_odm_adaptivity_parm_msg(void *sel, _adapter *adapter)
+{
+	HAL_DATA_TYPE *pHalData = GET_HAL_DATA(adapter);
+	DM_ODM_T *odm = &pHalData->odmpriv;
+
+	rtw_odm_adaptivity_config_msg(sel, adapter);
+
+	RTW_PRINT_SEL(sel, "%10s %16s %16s %22s %12s\n"
+		, "TH_L2H_ini", "TH_EDCCA_HL_diff", "TH_L2H_ini_mode2", "TH_EDCCA_HL_diff_mode2", "EDCCA_enable");
+	RTW_PRINT_SEL(sel, "0x%-8x %-16d 0x%-14x %-22d %-12d\n"
+		, (u8)odm->TH_L2H_ini
+		, odm->TH_EDCCA_HL_diff
+		, (u8)odm->TH_L2H_ini_mode2
+		, odm->TH_EDCCA_HL_diff_mode2
+		, odm->EDCCA_enable
+	);
+
+	RTW_PRINT_SEL(sel, "%15s %9s\n", "AdapEnableState", "Adap_Flag");
+	RTW_PRINT_SEL(sel, "%-15x %-9x\n"
+		, odm->Adaptivity_enable
+		, odm->adaptivity_flag
+	);
+}
+
+void rtw_odm_adaptivity_parm_set(_adapter *adapter, s8 TH_L2H_ini, s8 TH_EDCCA_HL_diff, s8 TH_L2H_ini_mode2, s8 TH_EDCCA_HL_diff_mode2, u8 EDCCA_enable)
+{
+	HAL_DATA_TYPE *pHalData = GET_HAL_DATA(adapter);
+	DM_ODM_T *odm = &pHalData->odmpriv;
+
+	odm->TH_L2H_ini = TH_L2H_ini;
+	odm->TH_EDCCA_HL_diff = TH_EDCCA_HL_diff;
+	odm->TH_L2H_ini_mode2 = TH_L2H_ini_mode2;
+	odm->TH_EDCCA_HL_diff_mode2 = TH_EDCCA_HL_diff_mode2;
+	odm->EDCCA_enable = EDCCA_enable;
+}
+
+void rtw_odm_get_perpkt_rssi(void *sel, _adapter *adapter)
+{
+	HAL_DATA_TYPE *hal_data = GET_HAL_DATA(adapter);
+	DM_ODM_T *odm = &(hal_data->odmpriv);
+
+	RTW_PRINT_SEL(sel, "RxRate = %s, RSSI_A = %d(%%), RSSI_B = %d(%%)\n",
+		      HDATA_RATE(odm->RxRate), odm->RSSI_A, odm->RSSI_B);
+}
+
+
+void rtw_odm_acquirespinlock(_adapter *adapter,	RT_SPINLOCK_TYPE type)
+{
+	PHAL_DATA_TYPE	pHalData = GET_HAL_DATA(adapter);
+	_irqL irqL;
+
+	switch (type) {
+	case RT_IQK_SPINLOCK:
+		_enter_critical_bh(&pHalData->IQKSpinLock, &irqL);
+	default:
+		break;
+	}
+}
+
+void rtw_odm_releasespinlock(_adapter *adapter,	RT_SPINLOCK_TYPE type)
+{
+	PHAL_DATA_TYPE	pHalData = GET_HAL_DATA(adapter);
+	_irqL irqL;
+
+	switch (type) {
+	case RT_IQK_SPINLOCK:
+		_exit_critical_bh(&pHalData->IQKSpinLock, &irqL);
+	default:
+		break;
+	}
+}
+
+#ifdef CONFIG_DFS_MASTER
+inline u8 rtw_odm_get_dfs_domain(_adapter *adapter)
+{
+	HAL_DATA_TYPE *hal_data = GET_HAL_DATA(adapter);
+	PDM_ODM_T pDM_Odm = &(hal_data->odmpriv);
+
+	return pDM_Odm->DFS_RegionDomain;
+}
+
+inline VOID rtw_odm_radar_detect_reset(_adapter *adapter)
+{
+	phydm_radar_detect_reset(GET_ODM(adapter));
+}
+
+inline VOID rtw_odm_radar_detect_disable(_adapter *adapter)
+{
+	phydm_radar_detect_disable(GET_ODM(adapter));
+}
+
+/* called after ch, bw is set */
+inline VOID rtw_odm_radar_detect_enable(_adapter *adapter)
+{
+	phydm_radar_detect_enable(GET_ODM(adapter));
+}
+
+inline BOOLEAN rtw_odm_radar_detect(_adapter *adapter)
+{
+	return phydm_radar_detect(GET_ODM(adapter));
+}
+#endif /* CONFIG_DFS_MASTER */
+
+void rtw_odm_parse_rx_phy_status_chinfo(union recv_frame *rframe, u8 *phys)
+{
+#ifndef DBG_RX_PHYSTATUS_CHINFO
+#define DBG_RX_PHYSTATUS_CHINFO 0
+#endif
+
+#if (ODM_PHY_STATUS_NEW_TYPE_SUPPORT == 1)
+	_adapter *adapter = rframe->u.hdr.adapter;
+	struct rx_pkt_attrib *attrib = &rframe->u.hdr.attrib;
+	HAL_DATA_TYPE *pHalData = GET_HAL_DATA(adapter);
+	PDM_ODM_T	pDM_Odm = &pHalData->odmpriv;
+	u8 *wlanhdr = get_recvframe_data(rframe);
+
+	if (pDM_Odm->SupportICType & ODM_IC_PHY_STATUE_NEW_TYPE) {
+		/*
+		* 8723D:
+		* type_0(CCK)
+		*     l_rxsc
+		*         is filled with primary channel SC, not real rxsc.
+		*         0:LSC, 1:USC
+		* type_1(OFDM)
+		*     rf_mode
+		*         RF bandwidth when RX
+		*     l_rxsc(legacy), ht_rxsc
+		*         see below RXSC N-series
+		* type_2(Not used)
+		*/
+		/*
+		* 8821C, 8822B:
+		* type_0(CCK)
+		*     l_rxsc
+		*         is filled with primary channel SC, not real rxsc.
+		*         0:LSC, 1:USC
+		* type_1(OFDM)
+		*     rf_mode
+		*         RF bandwidth when RX
+		*     l_rxsc(legacy), ht_rxsc
+		*         see below RXSC AC-series
+		* type_2(Not used)
+		*/
+
+		if ((*phys & 0xf) == 0) {
+			struct _Phy_Status_Rpt_Jaguar2_Type0 *phys_t0 = (struct _Phy_Status_Rpt_Jaguar2_Type0 *)phys;
+
+			if (DBG_RX_PHYSTATUS_CHINFO) {
+				RTW_PRINT("phys_t%u ta="MAC_FMT" %s, %s(band:%u, ch:%u, l_rxsc:%u)\n"
+					, *phys & 0xf
+					, MAC_ARG(get_ta(wlanhdr))
+					, is_broadcast_mac_addr(get_ra(wlanhdr)) ? "BC" : is_multicast_mac_addr(get_ra(wlanhdr)) ? "MC" : "UC"
+					, HDATA_RATE(attrib->data_rate)
+					, phys_t0->band, phys_t0->channel, phys_t0->rxsc
+				);
+			}
+
+		} else if ((*phys & 0xf) == 1) {
+			struct _Phy_Status_Rpt_Jaguar2_Type1 *phys_t1 = (struct _Phy_Status_Rpt_Jaguar2_Type1 *)phys;
+			u8 rxsc = (attrib->data_rate > DESC_RATE11M && attrib->data_rate < DESC_RATEMCS0) ? phys_t1->l_rxsc : phys_t1->ht_rxsc;
+			u8 pkt_cch = 0;
+			u8 pkt_bw = CHANNEL_WIDTH_20;
+
+			#if	ODM_IC_11N_SERIES_SUPPORT
+			if (pDM_Odm->SupportICType & ODM_IC_11N_SERIES) {
+				/* RXSC N-series */
+				#define RXSC_DUP	0
+				#define RXSC_LSC	1
+				#define RXSC_USC	2
+				#define RXSC_40M	3
+
+				static const s8 cch_offset_by_rxsc[4] = {0, -2, 2, 0};
+
+				if (phys_t1->rf_mode == 0) {
+					pkt_cch = phys_t1->channel;
+					pkt_bw = CHANNEL_WIDTH_20;
+				} else if (phys_t1->rf_mode == 1) {
+					if (rxsc == RXSC_LSC || rxsc == RXSC_USC) {
+						pkt_cch = phys_t1->channel + cch_offset_by_rxsc[rxsc];
+						pkt_bw = CHANNEL_WIDTH_20;
+					} else if (rxsc == RXSC_40M) {
+						pkt_cch = phys_t1->channel;
+						pkt_bw = CHANNEL_WIDTH_40;
+					}
+				} else
+					rtw_warn_on(1);
+
+				goto type1_end;
+			}
+			#endif /* ODM_IC_11N_SERIES_SUPPORT */
+
+			#if	ODM_IC_11AC_SERIES_SUPPORT
+			if (pDM_Odm->SupportICType & ODM_IC_11AC_SERIES) {
+				/* RXSC AC-series */
+				#define RXSC_DUP			0 /* 0: RX from all SC of current rf_mode */
+
+				#define RXSC_LL20M_OF_160M	8 /* 1~8: RX from 20MHz SC */
+				#define RXSC_L20M_OF_160M	6
+				#define RXSC_L20M_OF_80M	4
+				#define RXSC_L20M_OF_40M	2
+				#define RXSC_U20M_OF_40M	1
+				#define RXSC_U20M_OF_80M	3
+				#define RXSC_U20M_OF_160M	5
+				#define RXSC_UU20M_OF_160M	7
+
+				#define RXSC_L40M_OF_160M	12 /* 9~12: RX from 40MHz SC */
+				#define RXSC_L40M_OF_80M	10
+				#define RXSC_U40M_OF_80M	9
+				#define RXSC_U40M_OF_160M	11
+
+				#define RXSC_L80M_OF_160M	14 /* 13~14: RX from 80MHz SC */
+				#define RXSC_U80M_OF_160M	13
+
+				static const s8 cch_offset_by_rxsc[15] = {0, 2, -2, 6, -6, 10, -10, 14, -14, 4, -4, 12, -12, 8, -8};
+
+				if (phys_t1->rf_mode > 3) {
+					/* invalid rf_mode */
+					rtw_warn_on(1);
+					goto type1_end;
+				}
+
+				if (phys_t1->rf_mode == 0) {
+					/* RF 20MHz */
+					pkt_cch = phys_t1->channel;
+					pkt_bw = CHANNEL_WIDTH_20;
+					goto type1_end;
+				}
+
+				if (rxsc == 0) {
+					/* RF and RX with same BW */
+					if (attrib->data_rate >= DESC_RATEMCS0) {
+						pkt_cch = phys_t1->channel;
+						pkt_bw = phys_t1->rf_mode;
+					}
+					goto type1_end;
+				}
+
+				if ((phys_t1->rf_mode == 1 && rxsc >= 1 && rxsc <= 2) /* RF 40MHz, RX 20MHz */
+					|| (phys_t1->rf_mode == 2 && rxsc >= 1 && rxsc <= 4) /* RF 80MHz, RX 20MHz */
+					|| (phys_t1->rf_mode == 3 && rxsc >= 1 && rxsc <= 8) /* RF 160MHz, RX 20MHz */
+				) {
+					pkt_cch = phys_t1->channel + cch_offset_by_rxsc[rxsc];
+					pkt_bw = CHANNEL_WIDTH_20;
+				} else if ((phys_t1->rf_mode == 2 && rxsc >= 9 && rxsc <= 10) /* RF 80MHz, RX 40MHz */
+					|| (phys_t1->rf_mode == 3 && rxsc >= 9 && rxsc <= 12) /* RF 160MHz, RX 40MHz */
+				) {
+					if (attrib->data_rate >= DESC_RATEMCS0) {
+						pkt_cch = phys_t1->channel + cch_offset_by_rxsc[rxsc];
+						pkt_bw = CHANNEL_WIDTH_40;
+					}
+				} else if ((phys_t1->rf_mode == 3 && rxsc >= 13 && rxsc <= 14) /* RF 160MHz, RX 80MHz */
+				) {
+					if (attrib->data_rate >= DESC_RATEMCS0) {
+						pkt_cch = phys_t1->channel + cch_offset_by_rxsc[rxsc];
+						pkt_bw = CHANNEL_WIDTH_80;
+					}
+				} else
+					rtw_warn_on(1);
+
+			}
+			#endif /* ODM_IC_11AC_SERIES_SUPPORT */
+
+type1_end:
+			if (DBG_RX_PHYSTATUS_CHINFO) {
+				RTW_PRINT("phys_t%u ta="MAC_FMT" %s, %s(band:%u, ch:%u, rf_mode:%u, l_rxsc:%u, ht_rxsc:%u) => %u,%u\n"
+					, *phys & 0xf
+					, MAC_ARG(get_ta(wlanhdr))
+					, is_broadcast_mac_addr(get_ra(wlanhdr)) ? "BC" : is_multicast_mac_addr(get_ra(wlanhdr)) ? "MC" : "UC"
+					, HDATA_RATE(attrib->data_rate)
+					, phys_t1->band, phys_t1->channel, phys_t1->rf_mode, phys_t1->l_rxsc, phys_t1->ht_rxsc
+					, pkt_cch, pkt_bw
+				);
+			}
+
+			/* for now, only return cneter channel of 20MHz packet */
+			if (pkt_cch && pkt_bw == CHANNEL_WIDTH_20)
+				attrib->ch = pkt_cch;
+
+		} else {
+			struct _Phy_Status_Rpt_Jaguar2_Type2 *phys_t2 = (struct _Phy_Status_Rpt_Jaguar2_Type2 *)phys;
+
+			if (DBG_RX_PHYSTATUS_CHINFO) {
+				RTW_PRINT("phys_t%u ta="MAC_FMT" %s, %s(band:%u, ch:%u, l_rxsc:%u, ht_rxsc:%u)\n"
+					, *phys & 0xf
+					, MAC_ARG(get_ta(wlanhdr))
+					, is_broadcast_mac_addr(get_ra(wlanhdr)) ? "BC" : is_multicast_mac_addr(get_ra(wlanhdr)) ? "MC" : "UC"
+					, HDATA_RATE(attrib->data_rate)
+					, phys_t2->band, phys_t2->channel, phys_t2->l_rxsc, phys_t2->ht_rxsc
+				);
+			}
+		}
+	}
+#endif /* (ODM_PHY_STATUS_NEW_TYPE_SUPPORT == 1) */
+
+}
+

core/rtw_sdio.c

@@ -0,0 +1,98 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2015 - 2016 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * 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
+ *
+ ******************************************************************************/
+#define _RTW_SDIO_C_
+
+#include <drv_types.h>		/* struct dvobj_priv and etc. */
+#include <drv_types_sdio.h>	/* RTW_SDIO_ADDR_CMD52_GEN */
+
+/*
+ * Description:
+ *	Use SDIO cmd52 or cmd53 to read/write data
+ *
+ * Parameters:
+ *	d	pointer of device object(struct dvobj_priv)
+ *	addr	SDIO address, 17 bits
+ *	buf	buffer for I/O
+ *	len	length
+ *	write	0:read, 1:write
+ *	cmd52	0:cmd52, 1:cmd53
+ *
+ * Return:
+ *	_SUCCESS	I/O ok.
+ *	_FAIL		I/O fail.
+ */
+static u8 sdio_io(struct dvobj_priv *d, u32 addr, void *buf, size_t len, u8 write, u8 cmd52)
+{
+	int err;
+
+
+	if (cmd52)
+		addr = RTW_SDIO_ADDR_CMD52_GEN(addr);
+
+	if (write)
+		err = d->intf_ops->write(d, addr, buf, len, 0);
+	else
+		err = d->intf_ops->read(d, addr, buf, len, 0);
+	if (err) {
+		RTW_INFO("%s: [ERROR] %s FAIL! error(%d)\n",
+			 __FUNCTION__, write ? "write" : "read", err);
+		return _FAIL;
+	}
+
+	return _SUCCESS;
+}
+
+u8 rtw_sdio_read_cmd52(struct dvobj_priv *d, u32 addr, void *buf, size_t len)
+{
+	return sdio_io(d, addr, buf, len, 0, 1);
+}
+
+u8 rtw_sdio_read_cmd53(struct dvobj_priv *d, u32 addr, void *buf, size_t len)
+{
+	return sdio_io(d, addr, buf, len, 0, 0);
+}
+
+u8 rtw_sdio_write_cmd52(struct dvobj_priv *d, u32 addr, void *buf, size_t len)
+{
+	return sdio_io(d, addr, buf, len, 1, 1);
+}
+
+u8 rtw_sdio_write_cmd53(struct dvobj_priv *d, u32 addr, void *buf, size_t len)
+{
+	return sdio_io(d, addr, buf, len, 1, 0);
+}
+
+u8 rtw_sdio_f0_read(struct dvobj_priv *d, u32 addr, void *buf, size_t len)
+{
+	int err;
+	u8 ret;
+
+
+	ret = _SUCCESS;
+	addr = RTW_SDIO_ADDR_F0_GEN(addr);
+
+	err = d->intf_ops->read(d, addr, buf, len, 0);
+	if (err) {
+		RTW_INFO("%s: [ERROR] Read f0 register FAIL!\n", __FUNCTION__);
+		ret = _FAIL;
+	}
+
+
+	return ret;
+}

core/rtw_sreset.c

@@ -0,0 +1,346 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2012 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * 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
+ *
+ *
+ ******************************************************************************/
+
+#include <drv_types.h>
+#include <hal_data.h>
+#include <rtw_sreset.h>
+
+void sreset_init_value(_adapter *padapter)
+{
+#if defined(DBG_CONFIG_ERROR_DETECT)
+	HAL_DATA_TYPE	*pHalData = GET_HAL_DATA(padapter);
+	struct sreset_priv *psrtpriv = &pHalData->srestpriv;
+
+	_rtw_mutex_init(&psrtpriv->silentreset_mutex);
+	psrtpriv->silent_reset_inprogress = _FALSE;
+	psrtpriv->Wifi_Error_Status = WIFI_STATUS_SUCCESS;
+	psrtpriv->last_tx_time = 0;
+	psrtpriv->last_tx_complete_time = 0;
+#endif
+}
+void sreset_reset_value(_adapter *padapter)
+{
+#if defined(DBG_CONFIG_ERROR_DETECT)
+	HAL_DATA_TYPE	*pHalData = GET_HAL_DATA(padapter);
+	struct sreset_priv *psrtpriv = &pHalData->srestpriv;
+
+	psrtpriv->Wifi_Error_Status = WIFI_STATUS_SUCCESS;
+	psrtpriv->last_tx_time = 0;
+	psrtpriv->last_tx_complete_time = 0;
+#endif
+}
+
+u8 sreset_get_wifi_status(_adapter *padapter)
+{
+#if defined(DBG_CONFIG_ERROR_DETECT)
+	HAL_DATA_TYPE	*pHalData = GET_HAL_DATA(padapter);
+	struct sreset_priv *psrtpriv = &pHalData->srestpriv;
+
+	u8 status = WIFI_STATUS_SUCCESS;
+	u32 val32 = 0;
+	_irqL irqL;
+	if (psrtpriv->silent_reset_inprogress == _TRUE)
+		return status;
+	val32 = rtw_read32(padapter, REG_TXDMA_STATUS);
+	if (val32 == 0xeaeaeaea)
+		psrtpriv->Wifi_Error_Status = WIFI_IF_NOT_EXIST;
+	else if (val32 != 0) {
+		RTW_INFO("txdmastatu(%x)\n", val32);
+		psrtpriv->Wifi_Error_Status = WIFI_MAC_TXDMA_ERROR;
+	}
+
+	if (WIFI_STATUS_SUCCESS != psrtpriv->Wifi_Error_Status) {
+		RTW_INFO("==>%s error_status(0x%x)\n", __FUNCTION__, psrtpriv->Wifi_Error_Status);
+		status = (psrtpriv->Wifi_Error_Status & (~(USB_READ_PORT_FAIL | USB_WRITE_PORT_FAIL)));
+	}
+	RTW_INFO("==> %s wifi_status(0x%x)\n", __FUNCTION__, status);
+
+	/* status restore */
+	psrtpriv->Wifi_Error_Status = WIFI_STATUS_SUCCESS;
+
+	return status;
+#else
+	return WIFI_STATUS_SUCCESS;
+#endif
+}
+
+void sreset_set_wifi_error_status(_adapter *padapter, u32 status)
+{
+#if defined(DBG_CONFIG_ERROR_DETECT)
+	HAL_DATA_TYPE	*pHalData = GET_HAL_DATA(padapter);
+	pHalData->srestpriv.Wifi_Error_Status = status;
+#endif
+}
+
+void sreset_set_trigger_point(_adapter *padapter, s32 tgp)
+{
+#if defined(DBG_CONFIG_ERROR_DETECT)
+	HAL_DATA_TYPE	*pHalData = GET_HAL_DATA(padapter);
+	pHalData->srestpriv.dbg_trigger_point = tgp;
+#endif
+}
+
+bool sreset_inprogress(_adapter *padapter)
+{
+#if defined(DBG_CONFIG_ERROR_RESET)
+	HAL_DATA_TYPE	*pHalData = GET_HAL_DATA(padapter);
+	return pHalData->srestpriv.silent_reset_inprogress;
+#else
+	return _FALSE;
+#endif
+}
+
+void sreset_restore_security_station(_adapter *padapter)
+{
+	u8 EntryId = 0;
+	struct mlme_priv *mlmepriv = &padapter->mlmepriv;
+	struct sta_priv *pstapriv = &padapter->stapriv;
+	struct sta_info *psta;
+	struct security_priv *psecuritypriv = &(padapter->securitypriv);
+	struct mlme_ext_info	*pmlmeinfo = &padapter->mlmeextpriv.mlmext_info;
+
+	{
+		u8 val8;
+
+		if (pmlmeinfo->auth_algo == dot11AuthAlgrthm_8021X) {
+			val8 = 0xcc;
+#ifdef CONFIG_WAPI_SUPPORT
+		} else if (padapter->wapiInfo.bWapiEnable && pmlmeinfo->auth_algo == dot11AuthAlgrthm_WAPI) {
+			/* Disable TxUseDefaultKey, RxUseDefaultKey, RxBroadcastUseDefaultKey. */
+			val8 = 0x4c;
+#endif
+		} else
+			val8 = 0xcf;
+		rtw_hal_set_hwreg(padapter, HW_VAR_SEC_CFG, (u8 *)(&val8));
+	}
+
+#if 0
+	if ((padapter->securitypriv.dot11PrivacyAlgrthm == _WEP40_) ||
+	    (padapter->securitypriv.dot11PrivacyAlgrthm == _WEP104_)) {
+
+		for (EntryId = 0; EntryId < 4; EntryId++) {
+			if (EntryId == psecuritypriv->dot11PrivacyKeyIndex)
+				rtw_set_key(padapter, &padapter->securitypriv, EntryId, 1, _FALSE);
+			else
+				rtw_set_key(padapter, &padapter->securitypriv, EntryId, 0, _FALSE);
+		}
+
+	} else
+#endif
+		if ((padapter->securitypriv.dot11PrivacyAlgrthm == _TKIP_) ||
+		    (padapter->securitypriv.dot11PrivacyAlgrthm == _AES_)) {
+			psta = rtw_get_stainfo(pstapriv, get_bssid(mlmepriv));
+			if (psta == NULL) {
+				/* DEBUG_ERR( ("Set wpa_set_encryption: Obtain Sta_info fail\n")); */
+			} else {
+				/* pairwise key */
+				rtw_setstakey_cmd(padapter, psta, UNICAST_KEY, _FALSE);
+				/* group key */
+				rtw_set_key(padapter, &padapter->securitypriv, padapter->securitypriv.dot118021XGrpKeyid, 0, _FALSE);
+			}
+		}
+}
+
+void sreset_restore_network_station(_adapter *padapter)
+{
+	struct mlme_priv *mlmepriv = &padapter->mlmepriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	u8 doiqk = _FALSE;
+
+#if 0
+	{
+		/* ======================================================= */
+		/* reset related register of Beacon control */
+
+		/* set MSR to nolink */
+		Set_MSR(padapter, _HW_STATE_NOLINK_);
+		/* reject all data frame */
+		rtw_write16(padapter, REG_RXFLTMAP2, 0x00);
+		/* reset TSF */
+		rtw_write8(padapter, REG_DUAL_TSF_RST, (BIT(0) | BIT(1)));
+
+		/* disable update TSF */
+		SetBcnCtrlReg(padapter, BIT(4), 0);
+
+		/* ======================================================= */
+	}
+#endif
+
+	rtw_setopmode_cmd(padapter, Ndis802_11Infrastructure, _FALSE);
+
+	{
+		u8 threshold;
+#ifdef CONFIG_USB_HCI
+		/* TH=1 => means that invalidate usb rx aggregation */
+		/* TH=0 => means that validate usb rx aggregation, use init value. */
+		if (mlmepriv->htpriv.ht_option) {
+			if (padapter->registrypriv.wifi_spec == 1)
+				threshold = 1;
+			else
+				threshold = 0;
+			rtw_hal_set_hwreg(padapter, HW_VAR_RXDMA_AGG_PG_TH, (u8 *)(&threshold));
+		} else {
+			threshold = 1;
+			rtw_hal_set_hwreg(padapter, HW_VAR_RXDMA_AGG_PG_TH, (u8 *)(&threshold));
+		}
+#endif
+	}
+
+	doiqk = _TRUE;
+	rtw_hal_set_hwreg(padapter, HW_VAR_DO_IQK , &doiqk);
+
+	set_channel_bwmode(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
+
+	doiqk = _FALSE;
+	rtw_hal_set_hwreg(padapter , HW_VAR_DO_IQK , &doiqk);
+	/* disable dynamic functions, such as high power, DIG */
+	/*rtw_phydm_func_disable_all(padapter);*/
+
+	rtw_hal_set_hwreg(padapter, HW_VAR_BSSID, pmlmeinfo->network.MacAddress);
+
+	{
+		u8	join_type = 0;
+		rtw_hal_set_hwreg(padapter, HW_VAR_MLME_JOIN, (u8 *)(&join_type));
+	}
+
+	Set_MSR(padapter, (pmlmeinfo->state & 0x3));
+
+	mlmeext_joinbss_event_callback(padapter, 1);
+	/* restore Sequence No. */
+	rtw_hal_set_hwreg(padapter, HW_VAR_RESTORE_HW_SEQ, 0);
+
+	sreset_restore_security_station(padapter);
+}
+
+
+void sreset_restore_network_status(_adapter *padapter)
+{
+	struct mlme_priv *mlmepriv = &padapter->mlmepriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+
+	if (check_fwstate(mlmepriv, WIFI_STATION_STATE)) {
+		RTW_INFO(FUNC_ADPT_FMT" fwstate:0x%08x - WIFI_STATION_STATE\n", FUNC_ADPT_ARG(padapter), get_fwstate(mlmepriv));
+		sreset_restore_network_station(padapter);
+	} else if (check_fwstate(mlmepriv, WIFI_AP_STATE)) {
+		RTW_INFO(FUNC_ADPT_FMT" fwstate:0x%08x - WIFI_AP_STATE\n", FUNC_ADPT_ARG(padapter), get_fwstate(mlmepriv));
+		rtw_ap_restore_network(padapter);
+	} else if (check_fwstate(mlmepriv, WIFI_ADHOC_STATE))
+		RTW_INFO(FUNC_ADPT_FMT" fwstate:0x%08x - WIFI_ADHOC_STATE\n", FUNC_ADPT_ARG(padapter), get_fwstate(mlmepriv));
+	else
+		RTW_INFO(FUNC_ADPT_FMT" fwstate:0x%08x - ???\n", FUNC_ADPT_ARG(padapter), get_fwstate(mlmepriv));
+}
+
+void sreset_stop_adapter(_adapter *padapter)
+{
+	struct mlme_priv	*pmlmepriv = &(padapter->mlmepriv);
+	struct xmit_priv	*pxmitpriv = &padapter->xmitpriv;
+
+	if (padapter == NULL)
+		return;
+
+	RTW_INFO(FUNC_ADPT_FMT"\n", FUNC_ADPT_ARG(padapter));
+
+	rtw_netif_stop_queue(padapter->pnetdev);
+
+	rtw_cancel_all_timer(padapter);
+
+	/* TODO: OS and HCI independent */
+#if defined(PLATFORM_LINUX) && defined(CONFIG_USB_HCI)
+	tasklet_kill(&pxmitpriv->xmit_tasklet);
+#endif
+
+	if (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY))
+		rtw_scan_abort(padapter);
+
+	if (check_fwstate(pmlmepriv, _FW_UNDER_LINKING)) {
+		rtw_set_to_roam(padapter, 0);
+		_rtw_join_timeout_handler(padapter);
+	}
+
+}
+
+void sreset_start_adapter(_adapter *padapter)
+{
+	struct mlme_priv	*pmlmepriv = &(padapter->mlmepriv);
+	struct xmit_priv	*pxmitpriv = &padapter->xmitpriv;
+
+	if (padapter == NULL)
+		return;
+
+	RTW_INFO(FUNC_ADPT_FMT"\n", FUNC_ADPT_ARG(padapter));
+
+	if (check_fwstate(pmlmepriv, _FW_LINKED))
+		sreset_restore_network_status(padapter);
+
+	/* TODO: OS and HCI independent */
+#if defined(PLATFORM_LINUX) && defined(CONFIG_USB_HCI)
+	tasklet_hi_schedule(&pxmitpriv->xmit_tasklet);
+#endif
+
+	if (is_primary_adapter(padapter))
+		_set_timer(&adapter_to_dvobj(padapter)->dynamic_chk_timer, 2000);
+
+	rtw_netif_wake_queue(padapter->pnetdev);
+}
+
+void sreset_reset(_adapter *padapter)
+{
+#ifdef DBG_CONFIG_ERROR_RESET
+	HAL_DATA_TYPE	*pHalData = GET_HAL_DATA(padapter);
+	struct sreset_priv *psrtpriv = &pHalData->srestpriv;
+	struct pwrctrl_priv *pwrpriv = adapter_to_pwrctl(padapter);
+	struct mlme_priv	*pmlmepriv = &(padapter->mlmepriv);
+	struct xmit_priv	*pxmitpriv = &padapter->xmitpriv;
+	_irqL irqL;
+	u32 start = rtw_get_current_time();
+	struct dvobj_priv *psdpriv = padapter->dvobj;
+	struct debug_priv *pdbgpriv = &psdpriv->drv_dbg;
+
+	RTW_INFO("%s\n", __FUNCTION__);
+
+	psrtpriv->Wifi_Error_Status = WIFI_STATUS_SUCCESS;
+
+
+#ifdef CONFIG_LPS
+	rtw_set_ps_mode(padapter, PS_MODE_ACTIVE, 0, 0, "SRESET");
+#endif/* #ifdef CONFIG_LPS */
+
+	_enter_pwrlock(&pwrpriv->lock);
+
+	psrtpriv->silent_reset_inprogress = _TRUE;
+	pwrpriv->change_rfpwrstate = rf_off;
+
+	rtw_mi_sreset_adapter_hdl(padapter, _FALSE);/*sreset_stop_adapter*/
+#ifdef CONFIG_IPS
+	_ips_enter(padapter);
+	_ips_leave(padapter);
+#endif
+	rtw_mi_sreset_adapter_hdl(padapter, _TRUE);/*sreset_start_adapter*/
+
+	psrtpriv->silent_reset_inprogress = _FALSE;
+
+	_exit_pwrlock(&pwrpriv->lock);
+
+	RTW_INFO("%s done in %d ms\n", __FUNCTION__, rtw_get_passing_time_ms(start));
+	pdbgpriv->dbg_sreset_cnt++;
+#endif
+}

core/rtw_sta_mgt.c

@@ -0,0 +1,978 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * 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
+ *
+ *
+ ******************************************************************************/
+#define _RTW_STA_MGT_C_
+
+#include <drv_types.h>
+
+#if defined(PLATFORM_LINUX) && defined (PLATFORM_WINDOWS)
+
+	#error "Shall be Linux or Windows, but not both!\n"
+
+#endif
+
+
+bool test_st_match_rule(_adapter *adapter, u8 *local_naddr, u8 *local_port, u8 *remote_naddr, u8 *remote_port)
+{
+	if (ntohs(*((u16 *)local_port)) == 5001 || ntohs(*((u16 *)remote_port)) == 5001)
+		return _TRUE;
+	return _FALSE;
+}
+
+struct st_register test_st_reg = {
+	.s_proto = 0x06,
+	.rule = test_st_match_rule,
+};
+
+inline void rtw_st_ctl_init(struct st_ctl_t *st_ctl)
+{
+	_rtw_memset(st_ctl->reg, 0 , sizeof(struct st_register) * SESSION_TRACKER_REG_ID_NUM);
+	_rtw_init_queue(&st_ctl->tracker_q);
+}
+
+inline void rtw_st_ctl_clear_tracker_q(struct st_ctl_t *st_ctl)
+{
+	_irqL irqL;
+	_list *plist, *phead;
+	struct session_tracker *st;
+
+	_enter_critical_bh(&st_ctl->tracker_q.lock, &irqL);
+	phead = &st_ctl->tracker_q.queue;
+	plist = get_next(phead);
+	while (rtw_end_of_queue_search(phead, plist) == _FALSE) {
+		st = LIST_CONTAINOR(plist, struct session_tracker, list);
+		plist = get_next(plist);
+		rtw_list_delete(&st->list);
+		rtw_mfree((u8 *)st, sizeof(struct session_tracker));
+	}
+	_exit_critical_bh(&st_ctl->tracker_q.lock, &irqL);
+}
+
+inline void rtw_st_ctl_deinit(struct st_ctl_t *st_ctl)
+{
+	rtw_st_ctl_clear_tracker_q(st_ctl);
+	_rtw_deinit_queue(&st_ctl->tracker_q);
+}
+
+inline void rtw_st_ctl_register(struct st_ctl_t *st_ctl, u8 st_reg_id, struct st_register *reg)
+{
+	if (st_reg_id >= SESSION_TRACKER_REG_ID_NUM) {
+		rtw_warn_on(1);
+		return;
+	}
+
+	st_ctl->reg[st_reg_id].s_proto = reg->s_proto;
+	st_ctl->reg[st_reg_id].rule = reg->rule;
+}
+
+inline void rtw_st_ctl_unregister(struct st_ctl_t *st_ctl, u8 st_reg_id)
+{
+	int i;
+
+	if (st_reg_id >= SESSION_TRACKER_REG_ID_NUM) {
+		rtw_warn_on(1);
+		return;
+	}
+
+	st_ctl->reg[st_reg_id].s_proto = 0;
+	st_ctl->reg[st_reg_id].rule = NULL;
+
+	/* clear tracker queue if no session trecker registered */
+	for (i = 0; i < SESSION_TRACKER_REG_ID_NUM; i++)
+		if (st_ctl->reg[i].s_proto != 0)
+			break;
+	if (i >= SESSION_TRACKER_REG_ID_NUM)
+		rtw_st_ctl_clear_tracker_q(st_ctl);
+}
+
+inline bool rtw_st_ctl_chk_reg_s_proto(struct st_ctl_t *st_ctl, u8 s_proto)
+{
+	bool ret = _FALSE;
+	int i;
+
+	for (i = 0; i < SESSION_TRACKER_REG_ID_NUM; i++) {
+		if (st_ctl->reg[i].s_proto == s_proto) {
+			ret = _TRUE;
+			break;
+		}
+	}
+
+	return ret;
+}
+
+inline bool rtw_st_ctl_chk_reg_rule(struct st_ctl_t *st_ctl, _adapter *adapter, u8 *local_naddr, u8 *local_port, u8 *remote_naddr, u8 *remote_port)
+{
+	bool ret = _FALSE;
+	int i;
+	st_match_rule rule;
+
+	for (i = 0; i < SESSION_TRACKER_REG_ID_NUM; i++) {
+		rule = st_ctl->reg[i].rule;
+		if (rule && rule(adapter, local_naddr, local_port, remote_naddr, remote_port) == _TRUE) {
+			ret = _TRUE;
+			break;
+		}
+	}
+
+	return ret;
+}
+
+#define SESSION_TRACKER_FMT IP_FMT":"PORT_FMT" "IP_FMT":"PORT_FMT" %u %d"
+#define SESSION_TRACKER_ARG(st) IP_ARG(&(st)->local_naddr), PORT_ARG(&(st)->local_port), IP_ARG(&(st)->remote_naddr), PORT_ARG(&(st)->remote_port), (st)->status, rtw_get_passing_time_ms((st)->set_time)
+
+void dump_st_ctl(void *sel, struct st_ctl_t *st_ctl)
+{
+	int i;
+	_irqL irqL;
+	_list *plist, *phead;
+	struct session_tracker *st;
+
+	if (!DBG_SESSION_TRACKER)
+		return;
+
+	for (i = 0; i < SESSION_TRACKER_REG_ID_NUM; i++)
+		RTW_PRINT_SEL(sel, "reg%d: %u %p\n", i, st_ctl->reg[i].s_proto, st_ctl->reg[i].rule);
+
+	_enter_critical_bh(&st_ctl->tracker_q.lock, &irqL);
+	phead = &st_ctl->tracker_q.queue;
+	plist = get_next(phead);
+	while (rtw_end_of_queue_search(phead, plist) == _FALSE) {
+		st = LIST_CONTAINOR(plist, struct session_tracker, list);
+		plist = get_next(plist);
+
+		RTW_PRINT_SEL(sel, SESSION_TRACKER_FMT"\n", SESSION_TRACKER_ARG(st));
+	}
+	_exit_critical_bh(&st_ctl->tracker_q.lock, &irqL);
+
+}
+
+void _rtw_init_stainfo(struct sta_info *psta);
+void _rtw_init_stainfo(struct sta_info *psta)
+{
+
+
+	_rtw_memset((u8 *)psta, 0, sizeof(struct sta_info));
+
+	_rtw_spinlock_init(&psta->lock);
+	_rtw_init_listhead(&psta->list);
+	_rtw_init_listhead(&psta->hash_list);
+	/* _rtw_init_listhead(&psta->asoc_list); */
+	/* _rtw_init_listhead(&psta->sleep_list); */
+	/* _rtw_init_listhead(&psta->wakeup_list);	 */
+
+	_rtw_init_queue(&psta->sleep_q);
+	psta->sleepq_len = 0;
+
+	_rtw_init_sta_xmit_priv(&psta->sta_xmitpriv);
+	_rtw_init_sta_recv_priv(&psta->sta_recvpriv);
+
+#ifdef CONFIG_AP_MODE
+
+	_rtw_init_listhead(&psta->asoc_list);
+
+	_rtw_init_listhead(&psta->auth_list);
+
+	psta->expire_to = 0;
+
+	psta->flags = 0;
+
+	psta->capability = 0;
+
+	psta->bpairwise_key_installed = _FALSE;
+
+#ifdef CONFIG_RTW_80211R
+	psta->ft_pairwise_key_installed = _FALSE;
+#endif
+
+#ifdef CONFIG_NATIVEAP_MLME
+	psta->nonerp_set = 0;
+	psta->no_short_slot_time_set = 0;
+	psta->no_short_preamble_set = 0;
+	psta->no_ht_gf_set = 0;
+	psta->no_ht_set = 0;
+	psta->ht_20mhz_set = 0;
+	psta->ht_40mhz_intolerant = 0;
+#endif
+
+#ifdef CONFIG_TX_MCAST2UNI
+	psta->under_exist_checking = 0;
+#endif /* CONFIG_TX_MCAST2UNI */
+
+	psta->keep_alive_trycnt = 0;
+
+#endif /* CONFIG_AP_MODE	 */
+
+	rtw_st_ctl_init(&psta->st_ctl);
+
+
+}
+
+u32	_rtw_init_sta_priv(struct	sta_priv *pstapriv)
+{
+	struct sta_info *psta;
+	s32 i;
+
+
+	pstapriv->pallocated_stainfo_buf = rtw_zvmalloc(sizeof(struct sta_info) * NUM_STA + 4);
+
+	if (!pstapriv->pallocated_stainfo_buf)
+		return _FAIL;
+
+	pstapriv->pstainfo_buf = pstapriv->pallocated_stainfo_buf + 4 -
+			 ((SIZE_PTR)(pstapriv->pallocated_stainfo_buf) & 3);
+
+	_rtw_init_queue(&pstapriv->free_sta_queue);
+
+	_rtw_spinlock_init(&pstapriv->sta_hash_lock);
+
+	/* _rtw_init_queue(&pstapriv->asoc_q); */
+	pstapriv->asoc_sta_count = 0;
+	_rtw_init_queue(&pstapriv->sleep_q);
+	_rtw_init_queue(&pstapriv->wakeup_q);
+
+	psta = (struct sta_info *)(pstapriv->pstainfo_buf);
+
+
+	for (i = 0; i < NUM_STA; i++) {
+		_rtw_init_stainfo(psta);
+
+		_rtw_init_listhead(&(pstapriv->sta_hash[i]));
+
+		rtw_list_insert_tail(&psta->list, get_list_head(&pstapriv->free_sta_queue));
+
+		psta++;
+	}
+
+	pstapriv->adhoc_expire_to = 4; /* 4 * 2 = 8 sec */
+
+#ifdef CONFIG_AP_MODE
+
+	pstapriv->sta_dz_bitmap = 0;
+	pstapriv->tim_bitmap = 0;
+
+	_rtw_init_listhead(&pstapriv->asoc_list);
+	_rtw_init_listhead(&pstapriv->auth_list);
+	_rtw_spinlock_init(&pstapriv->asoc_list_lock);
+	_rtw_spinlock_init(&pstapriv->auth_list_lock);
+	pstapriv->asoc_list_cnt = 0;
+	pstapriv->auth_list_cnt = 0;
+
+	pstapriv->auth_to = 3; /* 3*2 = 6 sec */
+	pstapriv->assoc_to = 3;
+	/* pstapriv->expire_to = 900; */ /* 900*2 = 1800 sec = 30 min, expire after no any traffic. */
+	/* pstapriv->expire_to = 30; */ /* 30*2 = 60 sec = 1 min, expire after no any traffic. */
+#ifdef CONFIG_ACTIVE_KEEP_ALIVE_CHECK
+	pstapriv->expire_to = 3; /* 3*2 = 6 sec */
+#else
+	pstapriv->expire_to = 60;/* 60*2 = 120 sec = 2 min, expire after no any traffic. */
+#endif
+#ifdef CONFIG_ATMEL_RC_PATCH
+	_rtw_memset(pstapriv->atmel_rc_pattern, 0, ETH_ALEN);
+#endif
+	pstapriv->max_num_sta = NUM_STA;
+
+#endif
+
+#if CONFIG_RTW_MACADDR_ACL
+	_rtw_init_queue(&(pstapriv->acl_list.acl_node_q));
+#endif
+
+
+	return _SUCCESS;
+
+}
+
+inline int rtw_stainfo_offset(struct sta_priv *stapriv, struct sta_info *sta)
+{
+	int offset = (((u8 *)sta) - stapriv->pstainfo_buf) / sizeof(struct sta_info);
+
+	if (!stainfo_offset_valid(offset))
+		RTW_INFO("%s invalid offset(%d), out of range!!!", __func__, offset);
+
+	return offset;
+}
+
+inline struct sta_info *rtw_get_stainfo_by_offset(struct sta_priv *stapriv, int offset)
+{
+	if (!stainfo_offset_valid(offset))
+		RTW_INFO("%s invalid offset(%d), out of range!!!", __func__, offset);
+
+	return (struct sta_info *)(stapriv->pstainfo_buf + offset * sizeof(struct sta_info));
+}
+
+void	_rtw_free_sta_xmit_priv_lock(struct sta_xmit_priv *psta_xmitpriv);
+void	_rtw_free_sta_xmit_priv_lock(struct sta_xmit_priv *psta_xmitpriv)
+{
+
+	_rtw_spinlock_free(&psta_xmitpriv->lock);
+
+	_rtw_spinlock_free(&(psta_xmitpriv->be_q.sta_pending.lock));
+	_rtw_spinlock_free(&(psta_xmitpriv->bk_q.sta_pending.lock));
+	_rtw_spinlock_free(&(psta_xmitpriv->vi_q.sta_pending.lock));
+	_rtw_spinlock_free(&(psta_xmitpriv->vo_q.sta_pending.lock));
+}
+
+static void	_rtw_free_sta_recv_priv_lock(struct sta_recv_priv *psta_recvpriv)
+{
+
+	_rtw_spinlock_free(&psta_recvpriv->lock);
+
+	_rtw_spinlock_free(&(psta_recvpriv->defrag_q.lock));
+
+
+}
+
+void rtw_mfree_stainfo(struct sta_info *psta);
+void rtw_mfree_stainfo(struct sta_info *psta)
+{
+
+	if (&psta->lock != NULL)
+		_rtw_spinlock_free(&psta->lock);
+
+	_rtw_free_sta_xmit_priv_lock(&psta->sta_xmitpriv);
+	_rtw_free_sta_recv_priv_lock(&psta->sta_recvpriv);
+
+}
+
+
+/* this function is used to free the memory of lock || sema for all stainfos */
+void rtw_mfree_all_stainfo(struct sta_priv *pstapriv);
+void rtw_mfree_all_stainfo(struct sta_priv *pstapriv)
+{
+	_irqL	 irqL;
+	_list	*plist, *phead;
+	struct sta_info *psta = NULL;
+
+
+	_enter_critical_bh(&pstapriv->sta_hash_lock, &irqL);
+
+	phead = get_list_head(&pstapriv->free_sta_queue);
+	plist = get_next(phead);
+
+	while ((rtw_end_of_queue_search(phead, plist)) == _FALSE) {
+		psta = LIST_CONTAINOR(plist, struct sta_info , list);
+		plist = get_next(plist);
+
+		rtw_mfree_stainfo(psta);
+	}
+
+	_exit_critical_bh(&pstapriv->sta_hash_lock, &irqL);
+
+
+}
+
+void rtw_mfree_sta_priv_lock(struct	sta_priv *pstapriv);
+void rtw_mfree_sta_priv_lock(struct	sta_priv *pstapriv)
+{
+	rtw_mfree_all_stainfo(pstapriv); /* be done before free sta_hash_lock */
+
+	_rtw_spinlock_free(&pstapriv->free_sta_queue.lock);
+
+	_rtw_spinlock_free(&pstapriv->sta_hash_lock);
+	_rtw_spinlock_free(&pstapriv->wakeup_q.lock);
+	_rtw_spinlock_free(&pstapriv->sleep_q.lock);
+
+#ifdef CONFIG_AP_MODE
+	_rtw_spinlock_free(&pstapriv->asoc_list_lock);
+	_rtw_spinlock_free(&pstapriv->auth_list_lock);
+#endif
+
+}
+
+u32	_rtw_free_sta_priv(struct	sta_priv *pstapriv)
+{
+	_irqL	irqL;
+	_list	*phead, *plist;
+	struct sta_info *psta = NULL;
+	struct recv_reorder_ctrl *preorder_ctrl;
+	int	index;
+
+	if (pstapriv) {
+
+		/*	delete all reordering_ctrl_timer		*/
+		_enter_critical_bh(&pstapriv->sta_hash_lock, &irqL);
+		for (index = 0; index < NUM_STA; index++) {
+			phead = &(pstapriv->sta_hash[index]);
+			plist = get_next(phead);
+
+			while ((rtw_end_of_queue_search(phead, plist)) == _FALSE) {
+				int i;
+				psta = LIST_CONTAINOR(plist, struct sta_info , hash_list);
+				plist = get_next(plist);
+
+				for (i = 0; i < 16 ; i++) {
+					preorder_ctrl = &psta->recvreorder_ctrl[i];
+					_cancel_timer_ex(&preorder_ctrl->reordering_ctrl_timer);
+				}
+			}
+		}
+		_exit_critical_bh(&pstapriv->sta_hash_lock, &irqL);
+		/*===============================*/
+
+		rtw_mfree_sta_priv_lock(pstapriv);
+
+#if CONFIG_RTW_MACADDR_ACL
+		_rtw_deinit_queue(&(pstapriv->acl_list.acl_node_q));
+#endif
+
+		if (pstapriv->pallocated_stainfo_buf)
+			rtw_vmfree(pstapriv->pallocated_stainfo_buf, sizeof(struct sta_info) * NUM_STA + 4);
+	}
+
+	return _SUCCESS;
+}
+
+
+/* struct	sta_info *rtw_alloc_stainfo(_queue *pfree_sta_queue, unsigned char *hwaddr) */
+struct	sta_info *rtw_alloc_stainfo(struct	sta_priv *pstapriv, u8 *hwaddr)
+{
+	_irqL irqL, irqL2;
+	uint tmp_aid;
+	s32	index;
+	_list	*phash_list;
+	struct sta_info	*psta;
+	_queue *pfree_sta_queue;
+	struct recv_reorder_ctrl *preorder_ctrl;
+	int i = 0;
+	u16  wRxSeqInitialValue = 0xffff;
+
+
+	pfree_sta_queue = &pstapriv->free_sta_queue;
+
+	/* _enter_critical_bh(&(pfree_sta_queue->lock), &irqL); */
+	_enter_critical_bh(&(pstapriv->sta_hash_lock), &irqL2);
+	if (_rtw_queue_empty(pfree_sta_queue) == _TRUE) {
+		/* _exit_critical_bh(&(pfree_sta_queue->lock), &irqL); */
+		_exit_critical_bh(&(pstapriv->sta_hash_lock), &irqL2);
+		psta = NULL;
+	} else {
+		psta = LIST_CONTAINOR(get_next(&pfree_sta_queue->queue), struct sta_info, list);
+
+		rtw_list_delete(&(psta->list));
+
+		/* _exit_critical_bh(&(pfree_sta_queue->lock), &irqL); */
+
+		tmp_aid = psta->aid;
+
+		_rtw_init_stainfo(psta);
+
+		psta->padapter = pstapriv->padapter;
+
+		_rtw_memcpy(psta->hwaddr, hwaddr, ETH_ALEN);
+
+		index = wifi_mac_hash(hwaddr);
+
+
+		if (index >= NUM_STA) {
+			psta = NULL;
+			goto exit;
+		}
+		phash_list = &(pstapriv->sta_hash[index]);
+
+		/* _enter_critical_bh(&(pstapriv->sta_hash_lock), &irqL2); */
+
+		rtw_list_insert_tail(&psta->hash_list, phash_list);
+
+		pstapriv->asoc_sta_count++;
+
+		/* _exit_critical_bh(&(pstapriv->sta_hash_lock), &irqL2); */
+
+		/* Commented by Albert 2009/08/13
+		 * For the SMC router, the sequence number of first packet of WPS handshake will be 0.
+		 * In this case, this packet will be dropped by recv_decache function if we use the 0x00 as the default value for tid_rxseq variable.
+		 * So, we initialize the tid_rxseq variable as the 0xffff. */
+
+		for (i = 0; i < 16; i++)
+			_rtw_memcpy(&psta->sta_recvpriv.rxcache.tid_rxseq[i], &wRxSeqInitialValue, 2);
+
+
+		init_addba_retry_timer(pstapriv->padapter, psta);
+#ifdef CONFIG_IEEE80211W
+		init_dot11w_expire_timer(pstapriv->padapter, psta);
+#endif /* CONFIG_IEEE80211W */
+#ifdef CONFIG_TDLS
+		rtw_init_tdls_timer(pstapriv->padapter, psta);
+#endif /* CONFIG_TDLS */
+
+		/* for A-MPDU Rx reordering buffer control */
+		for (i = 0; i < 16 ; i++) {
+			preorder_ctrl = &psta->recvreorder_ctrl[i];
+
+			preorder_ctrl->padapter = pstapriv->padapter;
+
+			preorder_ctrl->enable = _FALSE;
+
+			preorder_ctrl->indicate_seq = 0xffff;
+#ifdef DBG_RX_SEQ
+			RTW_INFO("DBG_RX_SEQ %s:%d IndicateSeq: %d\n", __FUNCTION__, __LINE__,
+				 preorder_ctrl->indicate_seq);
+#endif
+			preorder_ctrl->wend_b = 0xffff;
+			/* preorder_ctrl->wsize_b = (NR_RECVBUFF-2); */
+			preorder_ctrl->wsize_b = 64;/* 64; */
+			preorder_ctrl->ampdu_size = RX_AMPDU_SIZE_INVALID;
+
+			_rtw_init_queue(&preorder_ctrl->pending_recvframe_queue);
+
+			rtw_init_recv_timer(preorder_ctrl);
+		}
+
+
+		/* init for DM */
+		psta->rssi_stat.UndecoratedSmoothedPWDB = (-1);
+		psta->rssi_stat.UndecoratedSmoothedCCK = (-1);
+#ifdef CONFIG_ATMEL_RC_PATCH
+		psta->flag_atmel_rc = 0;
+#endif
+		/* init for the sequence number of received management frame */
+		psta->RxMgmtFrameSeqNum = 0xffff;
+		psta->ra_rpt_linked = _FALSE;
+
+		rtw_alloc_macid(pstapriv->padapter, psta);
+
+	}
+
+exit:
+
+	_exit_critical_bh(&(pstapriv->sta_hash_lock), &irqL2);
+
+
+	if (psta)
+		rtw_mi_update_iface_status(&(pstapriv->padapter->mlmepriv), 0);
+
+	return psta;
+}
+
+
+/* using pstapriv->sta_hash_lock to protect */
+u32	rtw_free_stainfo(_adapter *padapter , struct sta_info *psta)
+{
+	int i;
+	_irqL irqL0;
+	_queue *pfree_sta_queue;
+	struct recv_reorder_ctrl *preorder_ctrl;
+	struct	sta_xmit_priv	*pstaxmitpriv;
+	struct	xmit_priv	*pxmitpriv = &padapter->xmitpriv;
+	struct	sta_priv *pstapriv = &padapter->stapriv;
+	struct hw_xmit *phwxmit;
+	int pending_qcnt[4];
+
+
+	if (psta == NULL)
+		goto exit;
+
+	_enter_critical_bh(&(pstapriv->sta_hash_lock), &irqL0);
+	rtw_list_delete(&psta->hash_list);
+	pstapriv->asoc_sta_count--;
+	_exit_critical_bh(&(pstapriv->sta_hash_lock), &irqL0);
+	rtw_mi_update_iface_status(&(padapter->mlmepriv), 0);
+
+
+	_enter_critical_bh(&psta->lock, &irqL0);
+	psta->state &= ~_FW_LINKED;
+	_exit_critical_bh(&psta->lock, &irqL0);
+
+	pfree_sta_queue = &pstapriv->free_sta_queue;
+
+
+	pstaxmitpriv = &psta->sta_xmitpriv;
+
+	/* rtw_list_delete(&psta->sleep_list); */
+
+	/* rtw_list_delete(&psta->wakeup_list); */
+
+	_enter_critical_bh(&pxmitpriv->lock, &irqL0);
+
+	rtw_free_xmitframe_queue(pxmitpriv, &psta->sleep_q);
+	psta->sleepq_len = 0;
+
+	/* vo */
+	/* _enter_critical_bh(&(pxmitpriv->vo_pending.lock), &irqL0); */
+	rtw_free_xmitframe_queue(pxmitpriv, &pstaxmitpriv->vo_q.sta_pending);
+	rtw_list_delete(&(pstaxmitpriv->vo_q.tx_pending));
+	phwxmit = pxmitpriv->hwxmits;
+	phwxmit->accnt -= pstaxmitpriv->vo_q.qcnt;
+	pending_qcnt[0] = pstaxmitpriv->vo_q.qcnt;
+	pstaxmitpriv->vo_q.qcnt = 0;
+	/* _exit_critical_bh(&(pxmitpriv->vo_pending.lock), &irqL0); */
+
+	/* vi */
+	/* _enter_critical_bh(&(pxmitpriv->vi_pending.lock), &irqL0); */
+	rtw_free_xmitframe_queue(pxmitpriv, &pstaxmitpriv->vi_q.sta_pending);
+	rtw_list_delete(&(pstaxmitpriv->vi_q.tx_pending));
+	phwxmit = pxmitpriv->hwxmits + 1;
+	phwxmit->accnt -= pstaxmitpriv->vi_q.qcnt;
+	pending_qcnt[1] = pstaxmitpriv->vi_q.qcnt;
+	pstaxmitpriv->vi_q.qcnt = 0;
+	/* _exit_critical_bh(&(pxmitpriv->vi_pending.lock), &irqL0); */
+
+	/* be */
+	/* _enter_critical_bh(&(pxmitpriv->be_pending.lock), &irqL0); */
+	rtw_free_xmitframe_queue(pxmitpriv, &pstaxmitpriv->be_q.sta_pending);
+	rtw_list_delete(&(pstaxmitpriv->be_q.tx_pending));
+	phwxmit = pxmitpriv->hwxmits + 2;
+	phwxmit->accnt -= pstaxmitpriv->be_q.qcnt;
+	pending_qcnt[2] = pstaxmitpriv->be_q.qcnt;
+	pstaxmitpriv->be_q.qcnt = 0;
+	/* _exit_critical_bh(&(pxmitpriv->be_pending.lock), &irqL0); */
+
+	/* bk */
+	/* _enter_critical_bh(&(pxmitpriv->bk_pending.lock), &irqL0); */
+	rtw_free_xmitframe_queue(pxmitpriv, &pstaxmitpriv->bk_q.sta_pending);
+	rtw_list_delete(&(pstaxmitpriv->bk_q.tx_pending));
+	phwxmit = pxmitpriv->hwxmits + 3;
+	phwxmit->accnt -= pstaxmitpriv->bk_q.qcnt;
+	pending_qcnt[3] = pstaxmitpriv->bk_q.qcnt;
+	pstaxmitpriv->bk_q.qcnt = 0;
+	/* _exit_critical_bh(&(pxmitpriv->bk_pending.lock), &irqL0); */
+
+	rtw_os_wake_queue_at_free_stainfo(padapter, pending_qcnt);
+
+	_exit_critical_bh(&pxmitpriv->lock, &irqL0);
+
+
+	/* re-init sta_info; 20061114 */ /* will be init in alloc_stainfo */
+	/* _rtw_init_sta_xmit_priv(&psta->sta_xmitpriv); */
+	/* _rtw_init_sta_recv_priv(&psta->sta_recvpriv); */
+#ifdef CONFIG_IEEE80211W
+	_cancel_timer_ex(&psta->dot11w_expire_timer);
+#endif /* CONFIG_IEEE80211W */
+	_cancel_timer_ex(&psta->addba_retry_timer);
+
+#ifdef CONFIG_TDLS
+	psta->tdls_sta_state = TDLS_STATE_NONE;
+	rtw_free_tdls_timer(psta);
+#endif /* CONFIG_TDLS */
+
+	/* for A-MPDU Rx reordering buffer control, cancel reordering_ctrl_timer */
+	for (i = 0; i < 16 ; i++) {
+		_irqL irqL;
+		_list	*phead, *plist;
+		union recv_frame *prframe;
+		_queue *ppending_recvframe_queue;
+		_queue *pfree_recv_queue = &padapter->recvpriv.free_recv_queue;
+
+		preorder_ctrl = &psta->recvreorder_ctrl[i];
+
+		_cancel_timer_ex(&preorder_ctrl->reordering_ctrl_timer);
+
+
+		ppending_recvframe_queue = &preorder_ctrl->pending_recvframe_queue;
+
+		_enter_critical_bh(&ppending_recvframe_queue->lock, &irqL);
+
+		phead =	get_list_head(ppending_recvframe_queue);
+		plist = get_next(phead);
+
+		while (!rtw_is_list_empty(phead)) {
+			prframe = LIST_CONTAINOR(plist, union recv_frame, u);
+
+			plist = get_next(plist);
+
+			rtw_list_delete(&(prframe->u.hdr.list));
+
+			rtw_free_recvframe(prframe, pfree_recv_queue);
+		}
+
+		_exit_critical_bh(&ppending_recvframe_queue->lock, &irqL);
+
+	}
+
+	if (!((psta->state & WIFI_AP_STATE) || MacAddr_isBcst(psta->hwaddr)))
+		rtw_hal_set_odm_var(padapter, HAL_ODM_STA_INFO, psta, _FALSE);
+
+
+	/* release mac id for non-bc/mc station, */
+	rtw_release_macid(pstapriv->padapter, psta);
+
+#ifdef CONFIG_AP_MODE
+
+	/*
+		_enter_critical_bh(&pstapriv->asoc_list_lock, &irqL0);
+		rtw_list_delete(&psta->asoc_list);
+		_exit_critical_bh(&pstapriv->asoc_list_lock, &irqL0);
+	*/
+	_enter_critical_bh(&pstapriv->auth_list_lock, &irqL0);
+	if (!rtw_is_list_empty(&psta->auth_list)) {
+		rtw_list_delete(&psta->auth_list);
+		pstapriv->auth_list_cnt--;
+	}
+	_exit_critical_bh(&pstapriv->auth_list_lock, &irqL0);
+
+	psta->expire_to = 0;
+#ifdef CONFIG_ATMEL_RC_PATCH
+	psta->flag_atmel_rc = 0;
+#endif
+	psta->sleepq_ac_len = 0;
+	psta->qos_info = 0;
+
+	psta->max_sp_len = 0;
+	psta->uapsd_bk = 0;
+	psta->uapsd_be = 0;
+	psta->uapsd_vi = 0;
+	psta->uapsd_vo = 0;
+
+	psta->has_legacy_ac = 0;
+
+#ifdef CONFIG_NATIVEAP_MLME
+
+	pstapriv->sta_dz_bitmap &= ~BIT(psta->aid);
+	pstapriv->tim_bitmap &= ~BIT(psta->aid);
+
+	/* rtw_indicate_sta_disassoc_event(padapter, psta); */
+
+	if ((psta->aid > 0) && (pstapriv->sta_aid[psta->aid - 1] == psta)) {
+		pstapriv->sta_aid[psta->aid - 1] = NULL;
+		psta->aid = 0;
+	}
+
+#endif /* CONFIG_NATIVEAP_MLME	 */
+
+#ifdef CONFIG_TX_MCAST2UNI
+	psta->under_exist_checking = 0;
+#endif /* CONFIG_TX_MCAST2UNI */
+
+#endif /* CONFIG_AP_MODE	 */
+
+	rtw_st_ctl_deinit(&psta->st_ctl);
+
+	_rtw_spinlock_free(&psta->lock);
+
+	/* _enter_critical_bh(&(pfree_sta_queue->lock), &irqL0); */
+	_enter_critical_bh(&(pstapriv->sta_hash_lock), &irqL0);
+	rtw_list_insert_tail(&psta->list, get_list_head(pfree_sta_queue));
+	_exit_critical_bh(&(pstapriv->sta_hash_lock), &irqL0);
+	/* _exit_critical_bh(&(pfree_sta_queue->lock), &irqL0); */
+
+exit:
+
+
+	return _SUCCESS;
+
+}
+
+/* free all stainfo which in sta_hash[all] */
+void rtw_free_all_stainfo(_adapter *padapter)
+{
+	_irqL	 irqL;
+	_list	*plist, *phead;
+	s32	index;
+	struct sta_info *psta = NULL;
+	struct	sta_priv *pstapriv = &padapter->stapriv;
+	struct sta_info *pbcmc_stainfo = rtw_get_bcmc_stainfo(padapter);
+	u8 free_sta_num = 0;
+	char free_sta_list[NUM_STA];
+	int stainfo_offset;
+
+
+	if (pstapriv->asoc_sta_count == 1)
+		goto exit;
+
+	_enter_critical_bh(&pstapriv->sta_hash_lock, &irqL);
+
+	for (index = 0; index < NUM_STA; index++) {
+		phead = &(pstapriv->sta_hash[index]);
+		plist = get_next(phead);
+
+		while ((rtw_end_of_queue_search(phead, plist)) == _FALSE) {
+			psta = LIST_CONTAINOR(plist, struct sta_info , hash_list);
+
+			plist = get_next(plist);
+
+			if (pbcmc_stainfo != psta) {
+				rtw_list_delete(&psta->hash_list);
+				/* rtw_free_stainfo(padapter , psta); */
+				stainfo_offset = rtw_stainfo_offset(pstapriv, psta);
+				if (stainfo_offset_valid(stainfo_offset))
+					free_sta_list[free_sta_num++] = stainfo_offset;
+			}
+
+		}
+	}
+
+	_exit_critical_bh(&pstapriv->sta_hash_lock, &irqL);
+
+
+	for (index = 0; index < free_sta_num; index++) {
+		psta = rtw_get_stainfo_by_offset(pstapriv, free_sta_list[index]);
+		rtw_free_stainfo(padapter , psta);
+	}
+
+exit:
+	return;
+}
+
+/* any station allocated can be searched by hash list */
+struct sta_info *rtw_get_stainfo(struct sta_priv *pstapriv, u8 *hwaddr)
+{
+
+	_irqL	 irqL;
+
+	_list	*plist, *phead;
+
+	struct sta_info *psta = NULL;
+
+	u32	index;
+
+	u8 *addr;
+
+	u8 bc_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+
+
+	if (hwaddr == NULL)
+		return NULL;
+
+	if (IS_MCAST(hwaddr))
+		addr = bc_addr;
+	else
+		addr = hwaddr;
+
+	index = wifi_mac_hash(addr);
+
+	_enter_critical_bh(&pstapriv->sta_hash_lock, &irqL);
+
+	phead = &(pstapriv->sta_hash[index]);
+	plist = get_next(phead);
+
+
+	while ((rtw_end_of_queue_search(phead, plist)) == _FALSE) {
+
+		psta = LIST_CONTAINOR(plist, struct sta_info, hash_list);
+
+		if ((_rtw_memcmp(psta->hwaddr, addr, ETH_ALEN)) == _TRUE) {
+			/* if found the matched address */
+			break;
+		}
+		psta = NULL;
+		plist = get_next(plist);
+	}
+
+	_exit_critical_bh(&pstapriv->sta_hash_lock, &irqL);
+	return psta;
+
+}
+
+u32 rtw_init_bcmc_stainfo(_adapter *padapter)
+{
+
+	struct sta_info	*psta;
+	struct tx_servq	*ptxservq;
+	u32 res = _SUCCESS;
+	NDIS_802_11_MAC_ADDRESS	bcast_addr = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+
+	struct	sta_priv *pstapriv = &padapter->stapriv;
+	/* _queue	*pstapending = &padapter->xmitpriv.bm_pending; */
+
+
+	psta = rtw_alloc_stainfo(pstapriv, bcast_addr);
+
+	if (psta == NULL) {
+		res = _FAIL;
+		goto exit;
+	}
+#ifdef CONFIG_BEAMFORMING
+	psta->txbf_gid = 63;
+	psta->txbf_paid = 0;
+#endif
+	ptxservq = &(psta->sta_xmitpriv.be_q);
+
+	/*
+		_enter_critical(&pstapending->lock, &irqL0);
+
+		if (rtw_is_list_empty(&ptxservq->tx_pending))
+			rtw_list_insert_tail(&ptxservq->tx_pending, get_list_head(pstapending));
+
+		_exit_critical(&pstapending->lock, &irqL0);
+	*/
+
+exit:
+	return _SUCCESS;
+
+}
+
+
+struct sta_info *rtw_get_bcmc_stainfo(_adapter *padapter)
+{
+	struct sta_info	*psta;
+	struct sta_priv	*pstapriv = &padapter->stapriv;
+	u8 bc_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+	psta = rtw_get_stainfo(pstapriv, bc_addr);
+	return psta;
+
+}
+
+#if CONFIG_RTW_MACADDR_ACL
+const char *const _acl_mode_str[] = {
+	"DISABLED",
+	"ACCEPT_UNLESS_LISTED",
+	"DENY_UNLESS_LISTED",
+};
+
+u8 rtw_access_ctrl(_adapter *adapter, u8 *mac_addr)
+{
+	u8 res = _TRUE;
+	_irqL irqL;
+	_list *list, *head;
+	struct rtw_wlan_acl_node *acl_node;
+	u8 match = _FALSE;
+	struct sta_priv *stapriv = &adapter->stapriv;
+	struct wlan_acl_pool *acl = &stapriv->acl_list;
+	_queue	*acl_node_q = &acl->acl_node_q;
+
+	_enter_critical_bh(&(acl_node_q->lock), &irqL);
+	head = get_list_head(acl_node_q);
+	list = get_next(head);
+	while (rtw_end_of_queue_search(head, list) == _FALSE) {
+		acl_node = LIST_CONTAINOR(list, struct rtw_wlan_acl_node, list);
+		list = get_next(list);
+
+		if (_rtw_memcmp(acl_node->addr, mac_addr, ETH_ALEN)) {
+			if (acl_node->valid == _TRUE) {
+				match = _TRUE;
+				break;
+			}
+		}
+	}
+	_exit_critical_bh(&(acl_node_q->lock), &irqL);
+
+	if (acl->mode == RTW_ACL_MODE_ACCEPT_UNLESS_LISTED)
+		res = (match == _TRUE) ?  _FALSE : _TRUE;
+	else if (acl->mode == RTW_ACL_MODE_DENY_UNLESS_LISTED)
+		res = (match == _TRUE) ?  _TRUE : _FALSE;
+	else
+		res = _TRUE;
+
+	return res;
+}
+
+void dump_macaddr_acl(void *sel, _adapter *adapter)
+{
+	struct sta_priv *stapriv = &adapter->stapriv;
+	struct wlan_acl_pool *acl = &stapriv->acl_list;
+	int i;
+
+	RTW_PRINT_SEL(sel, "mode:%s(%d)\n", acl_mode_str(acl->mode), acl->mode);
+	RTW_PRINT_SEL(sel, "num:%d/%d\n", acl->num, NUM_ACL);
+	for (i = 0; i < NUM_ACL; i++) {
+		if (acl->aclnode[i].valid == _FALSE)
+			continue;
+		RTW_PRINT_SEL(sel, MAC_FMT"\n", MAC_ARG(acl->aclnode[i].addr));
+	}
+}
+#endif /* CONFIG_RTW_MACADDR_ACL */

core/rtw_vht.c

@@ -0,0 +1,857 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * 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
+ *
+ *
+ ******************************************************************************/
+#define _RTW_VHT_C
+
+#include <drv_types.h>
+
+#ifdef CONFIG_80211AC_VHT
+/*				20/40/80,	ShortGI,	MCS Rate  */
+const u16 VHT_MCS_DATA_RATE[3][2][30] = {
+	{	{
+			13, 26, 39, 52, 78, 104, 117, 130, 156, 156,
+			26, 52, 78, 104, 156, 208, 234, 260, 312, 312,
+			39, 78, 117, 156, 234, 312, 351, 390, 468, 520
+		},			/* Long GI, 20MHz */
+		{
+			14, 29, 43, 58, 87, 116, 130, 144, 173, 173,
+			29, 58, 87, 116, 173, 231, 260, 289, 347, 347,
+			43,	87, 130, 173, 260, 347, 390,	433,	520, 578
+		}
+	},		/* Short GI, 20MHz */
+	{	{
+			27, 54, 81, 108, 162, 216, 243, 270, 324, 360,
+			54, 108, 162, 216, 324, 432, 486, 540, 648, 720,
+			81, 162, 243, 324, 486, 648, 729, 810, 972, 1080
+		}, 		/* Long GI, 40MHz */
+		{
+			30, 60, 90, 120, 180, 240, 270, 300, 360, 400,
+			60, 120, 180, 240, 360, 480, 540, 600, 720, 800,
+			90, 180, 270, 360, 540, 720, 810, 900, 1080, 1200
+		}
+	},		/* Short GI, 40MHz */
+	{	{
+			59, 117,  176, 234, 351, 468, 527, 585, 702, 780,
+			117, 234, 351, 468, 702, 936, 1053, 1170, 1404, 1560,
+			176, 351, 527, 702, 1053, 1404, 1580, 1755, 2106, 2340
+		},	/* Long GI, 80MHz */
+		{
+			65, 130, 195, 260, 390, 520, 585, 650, 780, 867,
+			130, 260, 390, 520, 780, 1040, 1170, 1300, 1560, 1734,
+			195, 390, 585, 780, 1170, 1560, 1755, 1950, 2340, 2600
+		}
+	}	/* Short GI, 80MHz */
+};
+
+u8	rtw_get_vht_highest_rate(u8 *pvht_mcs_map)
+{
+	u8	i, j;
+	u8	bit_map;
+	u8	vht_mcs_rate = 0;
+
+	for (i = 0; i < 2; i++) {
+		if (pvht_mcs_map[i] != 0xff) {
+			for (j = 0; j < 8; j += 2) {
+				bit_map = (pvht_mcs_map[i] >> j) & 3;
+
+				if (bit_map != 3)
+					vht_mcs_rate = MGN_VHT1SS_MCS7 + 10 * j / 2 + i * 40 + bit_map; /* VHT rate indications begin from 0x90 */
+			}
+		}
+	}
+
+	/* RTW_INFO("HighestVHTMCSRate is %x\n", vht_mcs_rate); */
+	return vht_mcs_rate;
+}
+
+u8	rtw_vht_mcsmap_to_nss(u8 *pvht_mcs_map)
+{
+	u8	i, j;
+	u8	bit_map;
+	u8	nss = 0;
+
+	for (i = 0; i < 2; i++) {
+		if (pvht_mcs_map[i] != 0xff) {
+			for (j = 0; j < 8; j += 2) {
+				bit_map = (pvht_mcs_map[i] >> j) & 3;
+
+				if (bit_map != 3)
+					nss++;
+			}
+		}
+	}
+
+	/* RTW_INFO("%s : %dSS\n", __FUNCTION__, nss); */
+	return nss;
+}
+
+void	rtw_vht_nss_to_mcsmap(u8 nss, u8 *target_mcs_map, u8 *cur_mcs_map)
+{
+	u8	i, j;
+	u8	cur_rate, target_rate;
+
+	for (i = 0; i < 2; i++) {
+		target_mcs_map[i] = 0;
+		for (j = 0; j < 8; j += 2) {
+			cur_rate = (cur_mcs_map[i] >> j) & 3;
+			if (cur_rate == 3) /* 0x3 indicates not supported that num of SS */
+				target_rate = 3;
+			else if (nss <= ((j / 2) + i * 4))
+				target_rate = 3;
+			else
+				target_rate = cur_rate;
+
+			target_mcs_map[i] |= (target_rate << j);
+		}
+	}
+
+	/* RTW_INFO("%s : %dSS\n", __FUNCTION__, nss); */
+}
+
+u16	rtw_vht_mcs_to_data_rate(u8 bw, u8 short_GI, u8 vht_mcs_rate)
+{
+	if (vht_mcs_rate > MGN_VHT3SS_MCS9)
+		vht_mcs_rate = MGN_VHT3SS_MCS9;
+	/* RTW_INFO("bw=%d, short_GI=%d, ((vht_mcs_rate - MGN_VHT1SS_MCS0)&0x3f)=%d\n", bw, short_GI, ((vht_mcs_rate - MGN_VHT1SS_MCS0)&0x3f)); */
+	return VHT_MCS_DATA_RATE[bw][short_GI][((vht_mcs_rate - MGN_VHT1SS_MCS0) & 0x3f)];
+}
+
+void	rtw_vht_use_default_setting(_adapter *padapter)
+{
+	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
+	struct vht_priv		*pvhtpriv = &pmlmepriv->vhtpriv;
+	struct registry_priv	*pregistrypriv = &padapter->registrypriv;
+	BOOLEAN		bHwLDPCSupport = _FALSE, bHwSTBCSupport = _FALSE;
+#ifdef CONFIG_BEAMFORMING
+	BOOLEAN		bHwSupportBeamformer = _FALSE, bHwSupportBeamformee = _FALSE;
+	u8	mu_bfer, mu_bfee;
+#endif /* CONFIG_BEAMFORMING */
+	u8	rf_type = 0;
+	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	pvhtpriv->sgi_80m = TEST_FLAG(pregistrypriv->short_gi, BIT2) ? _TRUE : _FALSE;
+
+	/* LDPC support */
+	rtw_hal_get_def_var(padapter, HAL_DEF_RX_LDPC, (u8 *)&bHwLDPCSupport);
+	CLEAR_FLAGS(pvhtpriv->ldpc_cap);
+	if (bHwLDPCSupport) {
+		if (TEST_FLAG(pregistrypriv->ldpc_cap, BIT0))
+			SET_FLAG(pvhtpriv->ldpc_cap, LDPC_VHT_ENABLE_RX);
+	}
+	rtw_hal_get_def_var(padapter, HAL_DEF_TX_LDPC, (u8 *)&bHwLDPCSupport);
+	if (bHwLDPCSupport) {
+		if (TEST_FLAG(pregistrypriv->ldpc_cap, BIT1))
+			SET_FLAG(pvhtpriv->ldpc_cap, LDPC_VHT_ENABLE_TX);
+	}
+	if (pvhtpriv->ldpc_cap)
+		RTW_INFO("[VHT] Support LDPC = 0x%02X\n", pvhtpriv->ldpc_cap);
+
+	/* STBC */
+	rtw_hal_get_def_var(padapter, HAL_DEF_TX_STBC, (u8 *)&bHwSTBCSupport);
+	CLEAR_FLAGS(pvhtpriv->stbc_cap);
+	if (bHwSTBCSupport) {
+		if (TEST_FLAG(pregistrypriv->stbc_cap, BIT1))
+			SET_FLAG(pvhtpriv->stbc_cap, STBC_VHT_ENABLE_TX);
+	}
+	rtw_hal_get_def_var(padapter, HAL_DEF_RX_STBC, (u8 *)&bHwSTBCSupport);
+	if (bHwSTBCSupport) {
+		if (TEST_FLAG(pregistrypriv->stbc_cap, BIT0))
+			SET_FLAG(pvhtpriv->stbc_cap, STBC_VHT_ENABLE_RX);
+	}
+	if (pvhtpriv->stbc_cap)
+		RTW_INFO("[VHT] Support STBC = 0x%02X\n", pvhtpriv->stbc_cap);
+
+	/* Beamforming setting */
+	CLEAR_FLAGS(pvhtpriv->beamform_cap);
+#ifdef CONFIG_BEAMFORMING
+	rtw_hal_get_def_var(padapter, HAL_DEF_EXPLICIT_BEAMFORMER, (u8 *)&bHwSupportBeamformer);
+	rtw_hal_get_def_var(padapter, HAL_DEF_EXPLICIT_BEAMFORMEE, (u8 *)&bHwSupportBeamformee);
+	mu_bfer = _FALSE;
+	mu_bfee = _FALSE;
+	rtw_hal_get_def_var(padapter, HAL_DEF_VHT_MU_BEAMFORMER, &mu_bfer);
+	rtw_hal_get_def_var(padapter, HAL_DEF_VHT_MU_BEAMFORMEE, &mu_bfee);
+	if (TEST_FLAG(pregistrypriv->beamform_cap, BIT0) && bHwSupportBeamformer) {
+#ifdef CONFIG_CONCURRENT_MODE
+		if ((pmlmeinfo->state & 0x03) == WIFI_FW_AP_STATE) {
+			SET_FLAG(pvhtpriv->beamform_cap, BEAMFORMING_VHT_BEAMFORMER_ENABLE);
+			RTW_INFO("[VHT] CONCURRENT AP Support Beamformer\n");
+			if (TEST_FLAG(pregistrypriv->beamform_cap, BIT(2))
+			    && (_TRUE == mu_bfer)) {
+				SET_FLAG(pvhtpriv->beamform_cap, BEAMFORMING_VHT_MU_MIMO_AP_ENABLE);
+				RTW_INFO("[VHT] Support MU-MIMO AP\n");
+			}
+		} else
+			RTW_INFO("[VHT] CONCURRENT not AP ;not allow  Support Beamformer\n");
+#else
+		SET_FLAG(pvhtpriv->beamform_cap, BEAMFORMING_VHT_BEAMFORMER_ENABLE);
+		RTW_INFO("[VHT] Support Beamformer\n");
+		if (TEST_FLAG(pregistrypriv->beamform_cap, BIT(2))
+		    && (_TRUE == mu_bfer)
+		    && ((pmlmeinfo->state & 0x03) == WIFI_FW_AP_STATE)) {
+			SET_FLAG(pvhtpriv->beamform_cap, BEAMFORMING_VHT_MU_MIMO_AP_ENABLE);
+			RTW_INFO("[VHT] Support MU-MIMO AP\n");
+		}
+#endif
+	}
+	if (TEST_FLAG(pregistrypriv->beamform_cap, BIT1) && bHwSupportBeamformee) {
+		SET_FLAG(pvhtpriv->beamform_cap, BEAMFORMING_VHT_BEAMFORMEE_ENABLE);
+		RTW_INFO("[VHT] Support Beamformee\n");
+		if (TEST_FLAG(pregistrypriv->beamform_cap, BIT(3))
+		    && (_TRUE == mu_bfee)
+		    && ((pmlmeinfo->state & 0x03) != WIFI_FW_AP_STATE)) {
+			SET_FLAG(pvhtpriv->beamform_cap, BEAMFORMING_VHT_MU_MIMO_STA_ENABLE);
+			RTW_INFO("[VHT] Support MU-MIMO STA\n");
+		}
+	}
+#endif /* CONFIG_BEAMFORMING */
+
+	pvhtpriv->ampdu_len = pregistrypriv->ampdu_factor;
+
+	rtw_hal_get_hwreg(padapter, HW_VAR_RF_TYPE, (u8 *)(&rf_type));
+
+	if (rf_type == RF_3T3R)
+		pvhtpriv->vht_mcs_map[0] = 0xea;	/* support 1SS MCS 0~9 2SS MCS 0~9 3SS MCS 0~9 */
+	else if (rf_type == RF_2T2R)
+		pvhtpriv->vht_mcs_map[0] = 0xfa;	/* support 1SS MCS 0~9 2SS MCS 0~9 */
+	else
+		pvhtpriv->vht_mcs_map[0] = 0xfe;	/* Only support 1SS MCS 0~9; */
+	pvhtpriv->vht_mcs_map[1] = 0xff;
+
+	if (pregistrypriv->vht_rate_sel == 1) {
+		pvhtpriv->vht_mcs_map[0] = 0xfc;	/* support 1SS MCS 0~7 */
+	} else if (pregistrypriv->vht_rate_sel == 2) {
+		pvhtpriv->vht_mcs_map[0] = 0xfd;	/* Support 1SS MCS 0~8 */
+	} else if (pregistrypriv->vht_rate_sel == 3) {
+		pvhtpriv->vht_mcs_map[0] = 0xfe;	/* Support 1SS MCS 0~9 */
+	} else if (pregistrypriv->vht_rate_sel == 4) {
+		pvhtpriv->vht_mcs_map[0] = 0xf0;	/* support 1SS MCS 0~7 2SS MCS 0~7 */
+	} else if (pregistrypriv->vht_rate_sel == 5) {
+		pvhtpriv->vht_mcs_map[0] = 0xf5;	/* support 1SS MCS 0~8 2SS MCS 0~8 */
+	} else if (pregistrypriv->vht_rate_sel == 6) {
+		pvhtpriv->vht_mcs_map[0] = 0xfa;	/* support 1SS MCS 0~9 2SS MCS 0~9 */
+	} else if (pregistrypriv->vht_rate_sel == 7) {
+		pvhtpriv->vht_mcs_map[0] = 0xf8;	/* support 1SS MCS 0-7 2SS MCS 0~9 */
+	} else if (pregistrypriv->vht_rate_sel == 8) {
+		pvhtpriv->vht_mcs_map[0] = 0xf9;	/* support 1SS MCS 0-8 2SS MCS 0~9 */
+	} else if (pregistrypriv->vht_rate_sel == 9) {
+		pvhtpriv->vht_mcs_map[0] = 0xf4;	/* support 1SS MCS 0-7 2SS MCS 0~8 */
+	}
+
+	pvhtpriv->vht_highest_rate = rtw_get_vht_highest_rate(pvhtpriv->vht_mcs_map);
+}
+
+u64	rtw_vht_rate_to_bitmap(u8 *pVHTRate)
+{
+
+	u8	i, j , tmpRate;
+	u64	RateBitmap = 0;
+	u8 Bits_3ss = 6;
+
+	for (i = j = 0; i < Bits_3ss; i += 2, j += 10) {
+		/* every two bits means single sptial stream */
+		tmpRate = (pVHTRate[0] >> i) & 3;
+
+		switch (tmpRate) {
+		case 2:
+			RateBitmap = RateBitmap | (0x03ff << j);
+			break;
+		case 1:
+			RateBitmap = RateBitmap | (0x01ff << j);
+			break;
+
+		case 0:
+			RateBitmap = RateBitmap | (0x00ff << j);
+			break;
+
+		default:
+			break;
+		}
+	}
+	RTW_INFO("RateBitmap=%016llx , pVHTRate[0]=%02x, pVHTRate[1]=%02x\n", RateBitmap, pVHTRate[0], pVHTRate[1]);
+	return RateBitmap;
+}
+
+void	update_sta_vht_info_apmode(_adapter *padapter, PVOID sta)
+{
+	struct sta_info	*psta = (struct sta_info *)sta;
+	struct mlme_priv	*pmlmepriv = &(padapter->mlmepriv);
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct vht_priv	*pvhtpriv_ap = &pmlmepriv->vhtpriv;
+	struct vht_priv	*pvhtpriv_sta = &psta->vhtpriv;
+	struct ht_priv		*phtpriv_sta = &psta->htpriv;
+	u8	cur_ldpc_cap = 0, cur_stbc_cap = 0, bw_mode = 0;
+	u16	cur_beamform_cap = 0;
+	u8	*pcap_mcs;
+
+	if (pvhtpriv_sta->vht_option == _FALSE)
+		return;
+
+	bw_mode = GET_VHT_OPERATING_MODE_FIELD_CHNL_WIDTH(&pvhtpriv_sta->vht_op_mode_notify);
+
+	/* if (bw_mode > psta->bw_mode) */
+	psta->bw_mode = bw_mode;
+
+	/* B4 Rx LDPC */
+	if (TEST_FLAG(pvhtpriv_ap->ldpc_cap, LDPC_VHT_ENABLE_TX) &&
+	    GET_VHT_CAPABILITY_ELE_RX_LDPC(pvhtpriv_sta->vht_cap)) {
+		SET_FLAG(cur_ldpc_cap, (LDPC_VHT_ENABLE_TX | LDPC_VHT_CAP_TX));
+		RTW_INFO("Current STA(%d) VHT LDPC = %02X\n", psta->aid, cur_ldpc_cap);
+	}
+	pvhtpriv_sta->ldpc_cap = cur_ldpc_cap;
+
+	if (psta->bw_mode > pmlmeext->cur_bwmode)
+		psta->bw_mode = pmlmeext->cur_bwmode;
+
+	if (psta->bw_mode == CHANNEL_WIDTH_80) {
+		/* B5 Short GI for 80 MHz */
+		pvhtpriv_sta->sgi_80m = (GET_VHT_CAPABILITY_ELE_SHORT_GI80M(pvhtpriv_sta->vht_cap) & pvhtpriv_ap->sgi_80m) ? _TRUE : _FALSE;
+		/* RTW_INFO("Current STA ShortGI80MHz = %d\n", pvhtpriv_sta->sgi_80m); */
+	} else if (psta->bw_mode >= CHANNEL_WIDTH_160) {
+		/* B5 Short GI for 80 MHz */
+		pvhtpriv_sta->sgi_80m = (GET_VHT_CAPABILITY_ELE_SHORT_GI160M(pvhtpriv_sta->vht_cap) & pvhtpriv_ap->sgi_80m) ? _TRUE : _FALSE;
+		/* RTW_INFO("Current STA ShortGI160MHz = %d\n", pvhtpriv_sta->sgi_80m); */
+	}
+
+	/* B8 B9 B10 Rx STBC */
+	if (TEST_FLAG(pvhtpriv_ap->stbc_cap, STBC_VHT_ENABLE_TX) &&
+	    GET_VHT_CAPABILITY_ELE_RX_STBC(pvhtpriv_sta->vht_cap)) {
+		SET_FLAG(cur_stbc_cap, (STBC_VHT_ENABLE_TX | STBC_VHT_CAP_TX));
+		RTW_INFO("Current STA(%d) VHT STBC = %02X\n", psta->aid, cur_stbc_cap);
+	}
+	pvhtpriv_sta->stbc_cap = cur_stbc_cap;
+
+#ifdef CONFIG_BEAMFORMING
+	/* B11 SU Beamformer Capable, the target supports Beamformer and we are Beamformee */
+	if (TEST_FLAG(pvhtpriv_ap->beamform_cap, BEAMFORMING_VHT_BEAMFORMER_ENABLE) &&
+	    GET_VHT_CAPABILITY_ELE_SU_BFEE(pvhtpriv_sta->vht_cap)) {
+		SET_FLAG(cur_beamform_cap, BEAMFORMING_VHT_BEAMFORMEE_ENABLE);
+		/*Shift to BEAMFORMING_VHT_BEAMFORMER_STS_CAP*/
+		SET_FLAG(cur_beamform_cap, GET_VHT_CAPABILITY_ELE_SU_BFEE_STS_CAP(pvhtpriv_sta->vht_cap) << 8);
+	}
+
+	/* B12 SU Beamformee Capable, the target supports Beamformee and we are Beamformer */
+	if (TEST_FLAG(pvhtpriv_ap->beamform_cap, BEAMFORMING_VHT_BEAMFORMEE_ENABLE) &&
+	    GET_VHT_CAPABILITY_ELE_SU_BFER(pvhtpriv_sta->vht_cap)) {
+		SET_FLAG(cur_beamform_cap, BEAMFORMING_VHT_BEAMFORMER_ENABLE);
+		/*Shit to BEAMFORMING_VHT_BEAMFORMEE_SOUND_DIM*/
+		SET_FLAG(cur_beamform_cap, GET_VHT_CAPABILITY_ELE_SU_BFER_SOUND_DIM_NUM(pvhtpriv_sta->vht_cap) << 12);
+	}
+	pvhtpriv_sta->beamform_cap = cur_beamform_cap;
+	if (cur_beamform_cap)
+		RTW_INFO("Current STA(%d) VHT Beamforming Setting = %02X\n", psta->aid, cur_beamform_cap);
+#endif
+
+	/* B23 B24 B25 Maximum A-MPDU Length Exponent */
+	pvhtpriv_sta->ampdu_len = GET_VHT_CAPABILITY_ELE_MAX_RXAMPDU_FACTOR(pvhtpriv_sta->vht_cap);
+
+	pcap_mcs = GET_VHT_CAPABILITY_ELE_RX_MCS(pvhtpriv_sta->vht_cap);
+	_rtw_memcpy(pvhtpriv_sta->vht_mcs_map, pcap_mcs, 2);
+
+	pvhtpriv_sta->vht_highest_rate = rtw_get_vht_highest_rate(pvhtpriv_sta->vht_mcs_map);
+
+}
+
+void	update_hw_vht_param(_adapter *padapter)
+{
+	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
+	struct vht_priv		*pvhtpriv = &pmlmepriv->vhtpriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	u8	ht_AMPDU_len;
+
+	ht_AMPDU_len = pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x03;
+
+	if (pvhtpriv->ampdu_len > ht_AMPDU_len)
+		rtw_hal_set_hwreg(padapter, HW_VAR_AMPDU_FACTOR, (u8 *)(&pvhtpriv->ampdu_len));
+}
+
+void VHT_caps_handler(_adapter *padapter, PNDIS_802_11_VARIABLE_IEs pIE)
+{
+	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
+	struct vht_priv		*pvhtpriv = &pmlmepriv->vhtpriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	u8	cur_ldpc_cap = 0, cur_stbc_cap = 0, rf_type = RF_1T1R;
+	u16	cur_beamform_cap = 0;
+	u8	*pcap_mcs;
+	u8	vht_mcs[2];
+
+	if (pIE == NULL)
+		return;
+
+	if (pvhtpriv->vht_option == _FALSE)
+		return;
+
+	pmlmeinfo->VHT_enable = 1;
+
+	/* B4 Rx LDPC */
+	if (TEST_FLAG(pvhtpriv->ldpc_cap, LDPC_VHT_ENABLE_TX) &&
+	    GET_VHT_CAPABILITY_ELE_RX_LDPC(pIE->data)) {
+		SET_FLAG(cur_ldpc_cap, (LDPC_VHT_ENABLE_TX | LDPC_VHT_CAP_TX));
+		RTW_INFO("Current VHT LDPC Setting = %02X\n", cur_ldpc_cap);
+	}
+	pvhtpriv->ldpc_cap = cur_ldpc_cap;
+
+	/* B5 Short GI for 80 MHz */
+	pvhtpriv->sgi_80m = (GET_VHT_CAPABILITY_ELE_SHORT_GI80M(pIE->data) & pvhtpriv->sgi_80m) ? _TRUE : _FALSE;
+	/* RTW_INFO("Current ShortGI80MHz = %d\n", pvhtpriv->sgi_80m); */
+
+	/* B8 B9 B10 Rx STBC */
+	if (TEST_FLAG(pvhtpriv->stbc_cap, STBC_VHT_ENABLE_TX) &&
+	    GET_VHT_CAPABILITY_ELE_RX_STBC(pIE->data)) {
+		SET_FLAG(cur_stbc_cap, (STBC_VHT_ENABLE_TX | STBC_VHT_CAP_TX));
+		RTW_INFO("Current VHT STBC Setting = %02X\n", cur_stbc_cap);
+	}
+	pvhtpriv->stbc_cap = cur_stbc_cap;
+#ifdef CONFIG_BEAMFORMING
+#ifdef RTW_BEAMFORMING_VERSION_2
+	/*
+	 * B11 SU Beamformer Capable,
+	 * the target supports Beamformer and we are Beamformee
+	 */
+	if (TEST_FLAG(pvhtpriv->beamform_cap, BEAMFORMING_VHT_BEAMFORMEE_ENABLE)
+	    && GET_VHT_CAPABILITY_ELE_SU_BFER(pIE->data)) {
+		SET_FLAG(cur_beamform_cap, BEAMFORMING_VHT_BEAMFORMEE_ENABLE);
+
+		/* Shift to BEAMFORMING_VHT_BEAMFORMEE_STS_CAP */
+		SET_FLAG(cur_beamform_cap, GET_VHT_CAPABILITY_ELE_SU_BFEE_STS_CAP(pIE->data) << 8);
+
+		/*
+		 * B19 MU Beamformer Capable,
+		 * the target supports Beamformer and we are Beamformee
+		 */
+		if (TEST_FLAG(pvhtpriv->beamform_cap, BEAMFORMING_VHT_MU_MIMO_STA_ENABLE)
+		    && GET_VHT_CAPABILITY_ELE_MU_BFER(pIE->data))
+			SET_FLAG(cur_beamform_cap, BEAMFORMING_VHT_MU_MIMO_STA_ENABLE);
+	}
+
+	/*
+	 * B12 SU Beamformee Capable,
+	 * the target supports Beamformee and we are Beamformer
+	 */
+	if (TEST_FLAG(pvhtpriv->beamform_cap, BEAMFORMING_VHT_BEAMFORMER_ENABLE)
+	    && GET_VHT_CAPABILITY_ELE_SU_BFEE(pIE->data)) {
+		SET_FLAG(cur_beamform_cap, BEAMFORMING_VHT_BEAMFORMER_ENABLE);
+
+		/* Shit to BEAMFORMING_VHT_BEAMFORMER_SOUND_DIM */
+		SET_FLAG(cur_beamform_cap, GET_VHT_CAPABILITY_ELE_SU_BFER_SOUND_DIM_NUM(pIE->data) << 12);
+
+		/*
+		 * B20 MU Beamformee Capable,
+		 * the target supports Beamformee and we are Beamformer
+		 */
+		if (TEST_FLAG(pvhtpriv->beamform_cap, BEAMFORMING_VHT_MU_MIMO_AP_ENABLE)
+		    && GET_VHT_CAPABILITY_ELE_MU_BFEE(pIE->data))
+			SET_FLAG(cur_beamform_cap, BEAMFORMING_VHT_MU_MIMO_AP_ENABLE);
+	}
+
+	pvhtpriv->beamform_cap = cur_beamform_cap;
+	RTW_INFO("Current VHT Beamforming Setting=0x%04X\n", cur_beamform_cap);
+#else /* !RTW_BEAMFORMING_VERSION_2 */
+	/* B11 SU Beamformer Capable, the target supports Beamformer and we are Beamformee */
+	if (TEST_FLAG(pvhtpriv->beamform_cap, BEAMFORMING_VHT_BEAMFORMER_ENABLE) &&
+	    GET_VHT_CAPABILITY_ELE_SU_BFEE(pIE->data)) {
+		SET_FLAG(cur_beamform_cap, BEAMFORMING_VHT_BEAMFORMEE_ENABLE);
+		/*Shift to BEAMFORMING_VHT_BEAMFORMER_STS_CAP*/
+		SET_FLAG(cur_beamform_cap, GET_VHT_CAPABILITY_ELE_SU_BFEE_STS_CAP(pIE->data) << 8);
+	}
+
+	/* B12 SU Beamformee Capable, the target supports Beamformee and we are Beamformer */
+	if (TEST_FLAG(pvhtpriv->beamform_cap, BEAMFORMING_VHT_BEAMFORMEE_ENABLE) &&
+	    GET_VHT_CAPABILITY_ELE_SU_BFER(pIE->data)) {
+		SET_FLAG(cur_beamform_cap, BEAMFORMING_VHT_BEAMFORMER_ENABLE);
+		/*Shit to BEAMFORMING_VHT_BEAMFORMEE_SOUND_DIM*/
+		SET_FLAG(cur_beamform_cap, GET_VHT_CAPABILITY_ELE_SU_BFER_SOUND_DIM_NUM(pIE->data) << 12);
+
+	}
+	pvhtpriv->beamform_cap = cur_beamform_cap;
+	if (cur_beamform_cap)
+		RTW_INFO("Current VHT Beamforming Setting = %02X\n", cur_beamform_cap);
+#endif /* !RTW_BEAMFORMING_VERSION_2 */
+#endif /* CONFIG_BEAMFORMING */
+	/* B23 B24 B25 Maximum A-MPDU Length Exponent */
+	pvhtpriv->ampdu_len = GET_VHT_CAPABILITY_ELE_MAX_RXAMPDU_FACTOR(pIE->data);
+
+	pcap_mcs = GET_VHT_CAPABILITY_ELE_RX_MCS(pIE->data);
+	_rtw_memcpy(vht_mcs, pcap_mcs, 2);
+
+	rtw_hal_get_hwreg(padapter, HW_VAR_RF_TYPE, (u8 *)(&rf_type));
+	if ((rf_type == RF_1T1R) || (rf_type == RF_1T2R))
+		vht_mcs[0] |= 0xfc;
+	else if (rf_type == RF_2T2R)
+		vht_mcs[0] |= 0xf0;
+	else if (rf_type == RF_3T3R)
+		vht_mcs[0] |= 0xc0;
+
+	_rtw_memcpy(pvhtpriv->vht_mcs_map, vht_mcs, 2);
+
+	pvhtpriv->vht_highest_rate = rtw_get_vht_highest_rate(pvhtpriv->vht_mcs_map);
+}
+
+void VHT_operation_handler(_adapter *padapter, PNDIS_802_11_VARIABLE_IEs pIE)
+{
+	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
+	struct vht_priv		*pvhtpriv = &pmlmepriv->vhtpriv;
+
+	if (pIE == NULL)
+		return;
+
+	if (pvhtpriv->vht_option == _FALSE)
+		return;
+}
+
+void rtw_process_vht_op_mode_notify(_adapter *padapter, u8 *pframe, PVOID sta)
+{
+	struct sta_info		*psta = (struct sta_info *)sta;
+	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
+	struct vht_priv		*pvhtpriv = &pmlmepriv->vhtpriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct registry_priv *regsty = adapter_to_regsty(padapter);
+	u8	target_bw;
+	u8	target_rxss, current_rxss;
+	u8	update_ra = _FALSE;
+	u8	vht_mcs_map[2] = {};
+
+	if (pvhtpriv->vht_option == _FALSE)
+		return;
+
+	target_bw = GET_VHT_OPERATING_MODE_FIELD_CHNL_WIDTH(pframe);
+	target_rxss = (GET_VHT_OPERATING_MODE_FIELD_RX_NSS(pframe) + 1);
+
+	if (target_bw != psta->bw_mode) {
+		if (hal_is_bw_support(padapter, target_bw)
+		    && REGSTY_IS_BW_5G_SUPPORT(regsty, target_bw)
+		   ) {
+			update_ra = _TRUE;
+			psta->bw_mode = target_bw;
+		}
+	}
+
+	current_rxss = rtw_vht_mcsmap_to_nss(psta->vhtpriv.vht_mcs_map);
+	if (target_rxss != current_rxss) {
+		update_ra = _TRUE;
+
+		rtw_vht_nss_to_mcsmap(target_rxss, vht_mcs_map, psta->vhtpriv.vht_mcs_map);
+		_rtw_memcpy(psta->vhtpriv.vht_mcs_map, vht_mcs_map, 2);
+
+		rtw_hal_update_sta_rate_mask(padapter, psta);
+	}
+
+	if (update_ra)
+		rtw_dm_ra_mask_wk_cmd(padapter, (u8 *)psta);
+}
+
+u32	rtw_build_vht_operation_ie(_adapter *padapter, u8 *pbuf, u8 channel)
+{
+	struct registry_priv	*pregistrypriv = &padapter->registrypriv;
+	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
+	struct vht_priv		*pvhtpriv = &pmlmepriv->vhtpriv;
+	/* struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv; */
+	u8	ChnlWidth, center_freq, bw_mode, rf_type = 0;
+	u32	len = 0;
+	u8	operation[5];
+
+	rtw_hal_get_hwreg(padapter, HW_VAR_RF_TYPE, (u8 *)(&rf_type));
+
+	_rtw_memset(operation, 0, 5);
+
+	bw_mode = REGSTY_BW_5G(pregistrypriv); /* TODO: control op bw with other info */
+
+	if (hal_chk_bw_cap(padapter, BW_CAP_80M | BW_CAP_160M)
+	    && REGSTY_BW_5G(pregistrypriv) >= CHANNEL_WIDTH_80
+	   ) {
+		center_freq = rtw_get_center_ch(channel, bw_mode, HAL_PRIME_CHNL_OFFSET_LOWER);
+		ChnlWidth = 1;
+	} else {
+		center_freq = 0;
+		ChnlWidth = 0;
+	}
+
+
+	SET_VHT_OPERATION_ELE_CHL_WIDTH(operation, ChnlWidth);
+	/* center frequency */
+	SET_VHT_OPERATION_ELE_CHL_CENTER_FREQ1(operation, center_freq);/* Todo: need to set correct center channel */
+	SET_VHT_OPERATION_ELE_CHL_CENTER_FREQ2(operation, 0);
+
+	if (padapter->registrypriv.rf_config != RF_MAX_TYPE)
+		rf_type = padapter->registrypriv.rf_config;
+
+	switch (rf_type) {
+	case RF_1T1R:
+		operation[3] = 0xfe;
+		operation[4] = 0xff;
+		break;
+	case RF_1T2R:
+	case RF_2T2R:
+	case RF_2T2R_GREEN:
+		operation[3] = 0xfa;
+		operation[4] = 0xff;
+		break;
+	case RF_2T3R:
+	case RF_2T4R:
+	case RF_3T3R:
+	case RF_3T4R:
+		operation[3] = 0xea;
+		operation[4] = 0xff;
+		break;
+	case RF_4T4R:
+		operation[3] = 0xaa;
+		operation[4] = 0xff;
+		break;
+	default:
+		RTW_INFO("%s, %d, unknown rf type\n", __func__, __LINE__);
+	}
+
+	rtw_set_ie(pbuf, EID_VHTOperation, 5, operation, &len);
+
+	return len;
+}
+
+u32	rtw_build_vht_op_mode_notify_ie(_adapter *padapter, u8 *pbuf, u8 bw)
+{
+	/* struct registry_priv *pregistrypriv = &padapter->registrypriv; */
+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+	struct vht_priv	*pvhtpriv = &pmlmepriv->vhtpriv;
+	u32	len = 0;
+	u8	opmode = 0, rf_type = 0;
+	u8	chnl_width, rx_nss;
+
+	chnl_width = bw;
+
+	rtw_hal_get_hwreg(padapter, HW_VAR_RF_TYPE, (u8 *)(&rf_type));
+	if (rf_type == RF_3T3R)
+		rx_nss = 3;
+	else if (rf_type == RF_2T2R)
+		rx_nss = 2;
+	else
+		rx_nss = 1;
+
+	SET_VHT_OPERATING_MODE_FIELD_CHNL_WIDTH(&opmode, chnl_width);
+	SET_VHT_OPERATING_MODE_FIELD_RX_NSS(&opmode, (rx_nss - 1));
+	SET_VHT_OPERATING_MODE_FIELD_RX_NSS_TYPE(&opmode, 0); /* Todo */
+
+	pvhtpriv->vht_op_mode_notify = opmode;
+
+	pbuf = rtw_set_ie(pbuf, EID_OpModeNotification, 1, &opmode, &len);
+
+	return len;
+}
+
+u32	rtw_build_vht_cap_ie(_adapter *padapter, u8 *pbuf)
+{
+	u8	bw, rf_type, rf_num, rx_stbc_nss = 0;
+	u16	HighestRate;
+	u8	*pcap, *pcap_mcs;
+	u32	len = 0;
+	u32 rx_packet_offset, max_recvbuf_sz;
+	struct registry_priv *pregistrypriv = &padapter->registrypriv;
+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+	struct vht_priv	*pvhtpriv = &pmlmepriv->vhtpriv;
+
+	pcap = pvhtpriv->vht_cap;
+	_rtw_memset(pcap, 0, 32);
+
+	/* B0 B1 Maximum MPDU Length */
+	rtw_hal_get_def_var(padapter, HAL_DEF_RX_PACKET_OFFSET, &rx_packet_offset);
+	rtw_hal_get_def_var(padapter, HAL_DEF_MAX_RECVBUF_SZ, &max_recvbuf_sz);
+
+	RTW_DBG("%s, line%d, Available RX buf size = %d bytes\n.", __FUNCTION__, __LINE__, max_recvbuf_sz - rx_packet_offset);
+
+	if ((max_recvbuf_sz - rx_packet_offset) >= 11454) {
+		SET_VHT_CAPABILITY_ELE_MAX_MPDU_LENGTH(pcap, 2);
+		RTW_INFO("%s, line%d, Set MAX MPDU len = 11454 bytes\n.", __FUNCTION__, __LINE__);
+	} else if ((max_recvbuf_sz - rx_packet_offset) >= 7991) {
+		SET_VHT_CAPABILITY_ELE_MAX_MPDU_LENGTH(pcap, 1);
+		RTW_INFO("%s, line%d, Set MAX MPDU len = 7991 bytes\n.", __FUNCTION__, __LINE__);
+	} else if ((max_recvbuf_sz - rx_packet_offset) >= 3895) {
+		SET_VHT_CAPABILITY_ELE_MAX_MPDU_LENGTH(pcap, 0);
+		RTW_INFO("%s, line%d, Set MAX MPDU len = 3895 bytes\n.", __FUNCTION__, __LINE__);
+	} else
+		RTW_ERR("%s, line%d, Error!! Available RX buf size < 3895 bytes\n.", __FUNCTION__, __LINE__);
+
+	/* B2 B3 Supported Channel Width Set */
+	if (hal_chk_bw_cap(padapter, BW_CAP_160M) && REGSTY_IS_BW_5G_SUPPORT(pregistrypriv, CHANNEL_WIDTH_160)) {
+		if (hal_chk_bw_cap(padapter, BW_CAP_80_80M) && REGSTY_IS_BW_5G_SUPPORT(pregistrypriv, CHANNEL_WIDTH_80_80))
+			SET_VHT_CAPABILITY_ELE_CHL_WIDTH(pcap, 2);
+		else
+			SET_VHT_CAPABILITY_ELE_CHL_WIDTH(pcap, 1);
+	} else
+		SET_VHT_CAPABILITY_ELE_CHL_WIDTH(pcap, 0);
+
+	/* B4 Rx LDPC */
+	if (TEST_FLAG(pvhtpriv->ldpc_cap, LDPC_VHT_ENABLE_RX))
+		SET_VHT_CAPABILITY_ELE_RX_LDPC(pcap, 1);
+
+	/* B5 ShortGI for 80MHz */
+	SET_VHT_CAPABILITY_ELE_SHORT_GI80M(pcap, pvhtpriv->sgi_80m ? 1 : 0); /* We can receive Short GI of 80M */
+
+	/* B6 ShortGI for 160MHz */
+	/* SET_VHT_CAPABILITY_ELE_SHORT_GI160M(pcap, pvhtpriv->sgi_80m? 1 : 0); */
+
+	/* B7 Tx STBC */
+	if (TEST_FLAG(pvhtpriv->stbc_cap, STBC_VHT_ENABLE_TX))
+		SET_VHT_CAPABILITY_ELE_TX_STBC(pcap, 1);
+
+	/* B8 B9 B10 Rx STBC */
+	if (TEST_FLAG(pvhtpriv->stbc_cap, STBC_VHT_ENABLE_RX)) {
+		rtw_hal_get_def_var(padapter, HAL_DEF_RX_STBC, (u8 *)(&rx_stbc_nss));
+
+		SET_VHT_CAPABILITY_ELE_RX_STBC(pcap, rx_stbc_nss);
+	}
+
+	/* B11 SU Beamformer Capable */
+	if (TEST_FLAG(pvhtpriv->beamform_cap, BEAMFORMING_VHT_BEAMFORMER_ENABLE)) {
+		SET_VHT_CAPABILITY_ELE_SU_BFER(pcap, 1);
+		/* B16 17 18 Number of Sounding Dimensions */
+		rtw_hal_get_def_var(padapter, HAL_DEF_BEAMFORMER_CAP, (u8 *)&rf_num);
+		SET_VHT_CAPABILITY_ELE_SOUNDING_DIMENSIONS(pcap, rf_num);
+		/* B19 MU Beamformer Capable */
+		if (TEST_FLAG(pvhtpriv->beamform_cap, BEAMFORMING_VHT_MU_MIMO_AP_ENABLE))
+			SET_VHT_CAPABILITY_ELE_MU_BFER(pcap, 1);
+	}
+
+	/* B12 SU Beamformee Capable */
+	if (TEST_FLAG(pvhtpriv->beamform_cap, BEAMFORMING_VHT_BEAMFORMEE_ENABLE)) {
+		SET_VHT_CAPABILITY_ELE_SU_BFEE(pcap, 1);
+		/* B13 14 15 Compressed Steering Number of Beamformer Antennas Supported */
+		rtw_hal_get_def_var(padapter, HAL_DEF_BEAMFORMEE_CAP, (u8 *)&rf_num);
+		SET_VHT_CAPABILITY_ELE_BFER_ANT_SUPP(pcap, rf_num);
+		/* B20 SU Beamformee Capable */
+		if (TEST_FLAG(pvhtpriv->beamform_cap, BEAMFORMING_VHT_MU_MIMO_STA_ENABLE))
+			SET_VHT_CAPABILITY_ELE_MU_BFEE(pcap, 1);
+	}
+
+	/* B21 VHT TXOP PS */
+	SET_VHT_CAPABILITY_ELE_TXOP_PS(pcap, 0);
+	/* B22 +HTC-VHT Capable */
+	SET_VHT_CAPABILITY_ELE_HTC_VHT(pcap, 1);
+	/* B23 24 25 Maximum A-MPDU Length Exponent */
+	if (pregistrypriv->ampdu_factor != 0xFE)
+		SET_VHT_CAPABILITY_ELE_MAX_RXAMPDU_FACTOR(pcap, pregistrypriv->ampdu_factor);
+	else
+		SET_VHT_CAPABILITY_ELE_MAX_RXAMPDU_FACTOR(pcap, 7);
+	/* B26 27 VHT Link Adaptation Capable */
+	SET_VHT_CAPABILITY_ELE_LINK_ADAPTION(pcap, 0);
+
+	pcap_mcs = GET_VHT_CAPABILITY_ELE_RX_MCS(pcap);
+	_rtw_memcpy(pcap_mcs, pvhtpriv->vht_mcs_map, 2);
+
+	pcap_mcs = GET_VHT_CAPABILITY_ELE_TX_MCS(pcap);
+	_rtw_memcpy(pcap_mcs, pvhtpriv->vht_mcs_map, 2);
+
+	/* find the largest bw supported by both registry and hal */
+	bw = hal_largest_bw(padapter, REGSTY_BW_5G(pregistrypriv));
+
+	HighestRate = VHT_MCS_DATA_RATE[bw][pvhtpriv->sgi_80m][((pvhtpriv->vht_highest_rate - MGN_VHT1SS_MCS0) & 0x3f)];
+	HighestRate = (HighestRate + 1) >> 1;
+
+	SET_VHT_CAPABILITY_ELE_MCS_RX_HIGHEST_RATE(pcap, HighestRate); /* indicate we support highest rx rate is 600Mbps. */
+	SET_VHT_CAPABILITY_ELE_MCS_TX_HIGHEST_RATE(pcap, HighestRate); /* indicate we support highest tx rate is 600Mbps. */
+
+	pbuf = rtw_set_ie(pbuf, EID_VHTCapability, 12, pcap, &len);
+
+	return len;
+}
+
+u32 rtw_restructure_vht_ie(_adapter *padapter, u8 *in_ie, u8 *out_ie, uint in_len, uint *pout_len)
+{
+	u32	ielen = 0, out_len = 0;
+	u8	cap_len = 0, notify_len = 0, notify_bw = 0, operation_bw = 0, supported_chnl_width = 0;
+	u8	*p, *pframe;
+	struct registry_priv *pregistrypriv = &padapter->registrypriv;
+	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
+	struct vht_priv	*pvhtpriv = &pmlmepriv->vhtpriv;
+
+	rtw_vht_use_default_setting(padapter);
+
+	p = rtw_get_ie(in_ie + 12, EID_VHTCapability, &ielen, in_len - 12);
+	if (p && ielen > 0) {
+		supported_chnl_width = GET_VHT_CAPABILITY_ELE_CHL_WIDTH(p + 2);
+
+		/* VHT Capabilities element */
+		cap_len = rtw_build_vht_cap_ie(padapter, out_ie + *pout_len);
+		*pout_len += cap_len;
+
+		/* Get HT BW */
+		p = rtw_get_ie(in_ie + 12, _HT_EXTRA_INFO_IE_, &ielen, in_len - 12);
+		if (p && ielen > 0) {
+			struct HT_info_element *pht_info = (struct HT_info_element *)(p + 2);
+			if (pht_info->infos[0] & BIT(2))
+				operation_bw = CHANNEL_WIDTH_40;
+			else
+				operation_bw = CHANNEL_WIDTH_20;
+		}
+
+		/* VHT Operation element */
+		p = rtw_get_ie(in_ie + 12, EID_VHTOperation, &ielen, in_len - 12);
+		if (p && ielen > 0) {
+			out_len = *pout_len;
+			if (GET_VHT_OPERATION_ELE_CHL_WIDTH(p + 2) >= 1) {
+				if (supported_chnl_width == 2)
+					operation_bw = CHANNEL_WIDTH_80_80;
+				else if (supported_chnl_width == 1)
+					operation_bw = CHANNEL_WIDTH_160;
+				else
+					operation_bw = CHANNEL_WIDTH_80;
+			}
+			pframe = rtw_set_ie(out_ie + out_len, EID_VHTOperation, ielen, p + 2 , pout_len);
+		}
+
+		/* find the largest bw supported by both registry and hal */
+		notify_bw = hal_largest_bw(padapter, REGSTY_BW_5G(pregistrypriv));
+
+		if (notify_bw > operation_bw)
+			notify_bw = operation_bw;
+
+		/* Operating Mode Notification element */
+		notify_len = rtw_build_vht_op_mode_notify_ie(padapter, out_ie + *pout_len, notify_bw);
+		*pout_len += notify_len;
+
+		pvhtpriv->vht_option = _TRUE;
+	}
+
+	return pvhtpriv->vht_option;
+
+}
+
+void VHTOnAssocRsp(_adapter *padapter)
+{
+	struct mlme_priv		*pmlmepriv = &padapter->mlmepriv;
+	struct vht_priv		*pvhtpriv = &pmlmepriv->vhtpriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	u8	ht_AMPDU_len;
+
+	RTW_INFO("%s\n", __FUNCTION__);
+
+	if (!pmlmeinfo->HT_enable)
+		return;
+
+	if (!pmlmeinfo->VHT_enable)
+		return;
+
+	ht_AMPDU_len = pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x03;
+
+	if (pvhtpriv->ampdu_len > ht_AMPDU_len)
+		rtw_hal_set_hwreg(padapter, HW_VAR_AMPDU_FACTOR, (u8 *)(&pvhtpriv->ampdu_len));
+
+	rtw_hal_set_hwreg(padapter, HW_VAR_AMPDU_MAX_TIME, (u8 *)(&pvhtpriv->vht_highest_rate));
+}
+
+#endif /* CONFIG_80211AC_VHT */

core/rtw_wapi_sms4.c

@@ -0,0 +1,908 @@
+#ifdef CONFIG_WAPI_SUPPORT
+
+#include <linux/unistd.h>
+#include <linux/etherdevice.h>
+#include <drv_types.h>
+#include <rtw_wapi.h>
+
+
+#ifdef CONFIG_WAPI_SW_SMS4
+
+#define WAPI_LITTLE_ENDIAN
+/* #define BIG_ENDIAN */
+#define ENCRYPT  0
+#define DECRYPT  1
+
+
+/**********************************************************
+ **********************************************************/
+const u8 Sbox[256] = {
+	0xd6, 0x90, 0xe9, 0xfe, 0xcc, 0xe1, 0x3d, 0xb7, 0x16, 0xb6, 0x14, 0xc2, 0x28, 0xfb, 0x2c, 0x05,
+	0x2b, 0x67, 0x9a, 0x76, 0x2a, 0xbe, 0x04, 0xc3, 0xaa, 0x44, 0x13, 0x26, 0x49, 0x86, 0x06, 0x99,
+	0x9c, 0x42, 0x50, 0xf4, 0x91, 0xef, 0x98, 0x7a, 0x33, 0x54, 0x0b, 0x43, 0xed, 0xcf, 0xac, 0x62,
+	0xe4, 0xb3, 0x1c, 0xa9, 0xc9, 0x08, 0xe8, 0x95, 0x80, 0xdf, 0x94, 0xfa, 0x75, 0x8f, 0x3f, 0xa6,
+	0x47, 0x07, 0xa7, 0xfc, 0xf3, 0x73, 0x17, 0xba, 0x83, 0x59, 0x3c, 0x19, 0xe6, 0x85, 0x4f, 0xa8,
+	0x68, 0x6b, 0x81, 0xb2, 0x71, 0x64, 0xda, 0x8b, 0xf8, 0xeb, 0x0f, 0x4b, 0x70, 0x56, 0x9d, 0x35,
+	0x1e, 0x24, 0x0e, 0x5e, 0x63, 0x58, 0xd1, 0xa2, 0x25, 0x22, 0x7c, 0x3b, 0x01, 0x21, 0x78, 0x87,
+	0xd4, 0x00, 0x46, 0x57, 0x9f, 0xd3, 0x27, 0x52, 0x4c, 0x36, 0x02, 0xe7, 0xa0, 0xc4, 0xc8, 0x9e,
+	0xea, 0xbf, 0x8a, 0xd2, 0x40, 0xc7, 0x38, 0xb5, 0xa3, 0xf7, 0xf2, 0xce, 0xf9, 0x61, 0x15, 0xa1,
+	0xe0, 0xae, 0x5d, 0xa4, 0x9b, 0x34, 0x1a, 0x55, 0xad, 0x93, 0x32, 0x30, 0xf5, 0x8c, 0xb1, 0xe3,
+	0x1d, 0xf6, 0xe2, 0x2e, 0x82, 0x66, 0xca, 0x60, 0xc0, 0x29, 0x23, 0xab, 0x0d, 0x53, 0x4e, 0x6f,
+	0xd5, 0xdb, 0x37, 0x45, 0xde, 0xfd, 0x8e, 0x2f, 0x03, 0xff, 0x6a, 0x72, 0x6d, 0x6c, 0x5b, 0x51,
+	0x8d, 0x1b, 0xaf, 0x92, 0xbb, 0xdd, 0xbc, 0x7f, 0x11, 0xd9, 0x5c, 0x41, 0x1f, 0x10, 0x5a, 0xd8,
+	0x0a, 0xc1, 0x31, 0x88, 0xa5, 0xcd, 0x7b, 0xbd, 0x2d, 0x74, 0xd0, 0x12, 0xb8, 0xe5, 0xb4, 0xb0,
+	0x89, 0x69, 0x97, 0x4a, 0x0c, 0x96, 0x77, 0x7e, 0x65, 0xb9, 0xf1, 0x09, 0xc5, 0x6e, 0xc6, 0x84,
+	0x18, 0xf0, 0x7d, 0xec, 0x3a, 0xdc, 0x4d, 0x20, 0x79, 0xee, 0x5f, 0x3e, 0xd7, 0xcb, 0x39, 0x48
+};
+
+const u32 CK[32] = {
+	0x00070e15, 0x1c232a31, 0x383f464d, 0x545b6269,
+	0x70777e85, 0x8c939aa1, 0xa8afb6bd, 0xc4cbd2d9,
+	0xe0e7eef5, 0xfc030a11, 0x181f262d, 0x343b4249,
+	0x50575e65, 0x6c737a81, 0x888f969d, 0xa4abb2b9,
+	0xc0c7ced5, 0xdce3eaf1, 0xf8ff060d, 0x141b2229,
+	0x30373e45, 0x4c535a61, 0x686f767d, 0x848b9299,
+	0xa0a7aeb5, 0xbcc3cad1, 0xd8dfe6ed, 0xf4fb0209,
+	0x10171e25, 0x2c333a41, 0x484f565d, 0x646b7279
+};
+
+#define Rotl(_x, _y) (((_x) << (_y)) | ((_x) >> (32 - (_y))))
+
+#define ByteSub(_A) (Sbox[(_A) >> 24 & 0xFF] << 24 | \
+		     Sbox[(_A) >> 16 & 0xFF] << 16 | \
+		     Sbox[(_A) >>  8 & 0xFF] <<  8 | \
+		     Sbox[(_A) & 0xFF])
+
+#define L1(_B) ((_B) ^ Rotl(_B, 2) ^ Rotl(_B, 10) ^ Rotl(_B, 18) ^ Rotl(_B, 24))
+#define L2(_B) ((_B) ^ Rotl(_B, 13) ^ Rotl(_B, 23))
+
+static void
+xor_block(void *dst, void *src1, void *src2)
+/* 128-bit xor: *dst = *src1 xor *src2. Pointers must be 32-bit aligned */
+{
+	((u32 *)dst)[0] = ((u32 *)src1)[0] ^ ((u32 *)src2)[0];
+	((u32 *)dst)[1] = ((u32 *)src1)[1] ^ ((u32 *)src2)[1];
+	((u32 *)dst)[2] = ((u32 *)src1)[2] ^ ((u32 *)src2)[2];
+	((u32 *)dst)[3] = ((u32 *)src1)[3] ^ ((u32 *)src2)[3];
+}
+
+
+void SMS4Crypt(u8 *Input, u8 *Output, u32 *rk)
+{
+	u32 r, mid, x0, x1, x2, x3, *p;
+	p = (u32 *)Input;
+	x0 = p[0];
+	x1 = p[1];
+	x2 = p[2];
+	x3 = p[3];
+#ifdef WAPI_LITTLE_ENDIAN
+	x0 = Rotl(x0, 16);
+	x0 = ((x0 & 0x00FF00FF) << 8) | ((x0 & 0xFF00FF00) >> 8);
+	x1 = Rotl(x1, 16);
+	x1 = ((x1 & 0x00FF00FF) << 8) | ((x1 & 0xFF00FF00) >> 8);
+	x2 = Rotl(x2, 16);
+	x2 = ((x2 & 0x00FF00FF) << 8) | ((x2 & 0xFF00FF00) >> 8);
+	x3 = Rotl(x3, 16);
+	x3 = ((x3 & 0x00FF00FF) << 8) | ((x3 & 0xFF00FF00) >> 8);
+#endif
+	for (r = 0; r < 32; r += 4) {
+		mid = x1 ^ x2 ^ x3 ^ rk[r + 0];
+		mid = ByteSub(mid);
+		x0 ^= L1(mid);
+		mid = x2 ^ x3 ^ x0 ^ rk[r + 1];
+		mid = ByteSub(mid);
+		x1 ^= L1(mid);
+		mid = x3 ^ x0 ^ x1 ^ rk[r + 2];
+		mid = ByteSub(mid);
+		x2 ^= L1(mid);
+		mid = x0 ^ x1 ^ x2 ^ rk[r + 3];
+		mid = ByteSub(mid);
+		x3 ^= L1(mid);
+	}
+#ifdef WAPI_LITTLE_ENDIAN
+	x0 = Rotl(x0, 16);
+	x0 = ((x0 & 0x00FF00FF) << 8) | ((x0 & 0xFF00FF00) >> 8);
+	x1 = Rotl(x1, 16);
+	x1 = ((x1 & 0x00FF00FF) << 8) | ((x1 & 0xFF00FF00) >> 8);
+	x2 = Rotl(x2, 16);
+	x2 = ((x2 & 0x00FF00FF) << 8) | ((x2 & 0xFF00FF00) >> 8);
+	x3 = Rotl(x3, 16);
+	x3 = ((x3 & 0x00FF00FF) << 8) | ((x3 & 0xFF00FF00) >> 8);
+#endif
+	p = (u32 *)Output;
+	p[0] = x3;
+	p[1] = x2;
+	p[2] = x1;
+	p[3] = x0;
+}
+
+
+
+void SMS4KeyExt(u8 *Key, u32 *rk, u32 CryptFlag)
+{
+	u32 r, mid, x0, x1, x2, x3, *p;
+
+	p = (u32 *)Key;
+	x0 = p[0];
+	x1 = p[1];
+	x2 = p[2];
+	x3 = p[3];
+#ifdef WAPI_LITTLE_ENDIAN
+	x0 = Rotl(x0, 16);
+	x0 = ((x0 & 0xFF00FF) << 8) | ((x0 & 0xFF00FF00) >> 8);
+	x1 = Rotl(x1, 16);
+	x1 = ((x1 & 0xFF00FF) << 8) | ((x1 & 0xFF00FF00) >> 8);
+	x2 = Rotl(x2, 16);
+	x2 = ((x2 & 0xFF00FF) << 8) | ((x2 & 0xFF00FF00) >> 8);
+	x3 = Rotl(x3, 16);
+	x3 = ((x3 & 0xFF00FF) << 8) | ((x3 & 0xFF00FF00) >> 8);
+#endif
+
+	x0 ^= 0xa3b1bac6;
+	x1 ^= 0x56aa3350;
+	x2 ^= 0x677d9197;
+	x3 ^= 0xb27022dc;
+	for (r = 0; r < 32; r += 4) {
+		mid = x1 ^ x2 ^ x3 ^ CK[r + 0];
+		mid = ByteSub(mid);
+		rk[r + 0] = x0 ^= L2(mid);
+		mid = x2 ^ x3 ^ x0 ^ CK[r + 1];
+		mid = ByteSub(mid);
+		rk[r + 1] = x1 ^= L2(mid);
+		mid = x3 ^ x0 ^ x1 ^ CK[r + 2];
+		mid = ByteSub(mid);
+		rk[r + 2] = x2 ^= L2(mid);
+		mid = x0 ^ x1 ^ x2 ^ CK[r + 3];
+		mid = ByteSub(mid);
+		rk[r + 3] = x3 ^= L2(mid);
+	}
+	if (CryptFlag == DECRYPT) {
+		for (r = 0; r < 16; r++)
+			mid = rk[r], rk[r] = rk[31 - r], rk[31 - r] = mid;
+	}
+}
+
+
+void WapiSMS4Cryption(u8 *Key, u8 *IV, u8 *Input, u16 InputLength,
+		      u8 *Output, u16 *OutputLength, u32 CryptFlag)
+{
+	u32 blockNum, i, j, rk[32];
+	u16 remainder;
+	u8 blockIn[16], blockOut[16], tempIV[16], k;
+
+	*OutputLength = 0;
+	remainder = InputLength & 0x0F;
+	blockNum = InputLength >> 4;
+	if (remainder != 0)
+		blockNum++;
+	else
+		remainder = 16;
+
+	for (k = 0; k < 16; k++)
+		tempIV[k] = IV[15 - k];
+
+	memcpy(blockIn, tempIV, 16);
+
+	SMS4KeyExt((u8 *)Key, rk, CryptFlag);
+
+	for (i = 0; i < blockNum - 1; i++) {
+		SMS4Crypt((u8 *)blockIn, blockOut, rk);
+		xor_block(&Output[i * 16], &Input[i * 16], blockOut);
+		memcpy(blockIn, blockOut, 16);
+	}
+
+	*OutputLength = i * 16;
+
+	SMS4Crypt((u8 *)blockIn, blockOut, rk);
+
+	for (j = 0; j < remainder; j++)
+		Output[i * 16 + j] = Input[i * 16 + j] ^ blockOut[j];
+	*OutputLength += remainder;
+
+}
+
+void WapiSMS4Encryption(u8 *Key, u8 *IV, u8 *Input, u16 InputLength,
+			u8 *Output, u16 *OutputLength)
+{
+
+	WapiSMS4Cryption(Key, IV, Input, InputLength, Output, OutputLength, ENCRYPT);
+}
+
+void WapiSMS4Decryption(u8 *Key, u8 *IV, u8 *Input, u16 InputLength,
+			u8 *Output, u16 *OutputLength)
+{
+	/* OFB mode: is also ENCRYPT flag */
+	WapiSMS4Cryption(Key, IV, Input, InputLength, Output, OutputLength, ENCRYPT);
+}
+
+void WapiSMS4CalculateMic(u8 *Key, u8 *IV, u8 *Input1, u8 Input1Length,
+		  u8 *Input2, u16 Input2Length, u8 *Output, u8 *OutputLength)
+{
+	u32 blockNum, i, remainder, rk[32];
+	u8 BlockIn[16], BlockOut[16], TempBlock[16], tempIV[16], k;
+
+	*OutputLength = 0;
+	remainder = Input1Length & 0x0F;
+	blockNum = Input1Length >> 4;
+
+	for (k = 0; k < 16; k++)
+		tempIV[k] = IV[15 - k];
+
+	memcpy(BlockIn, tempIV, 16);
+
+	SMS4KeyExt((u8 *)Key, rk, ENCRYPT);
+
+	SMS4Crypt((u8 *)BlockIn, BlockOut, rk);
+
+	for (i = 0; i < blockNum; i++) {
+		xor_block(BlockIn, (Input1 + i * 16), BlockOut);
+		SMS4Crypt((u8 *)BlockIn, BlockOut, rk);
+	}
+
+	if (remainder != 0) {
+		memset(TempBlock, 0, 16);
+		memcpy(TempBlock, (Input1 + blockNum * 16), remainder);
+
+		xor_block(BlockIn, TempBlock, BlockOut);
+		SMS4Crypt((u8 *)BlockIn, BlockOut, rk);
+	}
+
+	remainder = Input2Length & 0x0F;
+	blockNum = Input2Length >> 4;
+
+	for (i = 0; i < blockNum; i++) {
+		xor_block(BlockIn, (Input2 + i * 16), BlockOut);
+		SMS4Crypt((u8 *)BlockIn, BlockOut, rk);
+	}
+
+	if (remainder != 0) {
+		memset(TempBlock, 0, 16);
+		memcpy(TempBlock, (Input2 + blockNum * 16), remainder);
+
+		xor_block(BlockIn, TempBlock, BlockOut);
+		SMS4Crypt((u8 *)BlockIn, BlockOut, rk);
+	}
+
+	memcpy(Output, BlockOut, 16);
+	*OutputLength = 16;
+}
+
+void SecCalculateMicSMS4(
+	u8		KeyIdx,
+	u8        *MicKey,
+	u8        *pHeader,
+	u8        *pData,
+	u16       DataLen,
+	u8        *MicBuffer
+)
+{
+#if 0
+	struct ieee80211_hdr_3addr_qos *header;
+	u8 TempBuf[34], TempLen = 32, MicLen, QosOffset, *IV;
+	u16 *pTemp, fc;
+
+	WAPI_TRACE(WAPI_TX | WAPI_RX, "=========>%s\n", __FUNCTION__);
+
+	header = (struct ieee80211_hdr_3addr_qos *)pHeader;
+	memset(TempBuf, 0, 34);
+	memcpy(TempBuf, pHeader, 2); /* FrameCtrl */
+	pTemp = (u16 *)TempBuf;
+	*pTemp &= 0xc78f;       /* bit4,5,6,11,12,13 */
+
+	memcpy((TempBuf + 2), (pHeader + 4), 12); /* Addr1, Addr2 */
+	memcpy((TempBuf + 14), (pHeader + 22), 2); /* SeqCtrl */
+	pTemp = (u16 *)(TempBuf + 14);
+	*pTemp &= 0x000f;
+
+	memcpy((TempBuf + 16), (pHeader + 16), 6); /* Addr3 */
+
+	fc = le16_to_cpu(header->frame_ctl);
+
+
+
+	if (GetFrDs((u16 *)&fc) && GetToDs((u16 *)&fc)) {
+		memcpy((TempBuf + 22), (pHeader + 24), 6);
+		QosOffset = 30;
+	} else {
+		memset((TempBuf + 22), 0, 6);
+		QosOffset = 24;
+	}
+
+	if ((fc & 0x0088) == 0x0088) {
+		memcpy((TempBuf + 28), (pHeader + QosOffset), 2);
+		TempLen += 2;
+		/* IV = pHeader + QosOffset + 2 + SNAP_SIZE + sizeof(u16) + 2; */
+		IV = pHeader + QosOffset + 2 + 2;
+	} else {
+		IV = pHeader + QosOffset + 2;
+		/* IV = pHeader + QosOffset + SNAP_SIZE + sizeof(u16) + 2; */
+	}
+
+	TempBuf[TempLen - 1] = (u8)(DataLen & 0xff);
+	TempBuf[TempLen - 2] = (u8)((DataLen & 0xff00) >> 8);
+	TempBuf[TempLen - 4] = KeyIdx;
+
+	WAPI_DATA(WAPI_TX, "CalculateMic - KEY", MicKey, 16);
+	WAPI_DATA(WAPI_TX, "CalculateMic - IV", IV, 16);
+	WAPI_DATA(WAPI_TX, "CalculateMic - TempBuf", TempBuf, TempLen);
+	WAPI_DATA(WAPI_TX, "CalculateMic - pData", pData, DataLen);
+
+	WapiSMS4CalculateMic(MicKey, IV, TempBuf, TempLen,
+			     pData, DataLen, MicBuffer, &MicLen);
+
+	if (MicLen != 16)
+		WAPI_TRACE(WAPI_ERR, "%s: MIC Length Error!!\n", __FUNCTION__);
+
+	WAPI_TRACE(WAPI_TX | WAPI_RX, "<=========%s\n", __FUNCTION__);
+#endif
+}
+
+/* AddCount: 1 or 2.
+ *  If overflow, return 1,
+ *  else return 0.
+ */
+u8 WapiIncreasePN(u8 *PN, u8 AddCount)
+{
+	u8  i;
+
+	if (NULL == PN)
+		return 1;
+	/* YJ,test,091102 */
+	/*
+	if(AddCount == 2){
+		RTW_INFO("############################%s(): PN[0]=0x%x\n", __FUNCTION__, PN[0]);
+		if(PN[0] == 0x48){
+			PN[0] += AddCount;
+			return 1;
+		}else{
+			PN[0] += AddCount;
+			return 0;
+		}
+	}
+	*/
+	/* YJ,test,091102,end */
+
+	for (i = 0; i < 16; i++) {
+		if (PN[i] + AddCount <= 0xff) {
+			PN[i] += AddCount;
+			return 0;
+		} else {
+			PN[i] += AddCount;
+			AddCount = 1;
+		}
+	}
+	return 1;
+}
+
+
+void WapiGetLastRxUnicastPNForQoSData(
+	u8			UserPriority,
+	PRT_WAPI_STA_INFO    pWapiStaInfo,
+	u8 *PNOut
+)
+{
+	WAPI_TRACE(WAPI_RX, "===========> %s\n", __FUNCTION__);
+	switch (UserPriority) {
+	case 0:
+	case 3:
+		memcpy(PNOut, pWapiStaInfo->lastRxUnicastPNBEQueue, 16);
+		break;
+	case 1:
+	case 2:
+		memcpy(PNOut, pWapiStaInfo->lastRxUnicastPNBKQueue, 16);
+		break;
+	case 4:
+	case 5:
+		memcpy(PNOut, pWapiStaInfo->lastRxUnicastPNVIQueue, 16);
+		break;
+	case 6:
+	case 7:
+		memcpy(PNOut, pWapiStaInfo->lastRxUnicastPNVOQueue, 16);
+		break;
+	default:
+		WAPI_TRACE(WAPI_ERR, "%s: Unknown TID\n", __FUNCTION__);
+		break;
+	}
+	WAPI_TRACE(WAPI_RX, "<=========== %s\n", __FUNCTION__);
+}
+
+
+void WapiSetLastRxUnicastPNForQoSData(
+	u8		UserPriority,
+	u8           *PNIn,
+	PRT_WAPI_STA_INFO    pWapiStaInfo
+)
+{
+	WAPI_TRACE(WAPI_RX, "===========> %s\n", __FUNCTION__);
+	switch (UserPriority) {
+	case 0:
+	case 3:
+		memcpy(pWapiStaInfo->lastRxUnicastPNBEQueue, PNIn, 16);
+		break;
+	case 1:
+	case 2:
+		memcpy(pWapiStaInfo->lastRxUnicastPNBKQueue, PNIn, 16);
+		break;
+	case 4:
+	case 5:
+		memcpy(pWapiStaInfo->lastRxUnicastPNVIQueue, PNIn, 16);
+		break;
+	case 6:
+	case 7:
+		memcpy(pWapiStaInfo->lastRxUnicastPNVOQueue, PNIn, 16);
+		break;
+	default:
+		WAPI_TRACE(WAPI_ERR, "%s: Unknown TID\n", __FUNCTION__);
+		break;
+	}
+	WAPI_TRACE(WAPI_RX, "<=========== %s\n", __FUNCTION__);
+}
+
+
+/****************************************************************************
+ FALSE not RX-Reorder
+ TRUE do RX Reorder
+add to support WAPI to N-mode
+*****************************************************************************/
+u8 WapiCheckPnInSwDecrypt(
+	_adapter *padapter,
+	struct sk_buff *pskb
+)
+{
+	u8				ret = false;
+
+#if 0
+	struct ieee80211_hdr_3addr_qos *header;
+	u16				fc;
+	u8				*pDaddr, *pTaddr, *pRaddr;
+
+	header = (struct ieee80211_hdr_3addr_qos *)pskb->data;
+	pTaddr = header->addr2;
+	pRaddr = header->addr1;
+	fc = le16_to_cpu(header->frame_ctl);
+
+	if (GetToDs(&fc))
+		pDaddr = header->addr3;
+	else
+		pDaddr = header->addr1;
+
+	if ((_rtw_memcmp(pRaddr, padapter->pnetdev->dev_addr, ETH_ALEN) == 0)
+	    &&	!(pDaddr)
+	    && (GetFrameType(&fc) == WIFI_QOS_DATA_TYPE))
+		/* && ieee->pHTInfo->bCurrentHTSupport && */
+		/* ieee->pHTInfo->bCurRxReorderEnable) */
+		ret = false;
+	else
+		ret = true;
+#endif
+	WAPI_TRACE(WAPI_RX, "%s: return %d\n", __FUNCTION__, ret);
+	return ret;
+}
+
+int SecSMS4HeaderFillIV(_adapter *padapter, u8 *pxmitframe)
+{
+	struct pkt_attrib *pattrib = &((struct xmit_frame *)pxmitframe)->attrib;
+	u8 *frame = ((struct xmit_frame *)pxmitframe)->buf_addr + TXDESC_OFFSET;
+	u8 *pSecHeader = NULL, *pos = NULL, *pRA = NULL;
+	u8 bPNOverflow = false, bFindMatchPeer = false, hdr_len = 0;
+	PWLAN_HEADER_WAPI_EXTENSION pWapiExt = NULL;
+	PRT_WAPI_T         pWapiInfo = &padapter->wapiInfo;
+	PRT_WAPI_STA_INFO  pWapiSta = NULL;
+	int ret = 0;
+
+	WAPI_TRACE(WAPI_TX, "=========>%s\n", __FUNCTION__);
+
+	return ret;
+#if 0
+	hdr_len = sMacHdrLng;
+	if (GetFrameType(pskb->data) == WIFI_QOS_DATA_TYPE)
+		hdr_len += 2;
+	/* hdr_len += SNAP_SIZE + sizeof(u16); */
+
+	pos = skb_push(pskb, padapter->wapiInfo.extra_prefix_len);
+	memmove(pos, pos + padapter->wapiInfo.extra_prefix_len, hdr_len);
+
+	pSecHeader = pskb->data + hdr_len;
+	pWapiExt = (PWLAN_HEADER_WAPI_EXTENSION)pSecHeader;
+	pRA = pskb->data + 4;
+
+	WAPI_DATA(WAPI_TX, "FillIV - Before Fill IV", pskb->data, pskb->len);
+
+	/* Address 1 is always receiver's address */
+	if (IS_MCAST(pRA)) {
+		if (!pWapiInfo->wapiTxMsk.bTxEnable) {
+			WAPI_TRACE(WAPI_ERR, "%s: bTxEnable = 0!!\n", __FUNCTION__);
+			return -2;
+		}
+		if (pWapiInfo->wapiTxMsk.keyId <= 1) {
+			pWapiExt->KeyIdx = pWapiInfo->wapiTxMsk.keyId;
+			pWapiExt->Reserved = 0;
+			bPNOverflow = WapiIncreasePN(pWapiInfo->lastTxMulticastPN, 1);
+			memcpy(pWapiExt->PN, pWapiInfo->lastTxMulticastPN, 16);
+			if (bPNOverflow) {
+				/* Update MSK Notification. */
+				WAPI_TRACE(WAPI_ERR, "===============>%s():multicast PN overflow\n", __FUNCTION__);
+				rtw_wapi_app_event_handler(padapter, NULL, 0, pRA, false, false, true, 0, false);
+			}
+		} else {
+			WAPI_TRACE(WAPI_ERR, "%s: Invalid Wapi Multicast KeyIdx!!\n", __FUNCTION__);
+			ret = -3;
+		}
+	} else {
+		list_for_each_entry(pWapiSta, &pWapiInfo->wapiSTAUsedList, list) {
+			if (!memcmp(pWapiSta->PeerMacAddr, pRA, 6)) {
+				bFindMatchPeer = true;
+				break;
+			}
+		}
+		if (bFindMatchPeer) {
+			if ((!pWapiSta->wapiUskUpdate.bTxEnable) && (!pWapiSta->wapiUsk.bTxEnable)) {
+				WAPI_TRACE(WAPI_ERR, "%s: bTxEnable = 0!!\n", __FUNCTION__);
+				return -4;
+			}
+			if (pWapiSta->wapiUsk.keyId <= 1) {
+				if (pWapiSta->wapiUskUpdate.bTxEnable)
+					pWapiExt->KeyIdx = pWapiSta->wapiUskUpdate.keyId;
+				else
+					pWapiExt->KeyIdx = pWapiSta->wapiUsk.keyId;
+
+				pWapiExt->Reserved = 0;
+				bPNOverflow = WapiIncreasePN(pWapiSta->lastTxUnicastPN, 2);
+				memcpy(pWapiExt->PN, pWapiSta->lastTxUnicastPN, 16);
+				if (bPNOverflow) {
+					/* Update USK Notification. */
+					WAPI_TRACE(WAPI_ERR, "===============>%s():unicast PN overflow\n", __FUNCTION__);
+					rtw_wapi_app_event_handler(padapter, NULL, 0, pWapiSta->PeerMacAddr, false, true, false, 0, false);
+				}
+			} else {
+				WAPI_TRACE(WAPI_ERR, "%s: Invalid Wapi Unicast KeyIdx!!\n", __FUNCTION__);
+				ret = -5;
+			}
+		} else {
+			WAPI_TRACE(WAPI_ERR, "%s: Can not find Peer Sta "MAC_FMT"!!\n", __FUNCTION__, MAC_ARG(pRA));
+			ret = -6;
+		}
+	}
+
+	WAPI_DATA(WAPI_TX, "FillIV - After Fill IV", pskb->data, pskb->len);
+	WAPI_TRACE(WAPI_TX, "<=========%s\n", __FUNCTION__);
+	return ret;
+#endif
+}
+
+/* WAPI SW Enc: must have done Coalesce! */
+void SecSWSMS4Encryption(
+	_adapter *padapter,
+	u8 *pxmitframe
+)
+{
+	PRT_WAPI_T		pWapiInfo = &padapter->wapiInfo;
+	PRT_WAPI_STA_INFO   pWapiSta = NULL;
+	u8 *pframe = ((struct xmit_frame *)pxmitframe)->buf_addr + TXDESC_SIZE;
+	struct pkt_attrib *pattrib = &((struct xmit_frame *)pxmitframe)->attrib;
+
+	u8 *SecPtr = NULL, *pRA, *pMicKey = NULL, *pDataKey = NULL, *pIV = NULL;
+	u8 IVOffset, DataOffset, bFindMatchPeer = false, KeyIdx = 0, MicBuffer[16];
+	u16 OutputLength;
+
+	WAPI_TRACE(WAPI_TX, "=========>%s\n", __FUNCTION__);
+
+	WAPI_TRACE(WAPI_TX, "hdrlen: %d\n", pattrib->hdrlen);
+
+	return;
+
+	DataOffset = pattrib->hdrlen + pattrib->iv_len;
+
+	pRA = pframe + 4;
+
+
+	if (IS_MCAST(pRA)) {
+		KeyIdx = pWapiInfo->wapiTxMsk.keyId;
+		pIV = pWapiInfo->lastTxMulticastPN;
+		pMicKey = pWapiInfo->wapiTxMsk.micKey;
+		pDataKey = pWapiInfo->wapiTxMsk.dataKey;
+	} else {
+		if (!list_empty(&(pWapiInfo->wapiSTAUsedList))) {
+			list_for_each_entry(pWapiSta, &pWapiInfo->wapiSTAUsedList, list) {
+				if (0 == memcmp(pWapiSta->PeerMacAddr, pRA, 6)) {
+					bFindMatchPeer = true;
+					break;
+				}
+			}
+
+			if (bFindMatchPeer) {
+				if (pWapiSta->wapiUskUpdate.bTxEnable) {
+					KeyIdx = pWapiSta->wapiUskUpdate.keyId;
+					WAPI_TRACE(WAPI_TX, "%s(): Use update USK!! KeyIdx=%d\n", __FUNCTION__, KeyIdx);
+					pIV = pWapiSta->lastTxUnicastPN;
+					pMicKey = pWapiSta->wapiUskUpdate.micKey;
+					pDataKey = pWapiSta->wapiUskUpdate.dataKey;
+				} else {
+					KeyIdx = pWapiSta->wapiUsk.keyId;
+					WAPI_TRACE(WAPI_TX, "%s(): Use USK!! KeyIdx=%d\n", __FUNCTION__, KeyIdx);
+					pIV = pWapiSta->lastTxUnicastPN;
+					pMicKey = pWapiSta->wapiUsk.micKey;
+					pDataKey = pWapiSta->wapiUsk.dataKey;
+				}
+			} else {
+				WAPI_TRACE(WAPI_ERR, "%s: Can not find Peer Sta!!\n", __FUNCTION__);
+				return;
+			}
+		} else {
+			WAPI_TRACE(WAPI_ERR, "%s: wapiSTAUsedList is empty!!\n", __FUNCTION__);
+			return;
+		}
+	}
+
+	SecPtr = pframe;
+	SecCalculateMicSMS4(KeyIdx, pMicKey, SecPtr, (SecPtr + DataOffset), pattrib->pktlen, MicBuffer);
+
+	WAPI_DATA(WAPI_TX, "Encryption - MIC", MicBuffer, padapter->wapiInfo.extra_postfix_len);
+
+	memcpy(pframe + pattrib->hdrlen + pattrib->iv_len + pattrib->pktlen - pattrib->icv_len,
+	       (u8 *)MicBuffer,
+	       padapter->wapiInfo.extra_postfix_len
+	      );
+
+
+	WapiSMS4Encryption(pDataKey, pIV, (SecPtr + DataOffset), pattrib->pktlen + pattrib->icv_len, (SecPtr + DataOffset), &OutputLength);
+
+	WAPI_DATA(WAPI_TX, "Encryption - After SMS4 encryption", pframe, pattrib->hdrlen + pattrib->iv_len + pattrib->pktlen);
+
+	WAPI_TRACE(WAPI_TX, "<=========%s\n", __FUNCTION__);
+}
+
+u8 SecSWSMS4Decryption(
+	_adapter *padapter,
+	u8		*precv_frame,
+	struct recv_priv *precv_priv
+)
+{
+	PRT_WAPI_T pWapiInfo = &padapter->wapiInfo;
+	struct recv_frame_hdr *precv_hdr;
+	PRT_WAPI_STA_INFO   pWapiSta = NULL;
+	u8 IVOffset, DataOffset, bFindMatchPeer = false, bUseUpdatedKey = false;
+	u8 KeyIdx, MicBuffer[16], lastRxPNforQoS[16];
+	u8 *pRA, *pTA, *pMicKey, *pDataKey, *pLastRxPN, *pRecvPN, *pSecData, *pRecvMic, *pos;
+	u8 TID = 0;
+	u16 OutputLength, DataLen;
+	u8   bQosData;
+	struct sk_buff	*pskb;
+
+	WAPI_TRACE(WAPI_RX, "=========>%s\n", __FUNCTION__);
+
+	return 0;
+
+	precv_hdr = &((union recv_frame *)precv_frame)->u.hdr;
+	pskb = (struct sk_buff *)(precv_hdr->rx_data);
+	precv_hdr->bWapiCheckPNInDecrypt = WapiCheckPnInSwDecrypt(padapter, pskb);
+	WAPI_TRACE(WAPI_RX, "=========>%s: check PN  %d\n", __FUNCTION__, precv_hdr->bWapiCheckPNInDecrypt);
+	WAPI_DATA(WAPI_RX, "Decryption - Before decryption", pskb->data, pskb->len);
+
+	IVOffset = sMacHdrLng;
+	bQosData = GetFrameType(pskb->data) == WIFI_QOS_DATA_TYPE;
+	if (bQosData)
+		IVOffset += 2;
+
+	/* if(GetHTC()) */
+	/*	IVOffset += 4; */
+
+	/* IVOffset += SNAP_SIZE + sizeof(u16); */
+
+	DataOffset = IVOffset + padapter->wapiInfo.extra_prefix_len;
+
+	pRA = pskb->data + 4;
+	pTA = pskb->data + 10;
+	KeyIdx = *(pskb->data + IVOffset);
+	pRecvPN = pskb->data + IVOffset + 2;
+	pSecData = pskb->data + DataOffset;
+	DataLen = pskb->len - DataOffset;
+	pRecvMic = pskb->data + pskb->len - padapter->wapiInfo.extra_postfix_len;
+	TID = GetTid(pskb->data);
+
+	if (!list_empty(&(pWapiInfo->wapiSTAUsedList))) {
+		list_for_each_entry(pWapiSta, &pWapiInfo->wapiSTAUsedList, list) {
+			if (0 == memcmp(pWapiSta->PeerMacAddr, pTA, 6)) {
+				bFindMatchPeer = true;
+				break;
+			}
+		}
+	}
+
+	if (!bFindMatchPeer) {
+		WAPI_TRACE(WAPI_ERR, "%s: Can not find Peer Sta "MAC_FMT" for Key Info!!!\n", __FUNCTION__, MAC_ARG(pTA));
+		return false;
+	}
+
+	if (IS_MCAST(pRA)) {
+		WAPI_TRACE(WAPI_RX, "%s: Multicast decryption !!!\n", __FUNCTION__);
+		if (pWapiSta->wapiMsk.keyId == KeyIdx && pWapiSta->wapiMsk.bSet) {
+			pLastRxPN = pWapiSta->lastRxMulticastPN;
+			if (!WapiComparePN(pRecvPN, pLastRxPN)) {
+				WAPI_TRACE(WAPI_ERR, "%s: MSK PN is not larger than last, Dropped!!!\n", __FUNCTION__);
+				WAPI_DATA(WAPI_ERR, "pRecvPN:", pRecvPN, 16);
+				WAPI_DATA(WAPI_ERR, "pLastRxPN:", pLastRxPN, 16);
+				return false;
+			}
+
+			memcpy(pLastRxPN, pRecvPN, 16);
+			pMicKey = pWapiSta->wapiMsk.micKey;
+			pDataKey = pWapiSta->wapiMsk.dataKey;
+		} else if (pWapiSta->wapiMskUpdate.keyId == KeyIdx && pWapiSta->wapiMskUpdate.bSet) {
+			WAPI_TRACE(WAPI_RX, "%s: Use Updated MSK for Decryption !!!\n", __FUNCTION__);
+			bUseUpdatedKey = true;
+			memcpy(pWapiSta->lastRxMulticastPN, pRecvPN, 16);
+			pMicKey = pWapiSta->wapiMskUpdate.micKey;
+			pDataKey = pWapiSta->wapiMskUpdate.dataKey;
+		} else {
+			WAPI_TRACE(WAPI_ERR, "%s: Can not find MSK with matched KeyIdx(%d), Dropped !!!\n", __FUNCTION__, KeyIdx);
+			return false;
+		}
+	} else {
+		WAPI_TRACE(WAPI_RX, "%s: Unicast decryption !!!\n", __FUNCTION__);
+		if (pWapiSta->wapiUsk.keyId == KeyIdx && pWapiSta->wapiUsk.bSet) {
+			WAPI_TRACE(WAPI_RX, "%s: Use USK for Decryption!!!\n", __FUNCTION__);
+			if (precv_hdr->bWapiCheckPNInDecrypt) {
+				if (GetFrameType(pskb->data) == WIFI_QOS_DATA_TYPE) {
+					WapiGetLastRxUnicastPNForQoSData(TID, pWapiSta, lastRxPNforQoS);
+					pLastRxPN = lastRxPNforQoS;
+				} else
+					pLastRxPN = pWapiSta->lastRxUnicastPN;
+				if (!WapiComparePN(pRecvPN, pLastRxPN))
+					return false;
+				if (bQosData)
+					WapiSetLastRxUnicastPNForQoSData(TID, pRecvPN, pWapiSta);
+				else
+					memcpy(pWapiSta->lastRxUnicastPN, pRecvPN, 16);
+			} else
+				memcpy(precv_hdr->WapiTempPN, pRecvPN, 16);
+
+			if (check_fwstate(&padapter->mlmepriv, WIFI_STATION_STATE)) {
+				if ((pRecvPN[0] & 0x1) == 0) {
+					WAPI_TRACE(WAPI_ERR, "%s: Rx USK PN is not odd when Infra STA mode, Dropped !!!\n", __FUNCTION__);
+					return false;
+				}
+			}
+
+			pMicKey = pWapiSta->wapiUsk.micKey;
+			pDataKey = pWapiSta->wapiUsk.dataKey;
+		} else if (pWapiSta->wapiUskUpdate.keyId == KeyIdx && pWapiSta->wapiUskUpdate.bSet) {
+			WAPI_TRACE(WAPI_RX, "%s: Use Updated USK for Decryption!!!\n", __FUNCTION__);
+			if (pWapiSta->bAuthenticatorInUpdata)
+				bUseUpdatedKey = true;
+			else
+				bUseUpdatedKey = false;
+
+			if (bQosData)
+				WapiSetLastRxUnicastPNForQoSData(TID, pRecvPN, pWapiSta);
+			else
+				memcpy(pWapiSta->lastRxUnicastPN, pRecvPN, 16);
+			pMicKey = pWapiSta->wapiUskUpdate.micKey;
+			pDataKey = pWapiSta->wapiUskUpdate.dataKey;
+		} else {
+			WAPI_TRACE(WAPI_ERR, "%s: No valid USK!!!KeyIdx=%d pWapiSta->wapiUsk.keyId=%d pWapiSta->wapiUskUpdate.keyId=%d\n", __FUNCTION__, KeyIdx, pWapiSta->wapiUsk.keyId,
+				   pWapiSta->wapiUskUpdate.keyId);
+			/* dump_buf(pskb->data,pskb->len); */
+			return false;
+		}
+	}
+
+	WAPI_DATA(WAPI_RX, "Decryption - DataKey", pDataKey, 16);
+	WAPI_DATA(WAPI_RX, "Decryption - IV", pRecvPN, 16);
+	WapiSMS4Decryption(pDataKey, pRecvPN, pSecData, DataLen, pSecData, &OutputLength);
+
+	if (OutputLength != DataLen)
+		WAPI_TRACE(WAPI_ERR, "%s:  Output Length Error!!!!\n", __FUNCTION__);
+
+	WAPI_DATA(WAPI_RX, "Decryption - After decryption", pskb->data, pskb->len);
+
+	DataLen -= padapter->wapiInfo.extra_postfix_len;
+
+	SecCalculateMicSMS4(KeyIdx, pMicKey, pskb->data, pSecData, DataLen, MicBuffer);
+
+	WAPI_DATA(WAPI_RX, "Decryption - MIC received", pRecvMic, SMS4_MIC_LEN);
+	WAPI_DATA(WAPI_RX, "Decryption - MIC calculated", MicBuffer, SMS4_MIC_LEN);
+
+	if (0 == memcmp(MicBuffer, pRecvMic, padapter->wapiInfo.extra_postfix_len)) {
+		WAPI_TRACE(WAPI_RX, "%s: Check MIC OK!!\n", __FUNCTION__);
+		if (bUseUpdatedKey) {
+			/* delete the old key */
+			if (IS_MCAST(pRA)) {
+				WAPI_TRACE(WAPI_API, "%s(): AE use new update MSK!!\n", __FUNCTION__);
+				pWapiSta->wapiMsk.keyId = pWapiSta->wapiMskUpdate.keyId;
+				memcpy(pWapiSta->wapiMsk.dataKey, pWapiSta->wapiMskUpdate.dataKey, 16);
+				memcpy(pWapiSta->wapiMsk.micKey, pWapiSta->wapiMskUpdate.micKey, 16);
+				pWapiSta->wapiMskUpdate.bTxEnable = pWapiSta->wapiMskUpdate.bSet = false;
+			} else {
+				WAPI_TRACE(WAPI_API, "%s(): AE use new update USK!!\n", __FUNCTION__);
+				pWapiSta->wapiUsk.keyId = pWapiSta->wapiUskUpdate.keyId;
+				memcpy(pWapiSta->wapiUsk.dataKey, pWapiSta->wapiUskUpdate.dataKey, 16);
+				memcpy(pWapiSta->wapiUsk.micKey, pWapiSta->wapiUskUpdate.micKey, 16);
+				pWapiSta->wapiUskUpdate.bTxEnable = pWapiSta->wapiUskUpdate.bSet = false;
+			}
+		}
+	} else {
+		WAPI_TRACE(WAPI_ERR, "%s:  Check MIC Error, Dropped !!!!\n", __FUNCTION__);
+		return false;
+	}
+
+	pos = pskb->data;
+	memmove(pos + padapter->wapiInfo.extra_prefix_len, pos, IVOffset);
+	skb_pull(pskb, padapter->wapiInfo.extra_prefix_len);
+
+	WAPI_TRACE(WAPI_RX, "<=========%s\n", __FUNCTION__);
+
+	return true;
+}
+
+u32	rtw_sms4_encrypt(_adapter *padapter, u8 *pxmitframe)
+{
+
+	u8	*pframe;
+	u32 res = _SUCCESS;
+
+	WAPI_TRACE(WAPI_TX, "=========>%s\n", __FUNCTION__);
+
+	if ((!padapter->WapiSupport) || (!padapter->wapiInfo.bWapiEnable)) {
+		WAPI_TRACE(WAPI_TX, "<========== %s, WAPI not supported or enabled!\n", __FUNCTION__);
+		return _FAIL;
+	}
+
+	if (((struct xmit_frame *)pxmitframe)->buf_addr == NULL)
+		return _FAIL;
+
+	pframe = ((struct xmit_frame *)pxmitframe)->buf_addr + TXDESC_OFFSET;
+
+	SecSWSMS4Encryption(padapter, pxmitframe);
+
+	WAPI_TRACE(WAPI_TX, "<=========%s\n", __FUNCTION__);
+	return res;
+}
+
+u32	rtw_sms4_decrypt(_adapter *padapter, u8 *precvframe)
+{
+	u8	*pframe;
+	u32 res = _SUCCESS;
+
+	WAPI_TRACE(WAPI_RX, "=========>%s\n", __FUNCTION__);
+
+	if ((!padapter->WapiSupport) || (!padapter->wapiInfo.bWapiEnable)) {
+		WAPI_TRACE(WAPI_RX, "<========== %s, WAPI not supported or enabled!\n", __FUNCTION__);
+		return _FAIL;
+	}
+
+
+	/* drop packet when hw decrypt fail
+	* return tempraily */
+	return _FAIL;
+
+	/* pframe=(unsigned char *)((union recv_frame*)precvframe)->u.hdr.rx_data; */
+
+	if (false == SecSWSMS4Decryption(padapter, precvframe, &padapter->recvpriv)) {
+		WAPI_TRACE(WAPI_ERR, "%s():SMS4 decrypt frame error\n", __FUNCTION__);
+		return _FAIL;
+	}
+
+	WAPI_TRACE(WAPI_RX, "<=========%s\n", __FUNCTION__);
+	return res;
+}
+
+#else
+
+u32	rtw_sms4_encrypt(_adapter *padapter, u8 *pxmitframe)
+{
+	WAPI_TRACE(WAPI_TX, "=========>Dummy %s\n", __FUNCTION__);
+	WAPI_TRACE(WAPI_TX, "<=========Dummy %s\n", __FUNCTION__);
+	return _SUCCESS;
+}
+
+u32	rtw_sms4_decrypt(_adapter *padapter, u8 *precvframe)
+{
+	WAPI_TRACE(WAPI_RX, "=========>Dummy %s\n", __FUNCTION__);
+	WAPI_TRACE(WAPI_RX, "<=========Dummy %s\n", __FUNCTION__);
+	return _SUCCESS;
+}
+
+#endif
+
+#endif