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