drivers/staging/rt2860/pci_main_dev.c

   1 /*
   2  *************************************************************************
   3  * Ralink Tech Inc.
   4  * 5F., No.36, Taiyuan St., Jhubei City,
   5  * Hsinchu County 302,
   6  * Taiwan, R.O.C.
   7  *
   8  * (c) Copyright 2002-2007, Ralink Technology, Inc.
   9  *
  10  * This program is free software; you can redistribute it and/or modify  *
  11  * it under the terms of the GNU General Public License as published by  *
  12  * the Free Software Foundation; either version 2 of the License, or     *
  13  * (at your option) any later version.                                   *
  14  *                                                                       *
  15  * This program is distributed in the hope that it will be useful,       *
  16  * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
  17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
  18  * GNU General Public License for more details.                          *
  19  *                                                                       *
  20  * You should have received a copy of the GNU General Public License     *
  21  * along with this program; if not, write to the                         *
  22  * Free Software Foundation, Inc.,                                       *
  23  * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
  24  *                                                                       *
  25  *************************************************************************
  26
  27     Module Name:
  28     pci_main_dev.c
  29
  30     Abstract:
  31     Create and register network interface for PCI based chipsets in Linux platform.
  32
  33     Revision History:
  34     Who         When            What
  35     --------    ----------      ----------------------------------------------
  36 */
  37
  38 #include "rt_config.h"
  39 #include <linux/pci.h>
  40
  41 // Following information will be show when you run 'modinfo'
  42 // *** If you have a solution for the bug in current version of driver, please mail to me.
  43 // Otherwise post to forum in ralinktech's web site(www.ralinktech.com) and let all users help you. ***
  44 MODULE_AUTHOR("Jett Chen <jett_chen@ralinktech.com>");
  45 MODULE_DESCRIPTION("RT2860/RT3090 Wireless Lan Linux Driver");
  46 MODULE_LICENSE("GPL");
  47 MODULE_ALIAS("rt3090sta");
  48
  49 //
  50 // Function declarations
  51 //
  52 extern int rt28xx_close(IN struct net_device *net_dev);
  53 extern int rt28xx_open(struct net_device *net_dev);
  54
  55 static VOID __devexit rt2860_remove_one(struct pci_dev *pci_dev);
  56 static INT __devinit rt2860_probe(struct pci_dev *pci_dev, const struct pci_device_id  *ent);
  57 static void __exit rt2860_cleanup_module(void);
  58 static int __init rt2860_init_module(void);
  59
  60  static VOID RTMPInitPCIeDevice(
  61     IN  struct pci_dev   *pci_dev,
  62     IN PRTMP_ADAPTER     pAd);
  63
  64 #ifdef CONFIG_PM
  65 static int rt2860_suspend(struct pci_dev *pci_dev, pm_message_t state);
  66 static int rt2860_resume(struct pci_dev *pci_dev);
  67 #endif // CONFIG_PM //
  68
  69 //
  70 // Ralink PCI device table, include all supported chipsets
  71 //
  72 static struct pci_device_id rt2860_pci_tbl[] __devinitdata =
  73 {
  74 #ifdef RT2860
  75         {PCI_DEVICE(NIC_PCI_VENDOR_ID, NIC2860_PCI_DEVICE_ID)},         //RT28602.4G
  76         {PCI_DEVICE(NIC_PCI_VENDOR_ID, NIC2860_PCIe_DEVICE_ID)},
  77         {PCI_DEVICE(NIC_PCI_VENDOR_ID, NIC2760_PCI_DEVICE_ID)},
  78         {PCI_DEVICE(NIC_PCI_VENDOR_ID, NIC2790_PCIe_DEVICE_ID)},
  79         {PCI_DEVICE(VEN_AWT_PCI_VENDOR_ID, VEN_AWT_PCIe_DEVICE_ID)},
  80         {PCI_DEVICE(EDIMAX_PCI_VENDOR_ID, 0x7708)},
  81         {PCI_DEVICE(EDIMAX_PCI_VENDOR_ID, 0x7728)},
  82         {PCI_DEVICE(EDIMAX_PCI_VENDOR_ID, 0x7758)},
  83         {PCI_DEVICE(EDIMAX_PCI_VENDOR_ID, 0x7727)},
  84         {PCI_DEVICE(EDIMAX_PCI_VENDOR_ID, 0x7738)},
  85         {PCI_DEVICE(EDIMAX_PCI_VENDOR_ID, 0x7748)},
  86         {PCI_DEVICE(EDIMAX_PCI_VENDOR_ID, 0x7768)},
  87 #endif
  88 #ifdef RT3090
  89         {PCI_DEVICE(NIC_PCI_VENDOR_ID, NIC3090_PCIe_DEVICE_ID)},
  90         {PCI_DEVICE(NIC_PCI_VENDOR_ID, NIC3091_PCIe_DEVICE_ID)},
  91         {PCI_DEVICE(NIC_PCI_VENDOR_ID, NIC3092_PCIe_DEVICE_ID)},
  92 #endif // RT3090 //
  93 #ifdef RT3390
  94         {PCI_DEVICE(NIC_PCI_VENDOR_ID, NIC3390_PCIe_DEVICE_ID)},
  95         {PCI_DEVICE(NIC_PCI_VENDOR_ID, NIC3391_PCIe_DEVICE_ID)},
  96         {PCI_DEVICE(NIC_PCI_VENDOR_ID, NIC3392_PCIe_DEVICE_ID)},
  97 #endif // RT3390 //
  98     {0,}                // terminate list
  99 };
 100
 101 MODULE_DEVICE_TABLE(pci, rt2860_pci_tbl);
 102 #ifdef MODULE_VERSION
 103 MODULE_VERSION(STA_DRIVER_VERSION);
 104 #endif
 105
 106
 107 //
 108 // Our PCI driver structure
 109 //
 110 static struct pci_driver rt2860_driver =
 111 {
 112     name:       "rt2860",
 113     id_table:   rt2860_pci_tbl,
 114     probe:      rt2860_probe,
 115     remove:     __devexit_p(rt2860_remove_one),
 116 #ifdef CONFIG_PM
 117         suspend:        rt2860_suspend,
 118         resume:         rt2860_resume,
 119 #endif
 120 };
 121
 122
 123 /***************************************************************************
 124  *
 125  *      PCI device initialization related procedures.
 126  *
 127  ***************************************************************************/
 128 #ifdef CONFIG_PM
 129
 130 VOID RT2860RejectPendingPackets(
 131         IN      PRTMP_ADAPTER   pAd)
 132 {
 133         // clear PS packets
 134         // clear TxSw packets
 135 }
 136
 137 static int rt2860_suspend(
 138         struct pci_dev *pci_dev,
 139         pm_message_t state)
 140 {
 141         struct net_device *net_dev = pci_get_drvdata(pci_dev);
 142         PRTMP_ADAPTER pAd = (PRTMP_ADAPTER)NULL;
 143         INT32 retval = 0;
 144
 145
 146         DBGPRINT(RT_DEBUG_TRACE, ("===> rt2860_suspend()\n"));
 147
 148         if (net_dev == NULL)
 149         {
 150                 DBGPRINT(RT_DEBUG_ERROR, ("net_dev == NULL!\n"));
 151         }
 152         else
 153         {
 154                 GET_PAD_FROM_NET_DEV(pAd, net_dev);
 155
 156                 /* we can not use IFF_UP because ra0 down but ra1 up */
 157                 /* and 1 suspend/resume function for 1 module, not for each interface */
 158                 /* so Linux will call suspend/resume function once */
 159                 if (VIRTUAL_IF_NUM(pAd) > 0)
 160                 {
 161                         // avoid users do suspend after interface is down
 162
 163                         // stop interface
 164                         netif_carrier_off(net_dev);
 165                         netif_stop_queue(net_dev);
 166
 167                         // mark device as removed from system and therefore no longer available
 168                         netif_device_detach(net_dev);
 169
 170                         // mark halt flag
 171                         RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS);
 172                         RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_RADIO_OFF);
 173
 174                         // take down the device
 175                         rt28xx_close((PNET_DEV)net_dev);
 176
 177                         RT_MOD_DEC_USE_COUNT();
 178                 }
 179         }
 180
 181         // reference to http://vovo2000.com/type-lab/linux/kernel-api/linux-kernel-api.html
 182         // enable device to generate PME# when suspended
 183         // pci_choose_state(): Choose the power state of a PCI device to be suspended
 184         retval = pci_enable_wake(pci_dev, pci_choose_state(pci_dev, state), 1);
 185         // save the PCI configuration space of a device before suspending
 186         pci_save_state(pci_dev);
 187         // disable PCI device after use
 188         pci_disable_device(pci_dev);
 189
 190         retval = pci_set_power_state(pci_dev, pci_choose_state(pci_dev, state));
 191
 192         DBGPRINT(RT_DEBUG_TRACE, ("<=== rt2860_suspend()\n"));
 193         return retval;
 194 }
 195
 196 static int rt2860_resume(
 197         struct pci_dev *pci_dev)
 198 {
 199         struct net_device *net_dev = pci_get_drvdata(pci_dev);
 200         PRTMP_ADAPTER pAd = (PRTMP_ADAPTER)NULL;
 201         INT32 retval;
 202
 203
 204         // set the power state of a PCI device
 205         // PCI has 4 power states, DO (normal) ~ D3(less power)
 206         // in include/linux/pci.h, you can find that
 207         // #define PCI_D0          ((pci_power_t __force) 0)
 208         // #define PCI_D1          ((pci_power_t __force) 1)
 209         // #define PCI_D2          ((pci_power_t __force) 2)
 210         // #define PCI_D3hot       ((pci_power_t __force) 3)
 211         // #define PCI_D3cold      ((pci_power_t __force) 4)
 212         // #define PCI_UNKNOWN     ((pci_power_t __force) 5)
 213         // #define PCI_POWER_ERROR ((pci_power_t __force) -1)
 214         retval = pci_set_power_state(pci_dev, PCI_D0);
 215
 216         // restore the saved state of a PCI device
 217         pci_restore_state(pci_dev);
 218
 219         // initialize device before it's used by a driver
 220         if (pci_enable_device(pci_dev))
 221         {
 222                 printk("pci enable fail!\n");
 223                 return 0;
 224         }
 225
 226         DBGPRINT(RT_DEBUG_TRACE, ("===> rt2860_resume()\n"));
 227
 228         if (net_dev == NULL)
 229         {
 230                 DBGPRINT(RT_DEBUG_ERROR, ("net_dev == NULL!\n"));
 231         }
 232         else
 233                 GET_PAD_FROM_NET_DEV(pAd, net_dev);
 234
 235         if (pAd != NULL)
 236         {
 237                 /* we can not use IFF_UP because ra0 down but ra1 up */
 238                 /* and 1 suspend/resume function for 1 module, not for each interface */
 239                 /* so Linux will call suspend/resume function once */
 240                 if (VIRTUAL_IF_NUM(pAd) > 0)
 241                 {
 242                         // mark device as attached from system and restart if needed
 243                         netif_device_attach(net_dev);
 244
 245                         if (rt28xx_open((PNET_DEV)net_dev) != 0)
 246                         {
 247                                 // open fail
 248                                 DBGPRINT(RT_DEBUG_TRACE, ("<=== rt2860_resume()\n"));
 249                                 return 0;
 250                         }
 251
 252                         // increase MODULE use count
 253                         RT_MOD_INC_USE_COUNT();
 254
 255                         RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS);
 256                         RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_RADIO_OFF);
 257
 258                         netif_start_queue(net_dev);
 259                         netif_carrier_on(net_dev);
 260                         netif_wake_queue(net_dev);
 261                 }
 262         }
 263
 264         DBGPRINT(RT_DEBUG_TRACE, ("<=== rt2860_resume()\n"));
 265         return 0;
 266 }
 267 #endif // CONFIG_PM //
 268
 269
 270 static INT __init rt2860_init_module(VOID)
 271 {
 272         return pci_register_driver(&rt2860_driver);
 273 }
 274
 275
 276 //
 277 // Driver module unload function
 278 //
 279 static VOID __exit rt2860_cleanup_module(VOID)
 280 {
 281     pci_unregister_driver(&rt2860_driver);
 282 }
 283
 284 module_init(rt2860_init_module);
 285 module_exit(rt2860_cleanup_module);
 286
 287
 288 //
 289 // PCI device probe & initialization function
 290 //
 291 static INT __devinit   rt2860_probe(
 292     IN  struct pci_dev              *pci_dev,
 293     IN  const struct pci_device_id  *pci_id)
 294 {
 295         PRTMP_ADAPTER           pAd = (PRTMP_ADAPTER)NULL;
 296         struct  net_device              *net_dev;
 297         PVOID                           handle;
 298         PSTRING                         print_name;
 299         ULONG                           csr_addr;
 300         INT rv = 0;
 301         RTMP_OS_NETDEV_OP_HOOK  netDevHook;
 302
 303         DBGPRINT(RT_DEBUG_TRACE, ("===> rt2860_probe\n"));
 304
 305 //PCIDevInit==============================================
 306         // wake up and enable device
 307         if ((rv = pci_enable_device(pci_dev))!= 0)
 308         {
 309                 DBGPRINT(RT_DEBUG_ERROR, ("Enable PCI device failed, errno=%d!\n", rv));
 310                 return rv;
 311         }
 312
 313         print_name = pci_name(pci_dev);
 314
 315         if ((rv = pci_request_regions(pci_dev, print_name)) != 0)
 316         {
 317                 DBGPRINT(RT_DEBUG_ERROR, ("Request PCI resource failed, errno=%d!\n", rv));
 318                 goto err_out;
 319         }
 320
 321         // map physical address to virtual address for accessing register
 322         csr_addr = (unsigned long) ioremap(pci_resource_start(pci_dev, 0), pci_resource_len(pci_dev, 0));
 323         if (!csr_addr)
 324         {
 325                 DBGPRINT(RT_DEBUG_ERROR, ("ioremap failed for device %s, region 0x%lX @ 0x%lX\n",
 326                                         print_name, (ULONG)pci_resource_len(pci_dev, 0), (ULONG)pci_resource_start(pci_dev, 0)));
 327                 goto err_out_free_res;
 328         }
 329         else
 330         {
 331                 DBGPRINT(RT_DEBUG_TRACE, ("%s: at 0x%lx, VA 0x%lx, IRQ %d. \n",  print_name,
 332                                         (ULONG)pci_resource_start(pci_dev, 0), (ULONG)csr_addr, pci_dev->irq));
 333         }
 334
 335         // Set DMA master
 336         pci_set_master(pci_dev);
 337
 338
 339 //RtmpDevInit==============================================
 340         // Allocate RTMP_ADAPTER adapter structure
 341         handle = kmalloc(sizeof(struct os_cookie), GFP_KERNEL);
 342         if (handle == NULL)
 343         {
 344                 DBGPRINT(RT_DEBUG_ERROR, ("%s(): Allocate memory for os handle failed!\n", __func__));
 345                 goto err_out_iounmap;
 346         }
 347
 348         ((POS_COOKIE)handle)->pci_dev = pci_dev;
 349
 350         rv = RTMPAllocAdapterBlock(handle, &pAd);       //shiang: we may need the pci_dev for allocate structure of "RTMP_ADAPTER"
 351         if (rv != NDIS_STATUS_SUCCESS)
 352                 goto err_out_iounmap;
 353         // Here are the RTMP_ADAPTER structure with pci-bus specific parameters.
 354         pAd->CSRBaseAddress = (PUCHAR)csr_addr;
 355         DBGPRINT(RT_DEBUG_ERROR, ("pAd->CSRBaseAddress =0x%lx, csr_addr=0x%lx!\n", (ULONG)pAd->CSRBaseAddress, csr_addr));
 356         RtmpRaDevCtrlInit(pAd, RTMP_DEV_INF_PCI);
 357
 358
 359 //NetDevInit==============================================
 360         net_dev = RtmpPhyNetDevInit(pAd, &netDevHook);
 361         if (net_dev == NULL)
 362                 goto err_out_free_radev;
 363
 364         // Here are the net_device structure with pci-bus specific parameters.
 365         net_dev->irq = pci_dev->irq;            // Interrupt IRQ number
 366         net_dev->base_addr = csr_addr;          // Save CSR virtual address and irq to device structure
 367         pci_set_drvdata(pci_dev, net_dev);      // Set driver data
 368
 369 /* for supporting Network Manager */
 370         /* Set the sysfs physical device reference for the network logical device
 371           * if set prior to registration will cause a symlink during initialization.
 372          */
 373         SET_NETDEV_DEV(net_dev, &(pci_dev->dev));
 374
 375
 376 //All done, it's time to register the net device to linux kernel.
 377         // Register this device
 378         rv = RtmpOSNetDevAttach(net_dev, &netDevHook);
 379         if (rv)
 380                 goto err_out_free_netdev;
 381
 382         pAd->StaCfg.OriDevType = net_dev->type;
 383         RTMPInitPCIeDevice(pci_dev, pAd);
 384
 385         DBGPRINT(RT_DEBUG_TRACE, ("<=== rt2860_probe\n"));
 386
 387         return 0; // probe ok
 388
 389
 390         /* --------------------------- ERROR HANDLE --------------------------- */
 391 err_out_free_netdev:
 392         RtmpOSNetDevFree(net_dev);
 393
 394 err_out_free_radev:
 395         /* free RTMP_ADAPTER strcuture and os_cookie*/
 396         RTMPFreeAdapter(pAd);
 397
 398 err_out_iounmap:
 399         iounmap((void *)(csr_addr));
 400         release_mem_region(pci_resource_start(pci_dev, 0), pci_resource_len(pci_dev, 0));
 401
 402 err_out_free_res:
 403         pci_release_regions(pci_dev);
 404
 405 err_out:
 406         pci_disable_device(pci_dev);
 407
 408         DBGPRINT(RT_DEBUG_ERROR, ("<=== rt2860_probe failed with rv = %d!\n", rv));
 409
 410         return -ENODEV; /* probe fail */
 411 }
 412
 413
 414 static VOID __devexit rt2860_remove_one(
 415     IN  struct pci_dev  *pci_dev)
 416 {
 417         PNET_DEV        net_dev = pci_get_drvdata(pci_dev);
 418         RTMP_ADAPTER    *pAd = NULL;
 419         ULONG                   csr_addr = net_dev->base_addr; // pAd->CSRBaseAddress;
 420
 421         GET_PAD_FROM_NET_DEV(pAd, net_dev);
 422
 423     DBGPRINT(RT_DEBUG_TRACE, ("===> rt2860_remove_one\n"));
 424
 425         if (pAd != NULL)
 426         {
 427                 // Unregister/Free all allocated net_device.
 428                 RtmpPhyNetDevExit(pAd, net_dev);
 429
 430                 // Unmap CSR base address
 431                 iounmap((char *)(csr_addr));
 432
 433                 // release memory region
 434                 release_mem_region(pci_resource_start(pci_dev, 0), pci_resource_len(pci_dev, 0));
 435
 436                 // Free RTMP_ADAPTER related structures.
 437                 RtmpRaDevCtrlExit(pAd);
 438
 439         }
 440         else
 441         {
 442                 // Unregister network device
 443                 RtmpOSNetDevDetach(net_dev);
 444
 445                 // Unmap CSR base address
 446                 iounmap((char *)(net_dev->base_addr));
 447
 448                 // release memory region
 449                 release_mem_region(pci_resource_start(pci_dev, 0), pci_resource_len(pci_dev, 0));
 450         }
 451
 452         // Free the root net_device
 453         RtmpOSNetDevFree(net_dev);
 454
 455 }
 456
 457
 458 /*
 459 ========================================================================
 460 Routine Description:
 461     Check the chipset vendor/product ID.
 462
 463 Arguments:
 464     _dev_p                              Point to the PCI or USB device
 465
 466 Return Value:
 467     TRUE                                Check ok
 468         FALSE                           Check fail
 469
 470 Note:
 471 ========================================================================
 472 */
 473 BOOLEAN RT28XXChipsetCheck(
 474         IN void *_dev_p)
 475 {
 476         /* always TRUE */
 477         return TRUE;
 478 }
 479
 480
 481 /***************************************************************************
 482  *
 483  *      PCIe device initialization related procedures.
 484  *
 485  ***************************************************************************/
 486  static VOID RTMPInitPCIeDevice(
 487     IN  struct pci_dev   *pci_dev,
 488     IN PRTMP_ADAPTER     pAd)
 489 {
 490         USHORT  device_id;
 491         POS_COOKIE pObj;
 492
 493         pObj = (POS_COOKIE) pAd->OS_Cookie;
 494         pci_read_config_word(pci_dev, PCI_DEVICE_ID, &device_id);
 495         device_id = le2cpu16(device_id);
 496         pObj->DeviceID = device_id;
 497         if (
 498 #ifdef RT2860
 499                 (device_id == NIC2860_PCIe_DEVICE_ID) ||
 500                 (device_id == NIC2790_PCIe_DEVICE_ID) ||
 501                 (device_id == VEN_AWT_PCIe_DEVICE_ID) ||
 502 #endif
 503 #ifdef RT3090
 504                 (device_id == NIC3090_PCIe_DEVICE_ID) ||
 505                 (device_id == NIC3091_PCIe_DEVICE_ID) ||
 506                 (device_id == NIC3092_PCIe_DEVICE_ID) ||
 507 #endif // RT3090 //
 508                  0)
 509         {
 510                 UINT32 MacCsr0 = 0, Index= 0;
 511                 do
 512                 {
 513                         RTMP_IO_READ32(pAd, MAC_CSR0, &MacCsr0);
 514
 515                         if ((MacCsr0 != 0x00) && (MacCsr0 != 0xFFFFFFFF))
 516                                 break;
 517
 518                         RTMPusecDelay(10);
 519                 } while (Index++ < 100);
 520
 521                 // Support advanced power save after 2892/2790.
 522                 // MAC version at offset 0x1000 is 0x2872XXXX/0x2870XXXX(PCIe, USB, SDIO).
 523                 if ((MacCsr0&0xffff0000) != 0x28600000)
 524                 {
 525                         OPSTATUS_SET_FLAG(pAd, fOP_STATUS_PCIE_DEVICE);
 526                 }
 527         }
 528 }
 529
 530
 531 VOID RTMPInitPCIeLinkCtrlValue(
 532         IN      PRTMP_ADAPTER   pAd)
 533 {
 534     INT     pos;
 535     USHORT      reg16, data2, PCIePowerSaveLevel, Configuration;
 536         UINT32 MacValue;
 537     BOOLEAN     bFindIntel = FALSE;
 538         POS_COOKIE pObj;
 539
 540         pObj = (POS_COOKIE) pAd->OS_Cookie;
 541
 542         if (!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_PCIE_DEVICE))
 543                 return;
 544
 545     DBGPRINT(RT_DEBUG_TRACE, ("%s.===>\n", __func__));
 546         // Init EEPROM, and save settings
 547         if (!(IS_RT3090(pAd) || IS_RT3572(pAd) || IS_RT3390(pAd)))
 548         {
 549                 RT28xx_EEPROM_READ16(pAd, 0x22, PCIePowerSaveLevel);
 550                 pAd->PCIePowerSaveLevel = PCIePowerSaveLevel & 0xff;
 551
 552                 pAd->LnkCtrlBitMask = 0;
 553                 if ((PCIePowerSaveLevel&0xff) == 0xff)
 554                 {
 555                         OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_PCIE_DEVICE);
 556                         DBGPRINT(RT_DEBUG_TRACE, ("====> PCIePowerSaveLevel = 0x%x.\n", PCIePowerSaveLevel));
 557                         return;
 558                 }
 559         else
 560         {
 561                 PCIePowerSaveLevel &= 0x3;
 562                 RT28xx_EEPROM_READ16(pAd, 0x24, data2);
 563
 564                 if( !(((data2&0xff00) == 0x9200) && ((data2&0x80) !=0)) )
 565                 {
 566                         if (PCIePowerSaveLevel > 1 )
 567                                 PCIePowerSaveLevel = 1;
 568                 }
 569
 570                 DBGPRINT(RT_DEBUG_TRACE, ("====> Write 0x83 = 0x%x.\n", PCIePowerSaveLevel));
 571                 AsicSendCommandToMcu(pAd, 0x83, 0xff, (UCHAR)PCIePowerSaveLevel, 0x00);
 572                 RT28xx_EEPROM_READ16(pAd, 0x22, PCIePowerSaveLevel);
 573                 PCIePowerSaveLevel &= 0xff;
 574                 PCIePowerSaveLevel = PCIePowerSaveLevel >> 6;
 575                 switch(PCIePowerSaveLevel)
 576                 {
 577                                 case 0: // Only support L0
 578                                         pAd->LnkCtrlBitMask = 0;
 579                                 break;
 580                                 case 1: // Only enable L0s
 581                                         pAd->LnkCtrlBitMask = 1;
 582                                 break;
 583                                 case 2: // enable L1, L0s
 584                                         pAd->LnkCtrlBitMask = 3;
 585                                 break;
 586                                 case 3: // sync with host clk and enable L1, L0s
 587                                 pAd->LnkCtrlBitMask = 0x103;
 588                                 break;
 589                 }
 590                         RT28xx_EEPROM_READ16(pAd, 0x24, data2);
 591                         if ((PCIePowerSaveLevel&0xff) != 0xff)
 592                         {
 593                                 PCIePowerSaveLevel &= 0x3;
 594
 595                                 if( !(((data2&0xff00) == 0x9200) && ((data2&0x80) !=0)) )
 596                                 {
 597                                         if (PCIePowerSaveLevel > 1 )
 598                                                 PCIePowerSaveLevel = 1;
 599                                 }
 600
 601                                 DBGPRINT(RT_DEBUG_TRACE, ("====> rt28xx Write 0x83 Command = 0x%x.\n", PCIePowerSaveLevel));
 602                                                printk("\n\n\n%s:%d\n",__FUNCTION__,__LINE__);
 603
 604                                 AsicSendCommandToMcu(pAd, 0x83, 0xff, (UCHAR)PCIePowerSaveLevel, 0x00);
 605                         }
 606                 DBGPRINT(RT_DEBUG_TRACE, ("====> LnkCtrlBitMask = 0x%x.\n", pAd->LnkCtrlBitMask));
 607         }
 608         }
 609         else if (IS_RT3090(pAd) || IS_RT3572(pAd) || IS_RT3390(pAd))
 610         {
 611                 UCHAR   LinkCtrlSetting = 0;
 612
 613                 // Check 3090E special setting chip.
 614                         RT28xx_EEPROM_READ16(pAd, 0x24, data2);
 615                 if ((data2 == 0x9280) && ((pAd->MACVersion&0xffff) == 0x0211))
 616                 {
 617                         pAd->b3090ESpecialChip = TRUE;
 618                         DBGPRINT_RAW(RT_DEBUG_ERROR,("Special 3090E chip \n"));
 619                 }
 620
 621                 RTMP_IO_READ32(pAd, AUX_CTRL, &MacValue);
 622                 //enable WAKE_PCIE function, which forces to enable PCIE clock when mpu interrupt asserting.
 623                 //Force PCIE 125MHz CLK to toggle
 624                 MacValue |= 0x402;
 625                 RTMP_IO_WRITE32(pAd, AUX_CTRL, MacValue);
 626                 DBGPRINT_RAW(RT_DEBUG_ERROR,(" AUX_CTRL = 0x%32x\n", MacValue));
 627
 628
 629
 630                 // for RT30xx F and after, PCIe infterface, and for power solution 3
 631                 if ((IS_VERSION_AFTER_F(pAd))
 632                         && (pAd->StaCfg.PSControl.field.rt30xxPowerMode >= 2)
 633                         && (pAd->StaCfg.PSControl.field.rt30xxPowerMode <= 3))
 634                 {
 635                         RTMP_IO_READ32(pAd, AUX_CTRL, &MacValue);
 636                         DBGPRINT_RAW(RT_DEBUG_ERROR,(" Read AUX_CTRL = 0x%x\n", MacValue));
 637                         // turn on bit 12.
 638                         //enable 32KHz clock mode for power saving
 639                         MacValue |= 0x1000;
 640                         if (MacValue != 0xffffffff)
 641                         {
 642                                 RTMP_IO_WRITE32(pAd, AUX_CTRL, MacValue);
 643                                 DBGPRINT_RAW(RT_DEBUG_ERROR,(" Write AUX_CTRL = 0x%x\n", MacValue));
 644                                 // 1. if use PCIePowerSetting is 2 or 3, need to program OSC_CTRL to 0x3ff11.
 645                                 MacValue = 0x3ff11;
 646                                 RTMP_IO_WRITE32(pAd, OSC_CTRL, MacValue);
 647                                 DBGPRINT_RAW(RT_DEBUG_ERROR,(" OSC_CTRL = 0x%x\n", MacValue));
 648                                 // 2. Write PCI register Clk ref bit
 649                                 RTMPrt3xSetPCIePowerLinkCtrl(pAd);
 650                         }
 651                         else
 652                         {
 653                                 // Error read Aux_Ctrl value.  Force to use solution 1
 654                                 DBGPRINT(RT_DEBUG_ERROR,(" Error Value in AUX_CTRL = 0x%x\n", MacValue));
 655                                 pAd->StaCfg.PSControl.field.rt30xxPowerMode = 1;
 656                                 DBGPRINT(RT_DEBUG_ERROR,(" Force to use power solution1 \n"));
 657                         }
 658                 }
 659                 // 1. read setting from inf file.
 660
 661                 PCIePowerSaveLevel = (USHORT)pAd->StaCfg.PSControl.field.rt30xxPowerMode;
 662                 DBGPRINT(RT_DEBUG_ERROR, ("====> rt30xx Read PowerLevelMode =  0x%x.\n", PCIePowerSaveLevel));
 663                 // 2. Check EnableNewPS.
 664                 if (pAd->StaCfg.PSControl.field.EnableNewPS == FALSE)
 665                         PCIePowerSaveLevel = 1;
 666
 667                 if (IS_VERSION_BEFORE_F(pAd) && (pAd->b3090ESpecialChip == FALSE))
 668                 {
 669                         // Chip Version E only allow 1, So force set 1.
 670                         PCIePowerSaveLevel &= 0x1;
 671                         pAd->PCIePowerSaveLevel = (USHORT)PCIePowerSaveLevel;
 672                         DBGPRINT(RT_DEBUG_TRACE, ("====> rt30xx E Write 0x83 Command = 0x%x.\n", PCIePowerSaveLevel));
 673
 674                         AsicSendCommandToMcu(pAd, 0x83, 0xff, (UCHAR)PCIePowerSaveLevel, 0x00);
 675                 }
 676                 else
 677                 {
 678                         // Chip Version F and after only allow 1 or 2 or 3. This might be modified after new chip version come out.
 679                         if (!((PCIePowerSaveLevel == 1) || (PCIePowerSaveLevel == 3)))
 680                                 PCIePowerSaveLevel = 1;
 681                         DBGPRINT(RT_DEBUG_ERROR, ("====> rt30xx F Write 0x83 Command = 0x%x.\n", PCIePowerSaveLevel));
 682                         pAd->PCIePowerSaveLevel = (USHORT)PCIePowerSaveLevel;
 683                         // for 3090F , we need to add high-byte arg for 0x83 command to indicate the link control setting in
 684                         // PCI Configuration Space. Because firmware can't read PCI Configuration Space
 685                         if ((pAd->Rt3xxRalinkLinkCtrl & 0x2) && (pAd->Rt3xxHostLinkCtrl & 0x2))
 686                         {
 687                                 LinkCtrlSetting = 1;
 688                         }
 689                         DBGPRINT(RT_DEBUG_TRACE, ("====> rt30xxF LinkCtrlSetting = 0x%x.\n", LinkCtrlSetting));
 690                         AsicSendCommandToMcu(pAd, 0x83, 0xff, (UCHAR)PCIePowerSaveLevel, LinkCtrlSetting);
 691                 }
 692         }
 693
 694     // Find Ralink PCIe Device's Express Capability Offset
 695         pos = pci_find_capability(pObj->pci_dev, PCI_CAP_ID_EXP);
 696
 697     if (pos != 0)
 698     {
 699         // Ralink PCIe Device's Link Control Register Offset
 700         pAd->RLnkCtrlOffset = pos + PCI_EXP_LNKCTL;
 701         pci_read_config_word(pObj->pci_dev, pAd->RLnkCtrlOffset, &reg16);
 702         Configuration = le2cpu16(reg16);
 703         DBGPRINT(RT_DEBUG_TRACE, ("Read (Ralink PCIe Link Control Register) offset 0x%x = 0x%x\n",
 704                                     pAd->RLnkCtrlOffset, Configuration));
 705         pAd->RLnkCtrlConfiguration = (Configuration & 0x103);
 706         Configuration &= 0xfefc;
 707         Configuration |= (0x0);
 708 #ifdef RT2860
 709                 if ((pObj->DeviceID == NIC2860_PCIe_DEVICE_ID)
 710                         ||(pObj->DeviceID == NIC2790_PCIe_DEVICE_ID))
 711                 {
 712                         reg16 = cpu2le16(Configuration);
 713                         pci_write_config_word(pObj->pci_dev, pAd->RLnkCtrlOffset, reg16);
 714                         DBGPRINT(RT_DEBUG_TRACE, ("Write (Ralink PCIe Link Control Register)  offset 0x%x = 0x%x\n",
 715                                     pos + PCI_EXP_LNKCTL, Configuration));
 716                 }
 717 #endif // RT2860 //
 718
 719         RTMPFindHostPCIDev(pAd);
 720         if (pObj->parent_pci_dev)
 721         {
 722                 USHORT  vendor_id;
 723
 724                 pci_read_config_word(pObj->parent_pci_dev, PCI_VENDOR_ID, &vendor_id);
 725                 vendor_id = le2cpu16(vendor_id);
 726                 if (vendor_id == PCIBUS_INTEL_VENDOR)
 727                  {
 728                         bFindIntel = TRUE;
 729                         RTMP_SET_PSFLAG(pAd, fRTMP_PS_TOGGLE_L1);
 730                  }
 731
 732                 // Find PCI-to-PCI Bridge Express Capability Offset
 733                 pos = pci_find_capability(pObj->parent_pci_dev, PCI_CAP_ID_EXP);
 734
 735                 if (pos != 0)
 736                 {
 737                         BOOLEAN         bChange = FALSE;
 738                         // PCI-to-PCI Bridge Link Control Register Offset
 739                         pAd->HostLnkCtrlOffset = pos + PCI_EXP_LNKCTL;
 740                         pci_read_config_word(pObj->parent_pci_dev, pAd->HostLnkCtrlOffset, &reg16);
 741                         Configuration = le2cpu16(reg16);
 742                         DBGPRINT(RT_DEBUG_TRACE, ("Read (Host PCI-to-PCI Bridge Link Control Register) offset 0x%x = 0x%x\n",
 743                                                     pAd->HostLnkCtrlOffset, Configuration));
 744                         pAd->HostLnkCtrlConfiguration = (Configuration & 0x103);
 745                         Configuration &= 0xfefc;
 746                         Configuration |= (0x0);
 747
 748                         switch (pObj->DeviceID)
 749                         {
 750 #ifdef RT2860
 751                                 case NIC2860_PCIe_DEVICE_ID:
 752                                 case NIC2790_PCIe_DEVICE_ID:
 753                                         bChange = TRUE;
 754                                         break;
 755 #endif // RT2860 //
 756 #ifdef RT3090
 757                                 case NIC3090_PCIe_DEVICE_ID:
 758                                 case NIC3091_PCIe_DEVICE_ID:
 759                                 case NIC3092_PCIe_DEVICE_ID:
 760                                         if (bFindIntel == FALSE)
 761                                         bChange = TRUE;
 762                                         break;
 763 #endif // RT3090 //
 764                                 default:
 765                                         break;
 766                         }
 767
 768                         if (bChange)
 769                         {
 770                                 reg16 = cpu2le16(Configuration);
 771                                 pci_write_config_word(pObj->parent_pci_dev, pAd->HostLnkCtrlOffset, reg16);
 772                                 DBGPRINT(RT_DEBUG_TRACE, ("Write (Host PCI-to-PCI Bridge Link Control Register) offset 0x%x = 0x%x\n",
 773                                                 pAd->HostLnkCtrlOffset, Configuration));
 774                         }
 775                 }
 776                 else
 777                 {
 778                         pAd->HostLnkCtrlOffset = 0;
 779                         DBGPRINT(RT_DEBUG_ERROR, ("%s: cannot find PCI-to-PCI Bridge PCI Express Capability!\n", __func__));
 780                 }
 781         }
 782     }
 783     else
 784     {
 785         pAd->RLnkCtrlOffset = 0;
 786         pAd->HostLnkCtrlOffset = 0;
 787         DBGPRINT(RT_DEBUG_ERROR, ("%s: cannot find Ralink PCIe Device's PCI Express Capability!\n", __func__));
 788     }
 789
 790     if (bFindIntel == FALSE)
 791         {
 792                 DBGPRINT(RT_DEBUG_TRACE, ("Doesn't find Intel PCI host controller. \n"));
 793                 // Doesn't switch L0, L1, So set PCIePowerSaveLevel to 0xff
 794                 pAd->PCIePowerSaveLevel = 0xff;
 795                 if ((pAd->RLnkCtrlOffset != 0)
 796 #ifdef RT3090
 797                         && ((pObj->DeviceID == NIC3090_PCIe_DEVICE_ID)
 798                                 ||(pObj->DeviceID == NIC3091_PCIe_DEVICE_ID)
 799                                 ||(pObj->DeviceID == NIC3092_PCIe_DEVICE_ID))
 800 #endif // RT3090 //
 801                 )
 802                 {
 803                         pci_read_config_word(pObj->pci_dev, pAd->RLnkCtrlOffset, &reg16);
 804                         Configuration = le2cpu16(reg16);
 805                         DBGPRINT(RT_DEBUG_TRACE, ("Read (Ralink 30xx PCIe Link Control Register) offset 0x%x = 0x%x\n",
 806                                                 pAd->RLnkCtrlOffset, Configuration));
 807                         pAd->RLnkCtrlConfiguration = (Configuration & 0x103);
 808                         Configuration &= 0xfefc;
 809                         Configuration |= (0x0);
 810                         reg16 = cpu2le16(Configuration);
 811                         pci_write_config_word(pObj->pci_dev, pAd->RLnkCtrlOffset, reg16);
 812                         DBGPRINT(RT_DEBUG_TRACE, ("Write (Ralink PCIe Link Control Register)  offset 0x%x = 0x%x\n",
 813                                                 pos + PCI_EXP_LNKCTL, Configuration));
 814                 }
 815         }
 816 }
 817
 818 VOID RTMPFindHostPCIDev(
 819     IN  PRTMP_ADAPTER   pAd)
 820 {
 821     USHORT  reg16;
 822     UCHAR   reg8;
 823         UINT    DevFn;
 824     PPCI_DEV    pPci_dev;
 825         POS_COOKIE      pObj;
 826
 827         pObj = (POS_COOKIE) pAd->OS_Cookie;
 828
 829         if (!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_PCIE_DEVICE))
 830                 return;
 831
 832     DBGPRINT(RT_DEBUG_TRACE, ("%s.===>\n", __func__));
 833
 834     pObj->parent_pci_dev = NULL;
 835     if (pObj->pci_dev->bus->parent)
 836     {
 837         for (DevFn = 0; DevFn < 255; DevFn++)
 838         {
 839             pPci_dev = pci_get_slot(pObj->pci_dev->bus->parent, DevFn);
 840             if (pPci_dev)
 841             {
 842                 pci_read_config_word(pPci_dev, PCI_CLASS_DEVICE, &reg16);
 843                 reg16 = le2cpu16(reg16);
 844                 pci_read_config_byte(pPci_dev, PCI_CB_CARD_BUS, &reg8);
 845                 if ((reg16 == PCI_CLASS_BRIDGE_PCI) &&
 846                     (reg8 == pObj->pci_dev->bus->number))
 847                 {
 848                     pObj->parent_pci_dev = pPci_dev;
 849                 }
 850             }
 851         }
 852     }
 853 }
 854
 855 /*
 856         ========================================================================
 857
 858         Routine Description:
 859
 860         Arguments:
 861                 Level = RESTORE_HALT : Restore PCI host and Ralink PCIe Link Control field to its default value.
 862                 Level = Other Value : Restore from dot11 power save or radio off status. And force PCI host Link Control fields to 0x1
 863
 864         ========================================================================
 865 */
 866 VOID RTMPPCIeLinkCtrlValueRestore(
 867         IN      PRTMP_ADAPTER   pAd,
 868         IN   UCHAR              Level)
 869 {
 870         USHORT  PCIePowerSaveLevel, reg16;
 871         USHORT  Configuration;
 872         POS_COOKIE      pObj;
 873
 874         pObj = (POS_COOKIE) pAd->OS_Cookie;
 875
 876         if (!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_PCIE_DEVICE))
 877                 return;
 878
 879 #ifdef RT2860
 880         if (!((pObj->DeviceID == NIC2860_PCIe_DEVICE_ID)
 881                 ||(pObj->DeviceID == NIC2790_PCIe_DEVICE_ID)))
 882                 return;
 883 #endif // RT2860 //
 884         // Check PSControl Configuration
 885         if (pAd->StaCfg.PSControl.field.EnableNewPS == FALSE)
 886                 return TRUE;
 887
 888         //3090 will not execute the following codes.
 889         // Check interface : If not PCIe interface, return.
 890
 891 #ifdef RT3090
 892         if ((pObj->DeviceID == NIC3090_PCIe_DEVICE_ID)
 893                 ||(pObj->DeviceID == NIC3091_PCIe_DEVICE_ID)
 894                 ||(pObj->DeviceID == NIC3092_PCIe_DEVICE_ID))
 895                 return;
 896 #endif // RT3090 //
 897
 898         DBGPRINT(RT_DEBUG_TRACE, ("%s.===>\n", __func__));
 899         PCIePowerSaveLevel = pAd->PCIePowerSaveLevel;
 900         if ((PCIePowerSaveLevel&0xff) == 0xff)
 901         {
 902                 DBGPRINT(RT_DEBUG_TRACE,("return  \n"));
 903                 return;
 904         }
 905
 906         if (pObj->parent_pci_dev && (pAd->HostLnkCtrlOffset != 0))
 907     {
 908         PCI_REG_READ_WORD(pObj->parent_pci_dev, pAd->HostLnkCtrlOffset, Configuration);
 909         if ((Configuration != 0) &&
 910             (Configuration != 0xFFFF))
 911         {
 912                 Configuration &= 0xfefc;
 913                 // If call from interface down, restore to orginial setting.
 914                 if (Level == RESTORE_CLOSE)
 915                 {
 916                         Configuration |= pAd->HostLnkCtrlConfiguration;
 917                 }
 918                 else
 919                         Configuration |= 0x0;
 920             PCI_REG_WIRTE_WORD(pObj->parent_pci_dev, pAd->HostLnkCtrlOffset, Configuration);
 921                 DBGPRINT(RT_DEBUG_TRACE, ("Restore PCI host : offset 0x%x = 0x%x\n", pAd->HostLnkCtrlOffset, Configuration));
 922         }
 923         else
 924             DBGPRINT(RT_DEBUG_ERROR, ("Restore PCI host : PCI_REG_READ_WORD failed (Configuration = 0x%x)\n", Configuration));
 925     }
 926
 927     if (pObj->pci_dev && (pAd->RLnkCtrlOffset != 0))
 928     {
 929         PCI_REG_READ_WORD(pObj->pci_dev, pAd->RLnkCtrlOffset, Configuration);
 930         if ((Configuration != 0) &&
 931             (Configuration != 0xFFFF))
 932         {
 933                 Configuration &= 0xfefc;
 934                         // If call from interface down, restore to orginial setting.
 935                         if (Level == RESTORE_CLOSE)
 936                 Configuration |= pAd->RLnkCtrlConfiguration;
 937                         else
 938                                 Configuration |= 0x0;
 939             PCI_REG_WIRTE_WORD(pObj->pci_dev, pAd->RLnkCtrlOffset, Configuration);
 940                 DBGPRINT(RT_DEBUG_TRACE, ("Restore Ralink : offset 0x%x = 0x%x\n", pAd->RLnkCtrlOffset, Configuration));
 941         }
 942         else
 943             DBGPRINT(RT_DEBUG_ERROR, ("Restore Ralink : PCI_REG_READ_WORD failed (Configuration = 0x%x)\n", Configuration));
 944         }
 945
 946         DBGPRINT(RT_DEBUG_TRACE,("%s <===\n", __func__));
 947 }
 948
 949 /*
 950         ========================================================================
 951
 952         Routine Description:
 953
 954         Arguments:
 955                 Max : limit Host PCI and Ralink PCIe device's LINK CONTROL field's value.
 956                 Because now frequently set our device to mode 1 or mode 3 will cause problem.
 957
 958         ========================================================================
 959 */
 960 VOID RTMPPCIeLinkCtrlSetting(
 961         IN      PRTMP_ADAPTER   pAd,
 962         IN      USHORT          Max)
 963 {
 964         USHORT  PCIePowerSaveLevel, reg16;
 965         USHORT  Configuration;
 966         POS_COOKIE      pObj;
 967
 968         pObj = (POS_COOKIE) pAd->OS_Cookie;
 969
 970         if (!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_PCIE_DEVICE))
 971                 return;
 972
 973 #ifdef RT2860
 974         if (!((pObj->DeviceID == NIC2860_PCIe_DEVICE_ID)
 975                 ||(pObj->DeviceID == NIC2790_PCIe_DEVICE_ID)))
 976                 return;
 977 #endif // RT2860 //
 978         // Check PSControl Configuration
 979         if (pAd->StaCfg.PSControl.field.EnableNewPS == FALSE)
 980                 return TRUE;
 981
 982         // Check interface : If not PCIe interface, return.
 983         //Block 3090 to enter the following function
 984
 985 #ifdef RT3090
 986         if ((pObj->DeviceID == NIC3090_PCIe_DEVICE_ID)
 987                 ||(pObj->DeviceID == NIC3091_PCIe_DEVICE_ID)
 988                 ||(pObj->DeviceID == NIC3092_PCIe_DEVICE_ID))
 989                 return;
 990 #endif // RT3090 //
 991         if (!RTMP_TEST_PSFLAG(pAd, fRTMP_PS_CAN_GO_SLEEP))
 992         {
 993                 DBGPRINT(RT_DEBUG_INFO, ("RTMPPCIePowerLinkCtrl return on fRTMP_PS_CAN_GO_SLEEP flag\n"));
 994                 return;
 995         }
 996
 997         DBGPRINT(RT_DEBUG_TRACE,("%s===>\n", __func__));
 998         PCIePowerSaveLevel = pAd->PCIePowerSaveLevel;
 999         if ((PCIePowerSaveLevel&0xff) == 0xff)
1000         {
1001                 DBGPRINT(RT_DEBUG_TRACE,("return  \n"));
1002                 return;
1003         }
1004         PCIePowerSaveLevel = PCIePowerSaveLevel>>6;
1005
1006     // Skip non-exist deice right away
1007         if (pObj->parent_pci_dev && (pAd->HostLnkCtrlOffset != 0))
1008         {
1009         PCI_REG_READ_WORD(pObj->parent_pci_dev, pAd->HostLnkCtrlOffset, Configuration);
1010                 switch (PCIePowerSaveLevel)
1011                 {
1012                         case 0:
1013                                 // Set b0 and b1 of LinkControl (both 2892 and PCIe bridge) to 00
1014                                 Configuration &= 0xfefc;
1015                                 break;
1016                         case 1:
1017                                 // Set b0 and b1 of LinkControl (both 2892 and PCIe bridge) to 01
1018                                 Configuration &= 0xfefc;
1019                                 Configuration |= 0x1;
1020                                 break;
1021                         case 2:
1022                                 //  Set b0 and b1 of LinkControl (both 2892 and PCIe bridge) to 11
1023                                 Configuration &= 0xfefc;
1024                                 Configuration |= 0x3;
1025                                 break;
1026                         case 3:
1027                                 // Set b0 and b1 of LinkControl (both 2892 and PCIe bridge) to 11 and bit 8 of LinkControl of 2892 to 1
1028                                 Configuration &= 0xfefc;
1029                                 Configuration |= 0x103;
1030                                 break;
1031                 }
1032         PCI_REG_WIRTE_WORD(pObj->parent_pci_dev, pAd->HostLnkCtrlOffset, Configuration);
1033                 DBGPRINT(RT_DEBUG_TRACE, ("Write PCI host offset 0x%x = 0x%x\n", pAd->HostLnkCtrlOffset, Configuration));
1034         }
1035
1036         if (pObj->pci_dev && (pAd->RLnkCtrlOffset != 0))
1037         {
1038                 // first 2892 chip not allow to frequently set mode 3. will cause hang problem.
1039                 if (PCIePowerSaveLevel > Max)
1040                         PCIePowerSaveLevel = Max;
1041
1042         PCI_REG_READ_WORD(pObj->pci_dev, pAd->RLnkCtrlOffset, Configuration);
1043                 switch (PCIePowerSaveLevel)
1044                 {
1045                         case 0:
1046                                 // No PCI power safe
1047                                 // Set b0 and b1 of LinkControl (both 2892 and PCIe bridge) to 00 .
1048                                 Configuration &= 0xfefc;
1049                                 break;
1050                         case 1:
1051                                 //  L0
1052                                 // Set b0 and b1 of LinkControl (both 2892 and PCIe bridge) to 01 .
1053                                 Configuration &= 0xfefc;
1054                                 Configuration |= 0x1;
1055                                 break;
1056                         case 2:
1057                                 // L0 and L1
1058                                 //  Set b0 and b1 of LinkControl (both 2892 and PCIe bridge) to 11
1059                                 Configuration &= 0xfefc;
1060                                 Configuration |= 0x3;
1061                                 break;
1062                         case 3:
1063                                 // L0 , L1 and clock management.
1064                                 // Set b0 and b1 of LinkControl (both 2892 and PCIe bridge) to 11 and bit 8 of LinkControl of 2892 to 1
1065                                 Configuration &= 0xfefc;
1066                                 Configuration |= 0x103;
1067                               pAd->bPCIclkOff = TRUE;
1068                                 break;
1069                 }
1070         PCI_REG_WIRTE_WORD(pObj->pci_dev, pAd->RLnkCtrlOffset, Configuration);
1071                 DBGPRINT(RT_DEBUG_TRACE, ("Write Ralink device : offset 0x%x = 0x%x\n", pAd->RLnkCtrlOffset, Configuration));
1072         }
1073
1074         DBGPRINT(RT_DEBUG_TRACE,("RTMPPCIePowerLinkCtrl <==============\n"));
1075 }
1076
1077 /*
1078         ========================================================================
1079
1080         Routine Description:
1081                 1. Write a PCI register for rt30xx power solution 3
1082
1083         ========================================================================
1084 */
1085 VOID RTMPrt3xSetPCIePowerLinkCtrl(
1086         IN      PRTMP_ADAPTER   pAd)
1087 {
1088
1089         ULONG   HostConfiguration;
1090         ULONG   Configuration;
1091         ULONG   Vendor;
1092         ULONG   offset;
1093         POS_COOKIE      pObj;
1094         INT     pos;
1095         USHORT  reg16;
1096
1097         pObj = (POS_COOKIE) pAd->OS_Cookie;
1098
1099         DBGPRINT(RT_DEBUG_INFO, ("RTMPrt3xSetPCIePowerLinkCtrl.===> %x\n", pAd->StaCfg.PSControl.word));
1100
1101         // Check PSControl Configuration
1102         if (pAd->StaCfg.PSControl.field.EnableNewPS == FALSE)
1103                 return;
1104         RTMPFindHostPCIDev(pAd);
1105         if (pObj->parent_pci_dev)
1106         {
1107                 USHORT  vendor_id;
1108                 // Find PCI-to-PCI Bridge Express Capability Offset
1109                 pos = pci_find_capability(pObj->parent_pci_dev, PCI_CAP_ID_EXP);
1110
1111                 if (pos != 0)
1112                 {
1113                         pAd->HostLnkCtrlOffset = pos + PCI_EXP_LNKCTL;
1114                 }
1115         // If configurared to turn on L1.
1116         HostConfiguration = 0;
1117                 if (pAd->StaCfg.PSControl.field.rt30xxForceASPMTest == 1)
1118                 {
1119                                                 DBGPRINT(RT_DEBUG_TRACE, ("Enter,PSM : Force ASPM \n"));
1120
1121                         // Skip non-exist deice right away
1122                         if ((pAd->HostLnkCtrlOffset != 0))
1123                         {
1124                          PCI_REG_READ_WORD(pObj->parent_pci_dev, pAd->HostLnkCtrlOffset, HostConfiguration);
1125                                 // Prepare Configuration to write to Host
1126                                 HostConfiguration |= 0x3;
1127                                 PCI_REG_WIRTE_WORD(pObj->parent_pci_dev, pAd->HostLnkCtrlOffset, HostConfiguration);
1128                                 pAd->Rt3xxHostLinkCtrl = HostConfiguration;
1129                                 // Because in rt30xxForceASPMTest Mode, Force turn on L0s, L1.
1130                                 // Fix HostConfiguration bit0:1 = 0x3 for later use.
1131                                 HostConfiguration = 0x3;
1132                                 DBGPRINT(RT_DEBUG_TRACE, ("PSM : Force ASPM : Host device L1/L0s Value =  0x%x\n", HostConfiguration));
1133                         }
1134                 }
1135                 else if (pAd->StaCfg.PSControl.field.rt30xxFollowHostASPM == 1)
1136                 {
1137
1138                         // Skip non-exist deice right away
1139                         if ((pAd->HostLnkCtrlOffset != 0))
1140                         {
1141                          PCI_REG_READ_WORD(pObj->parent_pci_dev, pAd->HostLnkCtrlOffset, HostConfiguration);
1142                                 pAd->Rt3xxHostLinkCtrl = HostConfiguration;
1143                                 HostConfiguration &= 0x3;
1144                                 DBGPRINT(RT_DEBUG_TRACE, ("PSM : Follow Host ASPM : Host device L1/L0s Value =  0x%x\n", HostConfiguration));
1145                         }
1146                 }
1147         }
1148         // Prepare to write Ralink setting.
1149         // Find Ralink PCIe Device's Express Capability Offset
1150         pos = pci_find_capability(pObj->pci_dev, PCI_CAP_ID_EXP);
1151
1152     if (pos != 0)
1153     {
1154         // Ralink PCIe Device's Link Control Register Offset
1155        pAd->RLnkCtrlOffset = pos + PCI_EXP_LNKCTL;
1156         pci_read_config_word(pObj->pci_dev, pAd->RLnkCtrlOffset, &reg16);
1157         Configuration = le2cpu16(reg16);
1158         DBGPRINT(RT_DEBUG_TRACE, ("Read (Ralink PCIe Link Control Register) offset 0x%x = 0x%x\n",
1159                                                             pAd->RLnkCtrlOffset, Configuration));
1160                 Configuration |= 0x100;
1161                 if ((pAd->StaCfg.PSControl.field.rt30xxFollowHostASPM == 1)
1162                         || (pAd->StaCfg.PSControl.field.rt30xxForceASPMTest == 1))
1163                 {
1164                         switch(HostConfiguration)
1165                         {
1166                                 case 0:
1167                                         Configuration &= 0xffffffc;
1168                                         break;
1169                                 case 1:
1170                                         Configuration &= 0xffffffc;
1171                                         Configuration |= 0x1;
1172                                         break;
1173                                 case 2:
1174                                         Configuration &= 0xffffffc;
1175                                         Configuration |= 0x2;
1176                                         break;
1177                                 case 3:
1178                                         Configuration |= 0x3;
1179                                         break;
1180                         }
1181                 }
1182                 reg16 = cpu2le16(Configuration);
1183                 pci_write_config_word(pObj->pci_dev, pAd->RLnkCtrlOffset, reg16);
1184                 pAd->Rt3xxRalinkLinkCtrl = Configuration;
1185                 DBGPRINT(RT_DEBUG_TRACE, ("PSM :Write Ralink device L1/L0s Value =  0x%x\n", Configuration));
1186         }
1187         DBGPRINT(RT_DEBUG_INFO,("PSM :RTMPrt3xSetPCIePowerLinkCtrl <==============\n"));
1188 }