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