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