drivers/net/wireless/b43/main.c

   1 /*
   2
   3   Broadcom B43 wireless driver
   4
   5   Copyright (c) 2005 Martin Langer <martin-langer@gmx.de>
   6   Copyright (c) 2005 Stefano Brivio <stefano.brivio@polimi.it>
   7   Copyright (c) 2005, 2006 Michael Buesch <mb@bu3sch.de>
   8   Copyright (c) 2005 Danny van Dyk <kugelfang@gentoo.org>
   9   Copyright (c) 2005 Andreas Jaggi <andreas.jaggi@waterwave.ch>
  10
  11   Some parts of the code in this file are derived from the ipw2200
  12   driver  Copyright(c) 2003 - 2004 Intel Corporation.
  13
  14   This program is free software; you can redistribute it and/or modify
  15   it under the terms of the GNU General Public License as published by
  16   the Free Software Foundation; either version 2 of the License, or
  17   (at your option) any later version.
  18
  19   This program is distributed in the hope that it will be useful,
  20   but WITHOUT ANY WARRANTY; without even the implied warranty of
  21   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  22   GNU General Public License for more details.
  23
  24   You should have received a copy of the GNU General Public License
  25   along with this program; see the file COPYING.  If not, write to
  26   the Free Software Foundation, Inc., 51 Franklin Steet, Fifth Floor,
  27   Boston, MA 02110-1301, USA.
  28
  29 */
  30
  31 #include <linux/delay.h>
  32 #include <linux/init.h>
  33 #include <linux/moduleparam.h>
  34 #include <linux/if_arp.h>
  35 #include <linux/etherdevice.h>
  36 #include <linux/firmware.h>
  37 #include <linux/wireless.h>
  38 #include <linux/workqueue.h>
  39 #include <linux/skbuff.h>
  40 #include <linux/io.h>
  41 #include <linux/dma-mapping.h>
  42 #include <asm/unaligned.h>
  43
  44 #include "b43.h"
  45 #include "main.h"
  46 #include "debugfs.h"
  47 #include "phy_common.h"
  48 #include "phy_g.h"
  49 #include "phy_n.h"
  50 #include "dma.h"
  51 #include "pio.h"
  52 #include "sysfs.h"
  53 #include "xmit.h"
  54 #include "lo.h"
  55 #include "pcmcia.h"
  56
  57 MODULE_DESCRIPTION("Broadcom B43 wireless driver");
  58 MODULE_AUTHOR("Martin Langer");
  59 MODULE_AUTHOR("Stefano Brivio");
  60 MODULE_AUTHOR("Michael Buesch");
  61 MODULE_LICENSE("GPL");
  62
  63 MODULE_FIRMWARE(B43_SUPPORTED_FIRMWARE_ID);
  64
  65
  66 static int modparam_bad_frames_preempt;
  67 module_param_named(bad_frames_preempt, modparam_bad_frames_preempt, int, 0444);
  68 MODULE_PARM_DESC(bad_frames_preempt,
  69                  "enable(1) / disable(0) Bad Frames Preemption");
  70
  71 static char modparam_fwpostfix[16];
  72 module_param_string(fwpostfix, modparam_fwpostfix, 16, 0444);
  73 MODULE_PARM_DESC(fwpostfix, "Postfix for the .fw files to load.");
  74
  75 static int modparam_hwpctl;
  76 module_param_named(hwpctl, modparam_hwpctl, int, 0444);
  77 MODULE_PARM_DESC(hwpctl, "Enable hardware-side power control (default off)");
  78
  79 static int modparam_nohwcrypt;
  80 module_param_named(nohwcrypt, modparam_nohwcrypt, int, 0444);
  81 MODULE_PARM_DESC(nohwcrypt, "Disable hardware encryption.");
  82
  83 int b43_modparam_qos = 1;
  84 module_param_named(qos, b43_modparam_qos, int, 0444);
  85 MODULE_PARM_DESC(qos, "Enable QOS support (default on)");
  86
  87 static int modparam_btcoex = 1;
  88 module_param_named(btcoex, modparam_btcoex, int, 0444);
  89 MODULE_PARM_DESC(btcoex, "Enable Bluetooth coexistance (default on)");
  90
  91
  92 static const struct ssb_device_id b43_ssb_tbl[] = {
  93         SSB_DEVICE(SSB_VENDOR_BROADCOM, SSB_DEV_80211, 5),
  94         SSB_DEVICE(SSB_VENDOR_BROADCOM, SSB_DEV_80211, 6),
  95         SSB_DEVICE(SSB_VENDOR_BROADCOM, SSB_DEV_80211, 7),
  96         SSB_DEVICE(SSB_VENDOR_BROADCOM, SSB_DEV_80211, 9),
  97         SSB_DEVICE(SSB_VENDOR_BROADCOM, SSB_DEV_80211, 10),
  98         SSB_DEVICE(SSB_VENDOR_BROADCOM, SSB_DEV_80211, 11),
  99         SSB_DEVICE(SSB_VENDOR_BROADCOM, SSB_DEV_80211, 13),
 100         SSB_DEVICE(SSB_VENDOR_BROADCOM, SSB_DEV_80211, 15),
 101         SSB_DEVICE(SSB_VENDOR_BROADCOM, SSB_DEV_80211, 16),
 102         SSB_DEVTABLE_END
 103 };
 104
 105 MODULE_DEVICE_TABLE(ssb, b43_ssb_tbl);
 106
 107 /* Channel and ratetables are shared for all devices.
 108  * They can't be const, because ieee80211 puts some precalculated
 109  * data in there. This data is the same for all devices, so we don't
 110  * get concurrency issues */
 111 #define RATETAB_ENT(_rateid, _flags) \
 112         {                                                               \
 113                 .bitrate        = B43_RATE_TO_BASE100KBPS(_rateid),     \
 114                 .hw_value       = (_rateid),                            \
 115                 .flags          = (_flags),                             \
 116         }
 117
 118 /*
 119  * NOTE: When changing this, sync with xmit.c's
 120  *       b43_plcp_get_bitrate_idx_* functions!
 121  */
 122 static struct ieee80211_rate __b43_ratetable[] = {
 123         RATETAB_ENT(B43_CCK_RATE_1MB, 0),
 124         RATETAB_ENT(B43_CCK_RATE_2MB, IEEE80211_RATE_SHORT_PREAMBLE),
 125         RATETAB_ENT(B43_CCK_RATE_5MB, IEEE80211_RATE_SHORT_PREAMBLE),
 126         RATETAB_ENT(B43_CCK_RATE_11MB, IEEE80211_RATE_SHORT_PREAMBLE),
 127         RATETAB_ENT(B43_OFDM_RATE_6MB, 0),
 128         RATETAB_ENT(B43_OFDM_RATE_9MB, 0),
 129         RATETAB_ENT(B43_OFDM_RATE_12MB, 0),
 130         RATETAB_ENT(B43_OFDM_RATE_18MB, 0),
 131         RATETAB_ENT(B43_OFDM_RATE_24MB, 0),
 132         RATETAB_ENT(B43_OFDM_RATE_36MB, 0),
 133         RATETAB_ENT(B43_OFDM_RATE_48MB, 0),
 134         RATETAB_ENT(B43_OFDM_RATE_54MB, 0),
 135 };
 136
 137 #define b43_a_ratetable         (__b43_ratetable + 4)
 138 #define b43_a_ratetable_size    8
 139 #define b43_b_ratetable         (__b43_ratetable + 0)
 140 #define b43_b_ratetable_size    4
 141 #define b43_g_ratetable         (__b43_ratetable + 0)
 142 #define b43_g_ratetable_size    12
 143
 144 #define CHAN4G(_channel, _freq, _flags) {                       \
 145         .band                   = IEEE80211_BAND_2GHZ,          \
 146         .center_freq            = (_freq),                      \
 147         .hw_value               = (_channel),                   \
 148         .flags                  = (_flags),                     \
 149         .max_antenna_gain       = 0,                            \
 150         .max_power              = 30,                           \
 151 }
 152 static struct ieee80211_channel b43_2ghz_chantable[] = {
 153         CHAN4G(1, 2412, 0),
 154         CHAN4G(2, 2417, 0),
 155         CHAN4G(3, 2422, 0),
 156         CHAN4G(4, 2427, 0),
 157         CHAN4G(5, 2432, 0),
 158         CHAN4G(6, 2437, 0),
 159         CHAN4G(7, 2442, 0),
 160         CHAN4G(8, 2447, 0),
 161         CHAN4G(9, 2452, 0),
 162         CHAN4G(10, 2457, 0),
 163         CHAN4G(11, 2462, 0),
 164         CHAN4G(12, 2467, 0),
 165         CHAN4G(13, 2472, 0),
 166         CHAN4G(14, 2484, 0),
 167 };
 168 #undef CHAN4G
 169
 170 #define CHAN5G(_channel, _flags) {                              \
 171         .band                   = IEEE80211_BAND_5GHZ,          \
 172         .center_freq            = 5000 + (5 * (_channel)),      \
 173         .hw_value               = (_channel),                   \
 174         .flags                  = (_flags),                     \
 175         .max_antenna_gain       = 0,                            \
 176         .max_power              = 30,                           \
 177 }
 178 static struct ieee80211_channel b43_5ghz_nphy_chantable[] = {
 179         CHAN5G(32, 0),          CHAN5G(34, 0),
 180         CHAN5G(36, 0),          CHAN5G(38, 0),
 181         CHAN5G(40, 0),          CHAN5G(42, 0),
 182         CHAN5G(44, 0),          CHAN5G(46, 0),
 183         CHAN5G(48, 0),          CHAN5G(50, 0),
 184         CHAN5G(52, 0),          CHAN5G(54, 0),
 185         CHAN5G(56, 0),          CHAN5G(58, 0),
 186         CHAN5G(60, 0),          CHAN5G(62, 0),
 187         CHAN5G(64, 0),          CHAN5G(66, 0),
 188         CHAN5G(68, 0),          CHAN5G(70, 0),
 189         CHAN5G(72, 0),          CHAN5G(74, 0),
 190         CHAN5G(76, 0),          CHAN5G(78, 0),
 191         CHAN5G(80, 0),          CHAN5G(82, 0),
 192         CHAN5G(84, 0),          CHAN5G(86, 0),
 193         CHAN5G(88, 0),          CHAN5G(90, 0),
 194         CHAN5G(92, 0),          CHAN5G(94, 0),
 195         CHAN5G(96, 0),          CHAN5G(98, 0),
 196         CHAN5G(100, 0),         CHAN5G(102, 0),
 197         CHAN5G(104, 0),         CHAN5G(106, 0),
 198         CHAN5G(108, 0),         CHAN5G(110, 0),
 199         CHAN5G(112, 0),         CHAN5G(114, 0),
 200         CHAN5G(116, 0),         CHAN5G(118, 0),
 201         CHAN5G(120, 0),         CHAN5G(122, 0),
 202         CHAN5G(124, 0),         CHAN5G(126, 0),
 203         CHAN5G(128, 0),         CHAN5G(130, 0),
 204         CHAN5G(132, 0),         CHAN5G(134, 0),
 205         CHAN5G(136, 0),         CHAN5G(138, 0),
 206         CHAN5G(140, 0),         CHAN5G(142, 0),
 207         CHAN5G(144, 0),         CHAN5G(145, 0),
 208         CHAN5G(146, 0),         CHAN5G(147, 0),
 209         CHAN5G(148, 0),         CHAN5G(149, 0),
 210         CHAN5G(150, 0),         CHAN5G(151, 0),
 211         CHAN5G(152, 0),         CHAN5G(153, 0),
 212         CHAN5G(154, 0),         CHAN5G(155, 0),
 213         CHAN5G(156, 0),         CHAN5G(157, 0),
 214         CHAN5G(158, 0),         CHAN5G(159, 0),
 215         CHAN5G(160, 0),         CHAN5G(161, 0),
 216         CHAN5G(162, 0),         CHAN5G(163, 0),
 217         CHAN5G(164, 0),         CHAN5G(165, 0),
 218         CHAN5G(166, 0),         CHAN5G(168, 0),
 219         CHAN5G(170, 0),         CHAN5G(172, 0),
 220         CHAN5G(174, 0),         CHAN5G(176, 0),
 221         CHAN5G(178, 0),         CHAN5G(180, 0),
 222         CHAN5G(182, 0),         CHAN5G(184, 0),
 223         CHAN5G(186, 0),         CHAN5G(188, 0),
 224         CHAN5G(190, 0),         CHAN5G(192, 0),
 225         CHAN5G(194, 0),         CHAN5G(196, 0),
 226         CHAN5G(198, 0),         CHAN5G(200, 0),
 227         CHAN5G(202, 0),         CHAN5G(204, 0),
 228         CHAN5G(206, 0),         CHAN5G(208, 0),
 229         CHAN5G(210, 0),         CHAN5G(212, 0),
 230         CHAN5G(214, 0),         CHAN5G(216, 0),
 231         CHAN5G(218, 0),         CHAN5G(220, 0),
 232         CHAN5G(222, 0),         CHAN5G(224, 0),
 233         CHAN5G(226, 0),         CHAN5G(228, 0),
 234 };
 235
 236 static struct ieee80211_channel b43_5ghz_aphy_chantable[] = {
 237         CHAN5G(34, 0),          CHAN5G(36, 0),
 238         CHAN5G(38, 0),          CHAN5G(40, 0),
 239         CHAN5G(42, 0),          CHAN5G(44, 0),
 240         CHAN5G(46, 0),          CHAN5G(48, 0),
 241         CHAN5G(52, 0),          CHAN5G(56, 0),
 242         CHAN5G(60, 0),          CHAN5G(64, 0),
 243         CHAN5G(100, 0),         CHAN5G(104, 0),
 244         CHAN5G(108, 0),         CHAN5G(112, 0),
 245         CHAN5G(116, 0),         CHAN5G(120, 0),
 246         CHAN5G(124, 0),         CHAN5G(128, 0),
 247         CHAN5G(132, 0),         CHAN5G(136, 0),
 248         CHAN5G(140, 0),         CHAN5G(149, 0),
 249         CHAN5G(153, 0),         CHAN5G(157, 0),
 250         CHAN5G(161, 0),         CHAN5G(165, 0),
 251         CHAN5G(184, 0),         CHAN5G(188, 0),
 252         CHAN5G(192, 0),         CHAN5G(196, 0),
 253         CHAN5G(200, 0),         CHAN5G(204, 0),
 254         CHAN5G(208, 0),         CHAN5G(212, 0),
 255         CHAN5G(216, 0),
 256 };
 257 #undef CHAN5G
 258
 259 static struct ieee80211_supported_band b43_band_5GHz_nphy = {
 260         .band           = IEEE80211_BAND_5GHZ,
 261         .channels       = b43_5ghz_nphy_chantable,
 262         .n_channels     = ARRAY_SIZE(b43_5ghz_nphy_chantable),
 263         .bitrates       = b43_a_ratetable,
 264         .n_bitrates     = b43_a_ratetable_size,
 265 };
 266
 267 static struct ieee80211_supported_band b43_band_5GHz_aphy = {
 268         .band           = IEEE80211_BAND_5GHZ,
 269         .channels       = b43_5ghz_aphy_chantable,
 270         .n_channels     = ARRAY_SIZE(b43_5ghz_aphy_chantable),
 271         .bitrates       = b43_a_ratetable,
 272         .n_bitrates     = b43_a_ratetable_size,
 273 };
 274
 275 static struct ieee80211_supported_band b43_band_2GHz = {
 276         .band           = IEEE80211_BAND_2GHZ,
 277         .channels       = b43_2ghz_chantable,
 278         .n_channels     = ARRAY_SIZE(b43_2ghz_chantable),
 279         .bitrates       = b43_g_ratetable,
 280         .n_bitrates     = b43_g_ratetable_size,
 281 };
 282
 283 static void b43_wireless_core_exit(struct b43_wldev *dev);
 284 static int b43_wireless_core_init(struct b43_wldev *dev);
 285 static void b43_wireless_core_stop(struct b43_wldev *dev);
 286 static int b43_wireless_core_start(struct b43_wldev *dev);
 287
 288 static int b43_ratelimit(struct b43_wl *wl)
 289 {
 290         if (!wl || !wl->current_dev)
 291                 return 1;
 292         if (b43_status(wl->current_dev) < B43_STAT_STARTED)
 293                 return 1;
 294         /* We are up and running.
 295          * Ratelimit the messages to avoid DoS over the net. */
 296         return net_ratelimit();
 297 }
 298
 299 void b43info(struct b43_wl *wl, const char *fmt, ...)
 300 {
 301         va_list args;
 302
 303         if (!b43_ratelimit(wl))
 304                 return;
 305         va_start(args, fmt);
 306         printk(KERN_INFO "b43-%s: ",
 307                (wl && wl->hw) ? wiphy_name(wl->hw->wiphy) : "wlan");
 308         vprintk(fmt, args);
 309         va_end(args);
 310 }
 311
 312 void b43err(struct b43_wl *wl, const char *fmt, ...)
 313 {
 314         va_list args;
 315
 316         if (!b43_ratelimit(wl))
 317                 return;
 318         va_start(args, fmt);
 319         printk(KERN_ERR "b43-%s ERROR: ",
 320                (wl && wl->hw) ? wiphy_name(wl->hw->wiphy) : "wlan");
 321         vprintk(fmt, args);
 322         va_end(args);
 323 }
 324
 325 void b43warn(struct b43_wl *wl, const char *fmt, ...)
 326 {
 327         va_list args;
 328
 329         if (!b43_ratelimit(wl))
 330                 return;
 331         va_start(args, fmt);
 332         printk(KERN_WARNING "b43-%s warning: ",
 333                (wl && wl->hw) ? wiphy_name(wl->hw->wiphy) : "wlan");
 334         vprintk(fmt, args);
 335         va_end(args);
 336 }
 337
 338 #if B43_DEBUG
 339 void b43dbg(struct b43_wl *wl, const char *fmt, ...)
 340 {
 341         va_list args;
 342
 343         va_start(args, fmt);
 344         printk(KERN_DEBUG "b43-%s debug: ",
 345                (wl && wl->hw) ? wiphy_name(wl->hw->wiphy) : "wlan");
 346         vprintk(fmt, args);
 347         va_end(args);
 348 }
 349 #endif /* DEBUG */
 350
 351 static void b43_ram_write(struct b43_wldev *dev, u16 offset, u32 val)
 352 {
 353         u32 macctl;
 354
 355         B43_WARN_ON(offset % 4 != 0);
 356
 357         macctl = b43_read32(dev, B43_MMIO_MACCTL);
 358         if (macctl & B43_MACCTL_BE)
 359                 val = swab32(val);
 360
 361         b43_write32(dev, B43_MMIO_RAM_CONTROL, offset);
 362         mmiowb();
 363         b43_write32(dev, B43_MMIO_RAM_DATA, val);
 364 }
 365
 366 static inline void b43_shm_control_word(struct b43_wldev *dev,
 367                                         u16 routing, u16 offset)
 368 {
 369         u32 control;
 370
 371         /* "offset" is the WORD offset. */
 372         control = routing;
 373         control <<= 16;
 374         control |= offset;
 375         b43_write32(dev, B43_MMIO_SHM_CONTROL, control);
 376 }
 377
 378 u32 __b43_shm_read32(struct b43_wldev *dev, u16 routing, u16 offset)
 379 {
 380         u32 ret;
 381
 382         if (routing == B43_SHM_SHARED) {
 383                 B43_WARN_ON(offset & 0x0001);
 384                 if (offset & 0x0003) {
 385                         /* Unaligned access */
 386                         b43_shm_control_word(dev, routing, offset >> 2);
 387                         ret = b43_read16(dev, B43_MMIO_SHM_DATA_UNALIGNED);
 388                         ret <<= 16;
 389                         b43_shm_control_word(dev, routing, (offset >> 2) + 1);
 390                         ret |= b43_read16(dev, B43_MMIO_SHM_DATA);
 391
 392                         goto out;
 393                 }
 394                 offset >>= 2;
 395         }
 396         b43_shm_control_word(dev, routing, offset);
 397         ret = b43_read32(dev, B43_MMIO_SHM_DATA);
 398 out:
 399         return ret;
 400 }
 401
 402 u32 b43_shm_read32(struct b43_wldev *dev, u16 routing, u16 offset)
 403 {
 404         struct b43_wl *wl = dev->wl;
 405         unsigned long flags;
 406         u32 ret;
 407
 408         spin_lock_irqsave(&wl->shm_lock, flags);
 409         ret = __b43_shm_read32(dev, routing, offset);
 410         spin_unlock_irqrestore(&wl->shm_lock, flags);
 411
 412         return ret;
 413 }
 414
 415 u16 __b43_shm_read16(struct b43_wldev *dev, u16 routing, u16 offset)
 416 {
 417         u16 ret;
 418
 419         if (routing == B43_SHM_SHARED) {
 420                 B43_WARN_ON(offset & 0x0001);
 421                 if (offset & 0x0003) {
 422                         /* Unaligned access */
 423                         b43_shm_control_word(dev, routing, offset >> 2);
 424                         ret = b43_read16(dev, B43_MMIO_SHM_DATA_UNALIGNED);
 425
 426                         goto out;
 427                 }
 428                 offset >>= 2;
 429         }
 430         b43_shm_control_word(dev, routing, offset);
 431         ret = b43_read16(dev, B43_MMIO_SHM_DATA);
 432 out:
 433         return ret;
 434 }
 435
 436 u16 b43_shm_read16(struct b43_wldev *dev, u16 routing, u16 offset)
 437 {
 438         struct b43_wl *wl = dev->wl;
 439         unsigned long flags;
 440         u16 ret;
 441
 442         spin_lock_irqsave(&wl->shm_lock, flags);
 443         ret = __b43_shm_read16(dev, routing, offset);
 444         spin_unlock_irqrestore(&wl->shm_lock, flags);
 445
 446         return ret;
 447 }
 448
 449 void __b43_shm_write32(struct b43_wldev *dev, u16 routing, u16 offset, u32 value)
 450 {
 451         if (routing == B43_SHM_SHARED) {
 452                 B43_WARN_ON(offset & 0x0001);
 453                 if (offset & 0x0003) {
 454                         /* Unaligned access */
 455                         b43_shm_control_word(dev, routing, offset >> 2);
 456                         b43_write16(dev, B43_MMIO_SHM_DATA_UNALIGNED,
 457                                     (value >> 16) & 0xffff);
 458                         b43_shm_control_word(dev, routing, (offset >> 2) + 1);
 459                         b43_write16(dev, B43_MMIO_SHM_DATA, value & 0xffff);
 460                         return;
 461                 }
 462                 offset >>= 2;
 463         }
 464         b43_shm_control_word(dev, routing, offset);
 465         b43_write32(dev, B43_MMIO_SHM_DATA, value);
 466 }
 467
 468 void b43_shm_write32(struct b43_wldev *dev, u16 routing, u16 offset, u32 value)
 469 {
 470         struct b43_wl *wl = dev->wl;
 471         unsigned long flags;
 472
 473         spin_lock_irqsave(&wl->shm_lock, flags);
 474         __b43_shm_write32(dev, routing, offset, value);
 475         spin_unlock_irqrestore(&wl->shm_lock, flags);
 476 }
 477
 478 void __b43_shm_write16(struct b43_wldev *dev, u16 routing, u16 offset, u16 value)
 479 {
 480         if (routing == B43_SHM_SHARED) {
 481                 B43_WARN_ON(offset & 0x0001);
 482                 if (offset & 0x0003) {
 483                         /* Unaligned access */
 484                         b43_shm_control_word(dev, routing, offset >> 2);
 485                         b43_write16(dev, B43_MMIO_SHM_DATA_UNALIGNED, value);
 486                         return;
 487                 }
 488                 offset >>= 2;
 489         }
 490         b43_shm_control_word(dev, routing, offset);
 491         b43_write16(dev, B43_MMIO_SHM_DATA, value);
 492 }
 493
 494 void b43_shm_write16(struct b43_wldev *dev, u16 routing, u16 offset, u16 value)
 495 {
 496         struct b43_wl *wl = dev->wl;
 497         unsigned long flags;
 498
 499         spin_lock_irqsave(&wl->shm_lock, flags);
 500         __b43_shm_write16(dev, routing, offset, value);
 501         spin_unlock_irqrestore(&wl->shm_lock, flags);
 502 }
 503
 504 /* Read HostFlags */
 505 u64 b43_hf_read(struct b43_wldev * dev)
 506 {
 507         u64 ret;
 508
 509         ret = b43_shm_read16(dev, B43_SHM_SHARED, B43_SHM_SH_HOSTFHI);
 510         ret <<= 16;
 511         ret |= b43_shm_read16(dev, B43_SHM_SHARED, B43_SHM_SH_HOSTFMI);
 512         ret <<= 16;
 513         ret |= b43_shm_read16(dev, B43_SHM_SHARED, B43_SHM_SH_HOSTFLO);
 514
 515         return ret;
 516 }
 517
 518 /* Write HostFlags */
 519 void b43_hf_write(struct b43_wldev *dev, u64 value)
 520 {
 521         u16 lo, mi, hi;
 522
 523         lo = (value & 0x00000000FFFFULL);
 524         mi = (value & 0x0000FFFF0000ULL) >> 16;
 525         hi = (value & 0xFFFF00000000ULL) >> 32;
 526         b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_HOSTFLO, lo);
 527         b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_HOSTFMI, mi);
 528         b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_HOSTFHI, hi);
 529 }
 530
 531 void b43_tsf_read(struct b43_wldev *dev, u64 *tsf)
 532 {
 533         u32 low, high;
 534
 535         B43_WARN_ON(dev->dev->id.revision < 3);
 536
 537         /* The hardware guarantees us an atomic read, if we
 538          * read the low register first. */
 539         low = b43_read32(dev, B43_MMIO_REV3PLUS_TSF_LOW);
 540         high = b43_read32(dev, B43_MMIO_REV3PLUS_TSF_HIGH);
 541
 542         *tsf = high;
 543         *tsf <<= 32;
 544         *tsf |= low;
 545 }
 546
 547 static void b43_time_lock(struct b43_wldev *dev)
 548 {
 549         u32 macctl;
 550
 551         macctl = b43_read32(dev, B43_MMIO_MACCTL);
 552         macctl |= B43_MACCTL_TBTTHOLD;
 553         b43_write32(dev, B43_MMIO_MACCTL, macctl);
 554         /* Commit the write */
 555         b43_read32(dev, B43_MMIO_MACCTL);
 556 }
 557
 558 static void b43_time_unlock(struct b43_wldev *dev)
 559 {
 560         u32 macctl;
 561
 562         macctl = b43_read32(dev, B43_MMIO_MACCTL);
 563         macctl &= ~B43_MACCTL_TBTTHOLD;
 564         b43_write32(dev, B43_MMIO_MACCTL, macctl);
 565         /* Commit the write */
 566         b43_read32(dev, B43_MMIO_MACCTL);
 567 }
 568
 569 static void b43_tsf_write_locked(struct b43_wldev *dev, u64 tsf)
 570 {
 571         u32 low, high;
 572
 573         B43_WARN_ON(dev->dev->id.revision < 3);
 574
 575         low = tsf;
 576         high = (tsf >> 32);
 577         /* The hardware guarantees us an atomic write, if we
 578          * write the low register first. */
 579         b43_write32(dev, B43_MMIO_REV3PLUS_TSF_LOW, low);
 580         mmiowb();
 581         b43_write32(dev, B43_MMIO_REV3PLUS_TSF_HIGH, high);
 582         mmiowb();
 583 }
 584
 585 void b43_tsf_write(struct b43_wldev *dev, u64 tsf)
 586 {
 587         b43_time_lock(dev);
 588         b43_tsf_write_locked(dev, tsf);
 589         b43_time_unlock(dev);
 590 }
 591
 592 static
 593 void b43_macfilter_set(struct b43_wldev *dev, u16 offset, const u8 * mac)
 594 {
 595         static const u8 zero_addr[ETH_ALEN] = { 0 };
 596         u16 data;
 597
 598         if (!mac)
 599                 mac = zero_addr;
 600
 601         offset |= 0x0020;
 602         b43_write16(dev, B43_MMIO_MACFILTER_CONTROL, offset);
 603
 604         data = mac[0];
 605         data |= mac[1] << 8;
 606         b43_write16(dev, B43_MMIO_MACFILTER_DATA, data);
 607         data = mac[2];
 608         data |= mac[3] << 8;
 609         b43_write16(dev, B43_MMIO_MACFILTER_DATA, data);
 610         data = mac[4];
 611         data |= mac[5] << 8;
 612         b43_write16(dev, B43_MMIO_MACFILTER_DATA, data);
 613 }
 614
 615 static void b43_write_mac_bssid_templates(struct b43_wldev *dev)
 616 {
 617         const u8 *mac;
 618         const u8 *bssid;
 619         u8 mac_bssid[ETH_ALEN * 2];
 620         int i;
 621         u32 tmp;
 622
 623         bssid = dev->wl->bssid;
 624         mac = dev->wl->mac_addr;
 625
 626         b43_macfilter_set(dev, B43_MACFILTER_BSSID, bssid);
 627
 628         memcpy(mac_bssid, mac, ETH_ALEN);
 629         memcpy(mac_bssid + ETH_ALEN, bssid, ETH_ALEN);
 630
 631         /* Write our MAC address and BSSID to template ram */
 632         for (i = 0; i < ARRAY_SIZE(mac_bssid); i += sizeof(u32)) {
 633                 tmp = (u32) (mac_bssid[i + 0]);
 634                 tmp |= (u32) (mac_bssid[i + 1]) << 8;
 635                 tmp |= (u32) (mac_bssid[i + 2]) << 16;
 636                 tmp |= (u32) (mac_bssid[i + 3]) << 24;
 637                 b43_ram_write(dev, 0x20 + i, tmp);
 638         }
 639 }
 640
 641 static void b43_upload_card_macaddress(struct b43_wldev *dev)
 642 {
 643         b43_write_mac_bssid_templates(dev);
 644         b43_macfilter_set(dev, B43_MACFILTER_SELF, dev->wl->mac_addr);
 645 }
 646
 647 static void b43_set_slot_time(struct b43_wldev *dev, u16 slot_time)
 648 {
 649         /* slot_time is in usec. */
 650         if (dev->phy.type != B43_PHYTYPE_G)
 651                 return;
 652         b43_write16(dev, 0x684, 510 + slot_time);
 653         b43_shm_write16(dev, B43_SHM_SHARED, 0x0010, slot_time);
 654 }
 655
 656 static void b43_short_slot_timing_enable(struct b43_wldev *dev)
 657 {
 658         b43_set_slot_time(dev, 9);
 659 }
 660
 661 static void b43_short_slot_timing_disable(struct b43_wldev *dev)
 662 {
 663         b43_set_slot_time(dev, 20);
 664 }
 665
 666 /* Enable a Generic IRQ. "mask" is the mask of which IRQs to enable.
 667  * Returns the _previously_ enabled IRQ mask.
 668  */
 669 static inline u32 b43_interrupt_enable(struct b43_wldev *dev, u32 mask)
 670 {
 671         u32 old_mask;
 672
 673         old_mask = b43_read32(dev, B43_MMIO_GEN_IRQ_MASK);
 674         b43_write32(dev, B43_MMIO_GEN_IRQ_MASK, old_mask | mask);
 675
 676         return old_mask;
 677 }
 678
 679 /* Disable a Generic IRQ. "mask" is the mask of which IRQs to disable.
 680  * Returns the _previously_ enabled IRQ mask.
 681  */
 682 static inline u32 b43_interrupt_disable(struct b43_wldev *dev, u32 mask)
 683 {
 684         u32 old_mask;
 685
 686         old_mask = b43_read32(dev, B43_MMIO_GEN_IRQ_MASK);
 687         b43_write32(dev, B43_MMIO_GEN_IRQ_MASK, old_mask & ~mask);
 688
 689         return old_mask;
 690 }
 691
 692 /* Synchronize IRQ top- and bottom-half.
 693  * IRQs must be masked before calling this.
 694  * This must not be called with the irq_lock held.
 695  */
 696 static void b43_synchronize_irq(struct b43_wldev *dev)
 697 {
 698         synchronize_irq(dev->dev->irq);
 699         tasklet_kill(&dev->isr_tasklet);
 700 }
 701
 702 /* DummyTransmission function, as documented on
 703  * http://bcm-specs.sipsolutions.net/DummyTransmission
 704  */
 705 void b43_dummy_transmission(struct b43_wldev *dev)
 706 {
 707         struct b43_wl *wl = dev->wl;
 708         struct b43_phy *phy = &dev->phy;
 709         unsigned int i, max_loop;
 710         u16 value;
 711         u32 buffer[5] = {
 712                 0x00000000,
 713                 0x00D40000,
 714                 0x00000000,
 715                 0x01000000,
 716                 0x00000000,
 717         };
 718
 719         switch (phy->type) {
 720         case B43_PHYTYPE_A:
 721                 max_loop = 0x1E;
 722                 buffer[0] = 0x000201CC;
 723                 break;
 724         case B43_PHYTYPE_B:
 725         case B43_PHYTYPE_G:
 726                 max_loop = 0xFA;
 727                 buffer[0] = 0x000B846E;
 728                 break;
 729         default:
 730                 B43_WARN_ON(1);
 731                 return;
 732         }
 733
 734         spin_lock_irq(&wl->irq_lock);
 735         write_lock(&wl->tx_lock);
 736
 737         for (i = 0; i < 5; i++)
 738                 b43_ram_write(dev, i * 4, buffer[i]);
 739
 740         /* Commit writes */
 741         b43_read32(dev, B43_MMIO_MACCTL);
 742
 743         b43_write16(dev, 0x0568, 0x0000);
 744         b43_write16(dev, 0x07C0, 0x0000);
 745         value = ((phy->type == B43_PHYTYPE_A) ? 1 : 0);
 746         b43_write16(dev, 0x050C, value);
 747         b43_write16(dev, 0x0508, 0x0000);
 748         b43_write16(dev, 0x050A, 0x0000);
 749         b43_write16(dev, 0x054C, 0x0000);
 750         b43_write16(dev, 0x056A, 0x0014);
 751         b43_write16(dev, 0x0568, 0x0826);
 752         b43_write16(dev, 0x0500, 0x0000);
 753         b43_write16(dev, 0x0502, 0x0030);
 754
 755         if (phy->radio_ver == 0x2050 && phy->radio_rev <= 0x5)
 756                 b43_radio_write16(dev, 0x0051, 0x0017);
 757         for (i = 0x00; i < max_loop; i++) {
 758                 value = b43_read16(dev, 0x050E);
 759                 if (value & 0x0080)
 760                         break;
 761                 udelay(10);
 762         }
 763         for (i = 0x00; i < 0x0A; i++) {
 764                 value = b43_read16(dev, 0x050E);
 765                 if (value & 0x0400)
 766                         break;
 767                 udelay(10);
 768         }
 769         for (i = 0x00; i < 0x19; i++) {
 770                 value = b43_read16(dev, 0x0690);
 771                 if (!(value & 0x0100))
 772                         break;
 773                 udelay(10);
 774         }
 775         if (phy->radio_ver == 0x2050 && phy->radio_rev <= 0x5)
 776                 b43_radio_write16(dev, 0x0051, 0x0037);
 777
 778         write_unlock(&wl->tx_lock);
 779         spin_unlock_irq(&wl->irq_lock);
 780 }
 781
 782 static void key_write(struct b43_wldev *dev,
 783                       u8 index, u8 algorithm, const u8 * key)
 784 {
 785         unsigned int i;
 786         u32 offset;
 787         u16 value;
 788         u16 kidx;
 789
 790         /* Key index/algo block */
 791         kidx = b43_kidx_to_fw(dev, index);
 792         value = ((kidx << 4) | algorithm);
 793         b43_shm_write16(dev, B43_SHM_SHARED,
 794                         B43_SHM_SH_KEYIDXBLOCK + (kidx * 2), value);
 795
 796         /* Write the key to the Key Table Pointer offset */
 797         offset = dev->ktp + (index * B43_SEC_KEYSIZE);
 798         for (i = 0; i < B43_SEC_KEYSIZE; i += 2) {
 799                 value = key[i];
 800                 value |= (u16) (key[i + 1]) << 8;
 801                 b43_shm_write16(dev, B43_SHM_SHARED, offset + i, value);
 802         }
 803 }
 804
 805 static void keymac_write(struct b43_wldev *dev, u8 index, const u8 * addr)
 806 {
 807         u32 addrtmp[2] = { 0, 0, };
 808         u8 per_sta_keys_start = 8;
 809
 810         if (b43_new_kidx_api(dev))
 811                 per_sta_keys_start = 4;
 812
 813         B43_WARN_ON(index < per_sta_keys_start);
 814         /* We have two default TX keys and possibly two default RX keys.
 815          * Physical mac 0 is mapped to physical key 4 or 8, depending
 816          * on the firmware version.
 817          * So we must adjust the index here.
 818          */
 819         index -= per_sta_keys_start;
 820
 821         if (addr) {
 822                 addrtmp[0] = addr[0];
 823                 addrtmp[0] |= ((u32) (addr[1]) << 8);
 824                 addrtmp[0] |= ((u32) (addr[2]) << 16);
 825                 addrtmp[0] |= ((u32) (addr[3]) << 24);
 826                 addrtmp[1] = addr[4];
 827                 addrtmp[1] |= ((u32) (addr[5]) << 8);
 828         }
 829
 830         if (dev->dev->id.revision >= 5) {
 831                 /* Receive match transmitter address mechanism */
 832                 b43_shm_write32(dev, B43_SHM_RCMTA,
 833                                 (index * 2) + 0, addrtmp[0]);
 834                 b43_shm_write16(dev, B43_SHM_RCMTA,
 835                                 (index * 2) + 1, addrtmp[1]);
 836         } else {
 837                 /* RXE (Receive Engine) and
 838                  * PSM (Programmable State Machine) mechanism
 839                  */
 840                 if (index < 8) {
 841                         /* TODO write to RCM 16, 19, 22 and 25 */
 842                 } else {
 843                         b43_shm_write32(dev, B43_SHM_SHARED,
 844                                         B43_SHM_SH_PSM + (index * 6) + 0,
 845                                         addrtmp[0]);
 846                         b43_shm_write16(dev, B43_SHM_SHARED,
 847                                         B43_SHM_SH_PSM + (index * 6) + 4,
 848                                         addrtmp[1]);
 849                 }
 850         }
 851 }
 852
 853 static void do_key_write(struct b43_wldev *dev,
 854                          u8 index, u8 algorithm,
 855                          const u8 * key, size_t key_len, const u8 * mac_addr)
 856 {
 857         u8 buf[B43_SEC_KEYSIZE] = { 0, };
 858         u8 per_sta_keys_start = 8;
 859
 860         if (b43_new_kidx_api(dev))
 861                 per_sta_keys_start = 4;
 862
 863         B43_WARN_ON(index >= dev->max_nr_keys);
 864         B43_WARN_ON(key_len > B43_SEC_KEYSIZE);
 865
 866         if (index >= per_sta_keys_start)
 867                 keymac_write(dev, index, NULL); /* First zero out mac. */
 868         if (key)
 869                 memcpy(buf, key, key_len);
 870         key_write(dev, index, algorithm, buf);
 871         if (index >= per_sta_keys_start)
 872                 keymac_write(dev, index, mac_addr);
 873
 874         dev->key[index].algorithm = algorithm;
 875 }
 876
 877 static int b43_key_write(struct b43_wldev *dev,
 878                          int index, u8 algorithm,
 879                          const u8 * key, size_t key_len,
 880                          const u8 * mac_addr,
 881                          struct ieee80211_key_conf *keyconf)
 882 {
 883         int i;
 884         int sta_keys_start;
 885
 886         if (key_len > B43_SEC_KEYSIZE)
 887                 return -EINVAL;
 888         for (i = 0; i < dev->max_nr_keys; i++) {
 889                 /* Check that we don't already have this key. */
 890                 B43_WARN_ON(dev->key[i].keyconf == keyconf);
 891         }
 892         if (index < 0) {
 893                 /* Pairwise key. Get an empty slot for the key. */
 894                 if (b43_new_kidx_api(dev))
 895                         sta_keys_start = 4;
 896                 else
 897                         sta_keys_start = 8;
 898                 for (i = sta_keys_start; i < dev->max_nr_keys; i++) {
 899                         if (!dev->key[i].keyconf) {
 900                                 /* found empty */
 901                                 index = i;
 902                                 break;
 903                         }
 904                 }
 905                 if (index < 0) {
 906                         b43warn(dev->wl, "Out of hardware key memory\n");
 907                         return -ENOSPC;
 908                 }
 909         } else
 910                 B43_WARN_ON(index > 3);
 911
 912         do_key_write(dev, index, algorithm, key, key_len, mac_addr);
 913         if ((index <= 3) && !b43_new_kidx_api(dev)) {
 914                 /* Default RX key */
 915                 B43_WARN_ON(mac_addr);
 916                 do_key_write(dev, index + 4, algorithm, key, key_len, NULL);
 917         }
 918         keyconf->hw_key_idx = index;
 919         dev->key[index].keyconf = keyconf;
 920
 921         return 0;
 922 }
 923
 924 static int b43_key_clear(struct b43_wldev *dev, int index)
 925 {
 926         if (B43_WARN_ON((index < 0) || (index >= dev->max_nr_keys)))
 927                 return -EINVAL;
 928         do_key_write(dev, index, B43_SEC_ALGO_NONE,
 929                      NULL, B43_SEC_KEYSIZE, NULL);
 930         if ((index <= 3) && !b43_new_kidx_api(dev)) {
 931                 do_key_write(dev, index + 4, B43_SEC_ALGO_NONE,
 932                              NULL, B43_SEC_KEYSIZE, NULL);
 933         }
 934         dev->key[index].keyconf = NULL;
 935
 936         return 0;
 937 }
 938
 939 static void b43_clear_keys(struct b43_wldev *dev)
 940 {
 941         int i;
 942
 943         for (i = 0; i < dev->max_nr_keys; i++)
 944                 b43_key_clear(dev, i);
 945 }
 946
 947 static void b43_dump_keymemory(struct b43_wldev *dev)
 948 {
 949         unsigned int i, index, offset;
 950         DECLARE_MAC_BUF(macbuf);
 951         u8 mac[ETH_ALEN];
 952         u16 algo;
 953         u32 rcmta0;
 954         u16 rcmta1;
 955         u64 hf;
 956         struct b43_key *key;
 957
 958         if (!b43_debug(dev, B43_DBG_KEYS))
 959                 return;
 960
 961         hf = b43_hf_read(dev);
 962         b43dbg(dev->wl, "Hardware key memory dump:  USEDEFKEYS=%u\n",
 963                !!(hf & B43_HF_USEDEFKEYS));
 964         for (index = 0; index < dev->max_nr_keys; index++) {
 965                 key = &(dev->key[index]);
 966                 printk(KERN_DEBUG "Key slot %02u: %s",
 967                        index, (key->keyconf == NULL) ? " " : "*");
 968                 offset = dev->ktp + (index * B43_SEC_KEYSIZE);
 969                 for (i = 0; i < B43_SEC_KEYSIZE; i += 2) {
 970                         u16 tmp = b43_shm_read16(dev, B43_SHM_SHARED, offset + i);
 971                         printk("%02X%02X", (tmp & 0xFF), ((tmp >> 8) & 0xFF));
 972                 }
 973
 974                 algo = b43_shm_read16(dev, B43_SHM_SHARED,
 975                                       B43_SHM_SH_KEYIDXBLOCK + (index * 2));
 976                 printk("   Algo: %04X/%02X", algo, key->algorithm);
 977
 978                 if (index >= 4) {
 979                         rcmta0 = b43_shm_read32(dev, B43_SHM_RCMTA,
 980                                                 ((index - 4) * 2) + 0);
 981                         rcmta1 = b43_shm_read16(dev, B43_SHM_RCMTA,
 982                                                 ((index - 4) * 2) + 1);
 983                         *((__le32 *)(&mac[0])) = cpu_to_le32(rcmta0);
 984                         *((__le16 *)(&mac[4])) = cpu_to_le16(rcmta1);
 985                         printk("   MAC: %s",
 986                                print_mac(macbuf, mac));
 987                 } else
 988                         printk("   DEFAULT KEY");
 989                 printk("\n");
 990         }
 991 }
 992
 993 void b43_power_saving_ctl_bits(struct b43_wldev *dev, unsigned int ps_flags)
 994 {
 995         u32 macctl;
 996         u16 ucstat;
 997         bool hwps;
 998         bool awake;
 999         int i;
1000
1001         B43_WARN_ON((ps_flags & B43_PS_ENABLED) &&
1002                     (ps_flags & B43_PS_DISABLED));
1003         B43_WARN_ON((ps_flags & B43_PS_AWAKE) && (ps_flags & B43_PS_ASLEEP));
1004
1005         if (ps_flags & B43_PS_ENABLED) {
1006                 hwps = 1;
1007         } else if (ps_flags & B43_PS_DISABLED) {
1008                 hwps = 0;
1009         } else {
1010                 //TODO: If powersave is not off and FIXME is not set and we are not in adhoc
1011                 //      and thus is not an AP and we are associated, set bit 25
1012         }
1013         if (ps_flags & B43_PS_AWAKE) {
1014                 awake = 1;
1015         } else if (ps_flags & B43_PS_ASLEEP) {
1016                 awake = 0;
1017         } else {
1018                 //TODO: If the device is awake or this is an AP, or we are scanning, or FIXME,
1019                 //      or we are associated, or FIXME, or the latest PS-Poll packet sent was
1020                 //      successful, set bit26
1021         }
1022
1023 /* FIXME: For now we force awake-on and hwps-off */
1024         hwps = 0;
1025         awake = 1;
1026
1027         macctl = b43_read32(dev, B43_MMIO_MACCTL);
1028         if (hwps)
1029                 macctl |= B43_MACCTL_HWPS;
1030         else
1031                 macctl &= ~B43_MACCTL_HWPS;
1032         if (awake)
1033                 macctl |= B43_MACCTL_AWAKE;
1034         else
1035                 macctl &= ~B43_MACCTL_AWAKE;
1036         b43_write32(dev, B43_MMIO_MACCTL, macctl);
1037         /* Commit write */
1038         b43_read32(dev, B43_MMIO_MACCTL);
1039         if (awake && dev->dev->id.revision >= 5) {
1040                 /* Wait for the microcode to wake up. */
1041                 for (i = 0; i < 100; i++) {
1042                         ucstat = b43_shm_read16(dev, B43_SHM_SHARED,
1043                                                 B43_SHM_SH_UCODESTAT);
1044                         if (ucstat != B43_SHM_SH_UCODESTAT_SLEEP)
1045                                 break;
1046                         udelay(10);
1047                 }
1048         }
1049 }
1050
1051 void b43_wireless_core_reset(struct b43_wldev *dev, u32 flags)
1052 {
1053         u32 tmslow;
1054         u32 macctl;
1055
1056         flags |= B43_TMSLOW_PHYCLKEN;
1057         flags |= B43_TMSLOW_PHYRESET;
1058         ssb_device_enable(dev->dev, flags);
1059         msleep(2);              /* Wait for the PLL to turn on. */
1060
1061         /* Now take the PHY out of Reset again */
1062         tmslow = ssb_read32(dev->dev, SSB_TMSLOW);
1063         tmslow |= SSB_TMSLOW_FGC;
1064         tmslow &= ~B43_TMSLOW_PHYRESET;
1065         ssb_write32(dev->dev, SSB_TMSLOW, tmslow);
1066         ssb_read32(dev->dev, SSB_TMSLOW);       /* flush */
1067         msleep(1);
1068         tmslow &= ~SSB_TMSLOW_FGC;
1069         ssb_write32(dev->dev, SSB_TMSLOW, tmslow);
1070         ssb_read32(dev->dev, SSB_TMSLOW);       /* flush */
1071         msleep(1);
1072
1073         /* Turn Analog ON, but only if we already know the PHY-type.
1074          * This protects against very early setup where we don't know the
1075          * PHY-type, yet. wireless_core_reset will be called once again later,
1076          * when we know the PHY-type. */
1077         if (dev->phy.ops)
1078                 dev->phy.ops->switch_analog(dev, 1);
1079
1080         macctl = b43_read32(dev, B43_MMIO_MACCTL);
1081         macctl &= ~B43_MACCTL_GMODE;
1082         if (flags & B43_TMSLOW_GMODE)
1083                 macctl |= B43_MACCTL_GMODE;
1084         macctl |= B43_MACCTL_IHR_ENABLED;
1085         b43_write32(dev, B43_MMIO_MACCTL, macctl);
1086 }
1087
1088 static void handle_irq_transmit_status(struct b43_wldev *dev)
1089 {
1090         u32 v0, v1;
1091         u16 tmp;
1092         struct b43_txstatus stat;
1093
1094         while (1) {
1095                 v0 = b43_read32(dev, B43_MMIO_XMITSTAT_0);
1096                 if (!(v0 & 0x00000001))
1097                         break;
1098                 v1 = b43_read32(dev, B43_MMIO_XMITSTAT_1);
1099
1100                 stat.cookie = (v0 >> 16);
1101                 stat.seq = (v1 & 0x0000FFFF);
1102                 stat.phy_stat = ((v1 & 0x00FF0000) >> 16);
1103                 tmp = (v0 & 0x0000FFFF);
1104                 stat.frame_count = ((tmp & 0xF000) >> 12);
1105                 stat.rts_count = ((tmp & 0x0F00) >> 8);
1106                 stat.supp_reason = ((tmp & 0x001C) >> 2);
1107                 stat.pm_indicated = !!(tmp & 0x0080);
1108                 stat.intermediate = !!(tmp & 0x0040);
1109                 stat.for_ampdu = !!(tmp & 0x0020);
1110                 stat.acked = !!(tmp & 0x0002);
1111
1112                 b43_handle_txstatus(dev, &stat);
1113         }
1114 }
1115
1116 static void drain_txstatus_queue(struct b43_wldev *dev)
1117 {
1118         u32 dummy;
1119
1120         if (dev->dev->id.revision < 5)
1121                 return;
1122         /* Read all entries from the microcode TXstatus FIFO
1123          * and throw them away.
1124          */
1125         while (1) {
1126                 dummy = b43_read32(dev, B43_MMIO_XMITSTAT_0);
1127                 if (!(dummy & 0x00000001))
1128                         break;
1129                 dummy = b43_read32(dev, B43_MMIO_XMITSTAT_1);
1130         }
1131 }
1132
1133 static u32 b43_jssi_read(struct b43_wldev *dev)
1134 {
1135         u32 val = 0;
1136
1137         val = b43_shm_read16(dev, B43_SHM_SHARED, 0x08A);
1138         val <<= 16;
1139         val |= b43_shm_read16(dev, B43_SHM_SHARED, 0x088);
1140
1141         return val;
1142 }
1143
1144 static void b43_jssi_write(struct b43_wldev *dev, u32 jssi)
1145 {
1146         b43_shm_write16(dev, B43_SHM_SHARED, 0x088, (jssi & 0x0000FFFF));
1147         b43_shm_write16(dev, B43_SHM_SHARED, 0x08A, (jssi & 0xFFFF0000) >> 16);
1148 }
1149
1150 static void b43_generate_noise_sample(struct b43_wldev *dev)
1151 {
1152         b43_jssi_write(dev, 0x7F7F7F7F);
1153         b43_write32(dev, B43_MMIO_MACCMD,
1154                     b43_read32(dev, B43_MMIO_MACCMD) | B43_MACCMD_BGNOISE);
1155 }
1156
1157 static void b43_calculate_link_quality(struct b43_wldev *dev)
1158 {
1159         /* Top half of Link Quality calculation. */
1160
1161         if (dev->phy.type != B43_PHYTYPE_G)
1162                 return;
1163         if (dev->noisecalc.calculation_running)
1164                 return;
1165         dev->noisecalc.calculation_running = 1;
1166         dev->noisecalc.nr_samples = 0;
1167
1168         b43_generate_noise_sample(dev);
1169 }
1170
1171 static void handle_irq_noise(struct b43_wldev *dev)
1172 {
1173         struct b43_phy_g *phy = dev->phy.g;
1174         u16 tmp;
1175         u8 noise[4];
1176         u8 i, j;
1177         s32 average;
1178
1179         /* Bottom half of Link Quality calculation. */
1180
1181         if (dev->phy.type != B43_PHYTYPE_G)
1182                 return;
1183
1184         /* Possible race condition: It might be possible that the user
1185          * changed to a different channel in the meantime since we
1186          * started the calculation. We ignore that fact, since it's
1187          * not really that much of a problem. The background noise is
1188          * an estimation only anyway. Slightly wrong results will get damped
1189          * by the averaging of the 8 sample rounds. Additionally the
1190          * value is shortlived. So it will be replaced by the next noise
1191          * calculation round soon. */
1192
1193         B43_WARN_ON(!dev->noisecalc.calculation_running);
1194         *((__le32 *)noise) = cpu_to_le32(b43_jssi_read(dev));
1195         if (noise[0] == 0x7F || noise[1] == 0x7F ||
1196             noise[2] == 0x7F || noise[3] == 0x7F)
1197                 goto generate_new;
1198
1199         /* Get the noise samples. */
1200         B43_WARN_ON(dev->noisecalc.nr_samples >= 8);
1201         i = dev->noisecalc.nr_samples;
1202         noise[0] = clamp_val(noise[0], 0, ARRAY_SIZE(phy->nrssi_lt) - 1);
1203         noise[1] = clamp_val(noise[1], 0, ARRAY_SIZE(phy->nrssi_lt) - 1);
1204         noise[2] = clamp_val(noise[2], 0, ARRAY_SIZE(phy->nrssi_lt) - 1);
1205         noise[3] = clamp_val(noise[3], 0, ARRAY_SIZE(phy->nrssi_lt) - 1);
1206         dev->noisecalc.samples[i][0] = phy->nrssi_lt[noise[0]];
1207         dev->noisecalc.samples[i][1] = phy->nrssi_lt[noise[1]];
1208         dev->noisecalc.samples[i][2] = phy->nrssi_lt[noise[2]];
1209         dev->noisecalc.samples[i][3] = phy->nrssi_lt[noise[3]];
1210         dev->noisecalc.nr_samples++;
1211         if (dev->noisecalc.nr_samples == 8) {
1212                 /* Calculate the Link Quality by the noise samples. */
1213                 average = 0;
1214                 for (i = 0; i < 8; i++) {
1215                         for (j = 0; j < 4; j++)
1216                                 average += dev->noisecalc.samples[i][j];
1217                 }
1218                 average /= (8 * 4);
1219                 average *= 125;
1220                 average += 64;
1221                 average /= 128;
1222                 tmp = b43_shm_read16(dev, B43_SHM_SHARED, 0x40C);
1223                 tmp = (tmp / 128) & 0x1F;
1224                 if (tmp >= 8)
1225                         average += 2;
1226                 else
1227                         average -= 25;
1228                 if (tmp == 8)
1229                         average -= 72;
1230                 else
1231                         average -= 48;
1232
1233                 dev->stats.link_noise = average;
1234                 dev->noisecalc.calculation_running = 0;
1235                 return;
1236         }
1237 generate_new:
1238         b43_generate_noise_sample(dev);
1239 }
1240
1241 static void handle_irq_tbtt_indication(struct b43_wldev *dev)
1242 {
1243         if (b43_is_mode(dev->wl, NL80211_IFTYPE_AP)) {
1244                 ///TODO: PS TBTT
1245         } else {
1246                 if (1 /*FIXME: the last PSpoll frame was sent successfully */ )
1247                         b43_power_saving_ctl_bits(dev, 0);
1248         }
1249         if (b43_is_mode(dev->wl, NL80211_IFTYPE_ADHOC))
1250                 dev->dfq_valid = 1;
1251 }
1252
1253 static void handle_irq_atim_end(struct b43_wldev *dev)
1254 {
1255         if (dev->dfq_valid) {
1256                 b43_write32(dev, B43_MMIO_MACCMD,
1257                             b43_read32(dev, B43_MMIO_MACCMD)
1258                             | B43_MACCMD_DFQ_VALID);
1259                 dev->dfq_valid = 0;
1260         }
1261 }
1262
1263 static void handle_irq_pmq(struct b43_wldev *dev)
1264 {
1265         u32 tmp;
1266
1267         //TODO: AP mode.
1268
1269         while (1) {
1270                 tmp = b43_read32(dev, B43_MMIO_PS_STATUS);
1271                 if (!(tmp & 0x00000008))
1272                         break;
1273         }
1274         /* 16bit write is odd, but correct. */
1275         b43_write16(dev, B43_MMIO_PS_STATUS, 0x0002);
1276 }
1277
1278 static void b43_write_template_common(struct b43_wldev *dev,
1279                                       const u8 * data, u16 size,
1280                                       u16 ram_offset,
1281                                       u16 shm_size_offset, u8 rate)
1282 {
1283         u32 i, tmp;
1284         struct b43_plcp_hdr4 plcp;
1285
1286         plcp.data = 0;
1287         b43_generate_plcp_hdr(&plcp, size + FCS_LEN, rate);
1288         b43_ram_write(dev, ram_offset, le32_to_cpu(plcp.data));
1289         ram_offset += sizeof(u32);
1290         /* The PLCP is 6 bytes long, but we only wrote 4 bytes, yet.
1291          * So leave the first two bytes of the next write blank.
1292          */
1293         tmp = (u32) (data[0]) << 16;
1294         tmp |= (u32) (data[1]) << 24;
1295         b43_ram_write(dev, ram_offset, tmp);
1296         ram_offset += sizeof(u32);
1297         for (i = 2; i < size; i += sizeof(u32)) {
1298                 tmp = (u32) (data[i + 0]);
1299                 if (i + 1 < size)
1300                         tmp |= (u32) (data[i + 1]) << 8;
1301                 if (i + 2 < size)
1302                         tmp |= (u32) (data[i + 2]) << 16;
1303                 if (i + 3 < size)
1304                         tmp |= (u32) (data[i + 3]) << 24;
1305                 b43_ram_write(dev, ram_offset + i - 2, tmp);
1306         }
1307         b43_shm_write16(dev, B43_SHM_SHARED, shm_size_offset,
1308                         size + sizeof(struct b43_plcp_hdr6));
1309 }
1310
1311 /* Check if the use of the antenna that ieee80211 told us to
1312  * use is possible. This will fall back to DEFAULT.
1313  * "antenna_nr" is the antenna identifier we got from ieee80211. */
1314 u8 b43_ieee80211_antenna_sanitize(struct b43_wldev *dev,
1315                                   u8 antenna_nr)
1316 {
1317         u8 antenna_mask;
1318
1319         if (antenna_nr == 0) {
1320                 /* Zero means "use default antenna". That's always OK. */
1321                 return 0;
1322         }
1323
1324         /* Get the mask of available antennas. */
1325         if (dev->phy.gmode)
1326                 antenna_mask = dev->dev->bus->sprom.ant_available_bg;
1327         else
1328                 antenna_mask = dev->dev->bus->sprom.ant_available_a;
1329
1330         if (!(antenna_mask & (1 << (antenna_nr - 1)))) {
1331                 /* This antenna is not available. Fall back to default. */
1332                 return 0;
1333         }
1334
1335         return antenna_nr;
1336 }
1337
1338 /* Convert a b43 antenna number value to the PHY TX control value. */
1339 static u16 b43_antenna_to_phyctl(int antenna)
1340 {
1341         switch (antenna) {
1342         case B43_ANTENNA0:
1343                 return B43_TXH_PHY_ANT0;
1344         case B43_ANTENNA1:
1345                 return B43_TXH_PHY_ANT1;
1346         case B43_ANTENNA2:
1347                 return B43_TXH_PHY_ANT2;
1348         case B43_ANTENNA3:
1349                 return B43_TXH_PHY_ANT3;
1350         case B43_ANTENNA_AUTO:
1351                 return B43_TXH_PHY_ANT01AUTO;
1352         }
1353         B43_WARN_ON(1);
1354         return 0;
1355 }
1356
1357 static void b43_write_beacon_template(struct b43_wldev *dev,
1358                                       u16 ram_offset,
1359                                       u16 shm_size_offset)
1360 {
1361         unsigned int i, len, variable_len;
1362         const struct ieee80211_mgmt *bcn;
1363         const u8 *ie;
1364         bool tim_found = 0;
1365         unsigned int rate;
1366         u16 ctl;
1367         int antenna;
1368         struct ieee80211_tx_info *info = IEEE80211_SKB_CB(dev->wl->current_beacon);
1369
1370         bcn = (const struct ieee80211_mgmt *)(dev->wl->current_beacon->data);
1371         len = min((size_t) dev->wl->current_beacon->len,
1372                   0x200 - sizeof(struct b43_plcp_hdr6));
1373         rate = ieee80211_get_tx_rate(dev->wl->hw, info)->hw_value;
1374
1375         b43_write_template_common(dev, (const u8 *)bcn,
1376                                   len, ram_offset, shm_size_offset, rate);
1377
1378         /* Write the PHY TX control parameters. */
1379         antenna = B43_ANTENNA_DEFAULT;
1380         antenna = b43_antenna_to_phyctl(antenna);
1381         ctl = b43_shm_read16(dev, B43_SHM_SHARED, B43_SHM_SH_BEACPHYCTL);
1382         /* We can't send beacons with short preamble. Would get PHY errors. */
1383         ctl &= ~B43_TXH_PHY_SHORTPRMBL;
1384         ctl &= ~B43_TXH_PHY_ANT;
1385         ctl &= ~B43_TXH_PHY_ENC;
1386         ctl |= antenna;
1387         if (b43_is_cck_rate(rate))
1388                 ctl |= B43_TXH_PHY_ENC_CCK;
1389         else
1390                 ctl |= B43_TXH_PHY_ENC_OFDM;
1391         b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_BEACPHYCTL, ctl);
1392
1393         /* Find the position of the TIM and the DTIM_period value
1394          * and write them to SHM. */
1395         ie = bcn->u.beacon.variable;
1396         variable_len = len - offsetof(struct ieee80211_mgmt, u.beacon.variable);
1397         for (i = 0; i < variable_len - 2; ) {
1398                 uint8_t ie_id, ie_len;
1399
1400                 ie_id = ie[i];
1401                 ie_len = ie[i + 1];
1402                 if (ie_id == 5) {
1403                         u16 tim_position;
1404                         u16 dtim_period;
1405                         /* This is the TIM Information Element */
1406
1407                         /* Check whether the ie_len is in the beacon data range. */
1408                         if (variable_len < ie_len + 2 + i)
1409                                 break;
1410                         /* A valid TIM is at least 4 bytes long. */
1411                         if (ie_len < 4)
1412                                 break;
1413                         tim_found = 1;
1414
1415                         tim_position = sizeof(struct b43_plcp_hdr6);
1416                         tim_position += offsetof(struct ieee80211_mgmt, u.beacon.variable);
1417                         tim_position += i;
1418
1419                         dtim_period = ie[i + 3];
1420
1421                         b43_shm_write16(dev, B43_SHM_SHARED,
1422                                         B43_SHM_SH_TIMBPOS, tim_position);
1423                         b43_shm_write16(dev, B43_SHM_SHARED,
1424                                         B43_SHM_SH_DTIMPER, dtim_period);
1425                         break;
1426                 }
1427                 i += ie_len + 2;
1428         }
1429         if (!tim_found) {
1430                 /*
1431                  * If ucode wants to modify TIM do it behind the beacon, this
1432                  * will happen, for example, when doing mesh networking.
1433                  */
1434                 b43_shm_write16(dev, B43_SHM_SHARED,
1435                                 B43_SHM_SH_TIMBPOS,
1436                                 len + sizeof(struct b43_plcp_hdr6));
1437                 b43_shm_write16(dev, B43_SHM_SHARED,
1438                                 B43_SHM_SH_DTIMPER, 0);
1439         }
1440         b43dbg(dev->wl, "Updated beacon template at 0x%x\n", ram_offset);
1441 }
1442
1443 static void b43_write_probe_resp_plcp(struct b43_wldev *dev,
1444                                       u16 shm_offset, u16 size,
1445                                       struct ieee80211_rate *rate)
1446 {
1447         struct b43_plcp_hdr4 plcp;
1448         u32 tmp;
1449         __le16 dur;
1450
1451         plcp.data = 0;
1452         b43_generate_plcp_hdr(&plcp, size + FCS_LEN, rate->hw_value);
1453         dur = ieee80211_generic_frame_duration(dev->wl->hw,
1454                                                dev->wl->vif, size,
1455                                                rate);
1456         /* Write PLCP in two parts and timing for packet transfer */
1457         tmp = le32_to_cpu(plcp.data);
1458         b43_shm_write16(dev, B43_SHM_SHARED, shm_offset, tmp & 0xFFFF);
1459         b43_shm_write16(dev, B43_SHM_SHARED, shm_offset + 2, tmp >> 16);
1460         b43_shm_write16(dev, B43_SHM_SHARED, shm_offset + 6, le16_to_cpu(dur));
1461 }
1462
1463 /* Instead of using custom probe response template, this function
1464  * just patches custom beacon template by:
1465  * 1) Changing packet type
1466  * 2) Patching duration field
1467  * 3) Stripping TIM
1468  */
1469 static const u8 * b43_generate_probe_resp(struct b43_wldev *dev,
1470                                           u16 *dest_size,
1471                                           struct ieee80211_rate *rate)
1472 {
1473         const u8 *src_data;
1474         u8 *dest_data;
1475         u16 src_size, elem_size, src_pos, dest_pos;
1476         __le16 dur;
1477         struct ieee80211_hdr *hdr;
1478         size_t ie_start;
1479
1480         src_size = dev->wl->current_beacon->len;
1481         src_data = (const u8 *)dev->wl->current_beacon->data;
1482
1483         /* Get the start offset of the variable IEs in the packet. */
1484         ie_start = offsetof(struct ieee80211_mgmt, u.probe_resp.variable);
1485         B43_WARN_ON(ie_start != offsetof(struct ieee80211_mgmt, u.beacon.variable));
1486
1487         if (B43_WARN_ON(src_size < ie_start))
1488                 return NULL;
1489
1490         dest_data = kmalloc(src_size, GFP_ATOMIC);
1491         if (unlikely(!dest_data))
1492                 return NULL;
1493
1494         /* Copy the static data and all Information Elements, except the TIM. */
1495         memcpy(dest_data, src_data, ie_start);
1496         src_pos = ie_start;
1497         dest_pos = ie_start;
1498         for ( ; src_pos < src_size - 2; src_pos += elem_size) {
1499                 elem_size = src_data[src_pos + 1] + 2;
1500                 if (src_data[src_pos] == 5) {
1501                         /* This is the TIM. */
1502                         continue;
1503                 }
1504                 memcpy(dest_data + dest_pos, src_data + src_pos,
1505                        elem_size);
1506                 dest_pos += elem_size;
1507         }
1508         *dest_size = dest_pos;
1509         hdr = (struct ieee80211_hdr *)dest_data;
1510
1511         /* Set the frame control. */
1512         hdr->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT |
1513                                          IEEE80211_STYPE_PROBE_RESP);
1514         dur = ieee80211_generic_frame_duration(dev->wl->hw,
1515                                                dev->wl->vif, *dest_size,
1516                                                rate);
1517         hdr->duration_id = dur;
1518
1519         return dest_data;
1520 }
1521
1522 static void b43_write_probe_resp_template(struct b43_wldev *dev,
1523                                           u16 ram_offset,
1524                                           u16 shm_size_offset,
1525                                           struct ieee80211_rate *rate)
1526 {
1527         const u8 *probe_resp_data;
1528         u16 size;
1529
1530         size = dev->wl->current_beacon->len;
1531         probe_resp_data = b43_generate_probe_resp(dev, &size, rate);
1532         if (unlikely(!probe_resp_data))
1533                 return;
1534
1535         /* Looks like PLCP headers plus packet timings are stored for
1536          * all possible basic rates
1537          */
1538         b43_write_probe_resp_plcp(dev, 0x31A, size, &b43_b_ratetable[0]);
1539         b43_write_probe_resp_plcp(dev, 0x32C, size, &b43_b_ratetable[1]);
1540         b43_write_probe_resp_plcp(dev, 0x33E, size, &b43_b_ratetable[2]);
1541         b43_write_probe_resp_plcp(dev, 0x350, size, &b43_b_ratetable[3]);
1542
1543         size = min((size_t) size, 0x200 - sizeof(struct b43_plcp_hdr6));
1544         b43_write_template_common(dev, probe_resp_data,
1545                                   size, ram_offset, shm_size_offset,
1546                                   rate->hw_value);
1547         kfree(probe_resp_data);
1548 }
1549
1550 static void b43_upload_beacon0(struct b43_wldev *dev)
1551 {
1552         struct b43_wl *wl = dev->wl;
1553
1554         if (wl->beacon0_uploaded)
1555                 return;
1556         b43_write_beacon_template(dev, 0x68, 0x18);
1557         /* FIXME: Probe resp upload doesn't really belong here,
1558          *        but we don't use that feature anyway. */
1559         b43_write_probe_resp_template(dev, 0x268, 0x4A,
1560                                       &__b43_ratetable[3]);
1561         wl->beacon0_uploaded = 1;
1562 }
1563
1564 static void b43_upload_beacon1(struct b43_wldev *dev)
1565 {
1566         struct b43_wl *wl = dev->wl;
1567
1568         if (wl->beacon1_uploaded)
1569                 return;
1570         b43_write_beacon_template(dev, 0x468, 0x1A);
1571         wl->beacon1_uploaded = 1;
1572 }
1573
1574 static void handle_irq_beacon(struct b43_wldev *dev)
1575 {
1576         struct b43_wl *wl = dev->wl;
1577         u32 cmd, beacon0_valid, beacon1_valid;
1578
1579         if (!b43_is_mode(wl, NL80211_IFTYPE_AP) &&
1580             !b43_is_mode(wl, NL80211_IFTYPE_MESH_POINT))
1581                 return;
1582
1583         /* This is the bottom half of the asynchronous beacon update. */
1584
1585         /* Ignore interrupt in the future. */
1586         dev->irq_savedstate &= ~B43_IRQ_BEACON;
1587
1588         cmd = b43_read32(dev, B43_MMIO_MACCMD);
1589         beacon0_valid = (cmd & B43_MACCMD_BEACON0_VALID);
1590         beacon1_valid = (cmd & B43_MACCMD_BEACON1_VALID);
1591
1592         /* Schedule interrupt manually, if busy. */
1593         if (beacon0_valid && beacon1_valid) {
1594                 b43_write32(dev, B43_MMIO_GEN_IRQ_REASON, B43_IRQ_BEACON);
1595                 dev->irq_savedstate |= B43_IRQ_BEACON;
1596                 return;
1597         }
1598
1599         if (unlikely(wl->beacon_templates_virgin)) {
1600                 /* We never uploaded a beacon before.
1601                  * Upload both templates now, but only mark one valid. */
1602                 wl->beacon_templates_virgin = 0;
1603                 b43_upload_beacon0(dev);
1604                 b43_upload_beacon1(dev);
1605                 cmd = b43_read32(dev, B43_MMIO_MACCMD);
1606                 cmd |= B43_MACCMD_BEACON0_VALID;
1607                 b43_write32(dev, B43_MMIO_MACCMD, cmd);
1608         } else {
1609                 if (!beacon0_valid) {
1610                         b43_upload_beacon0(dev);
1611                         cmd = b43_read32(dev, B43_MMIO_MACCMD);
1612                         cmd |= B43_MACCMD_BEACON0_VALID;
1613                         b43_write32(dev, B43_MMIO_MACCMD, cmd);
1614                 } else if (!beacon1_valid) {
1615                         b43_upload_beacon1(dev);
1616                         cmd = b43_read32(dev, B43_MMIO_MACCMD);
1617                         cmd |= B43_MACCMD_BEACON1_VALID;
1618                         b43_write32(dev, B43_MMIO_MACCMD, cmd);
1619                 }
1620         }
1621 }
1622
1623 static void b43_beacon_update_trigger_work(struct work_struct *work)
1624 {
1625         struct b43_wl *wl = container_of(work, struct b43_wl,
1626                                          beacon_update_trigger);
1627         struct b43_wldev *dev;
1628
1629         mutex_lock(&wl->mutex);
1630         dev = wl->current_dev;
1631         if (likely(dev && (b43_status(dev) >= B43_STAT_INITIALIZED))) {
1632                 spin_lock_irq(&wl->irq_lock);
1633                 /* update beacon right away or defer to irq */
1634                 dev->irq_savedstate = b43_read32(dev, B43_MMIO_GEN_IRQ_MASK);
1635                 handle_irq_beacon(dev);
1636                 /* The handler might have updated the IRQ mask. */
1637                 b43_write32(dev, B43_MMIO_GEN_IRQ_MASK,
1638                             dev->irq_savedstate);
1639                 mmiowb();
1640                 spin_unlock_irq(&wl->irq_lock);
1641         }
1642         mutex_unlock(&wl->mutex);
1643 }
1644
1645 /* Asynchronously update the packet templates in template RAM.
1646  * Locking: Requires wl->irq_lock to be locked. */
1647 static void b43_update_templates(struct b43_wl *wl)
1648 {
1649         struct sk_buff *beacon;
1650
1651         /* This is the top half of the ansynchronous beacon update.
1652          * The bottom half is the beacon IRQ.
1653          * Beacon update must be asynchronous to avoid sending an
1654          * invalid beacon. This can happen for example, if the firmware
1655          * transmits a beacon while we are updating it. */
1656
1657         /* We could modify the existing beacon and set the aid bit in
1658          * the TIM field, but that would probably require resizing and
1659          * moving of data within the beacon template.
1660          * Simply request a new beacon and let mac80211 do the hard work. */
1661         beacon = ieee80211_beacon_get(wl->hw, wl->vif);
1662         if (unlikely(!beacon))
1663                 return;
1664
1665         if (wl->current_beacon)
1666                 dev_kfree_skb_any(wl->current_beacon);
1667         wl->current_beacon = beacon;
1668         wl->beacon0_uploaded = 0;
1669         wl->beacon1_uploaded = 0;
1670         queue_work(wl->hw->workqueue, &wl->beacon_update_trigger);
1671 }
1672
1673 static void b43_set_beacon_int(struct b43_wldev *dev, u16 beacon_int)
1674 {
1675         b43_time_lock(dev);
1676         if (dev->dev->id.revision >= 3) {
1677                 b43_write32(dev, B43_MMIO_TSF_CFP_REP, (beacon_int << 16));
1678                 b43_write32(dev, B43_MMIO_TSF_CFP_START, (beacon_int << 10));
1679         } else {
1680                 b43_write16(dev, 0x606, (beacon_int >> 6));
1681                 b43_write16(dev, 0x610, beacon_int);
1682         }
1683         b43_time_unlock(dev);
1684         b43dbg(dev->wl, "Set beacon interval to %u\n", beacon_int);
1685 }
1686
1687 static void b43_handle_firmware_panic(struct b43_wldev *dev)
1688 {
1689         u16 reason;
1690
1691         /* Read the register that contains the reason code for the panic. */
1692         reason = b43_shm_read16(dev, B43_SHM_SCRATCH, B43_FWPANIC_REASON_REG);
1693         b43err(dev->wl, "Whoopsy, firmware panic! Reason: %u\n", reason);
1694
1695         switch (reason) {
1696         default:
1697                 b43dbg(dev->wl, "The panic reason is unknown.\n");
1698                 /* fallthrough */
1699         case B43_FWPANIC_DIE:
1700                 /* Do not restart the controller or firmware.
1701                  * The device is nonfunctional from now on.
1702                  * Restarting would result in this panic to trigger again,
1703                  * so we avoid that recursion. */
1704                 break;
1705         case B43_FWPANIC_RESTART:
1706                 b43_controller_restart(dev, "Microcode panic");
1707                 break;
1708         }
1709 }
1710
1711 static void handle_irq_ucode_debug(struct b43_wldev *dev)
1712 {
1713         unsigned int i, cnt;
1714         u16 reason, marker_id, marker_line;
1715         __le16 *buf;
1716
1717         /* The proprietary firmware doesn't have this IRQ. */
1718         if (!dev->fw.opensource)
1719                 return;
1720
1721         /* Read the register that contains the reason code for this IRQ. */
1722         reason = b43_shm_read16(dev, B43_SHM_SCRATCH, B43_DEBUGIRQ_REASON_REG);
1723
1724         switch (reason) {
1725         case B43_DEBUGIRQ_PANIC:
1726                 b43_handle_firmware_panic(dev);
1727                 break;
1728         case B43_DEBUGIRQ_DUMP_SHM:
1729                 if (!B43_DEBUG)
1730                         break; /* Only with driver debugging enabled. */
1731                 buf = kmalloc(4096, GFP_ATOMIC);
1732                 if (!buf) {
1733                         b43dbg(dev->wl, "SHM-dump: Failed to allocate memory\n");
1734                         goto out;
1735                 }
1736                 for (i = 0; i < 4096; i += 2) {
1737                         u16 tmp = b43_shm_read16(dev, B43_SHM_SHARED, i);
1738                         buf[i / 2] = cpu_to_le16(tmp);
1739                 }
1740                 b43info(dev->wl, "Shared memory dump:\n");
1741                 print_hex_dump(KERN_INFO, "", DUMP_PREFIX_OFFSET,
1742                                16, 2, buf, 4096, 1);
1743                 kfree(buf);
1744                 break;
1745         case B43_DEBUGIRQ_DUMP_REGS:
1746                 if (!B43_DEBUG)
1747                         break; /* Only with driver debugging enabled. */
1748                 b43info(dev->wl, "Microcode register dump:\n");
1749                 for (i = 0, cnt = 0; i < 64; i++) {
1750                         u16 tmp = b43_shm_read16(dev, B43_SHM_SCRATCH, i);
1751                         if (cnt == 0)
1752                                 printk(KERN_INFO);
1753                         printk("r%02u: 0x%04X  ", i, tmp);
1754                         cnt++;
1755                         if (cnt == 6) {
1756                                 printk("\n");
1757                                 cnt = 0;
1758                         }
1759                 }
1760                 printk("\n");
1761                 break;
1762         case B43_DEBUGIRQ_MARKER:
1763                 if (!B43_DEBUG)
1764                         break; /* Only with driver debugging enabled. */
1765                 marker_id = b43_shm_read16(dev, B43_SHM_SCRATCH,
1766                                            B43_MARKER_ID_REG);
1767                 marker_line = b43_shm_read16(dev, B43_SHM_SCRATCH,
1768                                              B43_MARKER_LINE_REG);
1769                 b43info(dev->wl, "The firmware just executed the MARKER(%u) "
1770                         "at line number %u\n",
1771                         marker_id, marker_line);
1772                 break;
1773         default:
1774                 b43dbg(dev->wl, "Debug-IRQ triggered for unknown reason: %u\n",
1775                        reason);
1776         }
1777 out:
1778         /* Acknowledge the debug-IRQ, so the firmware can continue. */
1779         b43_shm_write16(dev, B43_SHM_SCRATCH,
1780                         B43_DEBUGIRQ_REASON_REG, B43_DEBUGIRQ_ACK);
1781 }
1782
1783 /* Interrupt handler bottom-half */
1784 static void b43_interrupt_tasklet(struct b43_wldev *dev)
1785 {
1786         u32 reason;
1787         u32 dma_reason[ARRAY_SIZE(dev->dma_reason)];
1788         u32 merged_dma_reason = 0;
1789         int i;
1790         unsigned long flags;
1791
1792         spin_lock_irqsave(&dev->wl->irq_lock, flags);
1793
1794         B43_WARN_ON(b43_status(dev) != B43_STAT_STARTED);
1795
1796         reason = dev->irq_reason;
1797         for (i = 0; i < ARRAY_SIZE(dma_reason); i++) {
1798                 dma_reason[i] = dev->dma_reason[i];
1799                 merged_dma_reason |= dma_reason[i];
1800         }
1801
1802         if (unlikely(reason & B43_IRQ_MAC_TXERR))
1803                 b43err(dev->wl, "MAC transmission error\n");
1804
1805         if (unlikely(reason & B43_IRQ_PHY_TXERR)) {
1806                 b43err(dev->wl, "PHY transmission error\n");
1807                 rmb();
1808                 if (unlikely(atomic_dec_and_test(&dev->phy.txerr_cnt))) {
1809                         atomic_set(&dev->phy.txerr_cnt,
1810                                    B43_PHY_TX_BADNESS_LIMIT);
1811                         b43err(dev->wl, "Too many PHY TX errors, "
1812                                         "restarting the controller\n");
1813                         b43_controller_restart(dev, "PHY TX errors");
1814                 }
1815         }
1816
1817         if (unlikely(merged_dma_reason & (B43_DMAIRQ_FATALMASK |
1818                                           B43_DMAIRQ_NONFATALMASK))) {
1819                 if (merged_dma_reason & B43_DMAIRQ_FATALMASK) {
1820                         b43err(dev->wl, "Fatal DMA error: "
1821                                "0x%08X, 0x%08X, 0x%08X, "
1822                                "0x%08X, 0x%08X, 0x%08X\n",
1823                                dma_reason[0], dma_reason[1],
1824                                dma_reason[2], dma_reason[3],
1825                                dma_reason[4], dma_reason[5]);
1826                         b43_controller_restart(dev, "DMA error");
1827                         mmiowb();
1828                         spin_unlock_irqrestore(&dev->wl->irq_lock, flags);
1829                         return;
1830                 }
1831                 if (merged_dma_reason & B43_DMAIRQ_NONFATALMASK) {
1832                         b43err(dev->wl, "DMA error: "
1833                                "0x%08X, 0x%08X, 0x%08X, "
1834                                "0x%08X, 0x%08X, 0x%08X\n",
1835                                dma_reason[0], dma_reason[1],
1836                                dma_reason[2], dma_reason[3],
1837                                dma_reason[4], dma_reason[5]);
1838                 }
1839         }
1840
1841         if (unlikely(reason & B43_IRQ_UCODE_DEBUG))
1842                 handle_irq_ucode_debug(dev);
1843         if (reason & B43_IRQ_TBTT_INDI)
1844                 handle_irq_tbtt_indication(dev);
1845         if (reason & B43_IRQ_ATIM_END)
1846                 handle_irq_atim_end(dev);
1847         if (reason & B43_IRQ_BEACON)
1848                 handle_irq_beacon(dev);
1849         if (reason & B43_IRQ_PMQ)
1850                 handle_irq_pmq(dev);
1851         if (reason & B43_IRQ_TXFIFO_FLUSH_OK)
1852                 ;/* TODO */
1853         if (reason & B43_IRQ_NOISESAMPLE_OK)
1854                 handle_irq_noise(dev);
1855
1856         /* Check the DMA reason registers for received data. */
1857         if (dma_reason[0] & B43_DMAIRQ_RX_DONE) {
1858                 if (b43_using_pio_transfers(dev))
1859                         b43_pio_rx(dev->pio.rx_queue);
1860                 else
1861                         b43_dma_rx(dev->dma.rx_ring);
1862         }
1863         B43_WARN_ON(dma_reason[1] & B43_DMAIRQ_RX_DONE);
1864         B43_WARN_ON(dma_reason[2] & B43_DMAIRQ_RX_DONE);
1865         B43_WARN_ON(dma_reason[3] & B43_DMAIRQ_RX_DONE);
1866         B43_WARN_ON(dma_reason[4] & B43_DMAIRQ_RX_DONE);
1867         B43_WARN_ON(dma_reason[5] & B43_DMAIRQ_RX_DONE);
1868
1869         if (reason & B43_IRQ_TX_OK)
1870                 handle_irq_transmit_status(dev);
1871
1872         b43_interrupt_enable(dev, dev->irq_savedstate);
1873         mmiowb();
1874         spin_unlock_irqrestore(&dev->wl->irq_lock, flags);
1875 }
1876
1877 static void b43_interrupt_ack(struct b43_wldev *dev, u32 reason)
1878 {
1879         b43_write32(dev, B43_MMIO_GEN_IRQ_REASON, reason);
1880
1881         b43_write32(dev, B43_MMIO_DMA0_REASON, dev->dma_reason[0]);
1882         b43_write32(dev, B43_MMIO_DMA1_REASON, dev->dma_reason[1]);
1883         b43_write32(dev, B43_MMIO_DMA2_REASON, dev->dma_reason[2]);
1884         b43_write32(dev, B43_MMIO_DMA3_REASON, dev->dma_reason[3]);
1885         b43_write32(dev, B43_MMIO_DMA4_REASON, dev->dma_reason[4]);
1886         b43_write32(dev, B43_MMIO_DMA5_REASON, dev->dma_reason[5]);
1887 }
1888
1889 /* Interrupt handler top-half */
1890 static irqreturn_t b43_interrupt_handler(int irq, void *dev_id)
1891 {
1892         irqreturn_t ret = IRQ_NONE;
1893         struct b43_wldev *dev = dev_id;
1894         u32 reason;
1895
1896         if (!dev)
1897                 return IRQ_NONE;
1898
1899         spin_lock(&dev->wl->irq_lock);
1900
1901         if (b43_status(dev) < B43_STAT_STARTED)
1902                 goto out;
1903         reason = b43_read32(dev, B43_MMIO_GEN_IRQ_REASON);
1904         if (reason == 0xffffffff)       /* shared IRQ */
1905                 goto out;
1906         ret = IRQ_HANDLED;
1907         reason &= b43_read32(dev, B43_MMIO_GEN_IRQ_MASK);
1908         if (!reason)
1909                 goto out;
1910
1911         dev->dma_reason[0] = b43_read32(dev, B43_MMIO_DMA0_REASON)
1912             & 0x0001DC00;
1913         dev->dma_reason[1] = b43_read32(dev, B43_MMIO_DMA1_REASON)
1914             & 0x0000DC00;
1915         dev->dma_reason[2] = b43_read32(dev, B43_MMIO_DMA2_REASON)
1916             & 0x0000DC00;
1917         dev->dma_reason[3] = b43_read32(dev, B43_MMIO_DMA3_REASON)
1918             & 0x0001DC00;
1919         dev->dma_reason[4] = b43_read32(dev, B43_MMIO_DMA4_REASON)
1920             & 0x0000DC00;
1921         dev->dma_reason[5] = b43_read32(dev, B43_MMIO_DMA5_REASON)
1922             & 0x0000DC00;
1923
1924         b43_interrupt_ack(dev, reason);
1925         /* disable all IRQs. They are enabled again in the bottom half. */
1926         dev->irq_savedstate = b43_interrupt_disable(dev, B43_IRQ_ALL);
1927         /* save the reason code and call our bottom half. */
1928         dev->irq_reason = reason;
1929         tasklet_schedule(&dev->isr_tasklet);
1930       out:
1931         mmiowb();
1932         spin_unlock(&dev->wl->irq_lock);
1933
1934         return ret;
1935 }
1936
1937 void b43_do_release_fw(struct b43_firmware_file *fw)
1938 {
1939         release_firmware(fw->data);
1940         fw->data = NULL;
1941         fw->filename = NULL;
1942 }
1943
1944 static void b43_release_firmware(struct b43_wldev *dev)
1945 {
1946         b43_do_release_fw(&dev->fw.ucode);
1947         b43_do_release_fw(&dev->fw.pcm);
1948         b43_do_release_fw(&dev->fw.initvals);
1949         b43_do_release_fw(&dev->fw.initvals_band);
1950 }
1951
1952 static void b43_print_fw_helptext(struct b43_wl *wl, bool error)
1953 {
1954         const char *text;
1955
1956         text = "You must go to "
1957                "http://wireless.kernel.org/en/users/Drivers/b43#devicefirmware "
1958                "and download the correct firmware for this driver version. "
1959                "Please carefully read all instructions on this website.\n";
1960         if (error)
1961                 b43err(wl, text);
1962         else
1963                 b43warn(wl, text);
1964 }
1965
1966 int b43_do_request_fw(struct b43_request_fw_context *ctx,
1967                       const char *name,
1968                       struct b43_firmware_file *fw)
1969 {
1970         const struct firmware *blob;
1971         struct b43_fw_header *hdr;
1972         u32 size;
1973         int err;
1974
1975         if (!name) {
1976                 /* Don't fetch anything. Free possibly cached firmware. */
1977                 /* FIXME: We should probably keep it anyway, to save some headache
1978                  * on suspend/resume with multiband devices. */
1979                 b43_do_release_fw(fw);
1980                 return 0;
1981         }
1982         if (fw->filename) {
1983                 if ((fw->type == ctx->req_type) &&
1984                     (strcmp(fw->filename, name) == 0))
1985                         return 0; /* Already have this fw. */
1986                 /* Free the cached firmware first. */
1987                 /* FIXME: We should probably do this later after we successfully
1988                  * got the new fw. This could reduce headache with multiband devices.
1989                  * We could also redesign this to cache the firmware for all possible
1990                  * bands all the time. */
1991                 b43_do_release_fw(fw);
1992         }
1993
1994         switch (ctx->req_type) {
1995         case B43_FWTYPE_PROPRIETARY:
1996                 snprintf(ctx->fwname, sizeof(ctx->fwname),
1997                          "b43%s/%s.fw",
1998                          modparam_fwpostfix, name);
1999                 break;
2000         case B43_FWTYPE_OPENSOURCE:
2001                 snprintf(ctx->fwname, sizeof(ctx->fwname),
2002                          "b43-open%s/%s.fw",
2003                          modparam_fwpostfix, name);
2004                 break;
2005         default:
2006                 B43_WARN_ON(1);
2007                 return -ENOSYS;
2008         }
2009         err = request_firmware(&blob, ctx->fwname, ctx->dev->dev->dev);
2010         if (err == -ENOENT) {
2011                 snprintf(ctx->errors[ctx->req_type],
2012                          sizeof(ctx->errors[ctx->req_type]),
2013                          "Firmware file \"%s\" not found\n", ctx->fwname);
2014                 return err;
2015         } else if (err) {
2016                 snprintf(ctx->errors[ctx->req_type],
2017                          sizeof(ctx->errors[ctx->req_type]),
2018                          "Firmware file \"%s\" request failed (err=%d)\n",
2019                          ctx->fwname, err);
2020                 return err;
2021         }
2022         if (blob->size < sizeof(struct b43_fw_header))
2023                 goto err_format;
2024         hdr = (struct b43_fw_header *)(blob->data);
2025         switch (hdr->type) {
2026         case B43_FW_TYPE_UCODE:
2027         case B43_FW_TYPE_PCM:
2028                 size = be32_to_cpu(hdr->size);
2029                 if (size != blob->size - sizeof(struct b43_fw_header))
2030                         goto err_format;
2031                 /* fallthrough */
2032         case B43_FW_TYPE_IV:
2033                 if (hdr->ver != 1)
2034                         goto err_format;
2035                 break;
2036         default:
2037                 goto err_format;
2038         }
2039
2040         fw->data = blob;
2041         fw->filename = name;
2042         fw->type = ctx->req_type;
2043
2044         return 0;
2045
2046 err_format:
2047         snprintf(ctx->errors[ctx->req_type],
2048                  sizeof(ctx->errors[ctx->req_type]),
2049                  "Firmware file \"%s\" format error.\n", ctx->fwname);
2050         release_firmware(blob);
2051
2052         return -EPROTO;
2053 }
2054
2055 static int b43_try_request_fw(struct b43_request_fw_context *ctx)
2056 {
2057         struct b43_wldev *dev = ctx->dev;
2058         struct b43_firmware *fw = &ctx->dev->fw;
2059         const u8 rev = ctx->dev->dev->id.revision;
2060         const char *filename;
2061         u32 tmshigh;
2062         int err;
2063
2064         /* Get microcode */
2065         tmshigh = ssb_read32(dev->dev, SSB_TMSHIGH);
2066         if ((rev >= 5) && (rev <= 10))
2067                 filename = "ucode5";
2068         else if ((rev >= 11) && (rev <= 12))
2069                 filename = "ucode11";
2070         else if (rev >= 13)
2071                 filename = "ucode13";
2072         else
2073                 goto err_no_ucode;
2074         err = b43_do_request_fw(ctx, filename, &fw->ucode);
2075         if (err)
2076                 goto err_load;
2077
2078         /* Get PCM code */
2079         if ((rev >= 5) && (rev <= 10))
2080                 filename = "pcm5";
2081         else if (rev >= 11)
2082                 filename = NULL;
2083         else
2084                 goto err_no_pcm;
2085         fw->pcm_request_failed = 0;
2086         err = b43_do_request_fw(ctx, filename, &fw->pcm);
2087         if (err == -ENOENT) {
2088                 /* We did not find a PCM file? Not fatal, but
2089                  * core rev <= 10 must do without hwcrypto then. */
2090                 fw->pcm_request_failed = 1;
2091         } else if (err)
2092                 goto err_load;
2093
2094         /* Get initvals */
2095         switch (dev->phy.type) {
2096         case B43_PHYTYPE_A:
2097                 if ((rev >= 5) && (rev <= 10)) {
2098                         if (tmshigh & B43_TMSHIGH_HAVE_2GHZ_PHY)
2099                                 filename = "a0g1initvals5";
2100                         else
2101                                 filename = "a0g0initvals5";
2102                 } else
2103                         goto err_no_initvals;
2104                 break;
2105         case B43_PHYTYPE_G:
2106                 if ((rev >= 5) && (rev <= 10))
2107                         filename = "b0g0initvals5";
2108                 else if (rev >= 13)
2109                         filename = "b0g0initvals13";
2110                 else
2111                         goto err_no_initvals;
2112                 break;
2113         case B43_PHYTYPE_N:
2114                 if ((rev >= 11) && (rev <= 12))
2115                         filename = "n0initvals11";
2116                 else
2117                         goto err_no_initvals;
2118                 break;
2119         default:
2120                 goto err_no_initvals;
2121         }
2122         err = b43_do_request_fw(ctx, filename, &fw->initvals);
2123         if (err)
2124                 goto err_load;
2125
2126         /* Get bandswitch initvals */
2127         switch (dev->phy.type) {
2128         case B43_PHYTYPE_A:
2129                 if ((rev >= 5) && (rev <= 10)) {
2130                         if (tmshigh & B43_TMSHIGH_HAVE_2GHZ_PHY)
2131                                 filename = "a0g1bsinitvals5";
2132                         else
2133                                 filename = "a0g0bsinitvals5";
2134                 } else if (rev >= 11)
2135                         filename = NULL;
2136                 else
2137                         goto err_no_initvals;
2138                 break;
2139         case B43_PHYTYPE_G:
2140                 if ((rev >= 5) && (rev <= 10))
2141                         filename = "b0g0bsinitvals5";
2142                 else if (rev >= 11)
2143                         filename = NULL;
2144                 else
2145                         goto err_no_initvals;
2146                 break;
2147         case B43_PHYTYPE_N:
2148                 if ((rev >= 11) && (rev <= 12))
2149                         filename = "n0bsinitvals11";
2150                 else
2151                         goto err_no_initvals;
2152                 break;
2153         default:
2154                 goto err_no_initvals;
2155         }
2156         err = b43_do_request_fw(ctx, filename, &fw->initvals_band);
2157         if (err)
2158                 goto err_load;
2159
2160         return 0;
2161
2162 err_no_ucode:
2163         err = ctx->fatal_failure = -EOPNOTSUPP;
2164         b43err(dev->wl, "The driver does not know which firmware (ucode) "
2165                "is required for your device (wl-core rev %u)\n", rev);
2166         goto error;
2167
2168 err_no_pcm:
2169         err = ctx->fatal_failure = -EOPNOTSUPP;
2170         b43err(dev->wl, "The driver does not know which firmware (PCM) "
2171                "is required for your device (wl-core rev %u)\n", rev);
2172         goto error;
2173
2174 err_no_initvals:
2175         err = ctx->fatal_failure = -EOPNOTSUPP;
2176         b43err(dev->wl, "The driver does not know which firmware (initvals) "
2177                "is required for your device (wl-core rev %u)\n", rev);
2178         goto error;
2179
2180 err_load:
2181         /* We failed to load this firmware image. The error message
2182          * already is in ctx->errors. Return and let our caller decide
2183          * what to do. */
2184         goto error;
2185
2186 error:
2187         b43_release_firmware(dev);
2188         return err;
2189 }
2190
2191 static int b43_request_firmware(struct b43_wldev *dev)
2192 {
2193         struct b43_request_fw_context *ctx;
2194         unsigned int i;
2195         int err;
2196         const char *errmsg;
2197
2198         ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
2199         if (!ctx)
2200                 return -ENOMEM;
2201         ctx->dev = dev;
2202
2203         ctx->req_type = B43_FWTYPE_PROPRIETARY;
2204         err = b43_try_request_fw(ctx);
2205         if (!err)
2206                 goto out; /* Successfully loaded it. */
2207         err = ctx->fatal_failure;
2208         if (err)
2209                 goto out;
2210
2211         ctx->req_type = B43_FWTYPE_OPENSOURCE;
2212         err = b43_try_request_fw(ctx);
2213         if (!err)
2214                 goto out; /* Successfully loaded it. */
2215         err = ctx->fatal_failure;
2216         if (err)
2217                 goto out;
2218
2219         /* Could not find a usable firmware. Print the errors. */
2220         for (i = 0; i < B43_NR_FWTYPES; i++) {
2221                 errmsg = ctx->errors[i];
2222                 if (strlen(errmsg))
2223                         b43err(dev->wl, errmsg);
2224         }
2225         b43_print_fw_helptext(dev->wl, 1);
2226         err = -ENOENT;
2227
2228 out:
2229         kfree(ctx);
2230         return err;
2231 }
2232
2233 static int b43_upload_microcode(struct b43_wldev *dev)
2234 {
2235         const size_t hdr_len = sizeof(struct b43_fw_header);
2236         const __be32 *data;
2237         unsigned int i, len;
2238         u16 fwrev, fwpatch, fwdate, fwtime;
2239         u32 tmp, macctl;
2240         int err = 0;
2241
2242         /* Jump the microcode PSM to offset 0 */
2243         macctl = b43_read32(dev, B43_MMIO_MACCTL);
2244         B43_WARN_ON(macctl & B43_MACCTL_PSM_RUN);
2245         macctl |= B43_MACCTL_PSM_JMP0;
2246         b43_write32(dev, B43_MMIO_MACCTL, macctl);
2247         /* Zero out all microcode PSM registers and shared memory. */
2248         for (i = 0; i < 64; i++)
2249                 b43_shm_write16(dev, B43_SHM_SCRATCH, i, 0);
2250         for (i = 0; i < 4096; i += 2)
2251                 b43_shm_write16(dev, B43_SHM_SHARED, i, 0);
2252
2253         /* Upload Microcode. */
2254         data = (__be32 *) (dev->fw.ucode.data->data + hdr_len);
2255         len = (dev->fw.ucode.data->size - hdr_len) / sizeof(__be32);
2256         b43_shm_control_word(dev, B43_SHM_UCODE | B43_SHM_AUTOINC_W, 0x0000);
2257         for (i = 0; i < len; i++) {
2258                 b43_write32(dev, B43_MMIO_SHM_DATA, be32_to_cpu(data[i]));
2259                 udelay(10);
2260         }
2261
2262         if (dev->fw.pcm.data) {
2263                 /* Upload PCM data. */
2264                 data = (__be32 *) (dev->fw.pcm.data->data + hdr_len);
2265                 len = (dev->fw.pcm.data->size - hdr_len) / sizeof(__be32);
2266                 b43_shm_control_word(dev, B43_SHM_HW, 0x01EA);
2267                 b43_write32(dev, B43_MMIO_SHM_DATA, 0x00004000);
2268                 /* No need for autoinc bit in SHM_HW */
2269                 b43_shm_control_word(dev, B43_SHM_HW, 0x01EB);
2270                 for (i = 0; i < len; i++) {
2271                         b43_write32(dev, B43_MMIO_SHM_DATA, be32_to_cpu(data[i]));
2272                         udelay(10);
2273                 }
2274         }
2275
2276         b43_write32(dev, B43_MMIO_GEN_IRQ_REASON, B43_IRQ_ALL);
2277
2278         /* Start the microcode PSM */
2279         macctl = b43_read32(dev, B43_MMIO_MACCTL);
2280         macctl &= ~B43_MACCTL_PSM_JMP0;
2281         macctl |= B43_MACCTL_PSM_RUN;
2282         b43_write32(dev, B43_MMIO_MACCTL, macctl);
2283
2284         /* Wait for the microcode to load and respond */
2285         i = 0;
2286         while (1) {
2287                 tmp = b43_read32(dev, B43_MMIO_GEN_IRQ_REASON);
2288                 if (tmp == B43_IRQ_MAC_SUSPENDED)
2289                         break;
2290                 i++;
2291                 if (i >= 20) {
2292                         b43err(dev->wl, "Microcode not responding\n");
2293                         b43_print_fw_helptext(dev->wl, 1);
2294                         err = -ENODEV;
2295                         goto error;
2296                 }
2297                 msleep_interruptible(50);
2298                 if (signal_pending(current)) {
2299                         err = -EINTR;
2300                         goto error;
2301                 }
2302         }
2303         b43_read32(dev, B43_MMIO_GEN_IRQ_REASON);       /* dummy read */
2304
2305         /* Get and check the revisions. */
2306         fwrev = b43_shm_read16(dev, B43_SHM_SHARED, B43_SHM_SH_UCODEREV);
2307         fwpatch = b43_shm_read16(dev, B43_SHM_SHARED, B43_SHM_SH_UCODEPATCH);
2308         fwdate = b43_shm_read16(dev, B43_SHM_SHARED, B43_SHM_SH_UCODEDATE);
2309         fwtime = b43_shm_read16(dev, B43_SHM_SHARED, B43_SHM_SH_UCODETIME);
2310
2311         if (fwrev <= 0x128) {
2312                 b43err(dev->wl, "YOUR FIRMWARE IS TOO OLD. Firmware from "
2313                        "binary drivers older than version 4.x is unsupported. "
2314                        "You must upgrade your firmware files.\n");
2315                 b43_print_fw_helptext(dev->wl, 1);
2316                 err = -EOPNOTSUPP;
2317                 goto error;
2318         }
2319         dev->fw.rev = fwrev;
2320         dev->fw.patch = fwpatch;
2321         dev->fw.opensource = (fwdate == 0xFFFF);
2322
2323         if (dev->fw.opensource) {
2324                 /* Patchlevel info is encoded in the "time" field. */
2325                 dev->fw.patch = fwtime;
2326                 b43info(dev->wl, "Loading OpenSource firmware version %u.%u%s\n",
2327                         dev->fw.rev, dev->fw.patch,
2328                         dev->fw.pcm_request_failed ? " (Hardware crypto not supported)" : "");
2329         } else {
2330                 b43info(dev->wl, "Loading firmware version %u.%u "
2331                         "(20%.2i-%.2i-%.2i %.2i:%.2i:%.2i)\n",
2332                         fwrev, fwpatch,
2333                         (fwdate >> 12) & 0xF, (fwdate >> 8) & 0xF, fwdate & 0xFF,
2334                         (fwtime >> 11) & 0x1F, (fwtime >> 5) & 0x3F, fwtime & 0x1F);
2335                 if (dev->fw.pcm_request_failed) {
2336                         b43warn(dev->wl, "No \"pcm5.fw\" firmware file found. "
2337                                 "Hardware accelerated cryptography is disabled.\n");
2338                         b43_print_fw_helptext(dev->wl, 0);
2339                 }
2340         }
2341
2342         if (b43_is_old_txhdr_format(dev)) {
2343                 /* We're over the deadline, but we keep support for old fw
2344                  * until it turns out to be in major conflict with something new. */
2345                 b43warn(dev->wl, "You are using an old firmware image. "
2346                         "Support for old firmware will be removed soon "
2347                         "(official deadline was July 2008).\n");
2348                 b43_print_fw_helptext(dev->wl, 0);
2349         }
2350
2351         return 0;
2352
2353 error:
2354         macctl = b43_read32(dev, B43_MMIO_MACCTL);
2355         macctl &= ~B43_MACCTL_PSM_RUN;
2356         macctl |= B43_MACCTL_PSM_JMP0;
2357         b43_write32(dev, B43_MMIO_MACCTL, macctl);
2358
2359         return err;
2360 }
2361
2362 static int b43_write_initvals(struct b43_wldev *dev,
2363                               const struct b43_iv *ivals,
2364                               size_t count,
2365                               size_t array_size)
2366 {
2367         const struct b43_iv *iv;
2368         u16 offset;
2369         size_t i;
2370         bool bit32;
2371
2372         BUILD_BUG_ON(sizeof(struct b43_iv) != 6);
2373         iv = ivals;
2374         for (i = 0; i < count; i++) {
2375                 if (array_size < sizeof(iv->offset_size))
2376                         goto err_format;
2377                 array_size -= sizeof(iv->offset_size);
2378                 offset = be16_to_cpu(iv->offset_size);
2379                 bit32 = !!(offset & B43_IV_32BIT);
2380                 offset &= B43_IV_OFFSET_MASK;
2381                 if (offset >= 0x1000)
2382                         goto err_format;
2383                 if (bit32) {
2384                         u32 value;
2385
2386                         if (array_size < sizeof(iv->data.d32))
2387                                 goto err_format;
2388                         array_size -= sizeof(iv->data.d32);
2389
2390                         value = get_unaligned_be32(&iv->data.d32);
2391                         b43_write32(dev, offset, value);
2392
2393                         iv = (const struct b43_iv *)((const uint8_t *)iv +
2394                                                         sizeof(__be16) +
2395                                                         sizeof(__be32));
2396                 } else {
2397                         u16 value;
2398
2399                         if (array_size < sizeof(iv->data.d16))
2400                                 goto err_format;
2401                         array_size -= sizeof(iv->data.d16);
2402
2403                         value = be16_to_cpu(iv->data.d16);
2404                         b43_write16(dev, offset, value);
2405
2406                         iv = (const struct b43_iv *)((const uint8_t *)iv +
2407                                                         sizeof(__be16) +
2408                                                         sizeof(__be16));
2409                 }
2410         }
2411         if (array_size)
2412                 goto err_format;
2413
2414         return 0;
2415
2416 err_format:
2417         b43err(dev->wl, "Initial Values Firmware file-format error.\n");
2418         b43_print_fw_helptext(dev->wl, 1);
2419
2420         return -EPROTO;
2421 }
2422
2423 static int b43_upload_initvals(struct b43_wldev *dev)
2424 {
2425         const size_t hdr_len = sizeof(struct b43_fw_header);
2426         const struct b43_fw_header *hdr;
2427         struct b43_firmware *fw = &dev->fw;
2428         const struct b43_iv *ivals;
2429         size_t count;
2430         int err;
2431
2432         hdr = (const struct b43_fw_header *)(fw->initvals.data->data);
2433         ivals = (const struct b43_iv *)(fw->initvals.data->data + hdr_len);
2434         count = be32_to_cpu(hdr->size);
2435         err = b43_write_initvals(dev, ivals, count,
2436                                  fw->initvals.data->size - hdr_len);
2437         if (err)
2438                 goto out;
2439         if (fw->initvals_band.data) {
2440                 hdr = (const struct b43_fw_header *)(fw->initvals_band.data->data);
2441                 ivals = (const struct b43_iv *)(fw->initvals_band.data->data + hdr_len);
2442                 count = be32_to_cpu(hdr->size);
2443                 err = b43_write_initvals(dev, ivals, count,
2444                                          fw->initvals_band.data->size - hdr_len);
2445                 if (err)
2446                         goto out;
2447         }
2448 out:
2449
2450         return err;
2451 }
2452
2453 /* Initialize the GPIOs
2454  * http://bcm-specs.sipsolutions.net/GPIO
2455  */
2456 static int b43_gpio_init(struct b43_wldev *dev)
2457 {
2458         struct ssb_bus *bus = dev->dev->bus;
2459         struct ssb_device *gpiodev, *pcidev = NULL;
2460         u32 mask, set;
2461
2462         b43_write32(dev, B43_MMIO_MACCTL, b43_read32(dev, B43_MMIO_MACCTL)
2463                     & ~B43_MACCTL_GPOUTSMSK);
2464
2465         b43_write16(dev, B43_MMIO_GPIO_MASK, b43_read16(dev, B43_MMIO_GPIO_MASK)
2466                     | 0x000F);
2467
2468         mask = 0x0000001F;
2469         set = 0x0000000F;
2470         if (dev->dev->bus->chip_id == 0x4301) {
2471                 mask |= 0x0060;
2472                 set |= 0x0060;
2473         }
2474         if (0 /* FIXME: conditional unknown */ ) {
2475                 b43_write16(dev, B43_MMIO_GPIO_MASK,
2476                             b43_read16(dev, B43_MMIO_GPIO_MASK)
2477                             | 0x0100);
2478                 mask |= 0x0180;
2479                 set |= 0x0180;
2480         }
2481         if (dev->dev->bus->sprom.boardflags_lo & B43_BFL_PACTRL) {
2482                 b43_write16(dev, B43_MMIO_GPIO_MASK,
2483                             b43_read16(dev, B43_MMIO_GPIO_MASK)
2484                             | 0x0200);
2485                 mask |= 0x0200;
2486                 set |= 0x0200;
2487         }
2488         if (dev->dev->id.revision >= 2)
2489                 mask |= 0x0010; /* FIXME: This is redundant. */
2490
2491 #ifdef CONFIG_SSB_DRIVER_PCICORE
2492         pcidev = bus->pcicore.dev;
2493 #endif
2494         gpiodev = bus->chipco.dev ? : pcidev;
2495         if (!gpiodev)
2496                 return 0;
2497         ssb_write32(gpiodev, B43_GPIO_CONTROL,
2498                     (ssb_read32(gpiodev, B43_GPIO_CONTROL)
2499                      & mask) | set);
2500
2501         return 0;
2502 }
2503
2504 /* Turn off all GPIO stuff. Call this on module unload, for example. */
2505 static void b43_gpio_cleanup(struct b43_wldev *dev)
2506 {
2507         struct ssb_bus *bus = dev->dev->bus;
2508         struct ssb_device *gpiodev, *pcidev = NULL;
2509
2510 #ifdef CONFIG_SSB_DRIVER_PCICORE
2511         pcidev = bus->pcicore.dev;
2512 #endif
2513         gpiodev = bus->chipco.dev ? : pcidev;
2514         if (!gpiodev)
2515                 return;
2516         ssb_write32(gpiodev, B43_GPIO_CONTROL, 0);
2517 }
2518
2519 /* http://bcm-specs.sipsolutions.net/EnableMac */
2520 void b43_mac_enable(struct b43_wldev *dev)
2521 {
2522         if (b43_debug(dev, B43_DBG_FIRMWARE)) {
2523                 u16 fwstate;
2524
2525                 fwstate = b43_shm_read16(dev, B43_SHM_SHARED,
2526                                          B43_SHM_SH_UCODESTAT);
2527                 if ((fwstate != B43_SHM_SH_UCODESTAT_SUSP) &&
2528                     (fwstate != B43_SHM_SH_UCODESTAT_SLEEP)) {
2529                         b43err(dev->wl, "b43_mac_enable(): The firmware "
2530                                "should be suspended, but current state is %u\n",
2531                                fwstate);
2532                 }
2533         }
2534
2535         dev->mac_suspended--;
2536         B43_WARN_ON(dev->mac_suspended < 0);
2537         if (dev->mac_suspended == 0) {
2538                 b43_write32(dev, B43_MMIO_MACCTL,
2539                             b43_read32(dev, B43_MMIO_MACCTL)
2540                             | B43_MACCTL_ENABLED);
2541                 b43_write32(dev, B43_MMIO_GEN_IRQ_REASON,
2542                             B43_IRQ_MAC_SUSPENDED);
2543                 /* Commit writes */
2544                 b43_read32(dev, B43_MMIO_MACCTL);
2545                 b43_read32(dev, B43_MMIO_GEN_IRQ_REASON);
2546                 b43_power_saving_ctl_bits(dev, 0);
2547         }
2548 }
2549
2550 /* http://bcm-specs.sipsolutions.net/SuspendMAC */
2551 void b43_mac_suspend(struct b43_wldev *dev)
2552 {
2553         int i;
2554         u32 tmp;
2555
2556         might_sleep();
2557         B43_WARN_ON(dev->mac_suspended < 0);
2558
2559         if (dev->mac_suspended == 0) {
2560                 b43_power_saving_ctl_bits(dev, B43_PS_AWAKE);
2561                 b43_write32(dev, B43_MMIO_MACCTL,
2562                             b43_read32(dev, B43_MMIO_MACCTL)
2563                             & ~B43_MACCTL_ENABLED);
2564                 /* force pci to flush the write */
2565                 b43_read32(dev, B43_MMIO_MACCTL);
2566                 for (i = 35; i; i--) {
2567                         tmp = b43_read32(dev, B43_MMIO_GEN_IRQ_REASON);
2568                         if (tmp & B43_IRQ_MAC_SUSPENDED)
2569                                 goto out;
2570                         udelay(10);
2571                 }
2572                 /* Hm, it seems this will take some time. Use msleep(). */
2573                 for (i = 40; i; i--) {
2574                         tmp = b43_read32(dev, B43_MMIO_GEN_IRQ_REASON);
2575                         if (tmp & B43_IRQ_MAC_SUSPENDED)
2576                                 goto out;
2577                         msleep(1);
2578                 }
2579                 b43err(dev->wl, "MAC suspend failed\n");
2580         }
2581 out:
2582         dev->mac_suspended++;
2583 }
2584
2585 static void b43_adjust_opmode(struct b43_wldev *dev)
2586 {
2587         struct b43_wl *wl = dev->wl;
2588         u32 ctl;
2589         u16 cfp_pretbtt;
2590
2591         ctl = b43_read32(dev, B43_MMIO_MACCTL);
2592         /* Reset status to STA infrastructure mode. */
2593         ctl &= ~B43_MACCTL_AP;
2594         ctl &= ~B43_MACCTL_KEEP_CTL;
2595         ctl &= ~B43_MACCTL_KEEP_BADPLCP;
2596         ctl &= ~B43_MACCTL_KEEP_BAD;
2597         ctl &= ~B43_MACCTL_PROMISC;
2598         ctl &= ~B43_MACCTL_BEACPROMISC;
2599         ctl |= B43_MACCTL_INFRA;
2600
2601         if (b43_is_mode(wl, NL80211_IFTYPE_AP) ||
2602             b43_is_mode(wl, NL80211_IFTYPE_MESH_POINT))
2603                 ctl |= B43_MACCTL_AP;
2604         else if (b43_is_mode(wl, NL80211_IFTYPE_ADHOC))
2605                 ctl &= ~B43_MACCTL_INFRA;
2606
2607         if (wl->filter_flags & FIF_CONTROL)
2608                 ctl |= B43_MACCTL_KEEP_CTL;
2609         if (wl->filter_flags & FIF_FCSFAIL)
2610                 ctl |= B43_MACCTL_KEEP_BAD;
2611         if (wl->filter_flags & FIF_PLCPFAIL)
2612                 ctl |= B43_MACCTL_KEEP_BADPLCP;
2613         if (wl->filter_flags & FIF_PROMISC_IN_BSS)
2614                 ctl |= B43_MACCTL_PROMISC;
2615         if (wl->filter_flags & FIF_BCN_PRBRESP_PROMISC)
2616                 ctl |= B43_MACCTL_BEACPROMISC;
2617
2618         /* Workaround: On old hardware the HW-MAC-address-filter
2619          * doesn't work properly, so always run promisc in filter
2620          * it in software. */
2621         if (dev->dev->id.revision <= 4)
2622                 ctl |= B43_MACCTL_PROMISC;
2623
2624         b43_write32(dev, B43_MMIO_MACCTL, ctl);
2625
2626         cfp_pretbtt = 2;
2627         if ((ctl & B43_MACCTL_INFRA) && !(ctl & B43_MACCTL_AP)) {
2628                 if (dev->dev->bus->chip_id == 0x4306 &&
2629                     dev->dev->bus->chip_rev == 3)
2630                         cfp_pretbtt = 100;
2631                 else
2632                         cfp_pretbtt = 50;
2633         }
2634         b43_write16(dev, 0x612, cfp_pretbtt);
2635 }
2636
2637 static void b43_rate_memory_write(struct b43_wldev *dev, u16 rate, int is_ofdm)
2638 {
2639         u16 offset;
2640
2641         if (is_ofdm) {
2642                 offset = 0x480;
2643                 offset += (b43_plcp_get_ratecode_ofdm(rate) & 0x000F) * 2;
2644         } else {
2645                 offset = 0x4C0;
2646                 offset += (b43_plcp_get_ratecode_cck(rate) & 0x000F) * 2;
2647         }
2648         b43_shm_write16(dev, B43_SHM_SHARED, offset + 0x20,
2649                         b43_shm_read16(dev, B43_SHM_SHARED, offset));
2650 }
2651
2652 static void b43_rate_memory_init(struct b43_wldev *dev)
2653 {
2654         switch (dev->phy.type) {
2655         case B43_PHYTYPE_A:
2656         case B43_PHYTYPE_G:
2657         case B43_PHYTYPE_N:
2658                 b43_rate_memory_write(dev, B43_OFDM_RATE_6MB, 1);
2659                 b43_rate_memory_write(dev, B43_OFDM_RATE_12MB, 1);
2660                 b43_rate_memory_write(dev, B43_OFDM_RATE_18MB, 1);
2661                 b43_rate_memory_write(dev, B43_OFDM_RATE_24MB, 1);
2662                 b43_rate_memory_write(dev, B43_OFDM_RATE_36MB, 1);
2663                 b43_rate_memory_write(dev, B43_OFDM_RATE_48MB, 1);
2664                 b43_rate_memory_write(dev, B43_OFDM_RATE_54MB, 1);
2665                 if (dev->phy.type == B43_PHYTYPE_A)
2666                         break;
2667                 /* fallthrough */
2668         case B43_PHYTYPE_B:
2669                 b43_rate_memory_write(dev, B43_CCK_RATE_1MB, 0);
2670                 b43_rate_memory_write(dev, B43_CCK_RATE_2MB, 0);
2671                 b43_rate_memory_write(dev, B43_CCK_RATE_5MB, 0);
2672                 b43_rate_memory_write(dev, B43_CCK_RATE_11MB, 0);
2673                 break;
2674         default:
2675                 B43_WARN_ON(1);
2676         }
2677 }
2678
2679 /* Set the default values for the PHY TX Control Words. */
2680 static void b43_set_phytxctl_defaults(struct b43_wldev *dev)
2681 {
2682         u16 ctl = 0;
2683
2684         ctl |= B43_TXH_PHY_ENC_CCK;
2685         ctl |= B43_TXH_PHY_ANT01AUTO;
2686         ctl |= B43_TXH_PHY_TXPWR;
2687
2688         b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_BEACPHYCTL, ctl);
2689         b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_ACKCTSPHYCTL, ctl);
2690         b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_PRPHYCTL, ctl);
2691 }
2692
2693 /* Set the TX-Antenna for management frames sent by firmware. */
2694 static void b43_mgmtframe_txantenna(struct b43_wldev *dev, int antenna)
2695 {
2696         u16 ant;
2697         u16 tmp;
2698
2699         ant = b43_antenna_to_phyctl(antenna);
2700
2701         /* For ACK/CTS */
2702         tmp = b43_shm_read16(dev, B43_SHM_SHARED, B43_SHM_SH_ACKCTSPHYCTL);
2703         tmp = (tmp & ~B43_TXH_PHY_ANT) | ant;
2704         b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_ACKCTSPHYCTL, tmp);
2705         /* For Probe Resposes */
2706         tmp = b43_shm_read16(dev, B43_SHM_SHARED, B43_SHM_SH_PRPHYCTL);
2707         tmp = (tmp & ~B43_TXH_PHY_ANT) | ant;
2708         b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_PRPHYCTL, tmp);
2709 }
2710
2711 /* This is the opposite of b43_chip_init() */
2712 static void b43_chip_exit(struct b43_wldev *dev)
2713 {
2714         b43_phy_exit(dev);
2715         b43_gpio_cleanup(dev);
2716         /* firmware is released later */
2717 }
2718
2719 /* Initialize the chip
2720  * http://bcm-specs.sipsolutions.net/ChipInit
2721  */
2722 static int b43_chip_init(struct b43_wldev *dev)
2723 {
2724         struct b43_phy *phy = &dev->phy;
2725         int err;
2726         u32 value32, macctl;
2727         u16 value16;
2728
2729         /* Initialize the MAC control */
2730         macctl = B43_MACCTL_IHR_ENABLED | B43_MACCTL_SHM_ENABLED;
2731         if (dev->phy.gmode)
2732                 macctl |= B43_MACCTL_GMODE;
2733         macctl |= B43_MACCTL_INFRA;
2734         b43_write32(dev, B43_MMIO_MACCTL, macctl);
2735
2736         err = b43_request_firmware(dev);
2737         if (err)
2738                 goto out;
2739         err = b43_upload_microcode(dev);
2740         if (err)
2741                 goto out;       /* firmware is released later */
2742
2743         err = b43_gpio_init(dev);
2744         if (err)
2745                 goto out;       /* firmware is released later */
2746
2747         err = b43_upload_initvals(dev);
2748         if (err)
2749                 goto err_gpio_clean;
2750
2751         /* Turn the Analog on and initialize the PHY. */
2752         phy->ops->switch_analog(dev, 1);
2753         err = b43_phy_init(dev);
2754         if (err)
2755                 goto err_gpio_clean;
2756
2757         /* Disable Interference Mitigation. */
2758         if (phy->ops->interf_mitigation)
2759                 phy->ops->interf_mitigation(dev, B43_INTERFMODE_NONE);
2760
2761         /* Select the antennae */
2762         if (phy->ops->set_rx_antenna)
2763                 phy->ops->set_rx_antenna(dev, B43_ANTENNA_DEFAULT);
2764         b43_mgmtframe_txantenna(dev, B43_ANTENNA_DEFAULT);
2765
2766         if (phy->type == B43_PHYTYPE_B) {
2767                 value16 = b43_read16(dev, 0x005E);
2768                 value16 |= 0x0004;
2769                 b43_write16(dev, 0x005E, value16);
2770         }
2771         b43_write32(dev, 0x0100, 0x01000000);
2772         if (dev->dev->id.revision < 5)
2773                 b43_write32(dev, 0x010C, 0x01000000);
2774
2775         b43_write32(dev, B43_MMIO_MACCTL, b43_read32(dev, B43_MMIO_MACCTL)
2776                     & ~B43_MACCTL_INFRA);
2777         b43_write32(dev, B43_MMIO_MACCTL, b43_read32(dev, B43_MMIO_MACCTL)
2778                     | B43_MACCTL_INFRA);
2779
2780         /* Probe Response Timeout value */
2781         /* FIXME: Default to 0, has to be set by ioctl probably... :-/ */
2782         b43_shm_write16(dev, B43_SHM_SHARED, 0x0074, 0x0000);
2783
2784         /* Initially set the wireless operation mode. */
2785         b43_adjust_opmode(dev);
2786
2787         if (dev->dev->id.revision < 3) {
2788                 b43_write16(dev, 0x060E, 0x0000);
2789                 b43_write16(dev, 0x0610, 0x8000);
2790                 b43_write16(dev, 0x0604, 0x0000);
2791                 b43_write16(dev, 0x0606, 0x0200);
2792         } else {
2793                 b43_write32(dev, 0x0188, 0x80000000);
2794                 b43_write32(dev, 0x018C, 0x02000000);
2795         }
2796         b43_write32(dev, B43_MMIO_GEN_IRQ_REASON, 0x00004000);
2797         b43_write32(dev, B43_MMIO_DMA0_IRQ_MASK, 0x0001DC00);
2798         b43_write32(dev, B43_MMIO_DMA1_IRQ_MASK, 0x0000DC00);
2799         b43_write32(dev, B43_MMIO_DMA2_IRQ_MASK, 0x0000DC00);
2800         b43_write32(dev, B43_MMIO_DMA3_IRQ_MASK, 0x0001DC00);
2801         b43_write32(dev, B43_MMIO_DMA4_IRQ_MASK, 0x0000DC00);
2802         b43_write32(dev, B43_MMIO_DMA5_IRQ_MASK, 0x0000DC00);
2803
2804         value32 = ssb_read32(dev->dev, SSB_TMSLOW);
2805         value32 |= 0x00100000;
2806         ssb_write32(dev->dev, SSB_TMSLOW, value32);
2807
2808         b43_write16(dev, B43_MMIO_POWERUP_DELAY,
2809                     dev->dev->bus->chipco.fast_pwrup_delay);
2810
2811         err = 0;
2812         b43dbg(dev->wl, "Chip initialized\n");
2813 out:
2814         return err;
2815
2816 err_gpio_clean:
2817         b43_gpio_cleanup(dev);
2818         return err;
2819 }
2820
2821 static void b43_periodic_every60sec(struct b43_wldev *dev)
2822 {
2823         const struct b43_phy_operations *ops = dev->phy.ops;
2824
2825         if (ops->pwork_60sec)
2826                 ops->pwork_60sec(dev);
2827
2828         /* Force check the TX power emission now. */
2829         b43_phy_txpower_check(dev, B43_TXPWR_IGNORE_TIME);
2830 }
2831
2832 static void b43_periodic_every30sec(struct b43_wldev *dev)
2833 {
2834         /* Update device statistics. */
2835         b43_calculate_link_quality(dev);
2836 }
2837
2838 static void b43_periodic_every15sec(struct b43_wldev *dev)
2839 {
2840         struct b43_phy *phy = &dev->phy;
2841         u16 wdr;
2842
2843         if (dev->fw.opensource) {
2844                 /* Check if the firmware is still alive.
2845                  * It will reset the watchdog counter to 0 in its idle loop. */
2846                 wdr = b43_shm_read16(dev, B43_SHM_SCRATCH, B43_WATCHDOG_REG);
2847                 if (unlikely(wdr)) {
2848                         b43err(dev->wl, "Firmware watchdog: The firmware died!\n");
2849                         b43_controller_restart(dev, "Firmware watchdog");
2850                         return;
2851                 } else {
2852                         b43_shm_write16(dev, B43_SHM_SCRATCH,
2853                                         B43_WATCHDOG_REG, 1);
2854                 }
2855         }
2856
2857         if (phy->ops->pwork_15sec)
2858                 phy->ops->pwork_15sec(dev);
2859
2860         atomic_set(&phy->txerr_cnt, B43_PHY_TX_BADNESS_LIMIT);
2861         wmb();
2862 }
2863
2864 static void do_periodic_work(struct b43_wldev *dev)
2865 {
2866         unsigned int state;
2867
2868         state = dev->periodic_state;
2869         if (state % 4 == 0)
2870                 b43_periodic_every60sec(dev);
2871         if (state % 2 == 0)
2872                 b43_periodic_every30sec(dev);
2873         b43_periodic_every15sec(dev);
2874 }
2875
2876 /* Periodic work locking policy:
2877  *      The whole periodic work handler is protected by
2878  *      wl->mutex. If another lock is needed somewhere in the
2879  *      pwork callchain, it's aquired in-place, where it's needed.
2880  */
2881 static void b43_periodic_work_handler(struct work_struct *work)
2882 {
2883         struct b43_wldev *dev = container_of(work, struct b43_wldev,
2884                                              periodic_work.work);
2885         struct b43_wl *wl = dev->wl;
2886         unsigned long delay;
2887
2888         mutex_lock(&wl->mutex);
2889
2890         if (unlikely(b43_status(dev) != B43_STAT_STARTED))
2891                 goto out;
2892         if (b43_debug(dev, B43_DBG_PWORK_STOP))
2893                 goto out_requeue;
2894
2895         do_periodic_work(dev);
2896
2897         dev->periodic_state++;
2898 out_requeue:
2899         if (b43_debug(dev, B43_DBG_PWORK_FAST))
2900                 delay = msecs_to_jiffies(50);
2901         else
2902                 delay = round_jiffies_relative(HZ * 15);
2903         queue_delayed_work(wl->hw->workqueue, &dev->periodic_work, delay);
2904 out:
2905         mutex_unlock(&wl->mutex);
2906 }
2907
2908 static void b43_periodic_tasks_setup(struct b43_wldev *dev)
2909 {
2910         struct delayed_work *work = &dev->periodic_work;
2911
2912         dev->periodic_state = 0;
2913         INIT_DELAYED_WORK(work, b43_periodic_work_handler);
2914         queue_delayed_work(dev->wl->hw->workqueue, work, 0);
2915 }
2916
2917 /* Check if communication with the device works correctly. */
2918 static int b43_validate_chipaccess(struct b43_wldev *dev)
2919 {
2920         u32 v, backup;
2921
2922         backup = b43_shm_read32(dev, B43_SHM_SHARED, 0);
2923
2924         /* Check for read/write and endianness problems. */
2925         b43_shm_write32(dev, B43_SHM_SHARED, 0, 0x55AAAA55);
2926         if (b43_shm_read32(dev, B43_SHM_SHARED, 0) != 0x55AAAA55)
2927                 goto error;
2928         b43_shm_write32(dev, B43_SHM_SHARED, 0, 0xAA5555AA);
2929         if (b43_shm_read32(dev, B43_SHM_SHARED, 0) != 0xAA5555AA)
2930                 goto error;
2931
2932         b43_shm_write32(dev, B43_SHM_SHARED, 0, backup);
2933
2934         if ((dev->dev->id.revision >= 3) && (dev->dev->id.revision <= 10)) {
2935                 /* The 32bit register shadows the two 16bit registers
2936                  * with update sideeffects. Validate this. */
2937                 b43_write16(dev, B43_MMIO_TSF_CFP_START, 0xAAAA);
2938                 b43_write32(dev, B43_MMIO_TSF_CFP_START, 0xCCCCBBBB);
2939                 if (b43_read16(dev, B43_MMIO_TSF_CFP_START_LOW) != 0xBBBB)
2940                         goto error;
2941                 if (b43_read16(dev, B43_MMIO_TSF_CFP_START_HIGH) != 0xCCCC)
2942                         goto error;
2943         }
2944         b43_write32(dev, B43_MMIO_TSF_CFP_START, 0);
2945
2946         v = b43_read32(dev, B43_MMIO_MACCTL);
2947         v |= B43_MACCTL_GMODE;
2948         if (v != (B43_MACCTL_GMODE | B43_MACCTL_IHR_ENABLED))
2949                 goto error;
2950
2951         return 0;
2952 error:
2953         b43err(dev->wl, "Failed to validate the chipaccess\n");
2954         return -ENODEV;
2955 }
2956
2957 static void b43_security_init(struct b43_wldev *dev)
2958 {
2959         dev->max_nr_keys = (dev->dev->id.revision >= 5) ? 58 : 20;
2960         B43_WARN_ON(dev->max_nr_keys > ARRAY_SIZE(dev->key));
2961         dev->ktp = b43_shm_read16(dev, B43_SHM_SHARED, B43_SHM_SH_KTP);
2962         /* KTP is a word address, but we address SHM bytewise.
2963          * So multiply by two.
2964          */
2965         dev->ktp *= 2;
2966         if (dev->dev->id.revision >= 5) {
2967                 /* Number of RCMTA address slots */
2968                 b43_write16(dev, B43_MMIO_RCMTA_COUNT, dev->max_nr_keys - 8);
2969         }
2970         b43_clear_keys(dev);
2971 }
2972
2973 static int b43_rng_read(struct hwrng *rng, u32 * data)
2974 {
2975         struct b43_wl *wl = (struct b43_wl *)rng->priv;
2976         unsigned long flags;
2977
2978         /* Don't take wl->mutex here, as it could deadlock with
2979          * hwrng internal locking. It's not needed to take
2980          * wl->mutex here, anyway. */
2981
2982         spin_lock_irqsave(&wl->irq_lock, flags);
2983         *data = b43_read16(wl->current_dev, B43_MMIO_RNG);
2984         spin_unlock_irqrestore(&wl->irq_lock, flags);
2985
2986         return (sizeof(u16));
2987 }
2988
2989 static void b43_rng_exit(struct b43_wl *wl)
2990 {
2991         if (wl->rng_initialized)
2992                 hwrng_unregister(&wl->rng);
2993 }
2994
2995 static int b43_rng_init(struct b43_wl *wl)
2996 {
2997         int err;
2998
2999         snprintf(wl->rng_name, ARRAY_SIZE(wl->rng_name),
3000                  "%s_%s", KBUILD_MODNAME, wiphy_name(wl->hw->wiphy));
3001         wl->rng.name = wl->rng_name;
3002         wl->rng.data_read = b43_rng_read;
3003         wl->rng.priv = (unsigned long)wl;
3004         wl->rng_initialized = 1;
3005         err = hwrng_register(&wl->rng);
3006         if (err) {
3007                 wl->rng_initialized = 0;
3008                 b43err(wl, "Failed to register the random "
3009                        "number generator (%d)\n", err);
3010         }
3011
3012         return err;
3013 }
3014
3015 static int b43_op_tx(struct ieee80211_hw *hw,
3016                      struct sk_buff *skb)
3017 {
3018         struct b43_wl *wl = hw_to_b43_wl(hw);
3019         struct b43_wldev *dev = wl->current_dev;
3020         unsigned long flags;
3021         int err;
3022
3023         if (unlikely(skb->len < 2 + 2 + 6)) {
3024                 /* Too short, this can't be a valid frame. */
3025                 goto drop_packet;
3026         }
3027         B43_WARN_ON(skb_shinfo(skb)->nr_frags);
3028         if (unlikely(!dev))
3029                 goto drop_packet;
3030
3031         /* Transmissions on seperate queues can run concurrently. */
3032         read_lock_irqsave(&wl->tx_lock, flags);
3033
3034         err = -ENODEV;
3035         if (likely(b43_status(dev) >= B43_STAT_STARTED)) {
3036                 if (b43_using_pio_transfers(dev))
3037                         err = b43_pio_tx(dev, skb);
3038                 else
3039                         err = b43_dma_tx(dev, skb);
3040         }
3041
3042         read_unlock_irqrestore(&wl->tx_lock, flags);
3043
3044         if (unlikely(err))
3045                 goto drop_packet;
3046         return NETDEV_TX_OK;
3047
3048 drop_packet:
3049         /* We can not transmit this packet. Drop it. */
3050         dev_kfree_skb_any(skb);
3051         return NETDEV_TX_OK;
3052 }
3053
3054 /* Locking: wl->irq_lock */
3055 static void b43_qos_params_upload(struct b43_wldev *dev,
3056                                   const struct ieee80211_tx_queue_params *p,
3057                                   u16 shm_offset)
3058 {
3059         u16 params[B43_NR_QOSPARAMS];
3060         int bslots, tmp;
3061         unsigned int i;
3062
3063         bslots = b43_read16(dev, B43_MMIO_RNG) & p->cw_min;
3064
3065         memset(&params, 0, sizeof(params));
3066
3067         params[B43_QOSPARAM_TXOP] = p->txop * 32;
3068         params[B43_QOSPARAM_CWMIN] = p->cw_min;
3069         params[B43_QOSPARAM_CWMAX] = p->cw_max;
3070         params[B43_QOSPARAM_CWCUR] = p->cw_min;
3071         params[B43_QOSPARAM_AIFS] = p->aifs;
3072         params[B43_QOSPARAM_BSLOTS] = bslots;
3073         params[B43_QOSPARAM_REGGAP] = bslots + p->aifs;
3074
3075         for (i = 0; i < ARRAY_SIZE(params); i++) {
3076                 if (i == B43_QOSPARAM_STATUS) {
3077                         tmp = b43_shm_read16(dev, B43_SHM_SHARED,
3078                                              shm_offset + (i * 2));
3079                         /* Mark the parameters as updated. */
3080                         tmp |= 0x100;
3081                         b43_shm_write16(dev, B43_SHM_SHARED,
3082                                         shm_offset + (i * 2),
3083                                         tmp);
3084                 } else {
3085                         b43_shm_write16(dev, B43_SHM_SHARED,
3086                                         shm_offset + (i * 2),
3087                                         params[i]);
3088                 }
3089         }
3090 }
3091
3092 /* Mapping of mac80211 queue numbers to b43 QoS SHM offsets. */
3093 static const u16 b43_qos_shm_offsets[] = {
3094         /* [mac80211-queue-nr] = SHM_OFFSET, */
3095         [0] = B43_QOS_VOICE,
3096         [1] = B43_QOS_VIDEO,
3097         [2] = B43_QOS_BESTEFFORT,
3098         [3] = B43_QOS_BACKGROUND,
3099 };
3100
3101 /* Update all QOS parameters in hardware. */
3102 static void b43_qos_upload_all(struct b43_wldev *dev)
3103 {
3104         struct b43_wl *wl = dev->wl;
3105         struct b43_qos_params *params;
3106         unsigned int i;
3107
3108         BUILD_BUG_ON(ARRAY_SIZE(b43_qos_shm_offsets) !=
3109                      ARRAY_SIZE(wl->qos_params));
3110
3111         b43_mac_suspend(dev);
3112         for (i = 0; i < ARRAY_SIZE(wl->qos_params); i++) {
3113                 params = &(wl->qos_params[i]);
3114                 b43_qos_params_upload(dev, &(params->p),
3115                                       b43_qos_shm_offsets[i]);
3116         }
3117         b43_mac_enable(dev);
3118 }
3119
3120 static void b43_qos_clear(struct b43_wl *wl)
3121 {
3122         struct b43_qos_params *params;
3123         unsigned int i;
3124
3125         /* Initialize QoS parameters to sane defaults. */
3126
3127         BUILD_BUG_ON(ARRAY_SIZE(b43_qos_shm_offsets) !=
3128                      ARRAY_SIZE(wl->qos_params));
3129
3130         for (i = 0; i < ARRAY_SIZE(wl->qos_params); i++) {
3131                 params = &(wl->qos_params[i]);
3132
3133                 switch (b43_qos_shm_offsets[i]) {
3134                 case B43_QOS_VOICE:
3135                         params->p.txop = 0;
3136                         params->p.aifs = 2;
3137                         params->p.cw_min = 0x0001;
3138                         params->p.cw_max = 0x0001;
3139                         break;
3140                 case B43_QOS_VIDEO:
3141                         params->p.txop = 0;
3142                         params->p.aifs = 2;
3143                         params->p.cw_min = 0x0001;
3144                         params->p.cw_max = 0x0001;
3145                         break;
3146                 case B43_QOS_BESTEFFORT:
3147                         params->p.txop = 0;
3148                         params->p.aifs = 3;
3149                         params->p.cw_min = 0x0001;
3150                         params->p.cw_max = 0x03FF;
3151                         break;
3152                 case B43_QOS_BACKGROUND:
3153                         params->p.txop = 0;
3154                         params->p.aifs = 7;
3155                         params->p.cw_min = 0x0001;
3156                         params->p.cw_max = 0x03FF;
3157                         break;
3158                 default:
3159                         B43_WARN_ON(1);
3160                 }
3161         }
3162 }
3163
3164 /* Initialize the core's QOS capabilities */
3165 static void b43_qos_init(struct b43_wldev *dev)
3166 {
3167         /* Upload the current QOS parameters. */
3168         b43_qos_upload_all(dev);
3169
3170         /* Enable QOS support. */
3171         b43_hf_write(dev, b43_hf_read(dev) | B43_HF_EDCF);
3172         b43_write16(dev, B43_MMIO_IFSCTL,
3173                     b43_read16(dev, B43_MMIO_IFSCTL)
3174                     | B43_MMIO_IFSCTL_USE_EDCF);
3175 }
3176
3177 static int b43_op_conf_tx(struct ieee80211_hw *hw, u16 _queue,
3178                           const struct ieee80211_tx_queue_params *params)
3179 {
3180         struct b43_wl *wl = hw_to_b43_wl(hw);
3181         struct b43_wldev *dev;
3182         unsigned int queue = (unsigned int)_queue;
3183         int err = -ENODEV;
3184
3185         if (queue >= ARRAY_SIZE(wl->qos_params)) {
3186                 /* Queue not available or don't support setting
3187                  * params on this queue. Return success to not
3188                  * confuse mac80211. */
3189                 return 0;
3190         }
3191         BUILD_BUG_ON(ARRAY_SIZE(b43_qos_shm_offsets) !=
3192                      ARRAY_SIZE(wl->qos_params));
3193
3194         mutex_lock(&wl->mutex);
3195         dev = wl->current_dev;
3196         if (unlikely(!dev || (b43_status(dev) < B43_STAT_INITIALIZED)))
3197                 goto out_unlock;
3198
3199         memcpy(&(wl->qos_params[queue].p), params, sizeof(*params));
3200         b43_mac_suspend(dev);
3201         b43_qos_params_upload(dev, &(wl->qos_params[queue].p),
3202                               b43_qos_shm_offsets[queue]);
3203         b43_mac_enable(dev);
3204         err = 0;
3205
3206 out_unlock:
3207         mutex_unlock(&wl->mutex);
3208
3209         return err;
3210 }
3211
3212 static int b43_op_get_tx_stats(struct ieee80211_hw *hw,
3213                                struct ieee80211_tx_queue_stats *stats)
3214 {
3215         struct b43_wl *wl = hw_to_b43_wl(hw);
3216         struct b43_wldev *dev = wl->current_dev;
3217         unsigned long flags;
3218         int err = -ENODEV;
3219
3220         if (!dev)
3221                 goto out;
3222         spin_lock_irqsave(&wl->irq_lock, flags);
3223         if (likely(b43_status(dev) >= B43_STAT_STARTED)) {
3224                 if (b43_using_pio_transfers(dev))
3225                         b43_pio_get_tx_stats(dev, stats);
3226                 else
3227                         b43_dma_get_tx_stats(dev, stats);
3228                 err = 0;
3229         }
3230         spin_unlock_irqrestore(&wl->irq_lock, flags);
3231 out:
3232         return err;
3233 }
3234
3235 static int b43_op_get_stats(struct ieee80211_hw *hw,
3236                             struct ieee80211_low_level_stats *stats)
3237 {
3238         struct b43_wl *wl = hw_to_b43_wl(hw);
3239         unsigned long flags;
3240
3241         spin_lock_irqsave(&wl->irq_lock, flags);
3242         memcpy(stats, &wl->ieee_stats, sizeof(*stats));
3243         spin_unlock_irqrestore(&wl->irq_lock, flags);
3244
3245         return 0;
3246 }
3247
3248 static void b43_put_phy_into_reset(struct b43_wldev *dev)
3249 {
3250         struct ssb_device *sdev = dev->dev;
3251         u32 tmslow;
3252
3253         tmslow = ssb_read32(sdev, SSB_TMSLOW);
3254         tmslow &= ~B43_TMSLOW_GMODE;
3255         tmslow |= B43_TMSLOW_PHYRESET;
3256         tmslow |= SSB_TMSLOW_FGC;
3257         ssb_write32(sdev, SSB_TMSLOW, tmslow);
3258         msleep(1);
3259
3260         tmslow = ssb_read32(sdev, SSB_TMSLOW);
3261         tmslow &= ~SSB_TMSLOW_FGC;
3262         tmslow |= B43_TMSLOW_PHYRESET;
3263         ssb_write32(sdev, SSB_TMSLOW, tmslow);
3264         msleep(1);
3265 }
3266
3267 static const char * band_to_string(enum ieee80211_band band)
3268 {
3269         switch (band) {
3270         case IEEE80211_BAND_5GHZ:
3271                 return "5";
3272         case IEEE80211_BAND_2GHZ:
3273                 return "2.4";
3274         default:
3275                 break;
3276         }
3277         B43_WARN_ON(1);
3278         return "";
3279 }
3280
3281 /* Expects wl->mutex locked */
3282 static int b43_switch_band(struct b43_wl *wl, struct ieee80211_channel *chan)
3283 {
3284         struct b43_wldev *up_dev = NULL;
3285         struct b43_wldev *down_dev;
3286         struct b43_wldev *d;
3287         int err;
3288         bool uninitialized_var(gmode);
3289         int prev_status;
3290
3291         /* Find a device and PHY which supports the band. */
3292         list_for_each_entry(d, &wl->devlist, list) {
3293                 switch (chan->band) {
3294                 case IEEE80211_BAND_5GHZ:
3295                         if (d->phy.supports_5ghz) {
3296                                 up_dev = d;
3297                                 gmode = 0;
3298                         }
3299                         break;
3300                 case IEEE80211_BAND_2GHZ:
3301                         if (d->phy.supports_2ghz) {
3302                                 up_dev = d;
3303                                 gmode = 1;
3304                         }
3305                         break;
3306                 default:
3307                         B43_WARN_ON(1);
3308                         return -EINVAL;
3309                 }
3310                 if (up_dev)
3311                         break;
3312         }
3313         if (!up_dev) {
3314                 b43err(wl, "Could not find a device for %s-GHz band operation\n",
3315                        band_to_string(chan->band));
3316                 return -ENODEV;
3317         }
3318         if ((up_dev == wl->current_dev) &&
3319             (!!wl->current_dev->phy.gmode == !!gmode)) {
3320                 /* This device is already running. */
3321                 return 0;
3322         }
3323         b43dbg(wl, "Switching to %s-GHz band\n",
3324                band_to_string(chan->band));
3325         down_dev = wl->current_dev;
3326
3327         prev_status = b43_status(down_dev);
3328         /* Shutdown the currently running core. */
3329         if (prev_status >= B43_STAT_STARTED)
3330                 b43_wireless_core_stop(down_dev);
3331         if (prev_status >= B43_STAT_INITIALIZED)
3332                 b43_wireless_core_exit(down_dev);
3333
3334         if (down_dev != up_dev) {
3335                 /* We switch to a different core, so we put PHY into
3336                  * RESET on the old core. */
3337                 b43_put_phy_into_reset(down_dev);
3338         }
3339
3340         /* Now start the new core. */
3341         up_dev->phy.gmode = gmode;
3342         if (prev_status >= B43_STAT_INITIALIZED) {
3343                 err = b43_wireless_core_init(up_dev);
3344                 if (err) {
3345                         b43err(wl, "Fatal: Could not initialize device for "
3346                                "selected %s-GHz band\n",
3347                                band_to_string(chan->band));
3348                         goto init_failure;
3349                 }
3350         }
3351         if (prev_status >= B43_STAT_STARTED) {
3352                 err = b43_wireless_core_start(up_dev);
3353                 if (err) {
3354                         b43err(wl, "Fatal: Coult not start device for "
3355                                "selected %s-GHz band\n",
3356                                band_to_string(chan->band));
3357                         b43_wireless_core_exit(up_dev);
3358                         goto init_failure;
3359                 }
3360         }
3361         B43_WARN_ON(b43_status(up_dev) != prev_status);
3362
3363         wl->current_dev = up_dev;
3364
3365         return 0;
3366 init_failure:
3367         /* Whoops, failed to init the new core. No core is operating now. */
3368         wl->current_dev = NULL;
3369         return err;
3370 }
3371
3372 /* Write the short and long frame retry limit values. */
3373 static void b43_set_retry_limits(struct b43_wldev *dev,
3374                                  unsigned int short_retry,
3375                                  unsigned int long_retry)
3376 {
3377         /* The retry limit is a 4-bit counter. Enforce this to avoid overflowing
3378          * the chip-internal counter. */
3379         short_retry = min(short_retry, (unsigned int)0xF);
3380         long_retry = min(long_retry, (unsigned int)0xF);
3381
3382         b43_shm_write16(dev, B43_SHM_SCRATCH, B43_SHM_SC_SRLIMIT,
3383                         short_retry);
3384         b43_shm_write16(dev, B43_SHM_SCRATCH, B43_SHM_SC_LRLIMIT,
3385                         long_retry);
3386 }
3387
3388 static int b43_op_config(struct ieee80211_hw *hw, u32 changed)
3389 {
3390         struct b43_wl *wl = hw_to_b43_wl(hw);
3391         struct b43_wldev *dev;
3392         struct b43_phy *phy;
3393         struct ieee80211_conf *conf = &hw->conf;
3394         unsigned long flags;
3395         int antenna;
3396         int err = 0;
3397
3398         mutex_lock(&wl->mutex);
3399
3400         /* Switch the band (if necessary). This might change the active core. */
3401         err = b43_switch_band(wl, conf->channel);
3402         if (err)
3403                 goto out_unlock_mutex;
3404         dev = wl->current_dev;
3405         phy = &dev->phy;
3406
3407         b43_mac_suspend(dev);
3408
3409         if (changed & IEEE80211_CONF_CHANGE_RETRY_LIMITS)
3410                 b43_set_retry_limits(dev, conf->short_frame_max_tx_count,
3411                                           conf->long_frame_max_tx_count);
3412         changed &= ~IEEE80211_CONF_CHANGE_RETRY_LIMITS;
3413         if (!changed)
3414                 goto out_mac_enable;
3415
3416         /* Switch to the requested channel.
3417          * The firmware takes care of races with the TX handler. */
3418         if (conf->channel->hw_value != phy->channel)
3419                 b43_switch_channel(dev, conf->channel->hw_value);
3420
3421         dev->wl->radiotap_enabled = !!(conf->flags & IEEE80211_CONF_RADIOTAP);
3422
3423         /* Adjust the desired TX power level. */
3424         if (conf->power_level != 0) {
3425                 spin_lock_irqsave(&wl->irq_lock, flags);
3426                 if (conf->power_level != phy->desired_txpower) {
3427                         phy->desired_txpower = conf->power_level;
3428                         b43_phy_txpower_check(dev, B43_TXPWR_IGNORE_TIME |
3429                                                    B43_TXPWR_IGNORE_TSSI);
3430                 }
3431                 spin_unlock_irqrestore(&wl->irq_lock, flags);
3432         }
3433
3434         /* Antennas for RX and management frame TX. */
3435         antenna = B43_ANTENNA_DEFAULT;
3436         b43_mgmtframe_txantenna(dev, antenna);
3437         antenna = B43_ANTENNA_DEFAULT;
3438         if (phy->ops->set_rx_antenna)
3439                 phy->ops->set_rx_antenna(dev, antenna);
3440
3441         /* Update templates for AP/mesh mode. */
3442         if (b43_is_mode(wl, NL80211_IFTYPE_AP) ||
3443             b43_is_mode(wl, NL80211_IFTYPE_MESH_POINT))
3444                 b43_set_beacon_int(dev, conf->beacon_int);
3445
3446         if (!!conf->radio_enabled != phy->radio_on) {
3447                 if (conf->radio_enabled) {
3448                         b43_software_rfkill(dev, RFKILL_STATE_UNBLOCKED);
3449                         b43info(dev->wl, "Radio turned on by software\n");
3450                         if (!dev->radio_hw_enable) {
3451                                 b43info(dev->wl, "The hardware RF-kill button "
3452                                         "still turns the radio physically off. "
3453                                         "Press the button to turn it on.\n");
3454                         }
3455                 } else {
3456                         b43_software_rfkill(dev, RFKILL_STATE_SOFT_BLOCKED);
3457                         b43info(dev->wl, "Radio turned off by software\n");
3458                 }
3459         }
3460
3461 out_mac_enable:
3462         b43_mac_enable(dev);
3463 out_unlock_mutex:
3464         mutex_unlock(&wl->mutex);
3465
3466         return err;
3467 }
3468
3469 static void b43_update_basic_rates(struct b43_wldev *dev, u32 brates)
3470 {
3471         struct ieee80211_supported_band *sband =
3472                 dev->wl->hw->wiphy->bands[b43_current_band(dev->wl)];
3473         struct ieee80211_rate *rate;
3474         int i;
3475         u16 basic, direct, offset, basic_offset, rateptr;
3476
3477         for (i = 0; i < sband->n_bitrates; i++) {
3478                 rate = &sband->bitrates[i];
3479
3480                 if (b43_is_cck_rate(rate->hw_value)) {
3481                         direct = B43_SHM_SH_CCKDIRECT;
3482                         basic = B43_SHM_SH_CCKBASIC;
3483                         offset = b43_plcp_get_ratecode_cck(rate->hw_value);
3484                         offset &= 0xF;
3485                 } else {
3486                         direct = B43_SHM_SH_OFDMDIRECT;
3487                         basic = B43_SHM_SH_OFDMBASIC;
3488                         offset = b43_plcp_get_ratecode_ofdm(rate->hw_value);
3489                         offset &= 0xF;
3490                 }
3491
3492                 rate = ieee80211_get_response_rate(sband, brates, rate->bitrate);
3493
3494                 if (b43_is_cck_rate(rate->hw_value)) {
3495                         basic_offset = b43_plcp_get_ratecode_cck(rate->hw_value);
3496                         basic_offset &= 0xF;
3497                 } else {
3498                         basic_offset = b43_plcp_get_ratecode_ofdm(rate->hw_value);
3499                         basic_offset &= 0xF;
3500                 }
3501
3502                 /*
3503                  * Get the pointer that we need to point to
3504                  * from the direct map
3505                  */
3506                 rateptr = b43_shm_read16(dev, B43_SHM_SHARED,
3507                                          direct + 2 * basic_offset);
3508                 /* and write it to the basic map */
3509                 b43_shm_write16(dev, B43_SHM_SHARED, basic + 2 * offset,
3510                                 rateptr);
3511         }
3512 }
3513
3514 static void b43_op_bss_info_changed(struct ieee80211_hw *hw,
3515                                     struct ieee80211_vif *vif,
3516                                     struct ieee80211_bss_conf *conf,
3517                                     u32 changed)
3518 {
3519         struct b43_wl *wl = hw_to_b43_wl(hw);
3520         struct b43_wldev *dev;
3521
3522         mutex_lock(&wl->mutex);
3523
3524         dev = wl->current_dev;
3525         if (!dev || b43_status(dev) < B43_STAT_STARTED)
3526                 goto out_unlock_mutex;
3527         b43_mac_suspend(dev);
3528
3529         if (changed & BSS_CHANGED_BASIC_RATES)
3530                 b43_update_basic_rates(dev, conf->basic_rates);
3531
3532         if (changed & BSS_CHANGED_ERP_SLOT) {
3533                 if (conf->use_short_slot)
3534                         b43_short_slot_timing_enable(dev);
3535                 else
3536                         b43_short_slot_timing_disable(dev);
3537         }
3538
3539         b43_mac_enable(dev);
3540 out_unlock_mutex:
3541         mutex_unlock(&wl->mutex);
3542
3543         return;
3544 }
3545
3546 static int b43_op_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
3547                           struct ieee80211_vif *vif, struct ieee80211_sta *sta,
3548                           struct ieee80211_key_conf *key)
3549 {
3550         struct b43_wl *wl = hw_to_b43_wl(hw);
3551         struct b43_wldev *dev;
3552         u8 algorithm;
3553         u8 index;
3554         int err;
3555 #if B43_DEBUG
3556         static const u8 bcast_addr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
3557 #endif
3558
3559         if (modparam_nohwcrypt)
3560                 return -ENOSPC; /* User disabled HW-crypto */
3561
3562         mutex_lock(&wl->mutex);
3563         spin_lock_irq(&wl->irq_lock);
3564         write_lock(&wl->tx_lock);
3565         /* Why do we need all this locking here?
3566          * mutex     -> Every config operation must take it.
3567          * irq_lock  -> We modify the dev->key array, which is accessed
3568          *              in the IRQ handlers.
3569          * tx_lock   -> We modify the dev->key array, which is accessed
3570          *              in the TX handler.
3571          */
3572
3573         dev = wl->current_dev;
3574         err = -ENODEV;
3575         if (!dev || b43_status(dev) < B43_STAT_INITIALIZED)
3576                 goto out_unlock;
3577
3578         if (dev->fw.pcm_request_failed) {
3579                 /* We don't have firmware for the crypto engine.
3580                  * Must use software-crypto. */
3581                 err = -EOPNOTSUPP;
3582                 goto out_unlock;
3583         }
3584
3585         err = -EINVAL;
3586         switch (key->alg) {
3587         case ALG_WEP:
3588                 if (key->keylen == LEN_WEP40)
3589                         algorithm = B43_SEC_ALGO_WEP40;
3590                 else
3591                         algorithm = B43_SEC_ALGO_WEP104;
3592                 break;
3593         case ALG_TKIP:
3594                 algorithm = B43_SEC_ALGO_TKIP;
3595                 break;
3596         case ALG_CCMP:
3597                 algorithm = B43_SEC_ALGO_AES;
3598                 break;
3599         default:
3600                 B43_WARN_ON(1);
3601                 goto out_unlock;
3602         }
3603         index = (u8) (key->keyidx);
3604         if (index > 3)
3605                 goto out_unlock;
3606
3607         switch (cmd) {
3608         case SET_KEY:
3609                 if (algorithm == B43_SEC_ALGO_TKIP) {
3610                         /* FIXME: No TKIP hardware encryption for now. */
3611                         err = -EOPNOTSUPP;
3612                         goto out_unlock;
3613                 }
3614
3615                 if (key->flags & IEEE80211_KEY_FLAG_PAIRWISE) {
3616                         if (WARN_ON(!sta)) {
3617                                 err = -EOPNOTSUPP;
3618                                 goto out_unlock;
3619                         }
3620                         /* Pairwise key with an assigned MAC address. */
3621                         err = b43_key_write(dev, -1, algorithm,
3622                                             key->key, key->keylen,
3623                                             sta->addr, key);
3624                 } else {
3625                         /* Group key */
3626                         err = b43_key_write(dev, index, algorithm,
3627                                             key->key, key->keylen, NULL, key);
3628                 }
3629                 if (err)
3630                         goto out_unlock;
3631
3632                 if (algorithm == B43_SEC_ALGO_WEP40 ||
3633                     algorithm == B43_SEC_ALGO_WEP104) {
3634                         b43_hf_write(dev, b43_hf_read(dev) | B43_HF_USEDEFKEYS);
3635                 } else {
3636                         b43_hf_write(dev,
3637                                      b43_hf_read(dev) & ~B43_HF_USEDEFKEYS);
3638                 }
3639                 key->flags |= IEEE80211_KEY_FLAG_GENERATE_IV;
3640                 break;
3641         case DISABLE_KEY: {
3642                 err = b43_key_clear(dev, key->hw_key_idx);
3643                 if (err)
3644                         goto out_unlock;
3645                 break;
3646         }
3647         default:
3648                 B43_WARN_ON(1);
3649         }
3650
3651 out_unlock:
3652         if (!err) {
3653                 b43dbg(wl, "%s hardware based encryption for keyidx: %d, "
3654                        "mac: %pM\n",
3655                        cmd == SET_KEY ? "Using" : "Disabling", key->keyidx,
3656                        sta ? sta->addr : bcast_addr);
3657                 b43_dump_keymemory(dev);
3658         }
3659         write_unlock(&wl->tx_lock);
3660         spin_unlock_irq(&wl->irq_lock);
3661         mutex_unlock(&wl->mutex);
3662
3663         return err;
3664 }
3665
3666 static void b43_op_configure_filter(struct ieee80211_hw *hw,
3667                                     unsigned int changed, unsigned int *fflags,
3668                                     int mc_count, struct dev_addr_list *mc_list)
3669 {
3670         struct b43_wl *wl = hw_to_b43_wl(hw);
3671         struct b43_wldev *dev = wl->current_dev;
3672         unsigned long flags;
3673
3674         if (!dev) {
3675                 *fflags = 0;
3676                 return;
3677         }
3678
3679         spin_lock_irqsave(&wl->irq_lock, flags);
3680         *fflags &= FIF_PROMISC_IN_BSS |
3681                   FIF_ALLMULTI |
3682                   FIF_FCSFAIL |
3683                   FIF_PLCPFAIL |
3684                   FIF_CONTROL |
3685                   FIF_OTHER_BSS |
3686                   FIF_BCN_PRBRESP_PROMISC;
3687
3688         changed &= FIF_PROMISC_IN_BSS |
3689                    FIF_ALLMULTI |
3690                    FIF_FCSFAIL |
3691                    FIF_PLCPFAIL |
3692                    FIF_CONTROL |
3693                    FIF_OTHER_BSS |
3694                    FIF_BCN_PRBRESP_PROMISC;
3695
3696         wl->filter_flags = *fflags;
3697
3698         if (changed && b43_status(dev) >= B43_STAT_INITIALIZED)
3699                 b43_adjust_opmode(dev);
3700         spin_unlock_irqrestore(&wl->irq_lock, flags);
3701 }
3702
3703 static int b43_op_config_interface(struct ieee80211_hw *hw,
3704                                    struct ieee80211_vif *vif,
3705                                    struct ieee80211_if_conf *conf)
3706 {
3707         struct b43_wl *wl = hw_to_b43_wl(hw);
3708         struct b43_wldev *dev = wl->current_dev;
3709         unsigned long flags;
3710
3711         if (!dev)
3712                 return -ENODEV;
3713         mutex_lock(&wl->mutex);
3714         spin_lock_irqsave(&wl->irq_lock, flags);
3715         B43_WARN_ON(wl->vif != vif);
3716         if (conf->bssid)
3717                 memcpy(wl->bssid, conf->bssid, ETH_ALEN);
3718         else
3719                 memset(wl->bssid, 0, ETH_ALEN);
3720         if (b43_status(dev) >= B43_STAT_INITIALIZED) {
3721                 if (b43_is_mode(wl, NL80211_IFTYPE_AP) ||
3722                     b43_is_mode(wl, NL80211_IFTYPE_MESH_POINT)) {
3723                         B43_WARN_ON(vif->type != wl->if_type);
3724                         if (conf->changed & IEEE80211_IFCC_BEACON)
3725                                 b43_update_templates(wl);
3726                 } else if (b43_is_mode(wl, NL80211_IFTYPE_ADHOC)) {
3727                         if (conf->changed & IEEE80211_IFCC_BEACON)
3728                                 b43_update_templates(wl);
3729                 }
3730                 b43_write_mac_bssid_templates(dev);
3731         }
3732         spin_unlock_irqrestore(&wl->irq_lock, flags);
3733         mutex_unlock(&wl->mutex);
3734
3735         return 0;
3736 }
3737
3738 /* Locking: wl->mutex */
3739 static void b43_wireless_core_stop(struct b43_wldev *dev)
3740 {
3741         struct b43_wl *wl = dev->wl;
3742         unsigned long flags;
3743
3744         if (b43_status(dev) < B43_STAT_STARTED)
3745                 return;
3746
3747         /* Disable and sync interrupts. We must do this before than
3748          * setting the status to INITIALIZED, as the interrupt handler
3749          * won't care about IRQs then. */
3750         spin_lock_irqsave(&wl->irq_lock, flags);
3751         dev->irq_savedstate = b43_interrupt_disable(dev, B43_IRQ_ALL);
3752         b43_read32(dev, B43_MMIO_GEN_IRQ_MASK); /* flush */
3753         spin_unlock_irqrestore(&wl->irq_lock, flags);
3754         b43_synchronize_irq(dev);
3755
3756         write_lock_irqsave(&wl->tx_lock, flags);
3757         b43_set_status(dev, B43_STAT_INITIALIZED);
3758         write_unlock_irqrestore(&wl->tx_lock, flags);
3759
3760         b43_pio_stop(dev);
3761         mutex_unlock(&wl->mutex);
3762         /* Must unlock as it would otherwise deadlock. No races here.
3763          * Cancel the possibly running self-rearming periodic work. */
3764         cancel_delayed_work_sync(&dev->periodic_work);
3765         mutex_lock(&wl->mutex);
3766
3767         b43_mac_suspend(dev);
3768         free_irq(dev->dev->irq, dev);
3769         b43dbg(wl, "Wireless interface stopped\n");
3770 }
3771
3772 /* Locking: wl->mutex */
3773 static int b43_wireless_core_start(struct b43_wldev *dev)
3774 {
3775         int err;
3776
3777         B43_WARN_ON(b43_status(dev) != B43_STAT_INITIALIZED);
3778
3779         drain_txstatus_queue(dev);
3780         err = request_irq(dev->dev->irq, b43_interrupt_handler,
3781                           IRQF_SHARED, KBUILD_MODNAME, dev);
3782         if (err) {
3783                 b43err(dev->wl, "Cannot request IRQ-%d\n", dev->dev->irq);
3784                 goto out;
3785         }
3786
3787         /* We are ready to run. */
3788         b43_set_status(dev, B43_STAT_STARTED);
3789
3790         /* Start data flow (TX/RX). */
3791         b43_mac_enable(dev);
3792         b43_interrupt_enable(dev, dev->irq_savedstate);
3793
3794         /* Start maintainance work */
3795         b43_periodic_tasks_setup(dev);
3796
3797         b43dbg(dev->wl, "Wireless interface started\n");
3798       out:
3799         return err;
3800 }
3801
3802 /* Get PHY and RADIO versioning numbers */
3803 static int b43_phy_versioning(struct b43_wldev *dev)
3804 {
3805         struct b43_phy *phy = &dev->phy;
3806         u32 tmp;
3807         u8 analog_type;
3808         u8 phy_type;
3809         u8 phy_rev;
3810         u16 radio_manuf;
3811         u16 radio_ver;
3812         u16 radio_rev;
3813         int unsupported = 0;
3814
3815         /* Get PHY versioning */
3816         tmp = b43_read16(dev, B43_MMIO_PHY_VER);
3817         analog_type = (tmp & B43_PHYVER_ANALOG) >> B43_PHYVER_ANALOG_SHIFT;
3818         phy_type = (tmp & B43_PHYVER_TYPE) >> B43_PHYVER_TYPE_SHIFT;
3819         phy_rev = (tmp & B43_PHYVER_VERSION);
3820         switch (phy_type) {
3821         case B43_PHYTYPE_A:
3822                 if (phy_rev >= 4)
3823                         unsupported = 1;
3824                 break;
3825         case B43_PHYTYPE_B:
3826                 if (phy_rev != 2 && phy_rev != 4 && phy_rev != 6
3827                     && phy_rev != 7)
3828                         unsupported = 1;
3829                 break;
3830         case B43_PHYTYPE_G:
3831                 if (phy_rev > 9)
3832                         unsupported = 1;
3833                 break;
3834 #ifdef CONFIG_B43_NPHY
3835         case B43_PHYTYPE_N:
3836                 if (phy_rev > 4)
3837                         unsupported = 1;
3838                 break;
3839 #endif
3840 #ifdef CONFIG_B43_PHY_LP
3841         case B43_PHYTYPE_LP:
3842                 if (phy_rev > 1)
3843                         unsupported = 1;
3844                 break;
3845 #endif
3846         default:
3847                 unsupported = 1;
3848         };
3849         if (unsupported) {
3850                 b43err(dev->wl, "FOUND UNSUPPORTED PHY "
3851                        "(Analog %u, Type %u, Revision %u)\n",
3852                        analog_type, phy_type, phy_rev);
3853                 return -EOPNOTSUPP;
3854         }
3855         b43dbg(dev->wl, "Found PHY: Analog %u, Type %u, Revision %u\n",
3856                analog_type, phy_type, phy_rev);
3857
3858         /* Get RADIO versioning */
3859         if (dev->dev->bus->chip_id == 0x4317) {
3860                 if (dev->dev->bus->chip_rev == 0)
3861                         tmp = 0x3205017F;
3862                 else if (dev->dev->bus->chip_rev == 1)
3863                         tmp = 0x4205017F;
3864                 else
3865                         tmp = 0x5205017F;
3866         } else {
3867                 b43_write16(dev, B43_MMIO_RADIO_CONTROL, B43_RADIOCTL_ID);
3868                 tmp = b43_read16(dev, B43_MMIO_RADIO_DATA_LOW);
3869                 b43_write16(dev, B43_MMIO_RADIO_CONTROL, B43_RADIOCTL_ID);
3870                 tmp |= (u32)b43_read16(dev, B43_MMIO_RADIO_DATA_HIGH) << 16;
3871         }
3872         radio_manuf = (tmp & 0x00000FFF);
3873         radio_ver = (tmp & 0x0FFFF000) >> 12;
3874         radio_rev = (tmp & 0xF0000000) >> 28;
3875         if (radio_manuf != 0x17F /* Broadcom */)
3876                 unsupported = 1;
3877         switch (phy_type) {
3878         case B43_PHYTYPE_A:
3879                 if (radio_ver != 0x2060)
3880                         unsupported = 1;
3881                 if (radio_rev != 1)
3882                         unsupported = 1;
3883                 if (radio_manuf != 0x17F)
3884                         unsupported = 1;
3885                 break;
3886         case B43_PHYTYPE_B:
3887                 if ((radio_ver & 0xFFF0) != 0x2050)
3888                         unsupported = 1;
3889                 break;
3890         case B43_PHYTYPE_G:
3891                 if (radio_ver != 0x2050)
3892                         unsupported = 1;
3893                 break;
3894         case B43_PHYTYPE_N:
3895                 if (radio_ver != 0x2055 && radio_ver != 0x2056)
3896                         unsupported = 1;
3897                 break;
3898         case B43_PHYTYPE_LP:
3899                 if (radio_ver != 0x2062)
3900                         unsupported = 1;
3901                 break;
3902         default:
3903                 B43_WARN_ON(1);
3904         }
3905         if (unsupported) {
3906                 b43err(dev->wl, "FOUND UNSUPPORTED RADIO "
3907                        "(Manuf 0x%X, Version 0x%X, Revision %u)\n",
3908                        radio_manuf, radio_ver, radio_rev);
3909                 return -EOPNOTSUPP;
3910         }
3911         b43dbg(dev->wl, "Found Radio: Manuf 0x%X, Version 0x%X, Revision %u\n",
3912                radio_manuf, radio_ver, radio_rev);
3913
3914         phy->radio_manuf = radio_manuf;
3915         phy->radio_ver = radio_ver;
3916         phy->radio_rev = radio_rev;
3917
3918         phy->analog = analog_type;
3919         phy->type = phy_type;
3920         phy->rev = phy_rev;
3921
3922         return 0;
3923 }
3924
3925 static void setup_struct_phy_for_init(struct b43_wldev *dev,
3926                                       struct b43_phy *phy)
3927 {
3928         phy->hardware_power_control = !!modparam_hwpctl;
3929         phy->next_txpwr_check_time = jiffies;
3930         /* PHY TX errors counter. */
3931         atomic_set(&phy->txerr_cnt, B43_PHY_TX_BADNESS_LIMIT);
3932 }
3933
3934 static void setup_struct_wldev_for_init(struct b43_wldev *dev)
3935 {
3936         dev->dfq_valid = 0;
3937
3938         /* Assume the radio is enabled. If it's not enabled, the state will
3939          * immediately get fixed on the first periodic work run. */
3940         dev->radio_hw_enable = 1;
3941
3942         /* Stats */
3943         memset(&dev->stats, 0, sizeof(dev->stats));
3944
3945         setup_struct_phy_for_init(dev, &dev->phy);
3946
3947         /* IRQ related flags */
3948         dev->irq_reason = 0;
3949         memset(dev->dma_reason, 0, sizeof(dev->dma_reason));
3950         dev->irq_savedstate = B43_IRQ_MASKTEMPLATE;
3951
3952         dev->mac_suspended = 1;
3953
3954         /* Noise calculation context */
3955         memset(&dev->noisecalc, 0, sizeof(dev->noisecalc));
3956 }
3957
3958 static void b43_bluetooth_coext_enable(struct b43_wldev *dev)
3959 {
3960         struct ssb_sprom *sprom = &dev->dev->bus->sprom;
3961         u64 hf;
3962
3963         if (!modparam_btcoex)
3964                 return;
3965         if (!(sprom->boardflags_lo & B43_BFL_BTCOEXIST))
3966                 return;
3967         if (dev->phy.type != B43_PHYTYPE_B && !dev->phy.gmode)
3968                 return;
3969
3970         hf = b43_hf_read(dev);
3971         if (sprom->boardflags_lo & B43_BFL_BTCMOD)
3972                 hf |= B43_HF_BTCOEXALT;
3973         else
3974                 hf |= B43_HF_BTCOEX;
3975         b43_hf_write(dev, hf);
3976 }
3977
3978 static void b43_bluetooth_coext_disable(struct b43_wldev *dev)
3979 {
3980         if (!modparam_btcoex)
3981                 return;
3982         //TODO
3983 }
3984
3985 static void b43_imcfglo_timeouts_workaround(struct b43_wldev *dev)
3986 {
3987 #ifdef CONFIG_SSB_DRIVER_PCICORE
3988         struct ssb_bus *bus = dev->dev->bus;
3989         u32 tmp;
3990
3991         if (bus->pcicore.dev &&
3992             bus->pcicore.dev->id.coreid == SSB_DEV_PCI &&
3993             bus->pcicore.dev->id.revision <= 5) {
3994                 /* IMCFGLO timeouts workaround. */
3995                 tmp = ssb_read32(dev->dev, SSB_IMCFGLO);
3996                 tmp &= ~SSB_IMCFGLO_REQTO;
3997                 tmp &= ~SSB_IMCFGLO_SERTO;
3998                 switch (bus->bustype) {
3999                 case SSB_BUSTYPE_PCI:
4000                 case SSB_BUSTYPE_PCMCIA:
4001                         tmp |= 0x32;
4002                         break;
4003                 case SSB_BUSTYPE_SSB:
4004                         tmp |= 0x53;
4005                         break;
4006                 }
4007                 ssb_write32(dev->dev, SSB_IMCFGLO, tmp);
4008         }
4009 #endif /* CONFIG_SSB_DRIVER_PCICORE */
4010 }
4011
4012 static void b43_set_synth_pu_delay(struct b43_wldev *dev, bool idle)
4013 {
4014         u16 pu_delay;
4015
4016         /* The time value is in microseconds. */
4017         if (dev->phy.type == B43_PHYTYPE_A)
4018                 pu_delay = 3700;
4019         else
4020                 pu_delay = 1050;
4021         if (b43_is_mode(dev->wl, NL80211_IFTYPE_ADHOC) || idle)
4022                 pu_delay = 500;
4023         if ((dev->phy.radio_ver == 0x2050) && (dev->phy.radio_rev == 8))
4024                 pu_delay = max(pu_delay, (u16)2400);
4025
4026         b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_SPUWKUP, pu_delay);
4027 }
4028
4029 /* Set the TSF CFP pre-TargetBeaconTransmissionTime. */
4030 static void b43_set_pretbtt(struct b43_wldev *dev)
4031 {
4032         u16 pretbtt;
4033
4034         /* The time value is in microseconds. */
4035         if (b43_is_mode(dev->wl, NL80211_IFTYPE_ADHOC)) {
4036                 pretbtt = 2;
4037         } else {
4038                 if (dev->phy.type == B43_PHYTYPE_A)
4039                         pretbtt = 120;
4040                 else
4041                         pretbtt = 250;
4042         }
4043         b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_PRETBTT, pretbtt);
4044         b43_write16(dev, B43_MMIO_TSF_CFP_PRETBTT, pretbtt);
4045 }
4046
4047 /* Shutdown a wireless core */
4048 /* Locking: wl->mutex */
4049 static void b43_wireless_core_exit(struct b43_wldev *dev)
4050 {
4051         u32 macctl;
4052
4053         B43_WARN_ON(b43_status(dev) > B43_STAT_INITIALIZED);
4054         if (b43_status(dev) != B43_STAT_INITIALIZED)
4055                 return;
4056         b43_set_status(dev, B43_STAT_UNINIT);
4057
4058         /* Stop the microcode PSM. */
4059         macctl = b43_read32(dev, B43_MMIO_MACCTL);
4060         macctl &= ~B43_MACCTL_PSM_RUN;
4061         macctl |= B43_MACCTL_PSM_JMP0;
4062         b43_write32(dev, B43_MMIO_MACCTL, macctl);
4063
4064         if (!dev->suspend_in_progress) {
4065                 b43_leds_exit(dev);
4066                 b43_rng_exit(dev->wl);
4067         }
4068         b43_dma_free(dev);
4069         b43_pio_free(dev);
4070         b43_chip_exit(dev);
4071         dev->phy.ops->switch_analog(dev, 0);
4072         if (dev->wl->current_beacon) {
4073                 dev_kfree_skb_any(dev->wl->current_beacon);
4074                 dev->wl->current_beacon = NULL;
4075         }
4076
4077         ssb_device_disable(dev->dev, 0);
4078         ssb_bus_may_powerdown(dev->dev->bus);
4079 }
4080
4081 /* Initialize a wireless core */
4082 static int b43_wireless_core_init(struct b43_wldev *dev)
4083 {
4084         struct b43_wl *wl = dev->wl;
4085         struct ssb_bus *bus = dev->dev->bus;
4086         struct ssb_sprom *sprom = &bus->sprom;
4087         struct b43_phy *phy = &dev->phy;
4088         int err;
4089         u64 hf;
4090         u32 tmp;
4091
4092         B43_WARN_ON(b43_status(dev) != B43_STAT_UNINIT);
4093
4094         err = ssb_bus_powerup(bus, 0);
4095         if (err)
4096                 goto out;
4097         if (!ssb_device_is_enabled(dev->dev)) {
4098                 tmp = phy->gmode ? B43_TMSLOW_GMODE : 0;
4099                 b43_wireless_core_reset(dev, tmp);
4100         }
4101
4102         /* Reset all data structures. */
4103         setup_struct_wldev_for_init(dev);
4104         phy->ops->prepare_structs(dev);
4105
4106         /* Enable IRQ routing to this device. */
4107         ssb_pcicore_dev_irqvecs_enable(&bus->pcicore, dev->dev);
4108
4109         b43_imcfglo_timeouts_workaround(dev);
4110         b43_bluetooth_coext_disable(dev);
4111         if (phy->ops->prepare_hardware) {
4112                 err = phy->ops->prepare_hardware(dev);
4113                 if (err)
4114                         goto err_busdown;
4115         }
4116         err = b43_chip_init(dev);
4117         if (err)
4118                 goto err_busdown;
4119         b43_shm_write16(dev, B43_SHM_SHARED,
4120                         B43_SHM_SH_WLCOREREV, dev->dev->id.revision);
4121         hf = b43_hf_read(dev);
4122         if (phy->type == B43_PHYTYPE_G) {
4123                 hf |= B43_HF_SYMW;
4124                 if (phy->rev == 1)
4125                         hf |= B43_HF_GDCW;
4126                 if (sprom->boardflags_lo & B43_BFL_PACTRL)
4127                         hf |= B43_HF_OFDMPABOOST;
4128         } else if (phy->type == B43_PHYTYPE_B) {
4129                 hf |= B43_HF_SYMW;
4130                 if (phy->rev >= 2 && phy->radio_ver == 0x2050)
4131                         hf &= ~B43_HF_GDCW;
4132         }
4133         b43_hf_write(dev, hf);
4134
4135         b43_set_retry_limits(dev, B43_DEFAULT_SHORT_RETRY_LIMIT,
4136                              B43_DEFAULT_LONG_RETRY_LIMIT);
4137         b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_SFFBLIM, 3);
4138         b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_LFFBLIM, 2);
4139
4140         /* Disable sending probe responses from firmware.
4141          * Setting the MaxTime to one usec will always trigger
4142          * a timeout, so we never send any probe resp.
4143          * A timeout of zero is infinite. */
4144         b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_PRMAXTIME, 1);
4145
4146         b43_rate_memory_init(dev);
4147         b43_set_phytxctl_defaults(dev);
4148
4149         /* Minimum Contention Window */
4150         if (phy->type == B43_PHYTYPE_B) {
4151                 b43_shm_write16(dev, B43_SHM_SCRATCH, B43_SHM_SC_MINCONT, 0x1F);
4152         } else {
4153                 b43_shm_write16(dev, B43_SHM_SCRATCH, B43_SHM_SC_MINCONT, 0xF);
4154         }
4155         /* Maximum Contention Window */
4156         b43_shm_write16(dev, B43_SHM_SCRATCH, B43_SHM_SC_MAXCONT, 0x3FF);
4157
4158         if ((dev->dev->bus->bustype == SSB_BUSTYPE_PCMCIA) || B43_FORCE_PIO) {
4159                 dev->__using_pio_transfers = 1;
4160                 err = b43_pio_init(dev);
4161         } else {
4162                 dev->__using_pio_transfers = 0;
4163                 err = b43_dma_init(dev);
4164         }
4165         if (err)
4166                 goto err_chip_exit;
4167         b43_qos_init(dev);
4168         b43_set_synth_pu_delay(dev, 1);
4169         b43_bluetooth_coext_enable(dev);
4170
4171         ssb_bus_powerup(bus, 1);        /* Enable dynamic PCTL */
4172         b43_upload_card_macaddress(dev);
4173         b43_security_init(dev);
4174         if (!dev->suspend_in_progress)
4175                 b43_rng_init(wl);
4176
4177         b43_set_status(dev, B43_STAT_INITIALIZED);
4178
4179         if (!dev->suspend_in_progress)
4180                 b43_leds_init(dev);
4181 out:
4182         return err;
4183
4184 err_chip_exit:
4185         b43_chip_exit(dev);
4186 err_busdown:
4187         ssb_bus_may_powerdown(bus);
4188         B43_WARN_ON(b43_status(dev) != B43_STAT_UNINIT);
4189         return err;
4190 }
4191
4192 static int b43_op_add_interface(struct ieee80211_hw *hw,
4193                                 struct ieee80211_if_init_conf *conf)
4194 {
4195         struct b43_wl *wl = hw_to_b43_wl(hw);
4196         struct b43_wldev *dev;
4197         unsigned long flags;
4198         int err = -EOPNOTSUPP;
4199
4200         /* TODO: allow WDS/AP devices to coexist */
4201
4202         if (conf->type != NL80211_IFTYPE_AP &&
4203             conf->type != NL80211_IFTYPE_MESH_POINT &&
4204             conf->type != NL80211_IFTYPE_STATION &&
4205             conf->type != NL80211_IFTYPE_WDS &&
4206             conf->type != NL80211_IFTYPE_ADHOC)
4207                 return -EOPNOTSUPP;
4208
4209         mutex_lock(&wl->mutex);
4210         if (wl->operating)
4211                 goto out_mutex_unlock;
4212
4213         b43dbg(wl, "Adding Interface type %d\n", conf->type);
4214
4215         dev = wl->current_dev;
4216         wl->operating = 1;
4217         wl->vif = conf->vif;
4218         wl->if_type = conf->type;
4219         memcpy(wl->mac_addr, conf->mac_addr, ETH_ALEN);
4220
4221         spin_lock_irqsave(&wl->irq_lock, flags);
4222         b43_adjust_opmode(dev);
4223         b43_set_pretbtt(dev);
4224         b43_set_synth_pu_delay(dev, 0);
4225         b43_upload_card_macaddress(dev);
4226         spin_unlock_irqrestore(&wl->irq_lock, flags);
4227
4228         err = 0;
4229  out_mutex_unlock:
4230         mutex_unlock(&wl->mutex);
4231
4232         return err;
4233 }
4234
4235 static void b43_op_remove_interface(struct ieee80211_hw *hw,
4236                                     struct ieee80211_if_init_conf *conf)
4237 {
4238         struct b43_wl *wl = hw_to_b43_wl(hw);
4239         struct b43_wldev *dev = wl->current_dev;
4240         unsigned long flags;
4241
4242         b43dbg(wl, "Removing Interface type %d\n", conf->type);
4243
4244         mutex_lock(&wl->mutex);
4245
4246         B43_WARN_ON(!wl->operating);
4247         B43_WARN_ON(wl->vif != conf->vif);
4248         wl->vif = NULL;
4249
4250         wl->operating = 0;
4251
4252         spin_lock_irqsave(&wl->irq_lock, flags);
4253         b43_adjust_opmode(dev);
4254         memset(wl->mac_addr, 0, ETH_ALEN);
4255         b43_upload_card_macaddress(dev);
4256         spin_unlock_irqrestore(&wl->irq_lock, flags);
4257
4258         mutex_unlock(&wl->mutex);
4259 }
4260
4261 static int b43_op_start(struct ieee80211_hw *hw)
4262 {
4263         struct b43_wl *wl = hw_to_b43_wl(hw);
4264         struct b43_wldev *dev = wl->current_dev;
4265         int did_init = 0;
4266         int err = 0;
4267         bool do_rfkill_exit = 0;
4268
4269         /* Kill all old instance specific information to make sure
4270          * the card won't use it in the short timeframe between start
4271          * and mac80211 reconfiguring it. */
4272         memset(wl->bssid, 0, ETH_ALEN);
4273         memset(wl->mac_addr, 0, ETH_ALEN);
4274         wl->filter_flags = 0;
4275         wl->radiotap_enabled = 0;
4276         b43_qos_clear(wl);
4277         wl->beacon0_uploaded = 0;
4278         wl->beacon1_uploaded = 0;
4279         wl->beacon_templates_virgin = 1;
4280
4281         /* First register RFkill.
4282          * LEDs that are registered later depend on it. */
4283         b43_rfkill_init(dev);
4284
4285         mutex_lock(&wl->mutex);
4286
4287         if (b43_status(dev) < B43_STAT_INITIALIZED) {
4288                 err = b43_wireless_core_init(dev);
4289                 if (err) {
4290                         do_rfkill_exit = 1;
4291                         goto out_mutex_unlock;
4292                 }
4293                 did_init = 1;
4294         }
4295
4296         if (b43_status(dev) < B43_STAT_STARTED) {
4297                 err = b43_wireless_core_start(dev);
4298                 if (err) {
4299                         if (did_init)
4300                                 b43_wireless_core_exit(dev);
4301                         do_rfkill_exit = 1;
4302                         goto out_mutex_unlock;
4303                 }
4304         }
4305
4306  out_mutex_unlock:
4307         mutex_unlock(&wl->mutex);
4308
4309         if (do_rfkill_exit)
4310                 b43_rfkill_exit(dev);
4311
4312         return err;
4313 }
4314
4315 static void b43_op_stop(struct ieee80211_hw *hw)
4316 {
4317         struct b43_wl *wl = hw_to_b43_wl(hw);
4318         struct b43_wldev *dev = wl->current_dev;
4319
4320         b43_rfkill_exit(dev);
4321         cancel_work_sync(&(wl->beacon_update_trigger));
4322
4323         mutex_lock(&wl->mutex);
4324         if (b43_status(dev) >= B43_STAT_STARTED)
4325                 b43_wireless_core_stop(dev);
4326         b43_wireless_core_exit(dev);
4327         mutex_unlock(&wl->mutex);
4328
4329         cancel_work_sync(&(wl->txpower_adjust_work));
4330 }
4331
4332 static int b43_op_beacon_set_tim(struct ieee80211_hw *hw,
4333                                  struct ieee80211_sta *sta, bool set)
4334 {
4335         struct b43_wl *wl = hw_to_b43_wl(hw);
4336         unsigned long flags;
4337
4338         spin_lock_irqsave(&wl->irq_lock, flags);
4339         b43_update_templates(wl);
4340         spin_unlock_irqrestore(&wl->irq_lock, flags);
4341
4342         return 0;
4343 }
4344
4345 static void b43_op_sta_notify(struct ieee80211_hw *hw,
4346                               struct ieee80211_vif *vif,
4347                               enum sta_notify_cmd notify_cmd,
4348                               struct ieee80211_sta *sta)
4349 {
4350         struct b43_wl *wl = hw_to_b43_wl(hw);
4351
4352         B43_WARN_ON(!vif || wl->vif != vif);
4353 }
4354
4355 static const struct ieee80211_ops b43_hw_ops = {
4356         .tx                     = b43_op_tx,
4357         .conf_tx                = b43_op_conf_tx,
4358         .add_interface          = b43_op_add_interface,
4359         .remove_interface       = b43_op_remove_interface,
4360         .config                 = b43_op_config,
4361         .bss_info_changed       = b43_op_bss_info_changed,
4362         .config_interface       = b43_op_config_interface,
4363         .configure_filter       = b43_op_configure_filter,
4364         .set_key                = b43_op_set_key,
4365         .get_stats              = b43_op_get_stats,
4366         .get_tx_stats           = b43_op_get_tx_stats,
4367         .start                  = b43_op_start,
4368         .stop                   = b43_op_stop,
4369         .set_tim                = b43_op_beacon_set_tim,
4370         .sta_notify             = b43_op_sta_notify,
4371 };
4372
4373 /* Hard-reset the chip. Do not call this directly.
4374  * Use b43_controller_restart()
4375  */
4376 static void b43_chip_reset(struct work_struct *work)
4377 {
4378         struct b43_wldev *dev =
4379             container_of(work, struct b43_wldev, restart_work);
4380         struct b43_wl *wl = dev->wl;
4381         int err = 0;
4382         int prev_status;
4383
4384         mutex_lock(&wl->mutex);
4385
4386         prev_status = b43_status(dev);
4387         /* Bring the device down... */
4388         if (prev_status >= B43_STAT_STARTED)
4389                 b43_wireless_core_stop(dev);
4390         if (prev_status >= B43_STAT_INITIALIZED)
4391                 b43_wireless_core_exit(dev);
4392
4393         /* ...and up again. */
4394         if (prev_status >= B43_STAT_INITIALIZED) {
4395                 err = b43_wireless_core_init(dev);
4396                 if (err)
4397                         goto out;
4398         }
4399         if (prev_status >= B43_STAT_STARTED) {
4400                 err = b43_wireless_core_start(dev);
4401                 if (err) {
4402                         b43_wireless_core_exit(dev);
4403                         goto out;
4404                 }
4405         }
4406 out:
4407         if (err)
4408                 wl->current_dev = NULL; /* Failed to init the dev. */
4409         mutex_unlock(&wl->mutex);
4410         if (err)
4411                 b43err(wl, "Controller restart FAILED\n");
4412         else
4413                 b43info(wl, "Controller restarted\n");
4414 }
4415
4416 static int b43_setup_bands(struct b43_wldev *dev,
4417                            bool have_2ghz_phy, bool have_5ghz_phy)
4418 {
4419         struct ieee80211_hw *hw = dev->wl->hw;
4420
4421         if (have_2ghz_phy)
4422                 hw->wiphy->bands[IEEE80211_BAND_2GHZ] = &b43_band_2GHz;
4423         if (dev->phy.type == B43_PHYTYPE_N) {
4424                 if (have_5ghz_phy)
4425                         hw->wiphy->bands[IEEE80211_BAND_5GHZ] = &b43_band_5GHz_nphy;
4426         } else {
4427                 if (have_5ghz_phy)
4428                         hw->wiphy->bands[IEEE80211_BAND_5GHZ] = &b43_band_5GHz_aphy;
4429         }
4430
4431         dev->phy.supports_2ghz = have_2ghz_phy;
4432         dev->phy.supports_5ghz = have_5ghz_phy;
4433
4434         return 0;
4435 }
4436
4437 static void b43_wireless_core_detach(struct b43_wldev *dev)
4438 {
4439         /* We release firmware that late to not be required to re-request
4440          * is all the time when we reinit the core. */
4441         b43_release_firmware(dev);
4442         b43_phy_free(dev);
4443 }
4444
4445 static int b43_wireless_core_attach(struct b43_wldev *dev)
4446 {
4447         struct b43_wl *wl = dev->wl;
4448         struct ssb_bus *bus = dev->dev->bus;
4449         struct pci_dev *pdev = bus->host_pci;
4450         int err;
4451         bool have_2ghz_phy = 0, have_5ghz_phy = 0;
4452         u32 tmp;
4453
4454         /* Do NOT do any device initialization here.
4455          * Do it in wireless_core_init() instead.
4456          * This function is for gathering basic information about the HW, only.
4457          * Also some structs may be set up here. But most likely you want to have
4458          * that in core_init(), too.
4459          */
4460
4461         err = ssb_bus_powerup(bus, 0);
4462         if (err) {
4463                 b43err(wl, "Bus powerup failed\n");
4464                 goto out;
4465         }
4466         /* Get the PHY type. */
4467         if (dev->dev->id.revision >= 5) {
4468                 u32 tmshigh;
4469
4470                 tmshigh = ssb_read32(dev->dev, SSB_TMSHIGH);
4471                 have_2ghz_phy = !!(tmshigh & B43_TMSHIGH_HAVE_2GHZ_PHY);
4472                 have_5ghz_phy = !!(tmshigh & B43_TMSHIGH_HAVE_5GHZ_PHY);
4473         } else
4474                 B43_WARN_ON(1);
4475
4476         dev->phy.gmode = have_2ghz_phy;
4477         tmp = dev->phy.gmode ? B43_TMSLOW_GMODE : 0;
4478         b43_wireless_core_reset(dev, tmp);
4479
4480         err = b43_phy_versioning(dev);
4481         if (err)
4482                 goto err_powerdown;
4483         /* Check if this device supports multiband. */
4484         if (!pdev ||
4485             (pdev->device != 0x4312 &&
4486              pdev->device != 0x4319 && pdev->device != 0x4324)) {
4487                 /* No multiband support. */
4488                 have_2ghz_phy = 0;
4489                 have_5ghz_phy = 0;
4490                 switch (dev->phy.type) {
4491                 case B43_PHYTYPE_A:
4492                         have_5ghz_phy = 1;
4493                         break;
4494                 case B43_PHYTYPE_G:
4495                 case B43_PHYTYPE_N:
4496                 case B43_PHYTYPE_LP:
4497                         have_2ghz_phy = 1;
4498                         break;
4499                 default:
4500                         B43_WARN_ON(1);
4501                 }
4502         }
4503         if (dev->phy.type == B43_PHYTYPE_A) {
4504                 /* FIXME */
4505                 b43err(wl, "IEEE 802.11a devices are unsupported\n");
4506                 err = -EOPNOTSUPP;
4507                 goto err_powerdown;
4508         }
4509         if (1 /* disable A-PHY */) {
4510                 /* FIXME: For now we disable the A-PHY on multi-PHY devices. */
4511                 if (dev->phy.type != B43_PHYTYPE_N) {
4512                         have_2ghz_phy = 1;
4513                         have_5ghz_phy = 0;
4514                 }
4515         }
4516
4517         err = b43_phy_allocate(dev);
4518         if (err)
4519                 goto err_powerdown;
4520
4521         dev->phy.gmode = have_2ghz_phy;
4522         tmp = dev->phy.gmode ? B43_TMSLOW_GMODE : 0;
4523         b43_wireless_core_reset(dev, tmp);
4524
4525         err = b43_validate_chipaccess(dev);
4526         if (err)
4527                 goto err_phy_free;
4528         err = b43_setup_bands(dev, have_2ghz_phy, have_5ghz_phy);
4529         if (err)
4530                 goto err_phy_free;
4531
4532         /* Now set some default "current_dev" */
4533         if (!wl->current_dev)
4534                 wl->current_dev = dev;
4535         INIT_WORK(&dev->restart_work, b43_chip_reset);
4536
4537         dev->phy.ops->switch_analog(dev, 0);
4538         ssb_device_disable(dev->dev, 0);
4539         ssb_bus_may_powerdown(bus);
4540
4541 out:
4542         return err;
4543
4544 err_phy_free:
4545         b43_phy_free(dev);
4546 err_powerdown:
4547         ssb_bus_may_powerdown(bus);
4548         return err;
4549 }
4550
4551 static void b43_one_core_detach(struct ssb_device *dev)
4552 {
4553         struct b43_wldev *wldev;
4554         struct b43_wl *wl;
4555
4556         /* Do not cancel ieee80211-workqueue based work here.
4557          * See comment in b43_remove(). */
4558
4559         wldev = ssb_get_drvdata(dev);
4560         wl = wldev->wl;
4561         b43_debugfs_remove_device(wldev);
4562         b43_wireless_core_detach(wldev);
4563         list_del(&wldev->list);
4564         wl->nr_devs--;
4565         ssb_set_drvdata(dev, NULL);
4566         kfree(wldev);
4567 }
4568
4569 static int b43_one_core_attach(struct ssb_device *dev, struct b43_wl *wl)
4570 {
4571         struct b43_wldev *wldev;
4572         struct pci_dev *pdev;
4573         int err = -ENOMEM;
4574
4575         if (!list_empty(&wl->devlist)) {
4576                 /* We are not the first core on this chip. */
4577                 pdev = dev->bus->host_pci;
4578                 /* Only special chips support more than one wireless
4579                  * core, although some of the other chips have more than
4580                  * one wireless core as well. Check for this and
4581                  * bail out early.
4582                  */
4583                 if (!pdev ||
4584                     ((pdev->device != 0x4321) &&
4585                      (pdev->device != 0x4313) && (pdev->device != 0x431A))) {
4586                         b43dbg(wl, "Ignoring unconnected 802.11 core\n");
4587                         return -ENODEV;
4588                 }
4589         }
4590
4591         wldev = kzalloc(sizeof(*wldev), GFP_KERNEL);
4592         if (!wldev)
4593                 goto out;
4594
4595         wldev->dev = dev;
4596         wldev->wl = wl;
4597         b43_set_status(wldev, B43_STAT_UNINIT);
4598         wldev->bad_frames_preempt = modparam_bad_frames_preempt;
4599         tasklet_init(&wldev->isr_tasklet,
4600                      (void (*)(unsigned long))b43_interrupt_tasklet,
4601                      (unsigned long)wldev);
4602         INIT_LIST_HEAD(&wldev->list);
4603
4604         err = b43_wireless_core_attach(wldev);
4605         if (err)
4606                 goto err_kfree_wldev;
4607
4608         list_add(&wldev->list, &wl->devlist);
4609         wl->nr_devs++;
4610         ssb_set_drvdata(dev, wldev);
4611         b43_debugfs_add_device(wldev);
4612
4613       out:
4614         return err;
4615
4616       err_kfree_wldev:
4617         kfree(wldev);
4618         return err;
4619 }
4620
4621 #define IS_PDEV(pdev, _vendor, _device, _subvendor, _subdevice)         ( \
4622         (pdev->vendor == PCI_VENDOR_ID_##_vendor) &&                    \
4623         (pdev->device == _device) &&                                    \
4624         (pdev->subsystem_vendor == PCI_VENDOR_ID_##_subvendor) &&       \
4625         (pdev->subsystem_device == _subdevice)                          )
4626
4627 static void b43_sprom_fixup(struct ssb_bus *bus)
4628 {
4629         struct pci_dev *pdev;
4630
4631         /* boardflags workarounds */
4632         if (bus->boardinfo.vendor == SSB_BOARDVENDOR_DELL &&
4633             bus->chip_id == 0x4301 && bus->boardinfo.rev == 0x74)
4634                 bus->sprom.boardflags_lo |= B43_BFL_BTCOEXIST;
4635         if (bus->boardinfo.vendor == PCI_VENDOR_ID_APPLE &&
4636             bus->boardinfo.type == 0x4E && bus->boardinfo.rev > 0x40)
4637                 bus->sprom.boardflags_lo |= B43_BFL_PACTRL;
4638         if (bus->bustype == SSB_BUSTYPE_PCI) {
4639                 pdev = bus->host_pci;
4640                 if (IS_PDEV(pdev, BROADCOM, 0x4318, ASUSTEK, 0x100F) ||
4641                     IS_PDEV(pdev, BROADCOM, 0x4320,    DELL, 0x0003) ||
4642                     IS_PDEV(pdev, BROADCOM, 0x4320,      HP, 0x12f8) ||
4643                     IS_PDEV(pdev, BROADCOM, 0x4320, LINKSYS, 0x0015) ||
4644                     IS_PDEV(pdev, BROADCOM, 0x4320, LINKSYS, 0x0014) ||
4645                     IS_PDEV(pdev, BROADCOM, 0x4320, LINKSYS, 0x0013) ||
4646                     IS_PDEV(pdev, BROADCOM, 0x4320, MOTOROLA, 0x7010))
4647                         bus->sprom.boardflags_lo &= ~B43_BFL_BTCOEXIST;
4648         }
4649 }
4650
4651 static void b43_wireless_exit(struct ssb_device *dev, struct b43_wl *wl)
4652 {
4653         struct ieee80211_hw *hw = wl->hw;
4654
4655         ssb_set_devtypedata(dev, NULL);
4656         ieee80211_free_hw(hw);
4657 }
4658
4659 static int b43_wireless_init(struct ssb_device *dev)
4660 {
4661         struct ssb_sprom *sprom = &dev->bus->sprom;
4662         struct ieee80211_hw *hw;
4663         struct b43_wl *wl;
4664         int err = -ENOMEM;
4665
4666         b43_sprom_fixup(dev->bus);
4667
4668         hw = ieee80211_alloc_hw(sizeof(*wl), &b43_hw_ops);
4669         if (!hw) {
4670                 b43err(NULL, "Could not allocate ieee80211 device\n");
4671                 goto out;
4672         }
4673
4674         /* fill hw info */
4675         hw->flags = IEEE80211_HW_RX_INCLUDES_FCS |
4676                     IEEE80211_HW_SIGNAL_DBM |
4677                     IEEE80211_HW_NOISE_DBM;
4678
4679         hw->wiphy->interface_modes =
4680                 BIT(NL80211_IFTYPE_AP) |
4681                 BIT(NL80211_IFTYPE_MESH_POINT) |
4682                 BIT(NL80211_IFTYPE_STATION) |
4683                 BIT(NL80211_IFTYPE_WDS) |
4684                 BIT(NL80211_IFTYPE_ADHOC);
4685
4686         hw->queues = b43_modparam_qos ? 4 : 1;
4687         hw->max_rates = 2;
4688         SET_IEEE80211_DEV(hw, dev->dev);
4689         if (is_valid_ether_addr(sprom->et1mac))
4690                 SET_IEEE80211_PERM_ADDR(hw, sprom->et1mac);
4691         else
4692                 SET_IEEE80211_PERM_ADDR(hw, sprom->il0mac);
4693
4694         /* Get and initialize struct b43_wl */
4695         wl = hw_to_b43_wl(hw);
4696         memset(wl, 0, sizeof(*wl));
4697         wl->hw = hw;
4698         spin_lock_init(&wl->irq_lock);
4699         rwlock_init(&wl->tx_lock);
4700         spin_lock_init(&wl->leds_lock);
4701         spin_lock_init(&wl->shm_lock);
4702         mutex_init(&wl->mutex);
4703         INIT_LIST_HEAD(&wl->devlist);
4704         INIT_WORK(&wl->beacon_update_trigger, b43_beacon_update_trigger_work);
4705         INIT_WORK(&wl->txpower_adjust_work, b43_phy_txpower_adjust_work);
4706
4707         ssb_set_devtypedata(dev, wl);
4708         b43info(wl, "Broadcom %04X WLAN found\n", dev->bus->chip_id);
4709         err = 0;
4710       out:
4711         return err;
4712 }
4713
4714 static int b43_probe(struct ssb_device *dev, const struct ssb_device_id *id)
4715 {
4716         struct b43_wl *wl;
4717         int err;
4718         int first = 0;
4719
4720         wl = ssb_get_devtypedata(dev);
4721         if (!wl) {
4722                 /* Probing the first core. Must setup common struct b43_wl */
4723                 first = 1;
4724                 err = b43_wireless_init(dev);
4725                 if (err)
4726                         goto out;
4727                 wl = ssb_get_devtypedata(dev);
4728                 B43_WARN_ON(!wl);
4729         }
4730         err = b43_one_core_attach(dev, wl);
4731         if (err)
4732                 goto err_wireless_exit;
4733
4734         if (first) {
4735                 err = ieee80211_register_hw(wl->hw);
4736                 if (err)
4737                         goto err_one_core_detach;
4738         }
4739
4740       out:
4741         return err;
4742
4743       err_one_core_detach:
4744         b43_one_core_detach(dev);
4745       err_wireless_exit:
4746         if (first)
4747                 b43_wireless_exit(dev, wl);
4748         return err;
4749 }
4750
4751 static void b43_remove(struct ssb_device *dev)
4752 {
4753         struct b43_wl *wl = ssb_get_devtypedata(dev);
4754         struct b43_wldev *wldev = ssb_get_drvdata(dev);
4755
4756         /* We must cancel any work here before unregistering from ieee80211,
4757          * as the ieee80211 unreg will destroy the workqueue. */
4758         cancel_work_sync(&wldev->restart_work);
4759
4760         B43_WARN_ON(!wl);
4761         if (wl->current_dev == wldev)
4762                 ieee80211_unregister_hw(wl->hw);
4763
4764         b43_one_core_detach(dev);
4765
4766         if (list_empty(&wl->devlist)) {
4767                 /* Last core on the chip unregistered.
4768                  * We can destroy common struct b43_wl.
4769                  */
4770                 b43_wireless_exit(dev, wl);
4771         }
4772 }
4773
4774 /* Perform a hardware reset. This can be called from any context. */
4775 void b43_controller_restart(struct b43_wldev *dev, const char *reason)
4776 {
4777         /* Must avoid requeueing, if we are in shutdown. */
4778         if (b43_status(dev) < B43_STAT_INITIALIZED)
4779                 return;
4780         b43info(dev->wl, "Controller RESET (%s) ...\n", reason);
4781         queue_work(dev->wl->hw->workqueue, &dev->restart_work);
4782 }
4783
4784 #ifdef CONFIG_PM
4785
4786 static int b43_suspend(struct ssb_device *dev, pm_message_t state)
4787 {
4788         struct b43_wldev *wldev = ssb_get_drvdata(dev);
4789         struct b43_wl *wl = wldev->wl;
4790
4791         b43dbg(wl, "Suspending...\n");
4792
4793         mutex_lock(&wl->mutex);
4794         wldev->suspend_in_progress = true;
4795         wldev->suspend_init_status = b43_status(wldev);
4796         if (wldev->suspend_init_status >= B43_STAT_STARTED)
4797                 b43_wireless_core_stop(wldev);
4798         if (wldev->suspend_init_status >= B43_STAT_INITIALIZED)
4799                 b43_wireless_core_exit(wldev);
4800         mutex_unlock(&wl->mutex);
4801
4802         b43dbg(wl, "Device suspended.\n");
4803
4804         return 0;
4805 }
4806
4807 static int b43_resume(struct ssb_device *dev)
4808 {
4809         struct b43_wldev *wldev = ssb_get_drvdata(dev);
4810         struct b43_wl *wl = wldev->wl;
4811         int err = 0;
4812
4813         b43dbg(wl, "Resuming...\n");
4814
4815         mutex_lock(&wl->mutex);
4816         if (wldev->suspend_init_status >= B43_STAT_INITIALIZED) {
4817                 err = b43_wireless_core_init(wldev);
4818                 if (err) {
4819                         b43err(wl, "Resume failed at core init\n");
4820                         goto out;
4821                 }
4822         }
4823         if (wldev->suspend_init_status >= B43_STAT_STARTED) {
4824                 err = b43_wireless_core_start(wldev);
4825                 if (err) {
4826                         b43_leds_exit(wldev);
4827                         b43_rng_exit(wldev->wl);
4828                         b43_wireless_core_exit(wldev);
4829                         b43err(wl, "Resume failed at core start\n");
4830                         goto out;
4831                 }
4832         }
4833         b43dbg(wl, "Device resumed.\n");
4834  out:
4835         wldev->suspend_in_progress = false;
4836         mutex_unlock(&wl->mutex);
4837         return err;
4838 }
4839
4840 #else /* CONFIG_PM */
4841 # define b43_suspend    NULL
4842 # define b43_resume     NULL
4843 #endif /* CONFIG_PM */
4844
4845 static struct ssb_driver b43_ssb_driver = {
4846         .name           = KBUILD_MODNAME,
4847         .id_table       = b43_ssb_tbl,
4848         .probe          = b43_probe,
4849         .remove         = b43_remove,
4850         .suspend        = b43_suspend,
4851         .resume         = b43_resume,
4852 };
4853
4854 static void b43_print_driverinfo(void)
4855 {
4856         const char *feat_pci = "", *feat_pcmcia = "", *feat_nphy = "",
4857                    *feat_leds = "", *feat_rfkill = "";
4858
4859 #ifdef CONFIG_B43_PCI_AUTOSELECT
4860         feat_pci = "P";
4861 #endif
4862 #ifdef CONFIG_B43_PCMCIA
4863         feat_pcmcia = "M";
4864 #endif
4865 #ifdef CONFIG_B43_NPHY
4866         feat_nphy = "N";
4867 #endif
4868 #ifdef CONFIG_B43_LEDS
4869         feat_leds = "L";
4870 #endif
4871 #ifdef CONFIG_B43_RFKILL
4872         feat_rfkill = "R";
4873 #endif
4874         printk(KERN_INFO "Broadcom 43xx driver loaded "
4875                "[ Features: %s%s%s%s%s, Firmware-ID: "
4876                B43_SUPPORTED_FIRMWARE_ID " ]\n",
4877                feat_pci, feat_pcmcia, feat_nphy,
4878                feat_leds, feat_rfkill);
4879 }
4880
4881 static int __init b43_init(void)
4882 {
4883         int err;
4884
4885         b43_debugfs_init();
4886         err = b43_pcmcia_init();
4887         if (err)
4888                 goto err_dfs_exit;
4889         err = ssb_driver_register(&b43_ssb_driver);
4890         if (err)
4891                 goto err_pcmcia_exit;
4892         b43_print_driverinfo();
4893
4894         return err;
4895
4896 err_pcmcia_exit:
4897         b43_pcmcia_exit();
4898 err_dfs_exit:
4899         b43_debugfs_exit();
4900         return err;
4901 }
4902
4903 static void __exit b43_exit(void)
4904 {
4905         ssb_driver_unregister(&b43_ssb_driver);
4906         b43_pcmcia_exit();
4907         b43_debugfs_exit();
4908 }
4909
4910 module_init(b43_init)
4911 module_exit(b43_exit)