kernel-bcm43xx-patch_2.6.18.1_for_PCI-E.patch

   1 Index: linux-2.6.18/drivers/net/wireless/bcm43xx/bcm43xx_dma.h
   2 ===================================================================
   3 --- linux-2.6.18.orig/drivers/net/wireless/bcm43xx/bcm43xx_dma.h
   4 +++ linux-2.6.18/drivers/net/wireless/bcm43xx/bcm43xx_dma.h
   5 @@ -314,6 +314,23 @@ int bcm43xx_dma_tx(struct bcm43xx_privat
   6                    struct ieee80211_txb *txb);
   7  void bcm43xx_dma_rx(struct bcm43xx_dmaring *ring);
   8
   9 +/* Helper function that returns the dma mask for this device. */
  10 +static inline
  11 +u64 bcm43xx_get_supported_dma_mask(struct bcm43xx_private *bcm)
  12 +{
  13 +       int dma64 = bcm43xx_read32(bcm, BCM43xx_CIR_SBTMSTATEHIGH) &
  14 +                                  BCM43xx_SBTMSTATEHIGH_DMA64BIT;
  15 +       u16 mmio_base = bcm43xx_dmacontroller_base(dma64, 0);
  16 +       u32 mask = BCM43xx_DMA32_TXADDREXT_MASK;
  17 +
  18 +       if (dma64)
  19 +               return DMA_64BIT_MASK;
  20 +       bcm43xx_write32(bcm, mmio_base + BCM43xx_DMA32_TXCTL, mask);
  21 +       if (bcm43xx_read32(bcm, mmio_base + BCM43xx_DMA32_TXCTL) & mask)
  22 +               return DMA_32BIT_MASK;
  23 +       return DMA_30BIT_MASK;
  24 +}
  25 +
  26  #else /* CONFIG_BCM43XX_DMA */
  27
  28
  29 Index: linux-2.6.18/drivers/net/wireless/bcm43xx/bcm43xx_main.c
  30 ===================================================================
  31 --- linux-2.6.18.orig/drivers/net/wireless/bcm43xx/bcm43xx_main.c
  32 +++ linux-2.6.18/drivers/net/wireless/bcm43xx/bcm43xx_main.c
  33 @@ -130,6 +130,10 @@ MODULE_PARM_DESC(fwpostfix, "Postfix for
  34         { PCI_VENDOR_ID_BROADCOM, 0x4301, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0 },
  35         /* Broadcom 4307 802.11b */
  36         { PCI_VENDOR_ID_BROADCOM, 0x4307, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0 },
  37 +       /* Broadcom 4311 802.11(a)/b/g */
  38 +       { PCI_VENDOR_ID_BROADCOM, 0x4311, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0 },
  39 +       /* Broadcom 4312 802.11a/b/g */
  40 +       { PCI_VENDOR_ID_BROADCOM, 0x4312, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0 },
  41         /* Broadcom 4318 802.11b/g */
  42         { PCI_VENDOR_ID_BROADCOM, 0x4318, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0 },
  43         /* Broadcom 4319 802.11a/b/g */
  44 @@ -2582,8 +2586,9 @@ static int bcm43xx_probe_cores(struct bc
  45         /* fetch sb_id_hi from core information registers */
  46         sb_id_hi = bcm43xx_read32(bcm, BCM43xx_CIR_SB_ID_HI);
  47
  48 -       core_id = (sb_id_hi & 0xFFF0) >> 4;
  49 -       core_rev = (sb_id_hi & 0xF);
  50 +       core_id = (sb_id_hi & 0x8FF0) >> 4;
  51 +       core_rev = (sb_id_hi & 0x7000) >> 8;
  52 +       core_rev |= (sb_id_hi & 0xF);
  53         core_vendor = (sb_id_hi & 0xFFFF0000) >> 16;
  54
  55         /* if present, chipcommon is always core 0; read the chipid from it */
  56 @@ -2693,6 +2698,7 @@ static int bcm43xx_probe_cores(struct bc
  57                 core = NULL;
  58                 switch (core_id) {
  59                 case BCM43xx_COREID_PCI:
  60 +               case BCM43xx_COREID_PCIE:
  61                         core = &bcm->core_pci;
  62                         if (core->available) {
  63                                 printk(KERN_WARNING PFX "Multiple PCI cores found.\n");
  64 @@ -2731,12 +2737,12 @@ static int bcm43xx_probe_cores(struct bc
  65                         case 6:
  66                         case 7:
  67                         case 9:
  68 +                       case 10:
  69                                 break;
  70                         default:
  71 -                               printk(KERN_ERR PFX "Error: Unsupported 80211 core revision %u\n",
  72 +                               printk(KERN_WARNING PFX
  73 +                                      "Unsupported 80211 core revision %u\n",
  74                                        core_rev);
  75 -                               err = -ENODEV;
  76 -                               goto out;
  77                         }
  78                         bcm->nr_80211_available++;
  79                         core->priv = ext_80211;
  80 @@ -2850,16 +2856,11 @@ static int bcm43xx_wireless_core_init(st
  81         u32 sbimconfiglow;
  82         u8 limit;
  83
  84 -       if (bcm->chip_rev < 5) {
  85 +       if (bcm->core_pci.rev <= 5 && bcm->core_pci.id != BCM43xx_COREID_PCIE) {
  86                 sbimconfiglow = bcm43xx_read32(bcm, BCM43xx_CIR_SBIMCONFIGLOW);
  87                 sbimconfiglow &= ~ BCM43xx_SBIMCONFIGLOW_REQUEST_TOUT_MASK;
  88                 sbimconfiglow &= ~ BCM43xx_SBIMCONFIGLOW_SERVICE_TOUT_MASK;
  89 -               if (bcm->bustype == BCM43xx_BUSTYPE_PCI)
  90 -                       sbimconfiglow |= 0x32;
  91 -               else if (bcm->bustype == BCM43xx_BUSTYPE_SB)
  92 -                       sbimconfiglow |= 0x53;
  93 -               else
  94 -                       assert(0);
  95 +               sbimconfiglow |= 0x32;
  96                 bcm43xx_write32(bcm, BCM43xx_CIR_SBIMCONFIGLOW, sbimconfiglow);
  97         }
  98
  99 @@ -2924,10 +2925,13 @@ static int bcm43xx_wireless_core_init(st
 100                 bcm43xx_write16(bcm, 0x043C, 0x000C);
 101
 102         if (active_wlcore) {
 103 -               if (bcm43xx_using_pio(bcm))
 104 +               if (bcm43xx_using_pio(bcm)) {
 105                         err = bcm43xx_pio_init(bcm);
 106 -               else
 107 +               } else {
 108                         err = bcm43xx_dma_init(bcm);
 109 +                       if (err == -ENOSYS)
 110 +                               err = bcm43xx_pio_init(bcm);
 111 +               }
 112                 if (err)
 113                         goto err_chip_cleanup;
 114         }
 115 @@ -2983,22 +2987,64 @@ static void bcm43xx_pcicore_broadcast_va
 116
 117  static int bcm43xx_pcicore_commit_settings(struct bcm43xx_private *bcm)
 118  {
 119 -       int err;
 120 -       struct bcm43xx_coreinfo *old_core;
 121 +       int err = 0;
 122
 123 -       old_core = bcm->current_core;
 124 -       err = bcm43xx_switch_core(bcm, &bcm->core_pci);
 125 -       if (err)
 126 -               goto out;
 127 +       bcm->irq_savedstate = bcm43xx_interrupt_disable(bcm, BCM43xx_IRQ_ALL);
 128
 129 -       bcm43xx_pcicore_broadcast_value(bcm, 0xfd8, 0x00000000);
 130 +       if (bcm->core_chipcommon.available) {
 131 +               err = bcm43xx_switch_core(bcm, &bcm->core_chipcommon);
 132 +               if (err)
 133 +                       goto out;
 134 +
 135 +               bcm43xx_pcicore_broadcast_value(bcm, 0xfd8, 0x00000000);
 136 +
 137 +               /* this function is always called when a PCI core is mapped */
 138 +               err = bcm43xx_switch_core(bcm, &bcm->core_pci);
 139 +               if (err)
 140 +                       goto out;
 141 +       } else
 142 +               bcm43xx_pcicore_broadcast_value(bcm, 0xfd8, 0x00000000);
 143 +
 144 +       bcm43xx_interrupt_enable(bcm, bcm->irq_savedstate);
 145
 146 -       bcm43xx_switch_core(bcm, old_core);
 147 -       assert(err == 0);
 148  out:
 149         return err;
 150  }
 151
 152 +static u32 bcm43xx_pcie_reg_read(struct bcm43xx_private *bcm, u32 address)
 153 +{
 154 +       bcm43xx_write32(bcm, BCM43xx_PCIECORE_REG_ADDR, address);
 155 +       return bcm43xx_read32(bcm, BCM43xx_PCIECORE_REG_DATA);
 156 +}
 157 +
 158 +static void bcm43xx_pcie_reg_write(struct bcm43xx_private *bcm, u32 address,
 159 +                                   u32 data)
 160 +{
 161 +       bcm43xx_write32(bcm, BCM43xx_PCIECORE_REG_ADDR, address);
 162 +       bcm43xx_write32(bcm, BCM43xx_PCIECORE_REG_DATA, data);
 163 +}
 164 +
 165 +static void bcm43xx_pcie_mdio_write(struct bcm43xx_private *bcm, u8 dev, u8 reg,
 166 +                                   u16 data)
 167 +{
 168 +       int i;
 169 +
 170 +       bcm43xx_write32(bcm, BCM43xx_PCIECORE_MDIO_CTL, 0x0082);
 171 +       bcm43xx_write32(bcm, BCM43xx_PCIECORE_MDIO_DATA, BCM43xx_PCIE_MDIO_ST |
 172 +                       BCM43xx_PCIE_MDIO_WT | (dev << BCM43xx_PCIE_MDIO_DEV) |
 173 +                       (reg << BCM43xx_PCIE_MDIO_REG) | BCM43xx_PCIE_MDIO_TA |
 174 +                       data);
 175 +       udelay(10);
 176 +
 177 +       for (i = 0; i < 10; i++) {
 178 +               if (bcm43xx_read32(bcm, BCM43xx_PCIECORE_MDIO_CTL) &
 179 +                   BCM43xx_PCIE_MDIO_TC)
 180 +                       break;
 181 +               msleep(1);
 182 +       }
 183 +       bcm43xx_write32(bcm, BCM43xx_PCIECORE_MDIO_CTL, 0);
 184 +}
 185 +
 186  /* Make an I/O Core usable. "core_mask" is the bitmask of the cores to enable.
 187   * To enable core 0, pass a core_mask of 1<<0
 188   */
 189 @@ -3018,7 +3064,8 @@ static int bcm43xx_setup_backplane_pci_c
 190         if (err)
 191                 goto out;
 192
 193 -       if (bcm->core_pci.rev < 6) {
 194 +       if (bcm->current_core->rev < 6 ||
 195 +               bcm->current_core->id == BCM43xx_COREID_PCI) {
 196                 value = bcm43xx_read32(bcm, BCM43xx_CIR_SBINTVEC);
 197                 value |= (1 << backplane_flag_nr);
 198                 bcm43xx_write32(bcm, BCM43xx_CIR_SBINTVEC, value);
 199 @@ -3036,21 +3083,46 @@ static int bcm43xx_setup_backplane_pci_c
 200                 }
 201         }
 202
 203 -       value = bcm43xx_read32(bcm, BCM43xx_PCICORE_SBTOPCI2);
 204 -       value |= BCM43xx_SBTOPCI2_PREFETCH | BCM43xx_SBTOPCI2_BURST;
 205 -       bcm43xx_write32(bcm, BCM43xx_PCICORE_SBTOPCI2, value);
 206 -
 207 -       if (bcm->core_pci.rev < 5) {
 208 -               value = bcm43xx_read32(bcm, BCM43xx_CIR_SBIMCONFIGLOW);
 209 -               value |= (2 << BCM43xx_SBIMCONFIGLOW_SERVICE_TOUT_SHIFT)
 210 -                        & BCM43xx_SBIMCONFIGLOW_SERVICE_TOUT_MASK;
 211 -               value |= (3 << BCM43xx_SBIMCONFIGLOW_REQUEST_TOUT_SHIFT)
 212 -                        & BCM43xx_SBIMCONFIGLOW_REQUEST_TOUT_MASK;
 213 -               bcm43xx_write32(bcm, BCM43xx_CIR_SBIMCONFIGLOW, value);
 214 -               err = bcm43xx_pcicore_commit_settings(bcm);
 215 -               assert(err == 0);
 216 +       if (bcm->current_core->id == BCM43xx_COREID_PCI) {
 217 +               value = bcm43xx_read32(bcm, BCM43xx_PCICORE_SBTOPCI2);
 218 +               value |= BCM43xx_SBTOPCI2_PREFETCH | BCM43xx_SBTOPCI2_BURST;
 219 +               bcm43xx_write32(bcm, BCM43xx_PCICORE_SBTOPCI2, value);
 220 +
 221 +               if (bcm->current_core->rev < 5) {
 222 +                       value = bcm43xx_read32(bcm, BCM43xx_CIR_SBIMCONFIGLOW);
 223 +                       value |= (2 << BCM43xx_SBIMCONFIGLOW_SERVICE_TOUT_SHIFT)
 224 +                                & BCM43xx_SBIMCONFIGLOW_SERVICE_TOUT_MASK;
 225 +                       value |= (3 << BCM43xx_SBIMCONFIGLOW_REQUEST_TOUT_SHIFT)
 226 +                                & BCM43xx_SBIMCONFIGLOW_REQUEST_TOUT_MASK;
 227 +                       bcm43xx_write32(bcm, BCM43xx_CIR_SBIMCONFIGLOW, value);
 228 +                       err = bcm43xx_pcicore_commit_settings(bcm);
 229 +                       assert(err == 0);
 230 +               } else if (bcm->current_core->rev >= 11) {
 231 +                       value = bcm43xx_read32(bcm, BCM43xx_PCICORE_SBTOPCI2);
 232 +                       value |= BCM43xx_SBTOPCI2_MEMREAD_MULTI;
 233 +                       bcm43xx_write32(bcm, BCM43xx_PCICORE_SBTOPCI2, value);
 234 +               }
 235 +       } else {
 236 +               if (bcm->current_core->rev == 0 || bcm->current_core->rev == 1) {
 237 +                       value = bcm43xx_pcie_reg_read(bcm, BCM43xx_PCIE_TLP_WORKAROUND);
 238 +                       value |= 0x8;
 239 +                       bcm43xx_pcie_reg_write(bcm, BCM43xx_PCIE_TLP_WORKAROUND,
 240 +                                              value);
 241 +               }
 242 +               if (bcm->current_core->rev == 0) {
 243 +                       bcm43xx_pcie_mdio_write(bcm, BCM43xx_MDIO_SERDES_RX,
 244 +                                               BCM43xx_SERDES_RXTIMER, 0x8128);
 245 +                       bcm43xx_pcie_mdio_write(bcm, BCM43xx_MDIO_SERDES_RX,
 246 +                                               BCM43xx_SERDES_CDR, 0x0100);
 247 +                       bcm43xx_pcie_mdio_write(bcm, BCM43xx_MDIO_SERDES_RX,
 248 +                                               BCM43xx_SERDES_CDR_BW, 0x1466);
 249 +               } else if (bcm->current_core->rev == 1) {
 250 +                       value = bcm43xx_pcie_reg_read(bcm, BCM43xx_PCIE_DLLP_LINKCTL);
 251 +                       value |= 0x40;
 252 +                       bcm43xx_pcie_reg_write(bcm, BCM43xx_PCIE_DLLP_LINKCTL,
 253 +                                              value);
 254 +               }
 255         }
 256 -
 257  out_switch_back:
 258         err = bcm43xx_switch_core(bcm, old_core);
 259  out:
 260 @@ -3635,7 +3707,7 @@ static int bcm43xx_read_phyinfo(struct b
 261                 bcm->ieee->freq_band = IEEE80211_24GHZ_BAND;
 262                 break;
 263         case BCM43xx_PHYTYPE_G:
 264 -               if (phy_rev > 7)
 265 +               if (phy_rev > 8)
 266                         phy_rev_ok = 0;
 267                 bcm->ieee->modulation = IEEE80211_OFDM_MODULATION |
 268                                         IEEE80211_CCK_MODULATION;
 269 @@ -3986,8 +4058,6 @@ static int bcm43xx_init_private(struct b
 270                                 struct net_device *net_dev,
 271                                 struct pci_dev *pci_dev)
 272  {
 273 -       int err;
 274 -
 275         bcm43xx_set_status(bcm, BCM43xx_STAT_UNINIT);
 276         bcm->ieee = netdev_priv(net_dev);
 277         bcm->softmac = ieee80211_priv(net_dev);
 278 @@ -4005,22 +4075,8 @@ static int bcm43xx_init_private(struct b
 279                      (void (*)(unsigned long))bcm43xx_interrupt_tasklet,
 280                      (unsigned long)bcm);
 281         tasklet_disable_nosync(&bcm->isr_tasklet);
 282 -       if (modparam_pio) {
 283 +       if (modparam_pio)
 284                 bcm->__using_pio = 1;
 285 -       } else {
 286 -               err = pci_set_dma_mask(pci_dev, DMA_30BIT_MASK);
 287 -               err |= pci_set_consistent_dma_mask(pci_dev, DMA_30BIT_MASK);
 288 -               if (err) {
 289 -#ifdef CONFIG_BCM43XX_PIO
 290 -                       printk(KERN_WARNING PFX "DMA not supported. Falling back to PIO.\n");
 291 -                       bcm->__using_pio = 1;
 292 -#else
 293 -                       printk(KERN_ERR PFX "FATAL: DMA not supported and PIO not configured. "
 294 -                                           "Recompile the driver with PIO support, please.\n");
 295 -                       return -ENODEV;
 296 -#endif /* CONFIG_BCM43XX_PIO */
 297 -               }
 298 -       }
 299         bcm->rts_threshold = BCM43xx_DEFAULT_RTS_THRESHOLD;
 300
 301         /* default to sw encryption for now */
 302 Index: linux-2.6.18/drivers/net/wireless/bcm43xx/bcm43xx_dma.c
 303 ===================================================================
 304 --- linux-2.6.18.orig/drivers/net/wireless/bcm43xx/bcm43xx_dma.c
 305 +++ linux-2.6.18/drivers/net/wireless/bcm43xx/bcm43xx_dma.c
 306 @@ -705,11 +705,30 @@ int bcm43xx_dma_init(struct bcm43xx_priv
 307         struct bcm43xx_dmaring *ring;
 308         int err = -ENOMEM;
 309         int dma64 = 0;
 310 -       u32 sbtmstatehi;
 311 +       u64 mask = bcm43xx_get_supported_dma_mask(bcm);
 312 +       int nobits;
 313
 314 -       sbtmstatehi = bcm43xx_read32(bcm, BCM43xx_CIR_SBTMSTATEHIGH);
 315 -       if (sbtmstatehi & BCM43xx_SBTMSTATEHIGH_DMA64BIT)
 316 +       if (mask == DMA_64BIT_MASK) {
 317                 dma64 = 1;
 318 +               nobits = 64;
 319 +       } else if (mask == DMA_32BIT_MASK)
 320 +               nobits = 32;
 321 +       else
 322 +               nobits = 30;
 323 +       err = pci_set_dma_mask(bcm->pci_dev, mask);
 324 +       err |= pci_set_consistent_dma_mask(bcm->pci_dev, mask);
 325 +       if (err) {
 326 +#ifdef CONFIG_BCM43XX_PIO
 327 +               printk(KERN_WARNING PFX "DMA not supported on this device."
 328 +                                       " Falling back to PIO.\n");
 329 +               bcm->__using_pio = 1;
 330 +               return -ENOSYS;
 331 +#else
 332 +               printk(KERN_ERR PFX "FATAL: DMA not supported and PIO not configured. "
 333 +                                   "Please recompile the driver with PIO support.\n");
 334 +               return -ENODEV;
 335 +#endif /* CONFIG_BCM43XX_PIO */
 336 +       }
 337
 338         /* setup TX DMA channels. */
 339         ring = bcm43xx_setup_dmaring(bcm, 0, 1, dma64);
 340 @@ -755,8 +774,7 @@ int bcm43xx_dma_init(struct bcm43xx_priv
 341                 dma->rx_ring3 = ring;
 342         }
 343
 344 -       dprintk(KERN_INFO PFX "%s DMA initialized\n",
 345 -                       dma64 ? "64-bit" : "32-bit");
 346 +       dprintk(KERN_INFO PFX "%d-bit DMA initialized\n", nobits);
 347         err = 0;
 348  out:
 349         return err;
 350 Index: linux-2.6.18/drivers/net/wireless/bcm43xx/bcm43xx.h
 351 ===================================================================
 352 --- linux-2.6.18.orig/drivers/net/wireless/bcm43xx/bcm43xx.h
 353 +++ linux-2.6.18/drivers/net/wireless/bcm43xx/bcm43xx.h
 354 @@ -159,6 +159,7 @@
 355
 356  /* Chipcommon registers. */
 357  #define BCM43xx_CHIPCOMMON_CAPABILITIES        0x04
 358 +#define BCM43xx_CHIPCOMMON_CTL                 0x28
 359  #define BCM43xx_CHIPCOMMON_PLLONDELAY          0xB0
 360  #define BCM43xx_CHIPCOMMON_FREFSELDELAY                0xB4
 361  #define BCM43xx_CHIPCOMMON_SLOWCLKCTL          0xB8
 362 @@ -172,6 +173,33 @@
 363  /* SBTOPCI2 values. */
 364  #define BCM43xx_SBTOPCI2_PREFETCH      0x4
 365  #define BCM43xx_SBTOPCI2_BURST         0x8
 366 +#define BCM43xx_SBTOPCI2_MEMREAD_MULTI 0x20
 367 +
 368 +/* PCI-E core registers. */
 369 +#define BCM43xx_PCIECORE_REG_ADDR      0x0130
 370 +#define BCM43xx_PCIECORE_REG_DATA      0x0134
 371 +#define BCM43xx_PCIECORE_MDIO_CTL      0x0128
 372 +#define BCM43xx_PCIECORE_MDIO_DATA     0x012C
 373 +
 374 +/* PCI-E registers. */
 375 +#define BCM43xx_PCIE_TLP_WORKAROUND    0x0004
 376 +#define BCM43xx_PCIE_DLLP_LINKCTL      0x0100
 377 +
 378 +/* PCI-E MDIO bits. */
 379 +#define BCM43xx_PCIE_MDIO_ST   0x40000000
 380 +#define BCM43xx_PCIE_MDIO_WT   0x10000000
 381 +#define BCM43xx_PCIE_MDIO_DEV  22
 382 +#define BCM43xx_PCIE_MDIO_REG  18
 383 +#define BCM43xx_PCIE_MDIO_TA   0x00020000
 384 +#define BCM43xx_PCIE_MDIO_TC   0x0100
 385 +
 386 +/* MDIO devices. */
 387 +#define BCM43xx_MDIO_SERDES_RX 0x1F
 388 +
 389 +/* SERDES RX registers. */
 390 +#define BCM43xx_SERDES_RXTIMER 0x2
 391 +#define BCM43xx_SERDES_CDR     0x6
 392 +#define BCM43xx_SERDES_CDR_BW  0x7
 393
 394  /* Chipcommon capabilities. */
 395  #define BCM43xx_CAPABILITIES_PCTL              0x00040000
 396 @@ -221,6 +249,7 @@
 397  #define BCM43xx_COREID_USB20_HOST       0x819
 398  #define BCM43xx_COREID_USB20_DEV        0x81a
 399  #define BCM43xx_COREID_SDIO_HOST        0x81b
 400 +#define BCM43xx_COREID_PCIE            0x820
 401
 402  /* Core Information Registers */
 403  #define BCM43xx_CIR_BASE               0xf00
 404 Index: linux-2.6.18/drivers/net/wireless/bcm43xx/bcm43xx_power.c
 405 ===================================================================
 406 --- linux-2.6.18.orig/drivers/net/wireless/bcm43xx/bcm43xx_power.c
 407 +++ linux-2.6.18/drivers/net/wireless/bcm43xx/bcm43xx_power.c
 408 @@ -153,8 +153,6 @@ int bcm43xx_pctl_init(struct bcm43xx_pri
 409         int err, maxfreq;
 410         struct bcm43xx_coreinfo *old_core;
 411
 412 -       if (!(bcm->chipcommon_capabilities & BCM43xx_CAPABILITIES_PCTL))
 413 -               return 0;
 414         old_core = bcm->current_core;
 415         err = bcm43xx_switch_core(bcm, &bcm->core_chipcommon);
 416         if (err == -ENODEV)
 417 @@ -162,11 +160,27 @@ int bcm43xx_pctl_init(struct bcm43xx_pri
 418         if (err)
 419                 goto out;
 420
 421 -       maxfreq = bcm43xx_pctl_clockfreqlimit(bcm, 1);
 422 -       bcm43xx_write32(bcm, BCM43xx_CHIPCOMMON_PLLONDELAY,
 423 -                       (maxfreq * 150 + 999999) / 1000000);
 424 -       bcm43xx_write32(bcm, BCM43xx_CHIPCOMMON_FREFSELDELAY,
 425 -                       (maxfreq * 15 + 999999) / 1000000);
 426 +       if (bcm->chip_id == 0x4321) {
 427 +               if (bcm->chip_rev == 0)
 428 +                       bcm43xx_write32(bcm, BCM43xx_CHIPCOMMON_CTL, 0x03A4);
 429 +               if (bcm->chip_rev == 1)
 430 +                       bcm43xx_write32(bcm, BCM43xx_CHIPCOMMON_CTL, 0x00A4);
 431 +       }
 432 +
 433 +       if (bcm->chipcommon_capabilities & BCM43xx_CAPABILITIES_PCTL) {
 434 +               if (bcm->current_core->rev >= 10) {
 435 +                       /* Set Idle Power clock rate to 1Mhz */
 436 +                       bcm43xx_write32(bcm, BCM43xx_CHIPCOMMON_SYSCLKCTL,
 437 +                                      (bcm43xx_read32(bcm, BCM43xx_CHIPCOMMON_SYSCLKCTL)
 438 +                                      & 0x0000FFFF) | 0x40000);
 439 +               } else {
 440 +                       maxfreq = bcm43xx_pctl_clockfreqlimit(bcm, 1);
 441 +                       bcm43xx_write32(bcm, BCM43xx_CHIPCOMMON_PLLONDELAY,
 442 +                                      (maxfreq * 150 + 999999) / 1000000);
 443 +                       bcm43xx_write32(bcm, BCM43xx_CHIPCOMMON_FREFSELDELAY,
 444 +                                      (maxfreq * 15 + 999999) / 1000000);
 445 +               }
 446 +       }
 447
 448         err = bcm43xx_switch_core(bcm, old_core);
 449         assert(err == 0);