Staging: rt28x0: updates from vendor's V2.1.0.0 drivers

Port changes from:

* 2009_0420_RT2860_Linux_STA_V2.1.0.0
* 2009_0302_RT2870_Linux_STA_v2.1.0.0
* 2009_0525_RT3070_Linux_STA_v2.1.1.0

to in-kernel drivers.


From the RT2860 driver release note:

[2.1.0.0]
1. New generation schema for multiple OS porting
2. Fixed Ad-hoc ping failed in noisy environment. (Probe Response has too
    many retry packet then cause "not enough space in MgmtRing")
3. Fixed WPA(2)PSK issue when group cipher of AP is WEP40 or WEP104.
4. Modified iwpriv ra0 get_site_survey:
	In scan list result: Security shows "NONE" when AP is OPEN/NONE,
	shows "WEP" when AP is OPEN/WEP or SHARED/WEP, shows
	"WPAPSK(WPA2PSK)/TKIP(AES)" when AP is WPAPSK(WPA2PSK)/TKIP(AES)
	shows "WPA(WPA2)/TKIP(AES)" when AP is WPA(WPA2)/TKIP(AES)
5. Support kthread.
6. Add New A band channel list region 15 contains the whole channels in
   the A band region 4 and the new CE channel 167,169,171,173
7. Add New IEEE802.11r functionality.
8. Fixed WPA2-Enterprise failed when AP reboot or turn off then turn on.
9. Fixed STA cannot connect to 11B only AP when the setting of is PHY_11GN.


From the RT2870 driver release note:

[V2.1.0.0]
1. New generation schema for multiple OS porting.
2. Fixed Ad-hoc ping failed in noisy environment. (Probe Response has too
   many retry packet then cause "not enough space in MgmtRing").
3. Fixed WPS failed with D-Link DIR-628 in 5GHz.
4. Change FastRoaming in DAT file to AutoRoaming.
5. Support kthread.
6. Add New A band channel list region 15 contains the whole channels in
   the A band region and the new CE channel 167,169,171,173.
7. New IEEE802.11r functionality.


From the RT3070 driver release note:

Version V2.1.1.0
       1. Linux kernel 2.6.29 support.
       2. Fix eFuse write from BIN file bug.

Version 2.1.0.0
       1. New generation schema for multiple OS porting
       2. Fixed Ad-hoc ping failed in noisy environment.
       3. Modified iwpriv ra0 get_site_survey:
       4. Change FastRoaming in DAT file to AutoRoaming.
       5. Support kthread.
       6. New IEEE802.11r functionality.


Tested with RT2860 and RT3070 chipsets.

Signed-off-by: Bartlomiej Zolnierkiewicz <bzolnier@gmail.com>
Signed-off-by: Greg Kroah-Hartman <gregkh@suse.de>

1 files changed, 873 insertions, 0 deletions

diff --git a/drivers/staging/rt2860/pci_main_dev.c b/drivers/staging/rt2860/pci_main_dev.c
new file mode 100644
index 000000000000..5f39c44d78f7
--- /dev/null
+++ b/drivers/staging/rt2860/pci_main_dev.c
@@ -0,0 +1,873 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * 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.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+    Module Name:
+    pci_main_dev.c
+
+    Abstract:
+    Create and register network interface for PCI based chipsets in Linux platform.
+
+    Revision History:
+    Who         When            What
+    --------    ----------      ----------------------------------------------
+*/
+
+#include "rt_config.h"
+#include <linux/pci.h>
+
+//
+// Function declarations
+//
+extern int rt28xx_close(IN struct net_device *net_dev);
+extern int rt28xx_open(struct net_device *net_dev);
+
+static VOID __devexit rt2860_remove_one(struct pci_dev *pci_dev);
+static INT __devinit rt2860_probe(struct pci_dev *pci_dev, const struct pci_device_id  *ent);
+static void __exit rt2860_cleanup_module(void);
+static int __init rt2860_init_module(void);
+
+ static VOID RTMPInitPCIeDevice(
+    IN  struct pci_dev   *pci_dev,
+    IN PRTMP_ADAPTER     pAd);
+
+#ifdef CONFIG_PM
+static int rt2860_suspend(struct pci_dev *pci_dev, pm_message_t state);
+static int rt2860_resume(struct pci_dev *pci_dev);
+#endif // CONFIG_PM //
+
+//
+// Ralink PCI device table, include all supported chipsets
+//
+static struct pci_device_id rt2860_pci_tbl[] __devinitdata =
+{
+	{PCI_DEVICE(NIC_PCI_VENDOR_ID, NIC2860_PCI_DEVICE_ID)},		//RT28602.4G
+	{PCI_DEVICE(NIC_PCI_VENDOR_ID, NIC2860_PCIe_DEVICE_ID)},
+	{PCI_DEVICE(NIC_PCI_VENDOR_ID, NIC2760_PCI_DEVICE_ID)},
+	{PCI_DEVICE(NIC_PCI_VENDOR_ID, NIC2790_PCIe_DEVICE_ID)},
+	{PCI_DEVICE(VEN_AWT_PCI_VENDOR_ID, VEN_AWT_PCIe_DEVICE_ID)},
+	{PCI_DEVICE(EDIMAX_PCI_VENDOR_ID, 0x7708)},
+	{PCI_DEVICE(EDIMAX_PCI_VENDOR_ID, 0x7728)},
+	{PCI_DEVICE(EDIMAX_PCI_VENDOR_ID, 0x7758)},
+	{PCI_DEVICE(EDIMAX_PCI_VENDOR_ID, 0x7727)},
+	{PCI_DEVICE(EDIMAX_PCI_VENDOR_ID, 0x7738)},
+	{PCI_DEVICE(EDIMAX_PCI_VENDOR_ID, 0x7748)},
+	{PCI_DEVICE(EDIMAX_PCI_VENDOR_ID, 0x7768)},
+    {0,}		// terminate list
+};
+
+MODULE_DEVICE_TABLE(pci, rt2860_pci_tbl);
+MODULE_LICENSE("GPL");
+#ifdef MODULE_VERSION
+MODULE_VERSION(STA_DRIVER_VERSION);
+#endif
+
+
+//
+// Our PCI driver structure
+//
+static struct pci_driver rt2860_driver =
+{
+    name:       "rt2860",
+    id_table:   rt2860_pci_tbl,
+    probe:      rt2860_probe,
+    remove:     __devexit_p(rt2860_remove_one),
+#ifdef CONFIG_PM
+	suspend:	rt2860_suspend,
+	resume:		rt2860_resume,
+#endif
+};
+
+
+/***************************************************************************
+ *
+ *	PCI device initialization related procedures.
+ *
+ ***************************************************************************/
+#ifdef CONFIG_PM
+
+VOID RT2860RejectPendingPackets(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	// clear PS packets
+	// clear TxSw packets
+}
+
+static int rt2860_suspend(
+	struct pci_dev *pci_dev,
+	pm_message_t state)
+{
+	struct net_device *net_dev = pci_get_drvdata(pci_dev);
+	PRTMP_ADAPTER pAd = (PRTMP_ADAPTER)NULL;
+	INT32 retval = 0;
+
+
+	DBGPRINT(RT_DEBUG_TRACE, ("===> rt2860_suspend()\n"));
+
+	if (net_dev == NULL)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("net_dev == NULL!\n"));
+	}
+	else
+	{
+		GET_PAD_FROM_NET_DEV(pAd, net_dev);
+
+		/* we can not use IFF_UP because ra0 down but ra1 up */
+		/* and 1 suspend/resume function for 1 module, not for each interface */
+		/* so Linux will call suspend/resume function once */
+		if (VIRTUAL_IF_NUM(pAd) > 0)
+		{
+			// avoid users do suspend after interface is down
+
+			// stop interface
+			netif_carrier_off(net_dev);
+			netif_stop_queue(net_dev);
+
+			// mark device as removed from system and therefore no longer available
+			netif_device_detach(net_dev);
+
+			// mark halt flag
+			RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS);
+			RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_RADIO_OFF);
+
+			// take down the device
+			rt28xx_close((PNET_DEV)net_dev);
+
+			RT_MOD_DEC_USE_COUNT();
+		}
+	}
+
+	// reference to http://vovo2000.com/type-lab/linux/kernel-api/linux-kernel-api.html
+	// enable device to generate PME# when suspended
+	// pci_choose_state(): Choose the power state of a PCI device to be suspended
+	retval = pci_enable_wake(pci_dev, pci_choose_state(pci_dev, state), 1);
+	// save the PCI configuration space of a device before suspending
+	pci_save_state(pci_dev);
+	// disable PCI device after use
+	pci_disable_device(pci_dev);
+
+	retval = pci_set_power_state(pci_dev, pci_choose_state(pci_dev, state));
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<=== rt2860_suspend()\n"));
+	return retval;
+}
+
+static int rt2860_resume(
+	struct pci_dev *pci_dev)
+{
+	struct net_device *net_dev = pci_get_drvdata(pci_dev);
+	PRTMP_ADAPTER pAd = (PRTMP_ADAPTER)NULL;
+	INT32 retval;
+
+
+	// set the power state of a PCI device
+	// PCI has 4 power states, DO (normal) ~ D3(less power)
+	// in include/linux/pci.h, you can find that
+	// #define PCI_D0          ((pci_power_t __force) 0)
+	// #define PCI_D1          ((pci_power_t __force) 1)
+	// #define PCI_D2          ((pci_power_t __force) 2)
+	// #define PCI_D3hot       ((pci_power_t __force) 3)
+	// #define PCI_D3cold      ((pci_power_t __force) 4)
+	// #define PCI_UNKNOWN     ((pci_power_t __force) 5)
+	// #define PCI_POWER_ERROR ((pci_power_t __force) -1)
+	retval = pci_set_power_state(pci_dev, PCI_D0);
+
+	// restore the saved state of a PCI device
+	pci_restore_state(pci_dev);
+
+	// initialize device before it's used by a driver
+	if (pci_enable_device(pci_dev))
+	{
+		printk("pci enable fail!\n");
+		return 0;
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("===> rt2860_resume()\n"));
+
+	if (net_dev == NULL)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("net_dev == NULL!\n"));
+	}
+	else
+		GET_PAD_FROM_NET_DEV(pAd, net_dev);
+
+	if (pAd != NULL)
+	{
+		/* we can not use IFF_UP because ra0 down but ra1 up */
+		/* and 1 suspend/resume function for 1 module, not for each interface */
+		/* so Linux will call suspend/resume function once */
+		if (VIRTUAL_IF_NUM(pAd) > 0)
+		{
+			// mark device as attached from system and restart if needed
+			netif_device_attach(net_dev);
+
+			if (rt28xx_open((PNET_DEV)net_dev) != 0)
+			{
+				// open fail
+				DBGPRINT(RT_DEBUG_TRACE, ("<=== rt2860_resume()\n"));
+				return 0;
+			}
+
+			// increase MODULE use count
+			RT_MOD_INC_USE_COUNT();
+
+			RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS);
+			RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_RADIO_OFF);
+
+			netif_start_queue(net_dev);
+			netif_carrier_on(net_dev);
+			netif_wake_queue(net_dev);
+		}
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<=== rt2860_resume()\n"));
+	return 0;
+}
+#endif // CONFIG_PM //
+
+
+static INT __init rt2860_init_module(VOID)
+{
+	return pci_register_driver(&rt2860_driver);
+}
+
+
+//
+// Driver module unload function
+//
+static VOID __exit rt2860_cleanup_module(VOID)
+{
+    pci_unregister_driver(&rt2860_driver);
+}
+
+module_init(rt2860_init_module);
+module_exit(rt2860_cleanup_module);
+
+
+//
+// PCI device probe & initialization function
+//
+static INT __devinit   rt2860_probe(
+    IN  struct pci_dev              *pci_dev,
+    IN  const struct pci_device_id  *pci_id)
+{
+	PRTMP_ADAPTER		pAd = (PRTMP_ADAPTER)NULL;
+	struct  net_device		*net_dev;
+	PVOID				handle;
+	PSTRING				print_name;
+	ULONG				csr_addr;
+	INT rv = 0;
+	RTMP_OS_NETDEV_OP_HOOK	netDevHook;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("===> rt2860_probe\n"));
+
+//PCIDevInit==============================================
+	// wake up and enable device
+	if ((rv = pci_enable_device(pci_dev))!= 0)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("Enable PCI device failed, errno=%d!\n", rv));
+		return rv;
+	}
+
+	print_name = pci_name(pci_dev);
+
+	if ((rv = pci_request_regions(pci_dev, print_name)) != 0)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("Request PCI resource failed, errno=%d!\n", rv));
+		goto err_out;
+	}
+
+	// map physical address to virtual address for accessing register
+	csr_addr = (unsigned long) ioremap(pci_resource_start(pci_dev, 0), pci_resource_len(pci_dev, 0));
+	if (!csr_addr)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("ioremap failed for device %s, region 0x%lX @ 0x%lX\n",
+					print_name, (ULONG)pci_resource_len(pci_dev, 0), (ULONG)pci_resource_start(pci_dev, 0)));
+		goto err_out_free_res;
+	}
+	else
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("%s: at 0x%lx, VA 0x%lx, IRQ %d. \n",  print_name,
+					(ULONG)pci_resource_start(pci_dev, 0), (ULONG)csr_addr, pci_dev->irq));
+	}
+
+	// Set DMA master
+	pci_set_master(pci_dev);
+
+
+//RtmpDevInit==============================================
+	// Allocate RTMP_ADAPTER adapter structure
+	handle = kmalloc(sizeof(struct os_cookie), GFP_KERNEL);
+	if (handle == NULL)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("%s(): Allocate memory for os handle failed!\n", __func__));
+		goto err_out_iounmap;
+	}
+
+	((POS_COOKIE)handle)->pci_dev = pci_dev;
+
+	rv = RTMPAllocAdapterBlock(handle, &pAd);	//shiang: we may need the pci_dev for allocate structure of "RTMP_ADAPTER"
+	if (rv != NDIS_STATUS_SUCCESS)
+		goto err_out_iounmap;
+	// Here are the RTMP_ADAPTER structure with pci-bus specific parameters.
+	pAd->CSRBaseAddress = (PUCHAR)csr_addr;
+	DBGPRINT(RT_DEBUG_ERROR, ("pAd->CSRBaseAddress =0x%lx, csr_addr=0x%lx!\n", (ULONG)pAd->CSRBaseAddress, csr_addr));
+	RtmpRaDevCtrlInit(pAd, RTMP_DEV_INF_PCI);
+
+
+//NetDevInit==============================================
+	net_dev = RtmpPhyNetDevInit(pAd, &netDevHook);
+	if (net_dev == NULL)
+		goto err_out_free_radev;
+
+	// Here are the net_device structure with pci-bus specific parameters.
+	net_dev->irq = pci_dev->irq;		// Interrupt IRQ number
+	net_dev->base_addr = csr_addr;		// Save CSR virtual address and irq to device structure
+	pci_set_drvdata(pci_dev, net_dev);	// Set driver data
+
+/* for supporting Network Manager */
+	/* Set the sysfs physical device reference for the network logical device
+	  * if set prior to registration will cause a symlink during initialization.
+	 */
+	SET_NETDEV_DEV(net_dev, &(pci_dev->dev));
+
+
+//All done, it's time to register the net device to linux kernel.
+	// Register this device
+	rv = RtmpOSNetDevAttach(net_dev, &netDevHook);
+	if (rv)
+		goto err_out_free_netdev;
+
+	pAd->StaCfg.OriDevType = net_dev->type;
+	RTMPInitPCIeDevice(pci_dev, pAd);
+
+#ifdef KTHREAD_SUPPORT
+#endif // KTHREAD_SUPPORT //
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<=== rt2860_probe\n"));
+
+	return 0; // probe ok
+
+
+	/* --------------------------- ERROR HANDLE --------------------------- */
+err_out_free_netdev:
+	RtmpOSNetDevFree(net_dev);
+
+err_out_free_radev:
+	/* free RTMP_ADAPTER strcuture and os_cookie*/
+	RTMPFreeAdapter(pAd);
+
+err_out_iounmap:
+	iounmap((void *)(csr_addr));
+	release_mem_region(pci_resource_start(pci_dev, 0), pci_resource_len(pci_dev, 0));
+
+err_out_free_res:
+	pci_release_regions(pci_dev);
+
+err_out:
+	pci_disable_device(pci_dev);
+
+	DBGPRINT(RT_DEBUG_ERROR, ("<=== rt2860_probe failed with rv = %d!\n", rv));
+
+	return -ENODEV; /* probe fail */
+}
+
+
+static VOID __devexit rt2860_remove_one(
+    IN  struct pci_dev  *pci_dev)
+{
+	PNET_DEV	net_dev = pci_get_drvdata(pci_dev);
+	RTMP_ADAPTER	*pAd = NULL;
+	ULONG			csr_addr = net_dev->base_addr; // pAd->CSRBaseAddress;
+
+	GET_PAD_FROM_NET_DEV(pAd, net_dev);
+
+    DBGPRINT(RT_DEBUG_TRACE, ("===> rt2860_remove_one\n"));
+
+	if (pAd != NULL)
+	{
+		// Unregister/Free all allocated net_device.
+		RtmpPhyNetDevExit(pAd, net_dev);
+
+		// Unmap CSR base address
+		iounmap((char *)(csr_addr));
+
+		// release memory region
+		release_mem_region(pci_resource_start(pci_dev, 0), pci_resource_len(pci_dev, 0));
+
+		// Free RTMP_ADAPTER related structures.
+		RtmpRaDevCtrlExit(pAd);
+
+	}
+	else
+	{
+		// Unregister network device
+		RtmpOSNetDevDetach(net_dev);
+
+		// Unmap CSR base address
+		iounmap((char *)(net_dev->base_addr));
+
+		// release memory region
+		release_mem_region(pci_resource_start(pci_dev, 0), pci_resource_len(pci_dev, 0));
+	}
+
+	// Free the root net_device
+	RtmpOSNetDevFree(net_dev);
+
+}
+
+
+/*
+========================================================================
+Routine Description:
+    Check the chipset vendor/product ID.
+
+Arguments:
+    _dev_p				Point to the PCI or USB device
+
+Return Value:
+    TRUE				Check ok
+	FALSE				Check fail
+
+Note:
+========================================================================
+*/
+BOOLEAN RT28XXChipsetCheck(
+	IN void *_dev_p)
+{
+	/* always TRUE */
+	return TRUE;
+}
+
+
+/***************************************************************************
+ *
+ *	PCIe device initialization related procedures.
+ *
+ ***************************************************************************/
+ static VOID RTMPInitPCIeDevice(
+    IN  struct pci_dev   *pci_dev,
+    IN PRTMP_ADAPTER     pAd)
+{
+	USHORT  device_id;
+	POS_COOKIE pObj;
+
+	pObj = (POS_COOKIE) pAd->OS_Cookie;
+	pci_read_config_word(pci_dev, PCI_DEVICE_ID, &device_id);
+	device_id = le2cpu16(device_id);
+	pObj->DeviceID = device_id;
+	OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_ADVANCE_POWER_SAVE_PCIE_DEVICE);
+	if (
+		(device_id == NIC2860_PCIe_DEVICE_ID) ||
+		(device_id == NIC2790_PCIe_DEVICE_ID) ||
+		(device_id == VEN_AWT_PCIe_DEVICE_ID) ||
+		 0)
+	{
+		UINT32 MacCsr0 = 0, Index= 0;
+		do
+		{
+			RTMP_IO_READ32(pAd, MAC_CSR0, &MacCsr0);
+
+			if ((MacCsr0 != 0x00) && (MacCsr0 != 0xFFFFFFFF))
+				break;
+
+			RTMPusecDelay(10);
+		} while (Index++ < 100);
+
+		// Support advanced power save after 2892/2790.
+		// MAC version at offset 0x1000 is 0x2872XXXX/0x2870XXXX(PCIe, USB, SDIO).
+		if ((MacCsr0&0xffff0000) != 0x28600000)
+		{
+			OPSTATUS_SET_FLAG(pAd, fOP_STATUS_ADVANCE_POWER_SAVE_PCIE_DEVICE);
+		}
+	}
+}
+
+
+VOID RTMPInitPCIeLinkCtrlValue(
+	IN	PRTMP_ADAPTER	pAd)
+{
+    INT     pos;
+    USHORT	reg16, data2, PCIePowerSaveLevel, Configuration;
+    BOOLEAN	bFindIntel = FALSE;
+	POS_COOKIE pObj;
+
+	pObj = (POS_COOKIE) pAd->OS_Cookie;
+
+	if (!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_ADVANCE_POWER_SAVE_PCIE_DEVICE))
+		return;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("%s.===>\n", __func__));
+	// Init EEPROM, and save settings
+	if (!IS_RT3090(pAd))
+	{
+		RT28xx_EEPROM_READ16(pAd, 0x22, PCIePowerSaveLevel);
+		pAd->PCIePowerSaveLevel = PCIePowerSaveLevel & 0xff;
+
+		if ((PCIePowerSaveLevel&0xff) == 0xff)
+		{
+			OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_ADVANCE_POWER_SAVE_PCIE_DEVICE);
+			DBGPRINT(RT_DEBUG_TRACE, ("====> PCIePowerSaveLevel = 0x%x.\n", PCIePowerSaveLevel));
+			return;
+		}
+	else
+	{
+		PCIePowerSaveLevel &= 0x3;
+		RT28xx_EEPROM_READ16(pAd, 0x24, data2);
+
+		if( !(((data2&0xff00) == 0x9200) && ((data2&0x80) !=0)) )
+		{
+			if (PCIePowerSaveLevel > 1 )
+				PCIePowerSaveLevel = 1;
+		}
+
+		DBGPRINT(RT_DEBUG_TRACE, ("====> Write 0x83 = 0x%x.\n", PCIePowerSaveLevel));
+		AsicSendCommandToMcu(pAd, 0x83, 0xff, (UCHAR)PCIePowerSaveLevel, 0x00);
+		RT28xx_EEPROM_READ16(pAd, 0x22, PCIePowerSaveLevel);
+		PCIePowerSaveLevel &= 0xff;
+		PCIePowerSaveLevel = PCIePowerSaveLevel >> 6;
+		switch(PCIePowerSaveLevel)
+		{
+				case 0:	// Only support L0
+					pAd->LnkCtrlBitMask = 0;
+				break;
+				case 1:	// Only enable L0s
+					pAd->LnkCtrlBitMask = 1;
+				break;
+				case 2:	// enable L1, L0s
+					pAd->LnkCtrlBitMask = 3;
+				break;
+				case 3:	// sync with host clk and enable L1, L0s
+				pAd->LnkCtrlBitMask = 0x103;
+				break;
+		}
+		DBGPRINT(RT_DEBUG_TRACE, ("====> LnkCtrlBitMask = 0x%x.\n", pAd->LnkCtrlBitMask));
+	}
+	}
+	else if (IS_RT3090(pAd))
+	{
+		// 1. read setting from inf file.
+		// .....
+		USHORT	PCIePowerSetting = 0;
+		/* code from windows, default value of rt30xxPowerMode = 0
+		PCIePowerSetting = pAd->StaCfg.PSControl.field.rt30xxPowerMode;
+		*/
+		DBGPRINT(RT_DEBUG_TRACE, ("====> rt30xx Read PowerLevelMode =  0x%x.\n", PCIePowerSetting));
+		// 2. Check EnableNewPS
+		/*
+		if (pAd->StaCfg.PSControl.field.EnableNewPS == FALSE)
+			PCIePowerSetting = 1;
+		*/
+
+		if ((pAd->MACVersion&0xffff) <= 0x0211)
+		{
+			// Chip Version E only allow 1
+			PCIePowerSetting = 1;
+			DBGPRINT(RT_DEBUG_TRACE, ("====> rt30xx Write 0x83 Command = 0x%x.\n", PCIePowerSetting));
+			AsicSendCommandToMcu(pAd, 0x83, 0xff, (UCHAR)PCIePowerSetting, 0x00);
+		}
+		else
+		{
+			// Chip Version F only allow 1 or 2
+			if ((PCIePowerSetting > 2) || (PCIePowerSetting == 0))
+				PCIePowerSetting = 1;
+			DBGPRINT(RT_DEBUG_TRACE, ("====> rt30xx Write 0x83 Command = 0x%x.\n", PCIePowerSetting));
+			AsicSendCommandToMcu(pAd, 0x83, 0xff, (UCHAR)PCIePowerSetting, 0x00);
+		}
+
+	}
+
+    // Find Ralink PCIe Device's Express Capability Offset
+	pos = pci_find_capability(pObj->pci_dev, PCI_CAP_ID_EXP);
+
+    if (pos != 0)
+    {
+        // Ralink PCIe Device's Link Control Register Offset
+        pAd->RLnkCtrlOffset = pos + PCI_EXP_LNKCTL;
+	pci_read_config_word(pObj->pci_dev, pAd->RLnkCtrlOffset, &reg16);
+        Configuration = le2cpu16(reg16);
+        DBGPRINT(RT_DEBUG_TRACE, ("Read (Ralink PCIe Link Control Register) offset 0x%x = 0x%x\n",
+                                    pAd->RLnkCtrlOffset, Configuration));
+        pAd->RLnkCtrlConfiguration = (Configuration & 0x103);
+        Configuration &= 0xfefc;
+        Configuration |= (0x0);
+		if ((pObj->DeviceID == NIC2860_PCIe_DEVICE_ID)
+			||(pObj->DeviceID == NIC2790_PCIe_DEVICE_ID))
+		{
+			reg16 = cpu2le16(Configuration);
+			pci_write_config_word(pObj->pci_dev, pAd->RLnkCtrlOffset, reg16);
+			DBGPRINT(RT_DEBUG_TRACE, ("Write (Ralink PCIe Link Control Register)  offset 0x%x = 0x%x\n",
+                                    pos + PCI_EXP_LNKCTL, Configuration));
+		}
+
+        RTMPFindHostPCIDev(pAd);
+        if (pObj->parent_pci_dev)
+        {
+		USHORT  vendor_id;
+
+		pci_read_config_word(pObj->parent_pci_dev, PCI_VENDOR_ID, &vendor_id);
+		vendor_id = le2cpu16(vendor_id);
+		if (vendor_id == PCIBUS_INTEL_VENDOR)
+			bFindIntel = TRUE;
+
+		// Find PCI-to-PCI Bridge Express Capability Offset
+		pos = pci_find_capability(pObj->parent_pci_dev, PCI_CAP_ID_EXP);
+
+		if (pos != 0)
+		{
+			BOOLEAN		bChange = FALSE;
+			// PCI-to-PCI Bridge Link Control Register Offset
+			pAd->HostLnkCtrlOffset = pos + PCI_EXP_LNKCTL;
+			pci_read_config_word(pObj->parent_pci_dev, pAd->HostLnkCtrlOffset, &reg16);
+			Configuration = le2cpu16(reg16);
+			DBGPRINT(RT_DEBUG_TRACE, ("Read (Host PCI-to-PCI Bridge Link Control Register) offset 0x%x = 0x%x\n",
+			                            pAd->HostLnkCtrlOffset, Configuration));
+			pAd->HostLnkCtrlConfiguration = (Configuration & 0x103);
+			Configuration &= 0xfefc;
+			Configuration |= (0x0);
+
+			switch (pObj->DeviceID)
+			{
+				case NIC2860_PCIe_DEVICE_ID:
+				case NIC2790_PCIe_DEVICE_ID:
+					bChange = TRUE;
+					break;
+				default:
+					break;
+			}
+
+			if (bChange)
+			{
+				reg16 = cpu2le16(Configuration);
+				pci_write_config_word(pObj->parent_pci_dev, pAd->HostLnkCtrlOffset, reg16);
+				DBGPRINT(RT_DEBUG_TRACE, ("Write (Host PCI-to-PCI Bridge Link Control Register) offset 0x%x = 0x%x\n",
+						pAd->HostLnkCtrlOffset, Configuration));
+			}
+		}
+		else
+		{
+			pAd->HostLnkCtrlOffset = 0;
+			DBGPRINT(RT_DEBUG_ERROR, ("%s: cannot find PCI-to-PCI Bridge PCI Express Capability!\n", __func__));
+		}
+        }
+    }
+    else
+    {
+        pAd->RLnkCtrlOffset = 0;
+        pAd->HostLnkCtrlOffset = 0;
+        DBGPRINT(RT_DEBUG_ERROR, ("%s: cannot find Ralink PCIe Device's PCI Express Capability!\n", __func__));
+    }
+
+    if (bFindIntel == FALSE)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("Doesn't find Intel PCI host controller. \n"));
+		// Doesn't switch L0, L1, So set PCIePowerSaveLevel to 0xff
+		pAd->PCIePowerSaveLevel = 0xff;
+		if ((pAd->RLnkCtrlOffset != 0)
+		)
+		{
+			pci_read_config_word(pObj->pci_dev, pAd->RLnkCtrlOffset, &reg16);
+			Configuration = le2cpu16(reg16);
+			DBGPRINT(RT_DEBUG_TRACE, ("Read (Ralink 30xx PCIe Link Control Register) offset 0x%x = 0x%x\n",
+			                        pAd->RLnkCtrlOffset, Configuration));
+			pAd->RLnkCtrlConfiguration = (Configuration & 0x103);
+			Configuration &= 0xfefc;
+			Configuration |= (0x0);
+			reg16 = cpu2le16(Configuration);
+			pci_write_config_word(pObj->pci_dev, pAd->RLnkCtrlOffset, reg16);
+			DBGPRINT(RT_DEBUG_TRACE, ("Write (Ralink PCIe Link Control Register)  offset 0x%x = 0x%x\n",
+			                        pos + PCI_EXP_LNKCTL, Configuration));
+		}
+	}
+}
+
+VOID RTMPFindHostPCIDev(
+    IN	PRTMP_ADAPTER	pAd)
+{
+    USHORT  reg16;
+    UCHAR   reg8;
+	UINT	DevFn;
+    PPCI_DEV    pPci_dev;
+	POS_COOKIE	pObj;
+
+	pObj = (POS_COOKIE) pAd->OS_Cookie;
+
+	if (!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_ADVANCE_POWER_SAVE_PCIE_DEVICE))
+		return;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("%s.===>\n", __func__));
+
+    pObj->parent_pci_dev = NULL;
+    if (pObj->pci_dev->bus->parent)
+    {
+        for (DevFn = 0; DevFn < 255; DevFn++)
+        {
+            pPci_dev = pci_get_slot(pObj->pci_dev->bus->parent, DevFn);
+            if (pPci_dev)
+            {
+                pci_read_config_word(pPci_dev, PCI_CLASS_DEVICE, &reg16);
+                reg16 = le2cpu16(reg16);
+                pci_read_config_byte(pPci_dev, PCI_CB_CARD_BUS, &reg8);
+                if ((reg16 == PCI_CLASS_BRIDGE_PCI) &&
+                    (reg8 == pObj->pci_dev->bus->number))
+                {
+                    pObj->parent_pci_dev = pPci_dev;
+                }
+            }
+        }
+    }
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+		Level = RESTORE_HALT : Restore PCI host and Ralink PCIe Link Control field to its default value.
+		Level = Other Value : Restore from dot11 power save or radio off status. And force PCI host Link Control fields to 0x1
+
+	========================================================================
+*/
+VOID RTMPPCIeLinkCtrlValueRestore(
+	IN	PRTMP_ADAPTER	pAd,
+	IN   UCHAR		Level)
+{
+	USHORT  PCIePowerSaveLevel, reg16;
+	USHORT	Configuration;
+	POS_COOKIE	pObj;
+
+	pObj = (POS_COOKIE) pAd->OS_Cookie;
+
+	if (!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_ADVANCE_POWER_SAVE_PCIE_DEVICE))
+		return;
+
+	if (!((pObj->DeviceID == NIC2860_PCIe_DEVICE_ID)
+		||(pObj->DeviceID == NIC2790_PCIe_DEVICE_ID)))
+		return;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("%s.===>\n", __func__));
+	PCIePowerSaveLevel = pAd->PCIePowerSaveLevel;
+	if ((PCIePowerSaveLevel&0xff) == 0xff)
+	{
+		DBGPRINT(RT_DEBUG_TRACE,("return  \n"));
+		return;
+	}
+
+	if (pObj->parent_pci_dev && (pAd->HostLnkCtrlOffset != 0))
+    {
+        PCI_REG_READ_WORD(pObj->parent_pci_dev, pAd->HostLnkCtrlOffset, Configuration);
+        if ((Configuration != 0) &&
+            (Configuration != 0xFFFF))
+        {
+		Configuration &= 0xfefc;
+		// If call from interface down, restore to orginial setting.
+		if (Level == RESTORE_CLOSE)
+		{
+			Configuration |= pAd->HostLnkCtrlConfiguration;
+		}
+		else
+			Configuration |= 0x0;
+            PCI_REG_WIRTE_WORD(pObj->parent_pci_dev, pAd->HostLnkCtrlOffset, Configuration);
+		DBGPRINT(RT_DEBUG_TRACE, ("Restore PCI host : offset 0x%x = 0x%x\n", pAd->HostLnkCtrlOffset, Configuration));
+        }
+        else
+            DBGPRINT(RT_DEBUG_ERROR, ("Restore PCI host : PCI_REG_READ_WORD failed (Configuration = 0x%x)\n", Configuration));
+    }
+
+    if (pObj->pci_dev && (pAd->RLnkCtrlOffset != 0))
+    {
+        PCI_REG_READ_WORD(pObj->pci_dev, pAd->RLnkCtrlOffset, Configuration);
+        if ((Configuration != 0) &&
+            (Configuration != 0xFFFF))
+        {
+		Configuration &= 0xfefc;
+			// If call from interface down, restore to orginial setting.
+			if (Level == RESTORE_CLOSE)
+		Configuration |= pAd->RLnkCtrlConfiguration;
+			else
+				Configuration |= 0x0;
+            PCI_REG_WIRTE_WORD(pObj->pci_dev, pAd->RLnkCtrlOffset, Configuration);
+		DBGPRINT(RT_DEBUG_TRACE, ("Restore Ralink : offset 0x%x = 0x%x\n", pAd->RLnkCtrlOffset, Configuration));
+        }
+        else
+            DBGPRINT(RT_DEBUG_ERROR, ("Restore Ralink : PCI_REG_READ_WORD failed (Configuration = 0x%x)\n", Configuration));
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE,("%s <===\n", __func__));
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+		Max : limit Host PCI and Ralink PCIe device's LINK CONTROL field's value.
+		Because now frequently set our device to mode 1 or mode 3 will cause problem.
+
+	========================================================================
+*/
+VOID RTMPPCIeLinkCtrlSetting(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	USHORT		Max)
+{
+	USHORT  PCIePowerSaveLevel, reg16;
+	USHORT	Configuration;
+	POS_COOKIE	pObj;
+
+	pObj = (POS_COOKIE) pAd->OS_Cookie;
+
+	if (!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_ADVANCE_POWER_SAVE_PCIE_DEVICE))
+		return;
+
+	if (!((pObj->DeviceID == NIC2860_PCIe_DEVICE_ID)
+		||(pObj->DeviceID == NIC2790_PCIe_DEVICE_ID)))
+		return;
+
+	DBGPRINT(RT_DEBUG_TRACE,("%s===>\n", __func__));
+	PCIePowerSaveLevel = pAd->PCIePowerSaveLevel;
+	if ((PCIePowerSaveLevel&0xff) == 0xff)
+	{
+		DBGPRINT(RT_DEBUG_TRACE,("return  \n"));
+		return;
+	}
+	PCIePowerSaveLevel = PCIePowerSaveLevel>>6;
+
+
+	if (pObj->pci_dev && (pAd->RLnkCtrlOffset != 0))
+	{
+		// first 2892 chip not allow to frequently set mode 3. will cause hang problem.
+		if (PCIePowerSaveLevel > Max)
+			PCIePowerSaveLevel = Max;
+
+        PCI_REG_READ_WORD(pObj->pci_dev, pAd->RLnkCtrlOffset, Configuration);
+		Configuration |= 0x100;
+        PCI_REG_WIRTE_WORD(pObj->pci_dev, pAd->RLnkCtrlOffset, Configuration);
+		DBGPRINT(RT_DEBUG_TRACE, ("Write Ralink device : offset 0x%x = 0x%x\n", pAd->RLnkCtrlOffset, Configuration));
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE,("RTMPPCIePowerLinkCtrl <==============\n"));
+}