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         rv = pci_enable_device(pci_dev);
 285
 286         if (rv != 0) {
 287                 DBGPRINT(RT_DEBUG_ERROR,
 288                          ("Enable PCI device failed, errno=%d!\n", rv));
 289                 return rv;
 290         }
 291
 292         print_name = (char *)pci_name(pci_dev);
 293
 294         rv = pci_request_regions(pci_dev, print_name);
 295
 296         if (rv != 0) {
 297                 DBGPRINT(RT_DEBUG_ERROR,
 298                          ("Request PCI resource failed, errno=%d!\n", rv));
 299                 goto err_out;
 300         }
 301         /* map physical address to virtual address for accessing register */
 302         csr_addr =
 303             (unsigned long)ioremap(pci_resource_start(pci_dev, 0),
 304                                    pci_resource_len(pci_dev, 0));
 305         if (!csr_addr) {
 306                 DBGPRINT(RT_DEBUG_ERROR,
 307                          ("ioremap failed for device %s, region 0x%lX @ 0x%lX\n",
 308                           print_name, (unsigned long)pci_resource_len(pci_dev, 0),
 309                           (unsigned long)pci_resource_start(pci_dev, 0)));
 310                 goto err_out_free_res;
 311         } else {
 312                 DBGPRINT(RT_DEBUG_TRACE,
 313                          ("%s: at 0x%lx, VA 0x%lx, IRQ %d. \n", print_name,
 314                           (unsigned long)pci_resource_start(pci_dev, 0),
 315                           (unsigned long)csr_addr, pci_dev->irq));
 316         }
 317
 318         /* Set DMA master */
 319         pci_set_master(pci_dev);
 320
 321 /*RtmpDevInit============================================== */
 322         /* Allocate struct rt_rtmp_adapter adapter structure */
 323         handle = kmalloc(sizeof(struct os_cookie), GFP_KERNEL);
 324         if (handle == NULL) {
 325                 DBGPRINT(RT_DEBUG_ERROR,
 326                          ("%s(): Allocate memory for os handle failed!\n",
 327                           __func__));
 328                 goto err_out_iounmap;
 329         }
 330
 331         ((struct os_cookie *)handle)->pci_dev = pci_dev;
 332
 333         rv = RTMPAllocAdapterBlock(handle, &pAd);       /*shiang: we may need the pci_dev for allocate structure of "struct rt_rtmp_adapter" */
 334         if (rv != NDIS_STATUS_SUCCESS)
 335                 goto err_out_iounmap;
 336         /* Here are the struct rt_rtmp_adapter structure with pci-bus specific parameters. */
 337         pAd->CSRBaseAddress = (u8 *)csr_addr;
 338         DBGPRINT(RT_DEBUG_ERROR,
 339                  ("pAd->CSRBaseAddress =0x%lx, csr_addr=0x%lx!\n",
 340                   (unsigned long)pAd->CSRBaseAddress, csr_addr));
 341         RtmpRaDevCtrlInit(pAd, RTMP_DEV_INF_PCI);
 342
 343 /*NetDevInit============================================== */
 344         net_dev = RtmpPhyNetDevInit(pAd, &netDevHook);
 345         if (net_dev == NULL)
 346                 goto err_out_free_radev;
 347
 348         /* Here are the net_device structure with pci-bus specific parameters. */
 349         net_dev->irq = pci_dev->irq;    /* Interrupt IRQ number */
 350         net_dev->base_addr = csr_addr;  /* Save CSR virtual address and irq to device structure */
 351         pci_set_drvdata(pci_dev, net_dev);      /* Set driver data */
 352
 353 /* for supporting Network Manager */
 354         /* Set the sysfs physical device reference for the network logical device
 355          * if set prior to registration will cause a symlink during initialization.
 356          */
 357         SET_NETDEV_DEV(net_dev, &(pci_dev->dev));
 358
 359 /*All done, it's time to register the net device to linux kernel. */
 360         /* Register this device */
 361         rv = RtmpOSNetDevAttach(net_dev, &netDevHook);
 362         if (rv)
 363                 goto err_out_free_netdev;
 364
 365         pAd->StaCfg.OriDevType = net_dev->type;
 366         RTMPInitPCIeDevice(pci_dev, pAd);
 367
 368         DBGPRINT(RT_DEBUG_TRACE, ("<=== rt2860_probe\n"));
 369
 370         return 0;               /* probe ok */
 371
 372         /* --------------------------- ERROR HANDLE --------------------------- */
 373 err_out_free_netdev:
 374         RtmpOSNetDevFree(net_dev);
 375
 376 err_out_free_radev:
 377         /* free struct rt_rtmp_adapter strcuture and os_cookie */
 378         RTMPFreeAdapter(pAd);
 379
 380 err_out_iounmap:
 381         iounmap((void *)(csr_addr));
 382         release_mem_region(pci_resource_start(pci_dev, 0),
 383                            pci_resource_len(pci_dev, 0));
 384
 385 err_out_free_res:
 386         pci_release_regions(pci_dev);
 387
 388 err_out:
 389         pci_disable_device(pci_dev);
 390
 391         DBGPRINT(RT_DEBUG_ERROR,
 392                  ("<=== rt2860_probe failed with rv = %d!\n", rv));
 393
 394         return -ENODEV;         /* probe fail */
 395 }
 396
 397 static void __devexit rt2860_remove_one(IN struct pci_dev *pci_dev)
 398 {
 399         struct net_device *net_dev = pci_get_drvdata(pci_dev);
 400         struct rt_rtmp_adapter *pAd = NULL;
 401         unsigned long csr_addr = net_dev->base_addr;    /* pAd->CSRBaseAddress; */
 402
 403         GET_PAD_FROM_NET_DEV(pAd, net_dev);
 404
 405         DBGPRINT(RT_DEBUG_TRACE, ("===> rt2860_remove_one\n"));
 406
 407         if (pAd != NULL) {
 408                 /* Unregister/Free all allocated net_device. */
 409                 RtmpPhyNetDevExit(pAd, net_dev);
 410
 411                 /* Unmap CSR base address */
 412                 iounmap((char *)(csr_addr));
 413
 414                 /* release memory region */
 415                 release_mem_region(pci_resource_start(pci_dev, 0),
 416                                    pci_resource_len(pci_dev, 0));
 417
 418                 /* Free struct rt_rtmp_adapter related structures. */
 419                 RtmpRaDevCtrlExit(pAd);
 420
 421         } else {
 422                 /* Unregister network device */
 423                 RtmpOSNetDevDetach(net_dev);
 424
 425                 /* Unmap CSR base address */
 426                 iounmap((char *)(net_dev->base_addr));
 427
 428                 /* release memory region */
 429                 release_mem_region(pci_resource_start(pci_dev, 0),
 430                                    pci_resource_len(pci_dev, 0));
 431         }
 432
 433         /* Free the root net_device */
 434         RtmpOSNetDevFree(net_dev);
 435
 436 }
 437
 438 /*
 439 ========================================================================
 440 Routine Description:
 441     Check the chipset vendor/product ID.
 442
 443 Arguments:
 444     _dev_p                              Point to the PCI or USB device
 445
 446 Return Value:
 447     TRUE                                Check ok
 448         FALSE                           Check fail
 449
 450 Note:
 451 ========================================================================
 452 */
 453 BOOLEAN RT28XXChipsetCheck(IN void *_dev_p)
 454 {
 455         /* always TRUE */
 456         return TRUE;
 457 }
 458
 459 /***************************************************************************
 460  *
 461  *      PCIe device initialization related procedures.
 462  *
 463  ***************************************************************************/
 464 static void RTMPInitPCIeDevice(struct pci_dev *pci_dev, struct rt_rtmp_adapter *pAd)
 465 {
 466         u16 device_id;
 467         struct os_cookie *pObj;
 468
 469         pObj = (struct os_cookie *)pAd->OS_Cookie;
 470         pci_read_config_word(pci_dev, PCI_DEVICE_ID, &device_id);
 471         device_id = le2cpu16(device_id);
 472         pObj->DeviceID = device_id;
 473         if (
 474 #ifdef RT2860
 475                    (device_id == NIC2860_PCIe_DEVICE_ID) ||
 476                    (device_id == NIC2790_PCIe_DEVICE_ID) ||
 477                    (device_id == VEN_AWT_PCIe_DEVICE_ID) ||
 478 #endif
 479 #ifdef RT3090
 480                    (device_id == NIC3090_PCIe_DEVICE_ID) ||
 481                    (device_id == NIC3091_PCIe_DEVICE_ID) ||
 482                    (device_id == NIC3092_PCIe_DEVICE_ID) ||
 483 #endif /* RT3090 // */
 484                    0) {
 485                 u32 MacCsr0 = 0, Index = 0;
 486                 do {
 487                         RTMP_IO_READ32(pAd, MAC_CSR0, &MacCsr0);
 488
 489                         if ((MacCsr0 != 0x00) && (MacCsr0 != 0xFFFFFFFF))
 490                                 break;
 491
 492                         RTMPusecDelay(10);
 493                 } while (Index++ < 100);
 494
 495                 /* Support advanced power save after 2892/2790. */
 496                 /* MAC version at offset 0x1000 is 0x2872XXXX/0x2870XXXX(PCIe, USB, SDIO). */
 497                 if ((MacCsr0 & 0xffff0000) != 0x28600000)
 498                         OPSTATUS_SET_FLAG(pAd, fOP_STATUS_PCIE_DEVICE);
 499         }
 500 }
 501
 502 void RTMPInitPCIeLinkCtrlValue(struct rt_rtmp_adapter *pAd)
 503 {
 504         int pos;
 505         u16 reg16, data2, PCIePowerSaveLevel, Configuration;
 506         u32 MacValue;
 507         BOOLEAN bFindIntel = FALSE;
 508         struct os_cookie *pObj;
 509
 510         pObj = (struct os_cookie *)pAd->OS_Cookie;
 511
 512         if (!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_PCIE_DEVICE))
 513                 return;
 514
 515         DBGPRINT(RT_DEBUG_TRACE, ("%s.===>\n", __func__));
 516         /* Init EEPROM, and save settings */
 517         if (!(IS_RT3090(pAd) || IS_RT3572(pAd) || IS_RT3390(pAd))) {
 518                 RT28xx_EEPROM_READ16(pAd, 0x22, PCIePowerSaveLevel);
 519                 pAd->PCIePowerSaveLevel = PCIePowerSaveLevel & 0xff;
 520
 521                 pAd->LnkCtrlBitMask = 0;
 522                 if ((PCIePowerSaveLevel & 0xff) == 0xff) {
 523                         OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_PCIE_DEVICE);
 524                         DBGPRINT(RT_DEBUG_TRACE,
 525                                  ("====> PCIePowerSaveLevel = 0x%x.\n",
 526                                   PCIePowerSaveLevel));
 527                         return;
 528                 } else {
 529                         PCIePowerSaveLevel &= 0x3;
 530                         RT28xx_EEPROM_READ16(pAd, 0x24, data2);
 531
 532                         if (!
 533                             (((data2 & 0xff00) == 0x9200)
 534                              && ((data2 & 0x80) != 0))) {
 535                                 if (PCIePowerSaveLevel > 1)
 536                                         PCIePowerSaveLevel = 1;
 537                         }
 538
 539                         DBGPRINT(RT_DEBUG_TRACE,
 540                                  ("====> Write 0x83 = 0x%x.\n",
 541                                   PCIePowerSaveLevel));
 542                         AsicSendCommandToMcu(pAd, 0x83, 0xff,
 543                                              (u8)PCIePowerSaveLevel, 0x00);
 544                         RT28xx_EEPROM_READ16(pAd, 0x22, PCIePowerSaveLevel);
 545                         PCIePowerSaveLevel &= 0xff;
 546                         PCIePowerSaveLevel = PCIePowerSaveLevel >> 6;
 547                         switch (PCIePowerSaveLevel) {
 548                         case 0: /* Only support L0 */
 549                                 pAd->LnkCtrlBitMask = 0;
 550                                 break;
 551                         case 1: /* Only enable L0s */
 552                                 pAd->LnkCtrlBitMask = 1;
 553                                 break;
 554                         case 2: /* enable L1, L0s */
 555                                 pAd->LnkCtrlBitMask = 3;
 556                                 break;
 557                         case 3: /* sync with host clk and enable L1, L0s */
 558                                 pAd->LnkCtrlBitMask = 0x103;
 559                                 break;
 560                         }
 561                         RT28xx_EEPROM_READ16(pAd, 0x24, data2);
 562                         if ((PCIePowerSaveLevel & 0xff) != 0xff) {
 563                                 PCIePowerSaveLevel &= 0x3;
 564
 565                                 if (!
 566                                     (((data2 & 0xff00) == 0x9200)
 567                                      && ((data2 & 0x80) != 0))) {
 568                                         if (PCIePowerSaveLevel > 1)
 569                                                 PCIePowerSaveLevel = 1;
 570                                 }
 571
 572                                 DBGPRINT(RT_DEBUG_TRACE,
 573                                          ("====> rt28xx Write 0x83 Command = 0x%x.\n",
 574                                           PCIePowerSaveLevel));
 575
 576                                 AsicSendCommandToMcu(pAd, 0x83, 0xff,
 577                                                      (u8)PCIePowerSaveLevel,
 578                                                      0x00);
 579                         }
 580                         DBGPRINT(RT_DEBUG_TRACE,
 581                                  ("====> LnkCtrlBitMask = 0x%x.\n",
 582                                   pAd->LnkCtrlBitMask));
 583                 }
 584         } else if (IS_RT3090(pAd) || IS_RT3572(pAd) || IS_RT3390(pAd)) {
 585                 u8 LinkCtrlSetting = 0;
 586
 587                 /* Check 3090E special setting chip. */
 588                 RT28xx_EEPROM_READ16(pAd, 0x24, data2);
 589                 if ((data2 == 0x9280) && ((pAd->MACVersion & 0xffff) == 0x0211)) {
 590                         pAd->b3090ESpecialChip = TRUE;
 591                         DBGPRINT_RAW(RT_DEBUG_ERROR, ("Special 3090E chip \n"));
 592                 }
 593
 594                 RTMP_IO_READ32(pAd, AUX_CTRL, &MacValue);
 595                 /*enable WAKE_PCIE function, which forces to enable PCIE clock when mpu interrupt asserting. */
 596                 /*Force PCIE 125MHz CLK to toggle */
 597                 MacValue |= 0x402;
 598                 RTMP_IO_WRITE32(pAd, AUX_CTRL, MacValue);
 599                 DBGPRINT_RAW(RT_DEBUG_ERROR,
 600                              (" AUX_CTRL = 0x%32x\n", MacValue));
 601
 602                 /* for RT30xx F and after, PCIe infterface, and for power solution 3 */
 603                 if ((IS_VERSION_AFTER_F(pAd))
 604                     && (pAd->StaCfg.PSControl.field.rt30xxPowerMode >= 2)
 605                     && (pAd->StaCfg.PSControl.field.rt30xxPowerMode <= 3)) {
 606                         RTMP_IO_READ32(pAd, AUX_CTRL, &MacValue);
 607                         DBGPRINT_RAW(RT_DEBUG_ERROR,
 608                                      (" Read AUX_CTRL = 0x%x\n", MacValue));
 609                         /* turn on bit 12. */
 610                         /*enable 32KHz clock mode for power saving */
 611                         MacValue |= 0x1000;
 612                         if (MacValue != 0xffffffff) {
 613                                 RTMP_IO_WRITE32(pAd, AUX_CTRL, MacValue);
 614                                 DBGPRINT_RAW(RT_DEBUG_ERROR,
 615                                              (" Write AUX_CTRL = 0x%x\n",
 616                                               MacValue));
 617                                 /* 1. if use PCIePowerSetting is 2 or 3, need to program OSC_CTRL to 0x3ff11. */
 618                                 MacValue = 0x3ff11;
 619                                 RTMP_IO_WRITE32(pAd, OSC_CTRL, MacValue);
 620                                 DBGPRINT_RAW(RT_DEBUG_ERROR,
 621                                              (" OSC_CTRL = 0x%x\n", MacValue));
 622                                 /* 2. Write PCI register Clk ref bit */
 623                                 RTMPrt3xSetPCIePowerLinkCtrl(pAd);
 624                         } else {
 625                                 /* Error read Aux_Ctrl value.  Force to use solution 1 */
 626                                 DBGPRINT(RT_DEBUG_ERROR,
 627                                          (" Error Value in AUX_CTRL = 0x%x\n",
 628                                           MacValue));
 629                                 pAd->StaCfg.PSControl.field.rt30xxPowerMode = 1;
 630                                 DBGPRINT(RT_DEBUG_ERROR,
 631                                          (" Force to use power solution1 \n"));
 632                         }
 633                 }
 634                 /* 1. read setting from inf file. */
 635
 636                 PCIePowerSaveLevel =
 637                     (u16)pAd->StaCfg.PSControl.field.rt30xxPowerMode;
 638                 DBGPRINT(RT_DEBUG_ERROR,
 639                          ("====> rt30xx Read PowerLevelMode =  0x%x.\n",
 640                           PCIePowerSaveLevel));
 641                 /* 2. Check EnableNewPS. */
 642                 if (pAd->StaCfg.PSControl.field.EnableNewPS == FALSE)
 643                         PCIePowerSaveLevel = 1;
 644
 645                 if (IS_VERSION_BEFORE_F(pAd)
 646                     && (pAd->b3090ESpecialChip == FALSE)) {
 647                         /* Chip Version E only allow 1, So force set 1. */
 648                         PCIePowerSaveLevel &= 0x1;
 649                         pAd->PCIePowerSaveLevel = (u16)PCIePowerSaveLevel;
 650                         DBGPRINT(RT_DEBUG_TRACE,
 651                                  ("====> rt30xx E Write 0x83 Command = 0x%x.\n",
 652                                   PCIePowerSaveLevel));
 653
 654                         AsicSendCommandToMcu(pAd, 0x83, 0xff,
 655                                              (u8)PCIePowerSaveLevel, 0x00);
 656                 } else {
 657                         /* Chip Version F and after only allow 1 or 2 or 3. This might be modified after new chip version come out. */
 658                         if (!
 659                             ((PCIePowerSaveLevel == 1)
 660                              || (PCIePowerSaveLevel == 3)))
 661                                 PCIePowerSaveLevel = 1;
 662                         DBGPRINT(RT_DEBUG_ERROR,
 663                                  ("====> rt30xx F Write 0x83 Command = 0x%x.\n",
 664                                   PCIePowerSaveLevel));
 665                         pAd->PCIePowerSaveLevel = (u16)PCIePowerSaveLevel;
 666                         /* for 3090F , we need to add high-byte arg for 0x83 command to indicate the link control setting in */
 667                         /* PCI Configuration Space. Because firmware can't read PCI Configuration Space */
 668                         if ((pAd->Rt3xxRalinkLinkCtrl & 0x2)
 669                             && (pAd->Rt3xxHostLinkCtrl & 0x2)) {
 670                                 LinkCtrlSetting = 1;
 671                         }
 672                         DBGPRINT(RT_DEBUG_TRACE,
 673                                  ("====> rt30xxF LinkCtrlSetting = 0x%x.\n",
 674                                   LinkCtrlSetting));
 675                         AsicSendCommandToMcu(pAd, 0x83, 0xff,
 676                                              (u8)PCIePowerSaveLevel,
 677                                              LinkCtrlSetting);
 678                 }
 679         }
 680         /* Find Ralink PCIe Device's Express Capability Offset */
 681         pos = pci_find_capability(pObj->pci_dev, PCI_CAP_ID_EXP);
 682
 683         if (pos != 0) {
 684                 /* Ralink PCIe Device's Link Control Register Offset */
 685                 pAd->RLnkCtrlOffset = pos + PCI_EXP_LNKCTL;
 686                 pci_read_config_word(pObj->pci_dev, pAd->RLnkCtrlOffset,
 687                                      &reg16);
 688                 Configuration = le2cpu16(reg16);
 689                 DBGPRINT(RT_DEBUG_TRACE,
 690                          ("Read (Ralink PCIe Link Control Register) offset 0x%x = 0x%x\n",
 691                           pAd->RLnkCtrlOffset, Configuration));
 692                 pAd->RLnkCtrlConfiguration = (Configuration & 0x103);
 693                 Configuration &= 0xfefc;
 694                 Configuration |= (0x0);
 695 #ifdef RT2860
 696                 if ((pObj->DeviceID == NIC2860_PCIe_DEVICE_ID)
 697                     || (pObj->DeviceID == NIC2790_PCIe_DEVICE_ID)) {
 698                         reg16 = cpu2le16(Configuration);
 699                         pci_write_config_word(pObj->pci_dev,
 700                                               pAd->RLnkCtrlOffset, reg16);
 701                         DBGPRINT(RT_DEBUG_TRACE,
 702                                  ("Write (Ralink PCIe Link Control Register)  offset 0x%x = 0x%x\n",
 703                                   pos + PCI_EXP_LNKCTL, Configuration));
 704                 }
 705 #endif /* RT2860 // */
 706
 707                 RTMPFindHostPCIDev(pAd);
 708                 if (pObj->parent_pci_dev) {
 709                         u16 vendor_id;
 710
 711                         pci_read_config_word(pObj->parent_pci_dev,
 712                                              PCI_VENDOR_ID, &vendor_id);
 713                         vendor_id = le2cpu16(vendor_id);
 714                         if (vendor_id == PCIBUS_INTEL_VENDOR) {
 715                                 bFindIntel = TRUE;
 716                                 RTMP_SET_PSFLAG(pAd, fRTMP_PS_TOGGLE_L1);
 717                         }
 718                         /* Find PCI-to-PCI Bridge Express Capability Offset */
 719                         pos =
 720                             pci_find_capability(pObj->parent_pci_dev,
 721                                                 PCI_CAP_ID_EXP);
 722
 723                         if (pos != 0) {
 724                                 BOOLEAN bChange = FALSE;
 725                                 /* PCI-to-PCI Bridge Link Control Register Offset */
 726                                 pAd->HostLnkCtrlOffset = pos + PCI_EXP_LNKCTL;
 727                                 pci_read_config_word(pObj->parent_pci_dev,
 728                                                      pAd->HostLnkCtrlOffset,
 729                                                      &reg16);
 730                                 Configuration = le2cpu16(reg16);
 731                                 DBGPRINT(RT_DEBUG_TRACE,
 732                                          ("Read (Host PCI-to-PCI Bridge Link Control Register) offset 0x%x = 0x%x\n",
 733                                           pAd->HostLnkCtrlOffset,
 734                                           Configuration));
 735                                 pAd->HostLnkCtrlConfiguration =
 736                                     (Configuration & 0x103);
 737                                 Configuration &= 0xfefc;
 738                                 Configuration |= (0x0);
 739
 740                                 switch (pObj->DeviceID) {
 741 #ifdef RT2860
 742                                 case NIC2860_PCIe_DEVICE_ID:
 743                                 case NIC2790_PCIe_DEVICE_ID:
 744                                         bChange = TRUE;
 745                                         break;
 746 #endif /* RT2860 // */
 747 #ifdef RT3090
 748                                 case NIC3090_PCIe_DEVICE_ID:
 749                                 case NIC3091_PCIe_DEVICE_ID:
 750                                 case NIC3092_PCIe_DEVICE_ID:
 751                                         if (bFindIntel == FALSE)
 752                                                 bChange = TRUE;
 753                                         break;
 754 #endif /* RT3090 // */
 755                                 default:
 756                                         break;
 757                                 }
 758
 759                                 if (bChange) {
 760                                         reg16 = cpu2le16(Configuration);
 761                                         pci_write_config_word(pObj->
 762                                                               parent_pci_dev,
 763                                                               pAd->
 764                                                               HostLnkCtrlOffset,
 765                                                               reg16);
 766                                         DBGPRINT(RT_DEBUG_TRACE,
 767                                                  ("Write (Host PCI-to-PCI Bridge Link Control Register) offset 0x%x = 0x%x\n",
 768                                                   pAd->HostLnkCtrlOffset,
 769                                                   Configuration));
 770                                 }
 771                         } else {
 772                                 pAd->HostLnkCtrlOffset = 0;
 773                                 DBGPRINT(RT_DEBUG_ERROR,
 774                                          ("%s: cannot find PCI-to-PCI Bridge PCI Express Capability!\n",
 775                                           __func__));
 776                         }
 777                 }
 778         } else {
 779                 pAd->RLnkCtrlOffset = 0;
 780                 pAd->HostLnkCtrlOffset = 0;
 781                 DBGPRINT(RT_DEBUG_ERROR,
 782                          ("%s: cannot find Ralink PCIe Device's PCI Express Capability!\n",
 783                           __func__));
 784         }
 785
 786         if (bFindIntel == FALSE) {
 787                 DBGPRINT(RT_DEBUG_TRACE,
 788                          ("Doesn't find Intel PCI host controller. \n"));
 789                 /* Doesn't switch L0, L1, So set PCIePowerSaveLevel to 0xff */
 790                 pAd->PCIePowerSaveLevel = 0xff;
 791                 if ((pAd->RLnkCtrlOffset != 0)
 792 #ifdef RT3090
 793                     && ((pObj->DeviceID == NIC3090_PCIe_DEVICE_ID)
 794                         || (pObj->DeviceID == NIC3091_PCIe_DEVICE_ID)
 795                         || (pObj->DeviceID == NIC3092_PCIe_DEVICE_ID))
 796 #endif /* RT3090 // */
 797                     ) {
 798                         pci_read_config_word(pObj->pci_dev, pAd->RLnkCtrlOffset,
 799                                              &reg16);
 800                         Configuration = le2cpu16(reg16);
 801                         DBGPRINT(RT_DEBUG_TRACE,
 802                                  ("Read (Ralink 30xx PCIe Link Control Register) offset 0x%x = 0x%x\n",
 803                                   pAd->RLnkCtrlOffset, Configuration));
 804                         pAd->RLnkCtrlConfiguration = (Configuration & 0x103);
 805                         Configuration &= 0xfefc;
 806                         Configuration |= (0x0);
 807                         reg16 = cpu2le16(Configuration);
 808                         pci_write_config_word(pObj->pci_dev,
 809                                               pAd->RLnkCtrlOffset, reg16);
 810                         DBGPRINT(RT_DEBUG_TRACE,
 811                                  ("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(struct rt_rtmp_adapter *pAd)
 818 {
 819         u16 reg16;
 820         u8 reg8;
 821         u32 DevFn;
 822         struct pci_dev *pPci_dev;
 823         struct os_cookie *pObj;
 824
 825         pObj = (struct os_cookie *)pAd->OS_Cookie;
 826
 827         if (!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_PCIE_DEVICE))
 828                 return;
 829
 830         DBGPRINT(RT_DEBUG_TRACE, ("%s.===>\n", __func__));
 831
 832         pObj->parent_pci_dev = NULL;
 833         if (pObj->pci_dev->bus->parent) {
 834                 for (DevFn = 0; DevFn < 255; DevFn++) {
 835                         pPci_dev =
 836                             pci_get_slot(pObj->pci_dev->bus->parent, DevFn);
 837                         if (pPci_dev) {
 838                                 pci_read_config_word(pPci_dev, PCI_CLASS_DEVICE,
 839                                                      &reg16);
 840                                 reg16 = le2cpu16(reg16);
 841                                 pci_read_config_byte(pPci_dev, PCI_CB_CARD_BUS,
 842                                                      &reg8);
 843                                 if ((reg16 == PCI_CLASS_BRIDGE_PCI)
 844                                     && (reg8 == pObj->pci_dev->bus->number)) {
 845                                         pObj->parent_pci_dev = pPci_dev;
 846                                 }
 847                         }
 848                 }
 849         }
 850 }
 851
 852 /*
 853         ========================================================================
 854
 855         Routine Description:
 856
 857         Arguments:
 858                 Level = RESTORE_HALT : Restore PCI host and Ralink PCIe Link Control field to its default value.
 859                 Level = Other Value : Restore from dot11 power save or radio off status. And force PCI host Link Control fields to 0x1
 860
 861         ========================================================================
 862 */
 863 void RTMPPCIeLinkCtrlValueRestore(struct rt_rtmp_adapter *pAd, u8 Level)
 864 {
 865         u16 PCIePowerSaveLevel, reg16;
 866         u16 Configuration;
 867         struct os_cookie *pObj;
 868
 869         pObj = (struct os_cookie *)pAd->OS_Cookie;
 870
 871         if (!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_PCIE_DEVICE))
 872                 return;
 873
 874 #ifdef RT2860
 875         if (!((pObj->DeviceID == NIC2860_PCIe_DEVICE_ID)
 876               || (pObj->DeviceID == NIC2790_PCIe_DEVICE_ID)))
 877                 return;
 878 #endif /* RT2860 // */
 879         /* Check PSControl Configuration */
 880         if (pAd->StaCfg.PSControl.field.EnableNewPS == FALSE)
 881                 return;
 882
 883         /*3090 will not execute the following codes. */
 884         /* Check interface : If not PCIe interface, return. */
 885
 886 #ifdef RT3090
 887         if ((pObj->DeviceID == NIC3090_PCIe_DEVICE_ID)
 888             || (pObj->DeviceID == NIC3091_PCIe_DEVICE_ID)
 889             || (pObj->DeviceID == NIC3092_PCIe_DEVICE_ID))
 890                 return;
 891 #endif /* RT3090 // */
 892
 893         DBGPRINT(RT_DEBUG_TRACE, ("%s.===>\n", __func__));
 894         PCIePowerSaveLevel = pAd->PCIePowerSaveLevel;
 895         if ((PCIePowerSaveLevel & 0xff) == 0xff) {
 896                 DBGPRINT(RT_DEBUG_TRACE, ("return  \n"));
 897                 return;
 898         }
 899
 900         if (pObj->parent_pci_dev && (pAd->HostLnkCtrlOffset != 0)) {
 901                 PCI_REG_READ_WORD(pObj->parent_pci_dev, pAd->HostLnkCtrlOffset,
 902                                   Configuration);
 903                 if ((Configuration != 0) && (Configuration != 0xFFFF)) {
 904                         Configuration &= 0xfefc;
 905                         /* If call from interface down, restore to orginial setting. */
 906                         if (Level == RESTORE_CLOSE)
 907                                 Configuration |= pAd->HostLnkCtrlConfiguration;
 908                         else
 909                                 Configuration |= 0x0;
 910                         PCI_REG_WIRTE_WORD(pObj->parent_pci_dev,
 911                                            pAd->HostLnkCtrlOffset,
 912                                            Configuration);
 913                         DBGPRINT(RT_DEBUG_TRACE,
 914                                  ("Restore PCI host : offset 0x%x = 0x%x\n",
 915                                   pAd->HostLnkCtrlOffset, Configuration));
 916                 } else
 917                         DBGPRINT(RT_DEBUG_ERROR,
 918                                  ("Restore PCI host : PCI_REG_READ_WORD failed (Configuration = 0x%x)\n",
 919                                   Configuration));
 920         }
 921
 922         if (pObj->pci_dev && (pAd->RLnkCtrlOffset != 0)) {
 923                 PCI_REG_READ_WORD(pObj->pci_dev, pAd->RLnkCtrlOffset,
 924                                   Configuration);
 925                 if ((Configuration != 0) && (Configuration != 0xFFFF)) {
 926                         Configuration &= 0xfefc;
 927                         /* If call from interface down, restore to orginial setting. */
 928                         if (Level == RESTORE_CLOSE)
 929                                 Configuration |= pAd->RLnkCtrlConfiguration;
 930                         else
 931                                 Configuration |= 0x0;
 932                         PCI_REG_WIRTE_WORD(pObj->pci_dev, pAd->RLnkCtrlOffset,
 933                                            Configuration);
 934                         DBGPRINT(RT_DEBUG_TRACE,
 935                                  ("Restore Ralink : offset 0x%x = 0x%x\n",
 936                                   pAd->RLnkCtrlOffset, Configuration));
 937                 } else
 938                         DBGPRINT(RT_DEBUG_ERROR,
 939                                  ("Restore Ralink : PCI_REG_READ_WORD failed (Configuration = 0x%x)\n",
 940                                   Configuration));
 941         }
 942
 943         DBGPRINT(RT_DEBUG_TRACE, ("%s <===\n", __func__));
 944 }
 945
 946 /*
 947         ========================================================================
 948
 949         Routine Description:
 950
 951         Arguments:
 952                 Max : limit Host PCI and Ralink PCIe device's LINK CONTROL field's value.
 953                 Because now frequently set our device to mode 1 or mode 3 will cause problem.
 954
 955         ========================================================================
 956 */
 957 void RTMPPCIeLinkCtrlSetting(struct rt_rtmp_adapter *pAd, u16 Max)
 958 {
 959         u16 PCIePowerSaveLevel, reg16;
 960         u16 Configuration;
 961         struct os_cookie *pObj;
 962
 963         pObj = (struct os_cookie *)pAd->OS_Cookie;
 964
 965         if (!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_PCIE_DEVICE))
 966                 return;
 967
 968 #ifdef RT2860
 969         if (!((pObj->DeviceID == NIC2860_PCIe_DEVICE_ID)
 970               || (pObj->DeviceID == NIC2790_PCIe_DEVICE_ID)))
 971                 return;
 972 #endif /* RT2860 // */
 973         /* Check PSControl Configuration */
 974         if (pAd->StaCfg.PSControl.field.EnableNewPS == FALSE)
 975                 return;
 976
 977         /* Check interface : If not PCIe interface, return. */
 978         /*Block 3090 to enter the following function */
 979
 980 #ifdef RT3090
 981         if ((pObj->DeviceID == NIC3090_PCIe_DEVICE_ID)
 982             || (pObj->DeviceID == NIC3091_PCIe_DEVICE_ID)
 983             || (pObj->DeviceID == NIC3092_PCIe_DEVICE_ID))
 984                 return;
 985 #endif /* RT3090 // */
 986         if (!RTMP_TEST_PSFLAG(pAd, fRTMP_PS_CAN_GO_SLEEP)) {
 987                 DBGPRINT(RT_DEBUG_INFO,
 988                          ("RTMPPCIePowerLinkCtrl return on fRTMP_PS_CAN_GO_SLEEP flag\n"));
 989                 return;
 990         }
 991
 992         DBGPRINT(RT_DEBUG_TRACE, ("%s===>\n", __func__));
 993         PCIePowerSaveLevel = pAd->PCIePowerSaveLevel;
 994         if ((PCIePowerSaveLevel & 0xff) == 0xff) {
 995                 DBGPRINT(RT_DEBUG_TRACE, ("return  \n"));
 996                 return;
 997         }
 998         PCIePowerSaveLevel = PCIePowerSaveLevel >> 6;
 999
1000         /* Skip non-exist deice right away */
1001         if (pObj->parent_pci_dev && (pAd->HostLnkCtrlOffset != 0)) {
1002                 PCI_REG_READ_WORD(pObj->parent_pci_dev, pAd->HostLnkCtrlOffset,
1003                                   Configuration);
1004                 switch (PCIePowerSaveLevel) {
1005                 case 0:
1006                         /* Set b0 and b1 of LinkControl (both 2892 and PCIe bridge) to 00 */
1007                         Configuration &= 0xfefc;
1008                         break;
1009                 case 1:
1010                         /* Set b0 and b1 of LinkControl (both 2892 and PCIe bridge) to 01 */
1011                         Configuration &= 0xfefc;
1012                         Configuration |= 0x1;
1013                         break;
1014                 case 2:
1015                         /*  Set b0 and b1 of LinkControl (both 2892 and PCIe bridge) to 11 */
1016                         Configuration &= 0xfefc;
1017                         Configuration |= 0x3;
1018                         break;
1019                 case 3:
1020                         /* Set b0 and b1 of LinkControl (both 2892 and PCIe bridge) to 11 and bit 8 of LinkControl of 2892 to 1 */
1021                         Configuration &= 0xfefc;
1022                         Configuration |= 0x103;
1023                         break;
1024                 }
1025                 PCI_REG_WIRTE_WORD(pObj->parent_pci_dev, pAd->HostLnkCtrlOffset,
1026                                    Configuration);
1027                 DBGPRINT(RT_DEBUG_TRACE,
1028                          ("Write PCI host offset 0x%x = 0x%x\n",
1029                           pAd->HostLnkCtrlOffset, Configuration));
1030         }
1031
1032         if (pObj->pci_dev && (pAd->RLnkCtrlOffset != 0)) {
1033                 /* first 2892 chip not allow to frequently set mode 3. will cause hang problem. */
1034                 if (PCIePowerSaveLevel > Max)
1035                         PCIePowerSaveLevel = Max;
1036
1037                 PCI_REG_READ_WORD(pObj->pci_dev, pAd->RLnkCtrlOffset,
1038                                   Configuration);
1039                 switch (PCIePowerSaveLevel) {
1040                 case 0:
1041                         /* No PCI power safe */
1042                         /* Set b0 and b1 of LinkControl (both 2892 and PCIe bridge) to 00 . */
1043                         Configuration &= 0xfefc;
1044                         break;
1045                 case 1:
1046                         /*  L0 */
1047                         /* Set b0 and b1 of LinkControl (both 2892 and PCIe bridge) to 01 . */
1048                         Configuration &= 0xfefc;
1049                         Configuration |= 0x1;
1050                         break;
1051                 case 2:
1052                         /* L0 and L1 */
1053                         /*  Set b0 and b1 of LinkControl (both 2892 and PCIe bridge) to 11 */
1054                         Configuration &= 0xfefc;
1055                         Configuration |= 0x3;
1056                         break;
1057                 case 3:
1058                         /* L0 , L1 and clock management. */
1059                         /* Set b0 and b1 of LinkControl (both 2892 and PCIe bridge) to 11 and bit 8 of LinkControl of 2892 to 1 */
1060                         Configuration &= 0xfefc;
1061                         Configuration |= 0x103;
1062                         pAd->bPCIclkOff = TRUE;
1063                         break;
1064                 }
1065                 PCI_REG_WIRTE_WORD(pObj->pci_dev, pAd->RLnkCtrlOffset,
1066                                    Configuration);
1067                 DBGPRINT(RT_DEBUG_TRACE,
1068                          ("Write Ralink device : offset 0x%x = 0x%x\n",
1069                           pAd->RLnkCtrlOffset, Configuration));
1070         }
1071
1072         DBGPRINT(RT_DEBUG_TRACE, ("RTMPPCIePowerLinkCtrl <==============\n"));
1073 }
1074
1075 /*
1076         ========================================================================
1077
1078         Routine Description:
1079                 1. Write a PCI register for rt30xx power solution 3
1080
1081         ========================================================================
1082 */
1083 void RTMPrt3xSetPCIePowerLinkCtrl(struct rt_rtmp_adapter *pAd)
1084 {
1085
1086         unsigned long HostConfiguration = 0;
1087         unsigned long Configuration;
1088         struct os_cookie *pObj;
1089         int pos;
1090         u16 reg16;
1091
1092         pObj = (struct os_cookie *)pAd->OS_Cookie;
1093
1094         DBGPRINT(RT_DEBUG_INFO,
1095                  ("RTMPrt3xSetPCIePowerLinkCtrl.===> %lx\n",
1096                   pAd->StaCfg.PSControl.word));
1097
1098         /* Check PSControl Configuration */
1099         if (pAd->StaCfg.PSControl.field.EnableNewPS == FALSE)
1100                 return;
1101         RTMPFindHostPCIDev(pAd);
1102         if (pObj->parent_pci_dev) {
1103                 /* Find PCI-to-PCI Bridge Express Capability Offset */
1104                 pos = pci_find_capability(pObj->parent_pci_dev, PCI_CAP_ID_EXP);
1105
1106                 if (pos != 0)
1107                         pAd->HostLnkCtrlOffset = pos + PCI_EXP_LNKCTL;
1108
1109                 /* If configurared to turn on L1. */
1110                 HostConfiguration = 0;
1111                 if (pAd->StaCfg.PSControl.field.rt30xxForceASPMTest == 1) {
1112                         DBGPRINT(RT_DEBUG_TRACE, ("Enter,PSM : Force ASPM\n"));
1113
1114                         /* Skip non-exist deice right away */
1115                         if ((pAd->HostLnkCtrlOffset != 0)) {
1116                                 PCI_REG_READ_WORD(pObj->parent_pci_dev,
1117                                                   pAd->HostLnkCtrlOffset,
1118                                                   HostConfiguration);
1119                                 /* Prepare Configuration to write to Host */
1120                                 HostConfiguration |= 0x3;
1121                                 PCI_REG_WIRTE_WORD(pObj->parent_pci_dev,
1122                                                    pAd->HostLnkCtrlOffset,
1123                                                    HostConfiguration);
1124                                 pAd->Rt3xxHostLinkCtrl = HostConfiguration;
1125                                 /* Because in rt30xxForceASPMTest Mode, Force turn on L0s, L1. */
1126                                 /* Fix HostConfiguration bit0:1 = 0x3 for later use. */
1127                                 HostConfiguration = 0x3;
1128                                 DBGPRINT(RT_DEBUG_TRACE,
1129                                          ("PSM : Force ASPM : "
1130                                           "Host device L1/L0s Value =  0x%lx\n",
1131                                           HostConfiguration));
1132                         }
1133                 } else if (pAd->StaCfg.PSControl.field.rt30xxFollowHostASPM ==
1134                            1) {
1135
1136                         /* Skip non-exist deice right away */
1137                         if ((pAd->HostLnkCtrlOffset != 0)) {
1138                                 PCI_REG_READ_WORD(pObj->parent_pci_dev,
1139                                                   pAd->HostLnkCtrlOffset,
1140                                                   HostConfiguration);
1141                                 pAd->Rt3xxHostLinkCtrl = HostConfiguration;
1142                                 HostConfiguration &= 0x3;
1143                                 DBGPRINT(RT_DEBUG_TRACE,
1144                                          ("PSM : Follow Host ASPM : "
1145                                           "Host device L1/L0s Value =  0x%lx\n",
1146                                           HostConfiguration));
1147                         }
1148                 }
1149         }
1150         /* Prepare to write Ralink setting. */
1151         /* Find Ralink PCIe Device's Express Capability Offset */
1152         pos = pci_find_capability(pObj->pci_dev, PCI_CAP_ID_EXP);
1153
1154         if (pos != 0) {
1155                 /* Ralink PCIe Device's Link Control Register Offset */
1156                 pAd->RLnkCtrlOffset = pos + PCI_EXP_LNKCTL;
1157                 pci_read_config_word(pObj->pci_dev, pAd->RLnkCtrlOffset,
1158                                      &reg16);
1159                 Configuration = le2cpu16(reg16);
1160                 DBGPRINT(RT_DEBUG_TRACE,
1161                          ("Read (Ralink PCIe Link Control Register) "
1162                           "offset 0x%x = 0x%lx\n",
1163                           pAd->RLnkCtrlOffset, Configuration));
1164                 Configuration |= 0x100;
1165                 if ((pAd->StaCfg.PSControl.field.rt30xxFollowHostASPM == 1)
1166                     || (pAd->StaCfg.PSControl.field.rt30xxForceASPMTest == 1)) {
1167                         switch (HostConfiguration) {
1168                         case 0:
1169                                 Configuration &= 0xffffffc;
1170                                 break;
1171                         case 1:
1172                                 Configuration &= 0xffffffc;
1173                                 Configuration |= 0x1;
1174                                 break;
1175                         case 2:
1176                                 Configuration &= 0xffffffc;
1177                                 Configuration |= 0x2;
1178                                 break;
1179                         case 3:
1180                                 Configuration |= 0x3;
1181                                 break;
1182                         }
1183                 }
1184                 reg16 = cpu2le16(Configuration);
1185                 pci_write_config_word(pObj->pci_dev, pAd->RLnkCtrlOffset,
1186                                       reg16);
1187                 pAd->Rt3xxRalinkLinkCtrl = Configuration;
1188                 DBGPRINT(RT_DEBUG_TRACE,
1189                          ("PSM :Write Ralink device L1/L0s Value =  0x%lx\n",
1190                           Configuration));
1191         }
1192         DBGPRINT(RT_DEBUG_INFO,
1193                  ("PSM :RTMPrt3xSetPCIePowerLinkCtrl <==============\n"));
1194 }