drivers/net/wireless/b43/main.c

   1 /*
   2
   3   Broadcom B43 wireless driver
   4
   5   Copyright (c) 2005 Martin Langer <[email protected]>
   6   Copyright (c) 2005 Stefano Brivio <[email protected]>
   7   Copyright (c) 2005-2009 Michael Buesch <[email protected]>
   8   Copyright (c) 2005 Danny van Dyk <[email protected]>
   9   Copyright (c) 2005 Andreas Jaggi <[email protected]>
  10
  11   SDIO support
  12   Copyright (c) 2009 Albert Herranz <[email protected]>
  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                 flags |= B43_TMSLOW_PHY_BANDWIDTH_20MHZ; /* Make 20 MHz def */
1155         ssb_device_enable(dev->dev, flags);
1156         msleep(2);              /* Wait for the PLL to turn on. */
1157
1158         /* Now take the PHY out of Reset again */
1159         tmslow = ssb_read32(dev->dev, SSB_TMSLOW);
1160         tmslow |= SSB_TMSLOW_FGC;
1161         tmslow &= ~B43_TMSLOW_PHYRESET;
1162         ssb_write32(dev->dev, SSB_TMSLOW, tmslow);
1163         ssb_read32(dev->dev, SSB_TMSLOW);       /* flush */
1164         msleep(1);
1165         tmslow &= ~SSB_TMSLOW_FGC;
1166         ssb_write32(dev->dev, SSB_TMSLOW, tmslow);
1167         ssb_read32(dev->dev, SSB_TMSLOW);       /* flush */
1168         msleep(1);
1169
1170         /* Turn Analog ON, but only if we already know the PHY-type.
1171          * This protects against very early setup where we don't know the
1172          * PHY-type, yet. wireless_core_reset will be called once again later,
1173          * when we know the PHY-type. */
1174         if (dev->phy.ops)
1175                 dev->phy.ops->switch_analog(dev, 1);
1176
1177         macctl = b43_read32(dev, B43_MMIO_MACCTL);
1178         macctl &= ~B43_MACCTL_GMODE;
1179         if (flags & B43_TMSLOW_GMODE)
1180                 macctl |= B43_MACCTL_GMODE;
1181         macctl |= B43_MACCTL_IHR_ENABLED;
1182         b43_write32(dev, B43_MMIO_MACCTL, macctl);
1183 }
1184
1185 static void handle_irq_transmit_status(struct b43_wldev *dev)
1186 {
1187         u32 v0, v1;
1188         u16 tmp;
1189         struct b43_txstatus stat;
1190
1191         while (1) {
1192                 v0 = b43_read32(dev, B43_MMIO_XMITSTAT_0);
1193                 if (!(v0 & 0x00000001))
1194                         break;
1195                 v1 = b43_read32(dev, B43_MMIO_XMITSTAT_1);
1196
1197                 stat.cookie = (v0 >> 16);
1198                 stat.seq = (v1 & 0x0000FFFF);
1199                 stat.phy_stat = ((v1 & 0x00FF0000) >> 16);
1200                 tmp = (v0 & 0x0000FFFF);
1201                 stat.frame_count = ((tmp & 0xF000) >> 12);
1202                 stat.rts_count = ((tmp & 0x0F00) >> 8);
1203                 stat.supp_reason = ((tmp & 0x001C) >> 2);
1204                 stat.pm_indicated = !!(tmp & 0x0080);
1205                 stat.intermediate = !!(tmp & 0x0040);
1206                 stat.for_ampdu = !!(tmp & 0x0020);
1207                 stat.acked = !!(tmp & 0x0002);
1208
1209                 b43_handle_txstatus(dev, &stat);
1210         }
1211 }
1212
1213 static void drain_txstatus_queue(struct b43_wldev *dev)
1214 {
1215         u32 dummy;
1216
1217         if (dev->dev->id.revision < 5)
1218                 return;
1219         /* Read all entries from the microcode TXstatus FIFO
1220          * and throw them away.
1221          */
1222         while (1) {
1223                 dummy = b43_read32(dev, B43_MMIO_XMITSTAT_0);
1224                 if (!(dummy & 0x00000001))
1225                         break;
1226                 dummy = b43_read32(dev, B43_MMIO_XMITSTAT_1);
1227         }
1228 }
1229
1230 static u32 b43_jssi_read(struct b43_wldev *dev)
1231 {
1232         u32 val = 0;
1233
1234         val = b43_shm_read16(dev, B43_SHM_SHARED, 0x08A);
1235         val <<= 16;
1236         val |= b43_shm_read16(dev, B43_SHM_SHARED, 0x088);
1237
1238         return val;
1239 }
1240
1241 static void b43_jssi_write(struct b43_wldev *dev, u32 jssi)
1242 {
1243         b43_shm_write16(dev, B43_SHM_SHARED, 0x088, (jssi & 0x0000FFFF));
1244         b43_shm_write16(dev, B43_SHM_SHARED, 0x08A, (jssi & 0xFFFF0000) >> 16);
1245 }
1246
1247 static void b43_generate_noise_sample(struct b43_wldev *dev)
1248 {
1249         b43_jssi_write(dev, 0x7F7F7F7F);
1250         b43_write32(dev, B43_MMIO_MACCMD,
1251                     b43_read32(dev, B43_MMIO_MACCMD) | B43_MACCMD_BGNOISE);
1252 }
1253
1254 static void b43_calculate_link_quality(struct b43_wldev *dev)
1255 {
1256         /* Top half of Link Quality calculation. */
1257
1258         if (dev->phy.type != B43_PHYTYPE_G)
1259                 return;
1260         if (dev->noisecalc.calculation_running)
1261                 return;
1262         dev->noisecalc.calculation_running = 1;
1263         dev->noisecalc.nr_samples = 0;
1264
1265         b43_generate_noise_sample(dev);
1266 }
1267
1268 static void handle_irq_noise(struct b43_wldev *dev)
1269 {
1270         struct b43_phy_g *phy = dev->phy.g;
1271         u16 tmp;
1272         u8 noise[4];
1273         u8 i, j;
1274         s32 average;
1275
1276         /* Bottom half of Link Quality calculation. */
1277
1278         if (dev->phy.type != B43_PHYTYPE_G)
1279                 return;
1280
1281         /* Possible race condition: It might be possible that the user
1282          * changed to a different channel in the meantime since we
1283          * started the calculation. We ignore that fact, since it's
1284          * not really that much of a problem. The background noise is
1285          * an estimation only anyway. Slightly wrong results will get damped
1286          * by the averaging of the 8 sample rounds. Additionally the
1287          * value is shortlived. So it will be replaced by the next noise
1288          * calculation round soon. */
1289
1290         B43_WARN_ON(!dev->noisecalc.calculation_running);
1291         *((__le32 *)noise) = cpu_to_le32(b43_jssi_read(dev));
1292         if (noise[0] == 0x7F || noise[1] == 0x7F ||
1293             noise[2] == 0x7F || noise[3] == 0x7F)
1294                 goto generate_new;
1295
1296         /* Get the noise samples. */
1297         B43_WARN_ON(dev->noisecalc.nr_samples >= 8);
1298         i = dev->noisecalc.nr_samples;
1299         noise[0] = clamp_val(noise[0], 0, ARRAY_SIZE(phy->nrssi_lt) - 1);
1300         noise[1] = clamp_val(noise[1], 0, ARRAY_SIZE(phy->nrssi_lt) - 1);
1301         noise[2] = clamp_val(noise[2], 0, ARRAY_SIZE(phy->nrssi_lt) - 1);
1302         noise[3] = clamp_val(noise[3], 0, ARRAY_SIZE(phy->nrssi_lt) - 1);
1303         dev->noisecalc.samples[i][0] = phy->nrssi_lt[noise[0]];
1304         dev->noisecalc.samples[i][1] = phy->nrssi_lt[noise[1]];
1305         dev->noisecalc.samples[i][2] = phy->nrssi_lt[noise[2]];
1306         dev->noisecalc.samples[i][3] = phy->nrssi_lt[noise[3]];
1307         dev->noisecalc.nr_samples++;
1308         if (dev->noisecalc.nr_samples == 8) {
1309                 /* Calculate the Link Quality by the noise samples. */
1310                 average = 0;
1311                 for (i = 0; i < 8; i++) {
1312                         for (j = 0; j < 4; j++)
1313                                 average += dev->noisecalc.samples[i][j];
1314                 }
1315                 average /= (8 * 4);
1316                 average *= 125;
1317                 average += 64;
1318                 average /= 128;
1319                 tmp = b43_shm_read16(dev, B43_SHM_SHARED, 0x40C);
1320                 tmp = (tmp / 128) & 0x1F;
1321                 if (tmp >= 8)
1322                         average += 2;
1323                 else
1324                         average -= 25;
1325                 if (tmp == 8)
1326                         average -= 72;
1327                 else
1328                         average -= 48;
1329
1330                 dev->stats.link_noise = average;
1331                 dev->noisecalc.calculation_running = 0;
1332                 return;
1333         }
1334 generate_new:
1335         b43_generate_noise_sample(dev);
1336 }
1337
1338 static void handle_irq_tbtt_indication(struct b43_wldev *dev)
1339 {
1340         if (b43_is_mode(dev->wl, NL80211_IFTYPE_AP)) {
1341                 ///TODO: PS TBTT
1342         } else {
1343                 if (1 /*FIXME: the last PSpoll frame was sent successfully */ )
1344                         b43_power_saving_ctl_bits(dev, 0);
1345         }
1346         if (b43_is_mode(dev->wl, NL80211_IFTYPE_ADHOC))
1347                 dev->dfq_valid = 1;
1348 }
1349
1350 static void handle_irq_atim_end(struct b43_wldev *dev)
1351 {
1352         if (dev->dfq_valid) {
1353                 b43_write32(dev, B43_MMIO_MACCMD,
1354                             b43_read32(dev, B43_MMIO_MACCMD)
1355                             | B43_MACCMD_DFQ_VALID);
1356                 dev->dfq_valid = 0;
1357         }
1358 }
1359
1360 static void handle_irq_pmq(struct b43_wldev *dev)
1361 {
1362         u32 tmp;
1363
1364         //TODO: AP mode.
1365
1366         while (1) {
1367                 tmp = b43_read32(dev, B43_MMIO_PS_STATUS);
1368                 if (!(tmp & 0x00000008))
1369                         break;
1370         }
1371         /* 16bit write is odd, but correct. */
1372         b43_write16(dev, B43_MMIO_PS_STATUS, 0x0002);
1373 }
1374
1375 static void b43_write_template_common(struct b43_wldev *dev,
1376                                       const u8 *data, u16 size,
1377                                       u16 ram_offset,
1378                                       u16 shm_size_offset, u8 rate)
1379 {
1380         u32 i, tmp;
1381         struct b43_plcp_hdr4 plcp;
1382
1383         plcp.data = 0;
1384         b43_generate_plcp_hdr(&plcp, size + FCS_LEN, rate);
1385         b43_ram_write(dev, ram_offset, le32_to_cpu(plcp.data));
1386         ram_offset += sizeof(u32);
1387         /* The PLCP is 6 bytes long, but we only wrote 4 bytes, yet.
1388          * So leave the first two bytes of the next write blank.
1389          */
1390         tmp = (u32) (data[0]) << 16;
1391         tmp |= (u32) (data[1]) << 24;
1392         b43_ram_write(dev, ram_offset, tmp);
1393         ram_offset += sizeof(u32);
1394         for (i = 2; i < size; i += sizeof(u32)) {
1395                 tmp = (u32) (data[i + 0]);
1396                 if (i + 1 < size)
1397                         tmp |= (u32) (data[i + 1]) << 8;
1398                 if (i + 2 < size)
1399                         tmp |= (u32) (data[i + 2]) << 16;
1400                 if (i + 3 < size)
1401                         tmp |= (u32) (data[i + 3]) << 24;
1402                 b43_ram_write(dev, ram_offset + i - 2, tmp);
1403         }
1404         b43_shm_write16(dev, B43_SHM_SHARED, shm_size_offset,
1405                         size + sizeof(struct b43_plcp_hdr6));
1406 }
1407
1408 /* Check if the use of the antenna that ieee80211 told us to
1409  * use is possible. This will fall back to DEFAULT.
1410  * "antenna_nr" is the antenna identifier we got from ieee80211. */
1411 u8 b43_ieee80211_antenna_sanitize(struct b43_wldev *dev,
1412                                   u8 antenna_nr)
1413 {
1414         u8 antenna_mask;
1415
1416         if (antenna_nr == 0) {
1417                 /* Zero means "use default antenna". That's always OK. */
1418                 return 0;
1419         }
1420
1421         /* Get the mask of available antennas. */
1422         if (dev->phy.gmode)
1423                 antenna_mask = dev->dev->bus->sprom.ant_available_bg;
1424         else
1425                 antenna_mask = dev->dev->bus->sprom.ant_available_a;
1426
1427         if (!(antenna_mask & (1 << (antenna_nr - 1)))) {
1428                 /* This antenna is not available. Fall back to default. */
1429                 return 0;
1430         }
1431
1432         return antenna_nr;
1433 }
1434
1435 /* Convert a b43 antenna number value to the PHY TX control value. */
1436 static u16 b43_antenna_to_phyctl(int antenna)
1437 {
1438         switch (antenna) {
1439         case B43_ANTENNA0:
1440                 return B43_TXH_PHY_ANT0;
1441         case B43_ANTENNA1:
1442                 return B43_TXH_PHY_ANT1;
1443         case B43_ANTENNA2:
1444                 return B43_TXH_PHY_ANT2;
1445         case B43_ANTENNA3:
1446                 return B43_TXH_PHY_ANT3;
1447         case B43_ANTENNA_AUTO0:
1448         case B43_ANTENNA_AUTO1:
1449                 return B43_TXH_PHY_ANT01AUTO;
1450         }
1451         B43_WARN_ON(1);
1452         return 0;
1453 }
1454
1455 static void b43_write_beacon_template(struct b43_wldev *dev,
1456                                       u16 ram_offset,
1457                                       u16 shm_size_offset)
1458 {
1459         unsigned int i, len, variable_len;
1460         const struct ieee80211_mgmt *bcn;
1461         const u8 *ie;
1462         bool tim_found = 0;
1463         unsigned int rate;
1464         u16 ctl;
1465         int antenna;
1466         struct ieee80211_tx_info *info = IEEE80211_SKB_CB(dev->wl->current_beacon);
1467
1468         bcn = (const struct ieee80211_mgmt *)(dev->wl->current_beacon->data);
1469         len = min((size_t) dev->wl->current_beacon->len,
1470                   0x200 - sizeof(struct b43_plcp_hdr6));
1471         rate = ieee80211_get_tx_rate(dev->wl->hw, info)->hw_value;
1472
1473         b43_write_template_common(dev, (const u8 *)bcn,
1474                                   len, ram_offset, shm_size_offset, rate);
1475
1476         /* Write the PHY TX control parameters. */
1477         antenna = B43_ANTENNA_DEFAULT;
1478         antenna = b43_antenna_to_phyctl(antenna);
1479         ctl = b43_shm_read16(dev, B43_SHM_SHARED, B43_SHM_SH_BEACPHYCTL);
1480         /* We can't send beacons with short preamble. Would get PHY errors. */
1481         ctl &= ~B43_TXH_PHY_SHORTPRMBL;
1482         ctl &= ~B43_TXH_PHY_ANT;
1483         ctl &= ~B43_TXH_PHY_ENC;
1484         ctl |= antenna;
1485         if (b43_is_cck_rate(rate))
1486                 ctl |= B43_TXH_PHY_ENC_CCK;
1487         else
1488                 ctl |= B43_TXH_PHY_ENC_OFDM;
1489         b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_BEACPHYCTL, ctl);
1490
1491         /* Find the position of the TIM and the DTIM_period value
1492          * and write them to SHM. */
1493         ie = bcn->u.beacon.variable;
1494         variable_len = len - offsetof(struct ieee80211_mgmt, u.beacon.variable);
1495         for (i = 0; i < variable_len - 2; ) {
1496                 uint8_t ie_id, ie_len;
1497
1498                 ie_id = ie[i];
1499                 ie_len = ie[i + 1];
1500                 if (ie_id == 5) {
1501                         u16 tim_position;
1502                         u16 dtim_period;
1503                         /* This is the TIM Information Element */
1504
1505                         /* Check whether the ie_len is in the beacon data range. */
1506                         if (variable_len < ie_len + 2 + i)
1507                                 break;
1508                         /* A valid TIM is at least 4 bytes long. */
1509                         if (ie_len < 4)
1510                                 break;
1511                         tim_found = 1;
1512
1513                         tim_position = sizeof(struct b43_plcp_hdr6);
1514                         tim_position += offsetof(struct ieee80211_mgmt, u.beacon.variable);
1515                         tim_position += i;
1516
1517                         dtim_period = ie[i + 3];
1518
1519                         b43_shm_write16(dev, B43_SHM_SHARED,
1520                                         B43_SHM_SH_TIMBPOS, tim_position);
1521                         b43_shm_write16(dev, B43_SHM_SHARED,
1522                                         B43_SHM_SH_DTIMPER, dtim_period);
1523                         break;
1524                 }
1525                 i += ie_len + 2;
1526         }
1527         if (!tim_found) {
1528                 /*
1529                  * If ucode wants to modify TIM do it behind the beacon, this
1530                  * will happen, for example, when doing mesh networking.
1531                  */
1532                 b43_shm_write16(dev, B43_SHM_SHARED,
1533                                 B43_SHM_SH_TIMBPOS,
1534                                 len + sizeof(struct b43_plcp_hdr6));
1535                 b43_shm_write16(dev, B43_SHM_SHARED,
1536                                 B43_SHM_SH_DTIMPER, 0);
1537         }
1538         b43dbg(dev->wl, "Updated beacon template at 0x%x\n", ram_offset);
1539 }
1540
1541 static void b43_upload_beacon0(struct b43_wldev *dev)
1542 {
1543         struct b43_wl *wl = dev->wl;
1544
1545         if (wl->beacon0_uploaded)
1546                 return;
1547         b43_write_beacon_template(dev, 0x68, 0x18);
1548         wl->beacon0_uploaded = 1;
1549 }
1550
1551 static void b43_upload_beacon1(struct b43_wldev *dev)
1552 {
1553         struct b43_wl *wl = dev->wl;
1554
1555         if (wl->beacon1_uploaded)
1556                 return;
1557         b43_write_beacon_template(dev, 0x468, 0x1A);
1558         wl->beacon1_uploaded = 1;
1559 }
1560
1561 static void handle_irq_beacon(struct b43_wldev *dev)
1562 {
1563         struct b43_wl *wl = dev->wl;
1564         u32 cmd, beacon0_valid, beacon1_valid;
1565
1566         if (!b43_is_mode(wl, NL80211_IFTYPE_AP) &&
1567             !b43_is_mode(wl, NL80211_IFTYPE_MESH_POINT))
1568                 return;
1569
1570         /* This is the bottom half of the asynchronous beacon update. */
1571
1572         /* Ignore interrupt in the future. */
1573         dev->irq_mask &= ~B43_IRQ_BEACON;
1574
1575         cmd = b43_read32(dev, B43_MMIO_MACCMD);
1576         beacon0_valid = (cmd & B43_MACCMD_BEACON0_VALID);
1577         beacon1_valid = (cmd & B43_MACCMD_BEACON1_VALID);
1578
1579         /* Schedule interrupt manually, if busy. */
1580         if (beacon0_valid && beacon1_valid) {
1581                 b43_write32(dev, B43_MMIO_GEN_IRQ_REASON, B43_IRQ_BEACON);
1582                 dev->irq_mask |= B43_IRQ_BEACON;
1583                 return;
1584         }
1585
1586         if (unlikely(wl->beacon_templates_virgin)) {
1587                 /* We never uploaded a beacon before.
1588                  * Upload both templates now, but only mark one valid. */
1589                 wl->beacon_templates_virgin = 0;
1590                 b43_upload_beacon0(dev);
1591                 b43_upload_beacon1(dev);
1592                 cmd = b43_read32(dev, B43_MMIO_MACCMD);
1593                 cmd |= B43_MACCMD_BEACON0_VALID;
1594                 b43_write32(dev, B43_MMIO_MACCMD, cmd);
1595         } else {
1596                 if (!beacon0_valid) {
1597                         b43_upload_beacon0(dev);
1598                         cmd = b43_read32(dev, B43_MMIO_MACCMD);
1599                         cmd |= B43_MACCMD_BEACON0_VALID;
1600                         b43_write32(dev, B43_MMIO_MACCMD, cmd);
1601                 } else if (!beacon1_valid) {
1602                         b43_upload_beacon1(dev);
1603                         cmd = b43_read32(dev, B43_MMIO_MACCMD);
1604                         cmd |= B43_MACCMD_BEACON1_VALID;
1605                         b43_write32(dev, B43_MMIO_MACCMD, cmd);
1606                 }
1607         }
1608 }
1609
1610 static void b43_do_beacon_update_trigger_work(struct b43_wldev *dev)
1611 {
1612         u32 old_irq_mask = dev->irq_mask;
1613
1614         /* update beacon right away or defer to irq */
1615         handle_irq_beacon(dev);
1616         if (old_irq_mask != dev->irq_mask) {
1617                 /* The handler updated the IRQ mask. */
1618                 B43_WARN_ON(!dev->irq_mask);
1619                 if (b43_read32(dev, B43_MMIO_GEN_IRQ_MASK)) {
1620                         b43_write32(dev, B43_MMIO_GEN_IRQ_MASK, dev->irq_mask);
1621                 } else {
1622                         /* Device interrupts are currently disabled. That means
1623                          * we just ran the hardirq handler and scheduled the
1624                          * IRQ thread. The thread will write the IRQ mask when
1625                          * it finished, so there's nothing to do here. Writing
1626                          * the mask _here_ would incorrectly re-enable IRQs. */
1627                 }
1628         }
1629 }
1630
1631 static void b43_beacon_update_trigger_work(struct work_struct *work)
1632 {
1633         struct b43_wl *wl = container_of(work, struct b43_wl,
1634                                          beacon_update_trigger);
1635         struct b43_wldev *dev;
1636
1637         mutex_lock(&wl->mutex);
1638         dev = wl->current_dev;
1639         if (likely(dev && (b43_status(dev) >= B43_STAT_INITIALIZED))) {
1640                 if (dev->dev->bus->bustype == SSB_BUSTYPE_SDIO) {
1641                         /* wl->mutex is enough. */
1642                         b43_do_beacon_update_trigger_work(dev);
1643                         mmiowb();
1644                 } else {
1645                         spin_lock_irq(&wl->hardirq_lock);
1646                         b43_do_beacon_update_trigger_work(dev);
1647                         mmiowb();
1648                         spin_unlock_irq(&wl->hardirq_lock);
1649                 }
1650         }
1651         mutex_unlock(&wl->mutex);
1652 }
1653
1654 /* Asynchronously update the packet templates in template RAM.
1655  * Locking: Requires wl->mutex to be locked. */
1656 static void b43_update_templates(struct b43_wl *wl)
1657 {
1658         struct sk_buff *beacon;
1659
1660         /* This is the top half of the ansynchronous beacon update.
1661          * The bottom half is the beacon IRQ.
1662          * Beacon update must be asynchronous to avoid sending an
1663          * invalid beacon. This can happen for example, if the firmware
1664          * transmits a beacon while we are updating it. */
1665
1666         /* We could modify the existing beacon and set the aid bit in
1667          * the TIM field, but that would probably require resizing and
1668          * moving of data within the beacon template.
1669          * Simply request a new beacon and let mac80211 do the hard work. */
1670         beacon = ieee80211_beacon_get(wl->hw, wl->vif);
1671         if (unlikely(!beacon))
1672                 return;
1673
1674         if (wl->current_beacon)
1675                 dev_kfree_skb_any(wl->current_beacon);
1676         wl->current_beacon = beacon;
1677         wl->beacon0_uploaded = 0;
1678         wl->beacon1_uploaded = 0;
1679         ieee80211_queue_work(wl->hw, &wl->beacon_update_trigger);
1680 }
1681
1682 static void b43_set_beacon_int(struct b43_wldev *dev, u16 beacon_int)
1683 {
1684         b43_time_lock(dev);
1685         if (dev->dev->id.revision >= 3) {
1686                 b43_write32(dev, B43_MMIO_TSF_CFP_REP, (beacon_int << 16));
1687                 b43_write32(dev, B43_MMIO_TSF_CFP_START, (beacon_int << 10));
1688         } else {
1689                 b43_write16(dev, 0x606, (beacon_int >> 6));
1690                 b43_write16(dev, 0x610, beacon_int);
1691         }
1692         b43_time_unlock(dev);
1693         b43dbg(dev->wl, "Set beacon interval to %u\n", beacon_int);
1694 }
1695
1696 static void b43_handle_firmware_panic(struct b43_wldev *dev)
1697 {
1698         u16 reason;
1699
1700         /* Read the register that contains the reason code for the panic. */
1701         reason = b43_shm_read16(dev, B43_SHM_SCRATCH, B43_FWPANIC_REASON_REG);
1702         b43err(dev->wl, "Whoopsy, firmware panic! Reason: %u\n", reason);
1703
1704         switch (reason) {
1705         default:
1706                 b43dbg(dev->wl, "The panic reason is unknown.\n");
1707                 /* fallthrough */
1708         case B43_FWPANIC_DIE:
1709                 /* Do not restart the controller or firmware.
1710                  * The device is nonfunctional from now on.
1711                  * Restarting would result in this panic to trigger again,
1712                  * so we avoid that recursion. */
1713                 break;
1714         case B43_FWPANIC_RESTART:
1715                 b43_controller_restart(dev, "Microcode panic");
1716                 break;
1717         }
1718 }
1719
1720 static void handle_irq_ucode_debug(struct b43_wldev *dev)
1721 {
1722         unsigned int i, cnt;
1723         u16 reason, marker_id, marker_line;
1724         __le16 *buf;
1725
1726         /* The proprietary firmware doesn't have this IRQ. */
1727         if (!dev->fw.opensource)
1728                 return;
1729
1730         /* Read the register that contains the reason code for this IRQ. */
1731         reason = b43_shm_read16(dev, B43_SHM_SCRATCH, B43_DEBUGIRQ_REASON_REG);
1732
1733         switch (reason) {
1734         case B43_DEBUGIRQ_PANIC:
1735                 b43_handle_firmware_panic(dev);
1736                 break;
1737         case B43_DEBUGIRQ_DUMP_SHM:
1738                 if (!B43_DEBUG)
1739                         break; /* Only with driver debugging enabled. */
1740                 buf = kmalloc(4096, GFP_ATOMIC);
1741                 if (!buf) {
1742                         b43dbg(dev->wl, "SHM-dump: Failed to allocate memory\n");
1743                         goto out;
1744                 }
1745                 for (i = 0; i < 4096; i += 2) {
1746                         u16 tmp = b43_shm_read16(dev, B43_SHM_SHARED, i);
1747                         buf[i / 2] = cpu_to_le16(tmp);
1748                 }
1749                 b43info(dev->wl, "Shared memory dump:\n");
1750                 print_hex_dump(KERN_INFO, "", DUMP_PREFIX_OFFSET,
1751                                16, 2, buf, 4096, 1);
1752                 kfree(buf);
1753                 break;
1754         case B43_DEBUGIRQ_DUMP_REGS:
1755                 if (!B43_DEBUG)
1756                         break; /* Only with driver debugging enabled. */
1757                 b43info(dev->wl, "Microcode register dump:\n");
1758                 for (i = 0, cnt = 0; i < 64; i++) {
1759                         u16 tmp = b43_shm_read16(dev, B43_SHM_SCRATCH, i);
1760                         if (cnt == 0)
1761                                 printk(KERN_INFO);
1762                         printk("r%02u: 0x%04X  ", i, tmp);
1763                         cnt++;
1764                         if (cnt == 6) {
1765                                 printk("\n");
1766                                 cnt = 0;
1767                         }
1768                 }
1769                 printk("\n");
1770                 break;
1771         case B43_DEBUGIRQ_MARKER:
1772                 if (!B43_DEBUG)
1773                         break; /* Only with driver debugging enabled. */
1774                 marker_id = b43_shm_read16(dev, B43_SHM_SCRATCH,
1775                                            B43_MARKER_ID_REG);
1776                 marker_line = b43_shm_read16(dev, B43_SHM_SCRATCH,
1777                                              B43_MARKER_LINE_REG);
1778                 b43info(dev->wl, "The firmware just executed the MARKER(%u) "
1779                         "at line number %u\n",
1780                         marker_id, marker_line);
1781                 break;
1782         default:
1783                 b43dbg(dev->wl, "Debug-IRQ triggered for unknown reason: %u\n",
1784                        reason);
1785         }
1786 out:
1787         /* Acknowledge the debug-IRQ, so the firmware can continue. */
1788         b43_shm_write16(dev, B43_SHM_SCRATCH,
1789                         B43_DEBUGIRQ_REASON_REG, B43_DEBUGIRQ_ACK);
1790 }
1791
1792 static void b43_do_interrupt_thread(struct b43_wldev *dev)
1793 {
1794         u32 reason;
1795         u32 dma_reason[ARRAY_SIZE(dev->dma_reason)];
1796         u32 merged_dma_reason = 0;
1797         int i;
1798
1799         if (unlikely(b43_status(dev) != B43_STAT_STARTED))
1800                 return;
1801
1802         reason = dev->irq_reason;
1803         for (i = 0; i < ARRAY_SIZE(dma_reason); i++) {
1804                 dma_reason[i] = dev->dma_reason[i];
1805                 merged_dma_reason |= dma_reason[i];
1806         }
1807
1808         if (unlikely(reason & B43_IRQ_MAC_TXERR))
1809                 b43err(dev->wl, "MAC transmission error\n");
1810
1811         if (unlikely(reason & B43_IRQ_PHY_TXERR)) {
1812                 b43err(dev->wl, "PHY transmission error\n");
1813                 rmb();
1814                 if (unlikely(atomic_dec_and_test(&dev->phy.txerr_cnt))) {
1815                         atomic_set(&dev->phy.txerr_cnt,
1816                                    B43_PHY_TX_BADNESS_LIMIT);
1817                         b43err(dev->wl, "Too many PHY TX errors, "
1818                                         "restarting the controller\n");
1819                         b43_controller_restart(dev, "PHY TX errors");
1820                 }
1821         }
1822
1823         if (unlikely(merged_dma_reason & (B43_DMAIRQ_FATALMASK |
1824                                           B43_DMAIRQ_NONFATALMASK))) {
1825                 if (merged_dma_reason & B43_DMAIRQ_FATALMASK) {
1826                         b43err(dev->wl, "Fatal DMA error: "
1827                                "0x%08X, 0x%08X, 0x%08X, "
1828                                "0x%08X, 0x%08X, 0x%08X\n",
1829                                dma_reason[0], dma_reason[1],
1830                                dma_reason[2], dma_reason[3],
1831                                dma_reason[4], dma_reason[5]);
1832                         b43err(dev->wl, "This device does not support DMA "
1833                                "on your system. It will now be switched to PIO.\n");
1834                         /* Fall back to PIO transfers if we get fatal DMA errors! */
1835                         dev->use_pio = 1;
1836                         b43_controller_restart(dev, "DMA error");
1837                         return;
1838                 }
1839                 if (merged_dma_reason & B43_DMAIRQ_NONFATALMASK) {
1840                         b43err(dev->wl, "DMA error: "
1841                                "0x%08X, 0x%08X, 0x%08X, "
1842                                "0x%08X, 0x%08X, 0x%08X\n",
1843                                dma_reason[0], dma_reason[1],
1844                                dma_reason[2], dma_reason[3],
1845                                dma_reason[4], dma_reason[5]);
1846                 }
1847         }
1848
1849         if (unlikely(reason & B43_IRQ_UCODE_DEBUG))
1850                 handle_irq_ucode_debug(dev);
1851         if (reason & B43_IRQ_TBTT_INDI)
1852                 handle_irq_tbtt_indication(dev);
1853         if (reason & B43_IRQ_ATIM_END)
1854                 handle_irq_atim_end(dev);
1855         if (reason & B43_IRQ_BEACON)
1856                 handle_irq_beacon(dev);
1857         if (reason & B43_IRQ_PMQ)
1858                 handle_irq_pmq(dev);
1859         if (reason & B43_IRQ_TXFIFO_FLUSH_OK)
1860                 ;/* TODO */
1861         if (reason & B43_IRQ_NOISESAMPLE_OK)
1862                 handle_irq_noise(dev);
1863
1864         /* Check the DMA reason registers for received data. */
1865         if (dma_reason[0] & B43_DMAIRQ_RX_DONE) {
1866                 if (b43_using_pio_transfers(dev))
1867                         b43_pio_rx(dev->pio.rx_queue);
1868                 else
1869                         b43_dma_rx(dev->dma.rx_ring);
1870         }
1871         B43_WARN_ON(dma_reason[1] & B43_DMAIRQ_RX_DONE);
1872         B43_WARN_ON(dma_reason[2] & B43_DMAIRQ_RX_DONE);
1873         B43_WARN_ON(dma_reason[3] & B43_DMAIRQ_RX_DONE);
1874         B43_WARN_ON(dma_reason[4] & B43_DMAIRQ_RX_DONE);
1875         B43_WARN_ON(dma_reason[5] & B43_DMAIRQ_RX_DONE);
1876
1877         if (reason & B43_IRQ_TX_OK)
1878                 handle_irq_transmit_status(dev);
1879
1880         /* Re-enable interrupts on the device by restoring the current interrupt mask. */
1881         b43_write32(dev, B43_MMIO_GEN_IRQ_MASK, dev->irq_mask);
1882
1883 #if B43_DEBUG
1884         if (b43_debug(dev, B43_DBG_VERBOSESTATS)) {
1885                 dev->irq_count++;
1886                 for (i = 0; i < ARRAY_SIZE(dev->irq_bit_count); i++) {
1887                         if (reason & (1 << i))
1888                                 dev->irq_bit_count[i]++;
1889                 }
1890         }
1891 #endif
1892 }
1893
1894 /* Interrupt thread handler. Handles device interrupts in thread context. */
1895 static irqreturn_t b43_interrupt_thread_handler(int irq, void *dev_id)
1896 {
1897         struct b43_wldev *dev = dev_id;
1898
1899         mutex_lock(&dev->wl->mutex);
1900         b43_do_interrupt_thread(dev);
1901         mmiowb();
1902         mutex_unlock(&dev->wl->mutex);
1903
1904         return IRQ_HANDLED;
1905 }
1906
1907 static irqreturn_t b43_do_interrupt(struct b43_wldev *dev)
1908 {
1909         u32 reason;
1910
1911         /* This code runs under wl->hardirq_lock, but _only_ on non-SDIO busses.
1912          * On SDIO, this runs under wl->mutex. */
1913
1914         reason = b43_read32(dev, B43_MMIO_GEN_IRQ_REASON);
1915         if (reason == 0xffffffff)       /* shared IRQ */
1916                 return IRQ_NONE;
1917         reason &= dev->irq_mask;
1918         if (!reason)
1919                 return IRQ_HANDLED;
1920
1921         dev->dma_reason[0] = b43_read32(dev, B43_MMIO_DMA0_REASON)
1922             & 0x0001DC00;
1923         dev->dma_reason[1] = b43_read32(dev, B43_MMIO_DMA1_REASON)
1924             & 0x0000DC00;
1925         dev->dma_reason[2] = b43_read32(dev, B43_MMIO_DMA2_REASON)
1926             & 0x0000DC00;
1927         dev->dma_reason[3] = b43_read32(dev, B43_MMIO_DMA3_REASON)
1928             & 0x0001DC00;
1929         dev->dma_reason[4] = b43_read32(dev, B43_MMIO_DMA4_REASON)
1930             & 0x0000DC00;
1931 /* Unused ring
1932         dev->dma_reason[5] = b43_read32(dev, B43_MMIO_DMA5_REASON)
1933             & 0x0000DC00;
1934 */
1935
1936         /* ACK the interrupt. */
1937         b43_write32(dev, B43_MMIO_GEN_IRQ_REASON, reason);
1938         b43_write32(dev, B43_MMIO_DMA0_REASON, dev->dma_reason[0]);
1939         b43_write32(dev, B43_MMIO_DMA1_REASON, dev->dma_reason[1]);
1940         b43_write32(dev, B43_MMIO_DMA2_REASON, dev->dma_reason[2]);
1941         b43_write32(dev, B43_MMIO_DMA3_REASON, dev->dma_reason[3]);
1942         b43_write32(dev, B43_MMIO_DMA4_REASON, dev->dma_reason[4]);
1943 /* Unused ring
1944         b43_write32(dev, B43_MMIO_DMA5_REASON, dev->dma_reason[5]);
1945 */
1946
1947         /* Disable IRQs on the device. The IRQ thread handler will re-enable them. */
1948         b43_write32(dev, B43_MMIO_GEN_IRQ_MASK, 0);
1949         /* Save the reason bitmasks for the IRQ thread handler. */
1950         dev->irq_reason = reason;
1951
1952         return IRQ_WAKE_THREAD;
1953 }
1954
1955 /* Interrupt handler top-half. This runs with interrupts disabled. */
1956 static irqreturn_t b43_interrupt_handler(int irq, void *dev_id)
1957 {
1958         struct b43_wldev *dev = dev_id;
1959         irqreturn_t ret;
1960
1961         if (unlikely(b43_status(dev) < B43_STAT_STARTED))
1962                 return IRQ_NONE;
1963
1964         spin_lock(&dev->wl->hardirq_lock);
1965         ret = b43_do_interrupt(dev);
1966         mmiowb();
1967         spin_unlock(&dev->wl->hardirq_lock);
1968
1969         return ret;
1970 }
1971
1972 /* SDIO interrupt handler. This runs in process context. */
1973 static void b43_sdio_interrupt_handler(struct b43_wldev *dev)
1974 {
1975         struct b43_wl *wl = dev->wl;
1976         irqreturn_t ret;
1977
1978         mutex_lock(&wl->mutex);
1979
1980         ret = b43_do_interrupt(dev);
1981         if (ret == IRQ_WAKE_THREAD)
1982                 b43_do_interrupt_thread(dev);
1983
1984         mutex_unlock(&wl->mutex);
1985 }
1986
1987 void b43_do_release_fw(struct b43_firmware_file *fw)
1988 {
1989         release_firmware(fw->data);
1990         fw->data = NULL;
1991         fw->filename = NULL;
1992 }
1993
1994 static void b43_release_firmware(struct b43_wldev *dev)
1995 {
1996         b43_do_release_fw(&dev->fw.ucode);
1997         b43_do_release_fw(&dev->fw.pcm);
1998         b43_do_release_fw(&dev->fw.initvals);
1999         b43_do_release_fw(&dev->fw.initvals_band);
2000 }
2001
2002 static void b43_print_fw_helptext(struct b43_wl *wl, bool error)
2003 {
2004         const char text[] =
2005                 "You must go to " \
2006                 "http://wireless.kernel.org/en/users/Drivers/b43#devicefirmware " \
2007                 "and download the correct firmware for this driver version. " \
2008                 "Please carefully read all instructions on this website.\n";
2009
2010         if (error)
2011                 b43err(wl, text);
2012         else
2013                 b43warn(wl, text);
2014 }
2015
2016 int b43_do_request_fw(struct b43_request_fw_context *ctx,
2017                       const char *name,
2018                       struct b43_firmware_file *fw)
2019 {
2020         const struct firmware *blob;
2021         struct b43_fw_header *hdr;
2022         u32 size;
2023         int err;
2024
2025         if (!name) {
2026                 /* Don't fetch anything. Free possibly cached firmware. */
2027                 /* FIXME: We should probably keep it anyway, to save some headache
2028                  * on suspend/resume with multiband devices. */
2029                 b43_do_release_fw(fw);
2030                 return 0;
2031         }
2032         if (fw->filename) {
2033                 if ((fw->type == ctx->req_type) &&
2034                     (strcmp(fw->filename, name) == 0))
2035                         return 0; /* Already have this fw. */
2036                 /* Free the cached firmware first. */
2037                 /* FIXME: We should probably do this later after we successfully
2038                  * got the new fw. This could reduce headache with multiband devices.
2039                  * We could also redesign this to cache the firmware for all possible
2040                  * bands all the time. */
2041                 b43_do_release_fw(fw);
2042         }
2043
2044         switch (ctx->req_type) {
2045         case B43_FWTYPE_PROPRIETARY:
2046                 snprintf(ctx->fwname, sizeof(ctx->fwname),
2047                          "b43%s/%s.fw",
2048                          modparam_fwpostfix, name);
2049                 break;
2050         case B43_FWTYPE_OPENSOURCE:
2051                 snprintf(ctx->fwname, sizeof(ctx->fwname),
2052                          "b43-open%s/%s.fw",
2053                          modparam_fwpostfix, name);
2054                 break;
2055         default:
2056                 B43_WARN_ON(1);
2057                 return -ENOSYS;
2058         }
2059         err = request_firmware(&blob, ctx->fwname, ctx->dev->dev->dev);
2060         if (err == -ENOENT) {
2061                 snprintf(ctx->errors[ctx->req_type],
2062                          sizeof(ctx->errors[ctx->req_type]),
2063                          "Firmware file \"%s\" not found\n", ctx->fwname);
2064                 return err;
2065         } else if (err) {
2066                 snprintf(ctx->errors[ctx->req_type],
2067                          sizeof(ctx->errors[ctx->req_type]),
2068                          "Firmware file \"%s\" request failed (err=%d)\n",
2069                          ctx->fwname, err);
2070                 return err;
2071         }
2072         if (blob->size < sizeof(struct b43_fw_header))
2073                 goto err_format;
2074         hdr = (struct b43_fw_header *)(blob->data);
2075         switch (hdr->type) {
2076         case B43_FW_TYPE_UCODE:
2077         case B43_FW_TYPE_PCM:
2078                 size = be32_to_cpu(hdr->size);
2079                 if (size != blob->size - sizeof(struct b43_fw_header))
2080                         goto err_format;
2081                 /* fallthrough */
2082         case B43_FW_TYPE_IV:
2083                 if (hdr->ver != 1)
2084                         goto err_format;
2085                 break;
2086         default:
2087                 goto err_format;
2088         }
2089
2090         fw->data = blob;
2091         fw->filename = name;
2092         fw->type = ctx->req_type;
2093
2094         return 0;
2095
2096 err_format:
2097         snprintf(ctx->errors[ctx->req_type],
2098                  sizeof(ctx->errors[ctx->req_type]),
2099                  "Firmware file \"%s\" format error.\n", ctx->fwname);
2100         release_firmware(blob);
2101
2102         return -EPROTO;
2103 }
2104
2105 static int b43_try_request_fw(struct b43_request_fw_context *ctx)
2106 {
2107         struct b43_wldev *dev = ctx->dev;
2108         struct b43_firmware *fw = &ctx->dev->fw;
2109         const u8 rev = ctx->dev->dev->id.revision;
2110         const char *filename;
2111         u32 tmshigh;
2112         int err;
2113
2114         /* Get microcode */
2115         tmshigh = ssb_read32(dev->dev, SSB_TMSHIGH);
2116         if ((rev >= 5) && (rev <= 10))
2117                 filename = "ucode5";
2118         else if ((rev >= 11) && (rev <= 12))
2119                 filename = "ucode11";
2120         else if (rev == 13)
2121                 filename = "ucode13";
2122         else if (rev == 14)
2123                 filename = "ucode14";
2124         else if (rev == 15)
2125                 filename = "ucode15";
2126         else if ((rev >= 16) && (rev <= 20))
2127                 filename = "ucode16_mimo";
2128         else
2129                 goto err_no_ucode;
2130         err = b43_do_request_fw(ctx, filename, &fw->ucode);
2131         if (err)
2132                 goto err_load;
2133
2134         /* Get PCM code */
2135         if ((rev >= 5) && (rev <= 10))
2136                 filename = "pcm5";
2137         else if (rev >= 11)
2138                 filename = NULL;
2139         else
2140                 goto err_no_pcm;
2141         fw->pcm_request_failed = 0;
2142         err = b43_do_request_fw(ctx, filename, &fw->pcm);
2143         if (err == -ENOENT) {
2144                 /* We did not find a PCM file? Not fatal, but
2145                  * core rev <= 10 must do without hwcrypto then. */
2146                 fw->pcm_request_failed = 1;
2147         } else if (err)
2148                 goto err_load;
2149
2150         /* Get initvals */
2151         switch (dev->phy.type) {
2152         case B43_PHYTYPE_A:
2153                 if ((rev >= 5) && (rev <= 10)) {
2154                         if (tmshigh & B43_TMSHIGH_HAVE_2GHZ_PHY)
2155                                 filename = "a0g1initvals5";
2156                         else
2157                                 filename = "a0g0initvals5";
2158                 } else
2159                         goto err_no_initvals;
2160                 break;
2161         case B43_PHYTYPE_G:
2162                 if ((rev >= 5) && (rev <= 10))
2163                         filename = "b0g0initvals5";
2164                 else if (rev >= 13)
2165                         filename = "b0g0initvals13";
2166                 else
2167                         goto err_no_initvals;
2168                 break;
2169         case B43_PHYTYPE_N:
2170                 if (rev >= 16)
2171                         filename = "n0initvals16";
2172                 else 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 >= 16)
2217                         filename = "n0bsinitvals16";
2218                 else if ((rev >= 11) && (rev <= 12))
2219                         filename = "n0bsinitvals11";
2220                 else
2221                         goto err_no_initvals;
2222                 break;
2223         case B43_PHYTYPE_LP:
2224                 if (rev == 13)
2225                         filename = "lp0bsinitvals13";
2226                 else if (rev == 14)
2227                         filename = "lp0bsinitvals14";
2228                 else if (rev >= 15)
2229                         filename = "lp0bsinitvals15";
2230                 else
2231                         goto err_no_initvals;
2232                 break;
2233         default:
2234                 goto err_no_initvals;
2235         }
2236         err = b43_do_request_fw(ctx, filename, &fw->initvals_band);
2237         if (err)
2238                 goto err_load;
2239
2240         return 0;
2241
2242 err_no_ucode:
2243         err = ctx->fatal_failure = -EOPNOTSUPP;
2244         b43err(dev->wl, "The driver does not know which firmware (ucode) "
2245                "is required for your device (wl-core rev %u)\n", rev);
2246         goto error;
2247
2248 err_no_pcm:
2249         err = ctx->fatal_failure = -EOPNOTSUPP;
2250         b43err(dev->wl, "The driver does not know which firmware (PCM) "
2251                "is required for your device (wl-core rev %u)\n", rev);
2252         goto error;
2253
2254 err_no_initvals:
2255         err = ctx->fatal_failure = -EOPNOTSUPP;
2256         b43err(dev->wl, "The driver does not know which firmware (initvals) "
2257                "is required for your device (wl-core rev %u)\n", rev);
2258         goto error;
2259
2260 err_load:
2261         /* We failed to load this firmware image. The error message
2262          * already is in ctx->errors. Return and let our caller decide
2263          * what to do. */
2264         goto error;
2265
2266 error:
2267         b43_release_firmware(dev);
2268         return err;
2269 }
2270
2271 static int b43_request_firmware(struct b43_wldev *dev)
2272 {
2273         struct b43_request_fw_context *ctx;
2274         unsigned int i;
2275         int err;
2276         const char *errmsg;
2277
2278         ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
2279         if (!ctx)
2280                 return -ENOMEM;
2281         ctx->dev = dev;
2282
2283         ctx->req_type = B43_FWTYPE_PROPRIETARY;
2284         err = b43_try_request_fw(ctx);
2285         if (!err)
2286                 goto out; /* Successfully loaded it. */
2287         err = ctx->fatal_failure;
2288         if (err)
2289                 goto out;
2290
2291         ctx->req_type = B43_FWTYPE_OPENSOURCE;
2292         err = b43_try_request_fw(ctx);
2293         if (!err)
2294                 goto out; /* Successfully loaded it. */
2295         err = ctx->fatal_failure;
2296         if (err)
2297                 goto out;
2298
2299         /* Could not find a usable firmware. Print the errors. */
2300         for (i = 0; i < B43_NR_FWTYPES; i++) {
2301                 errmsg = ctx->errors[i];
2302                 if (strlen(errmsg))
2303                         b43err(dev->wl, errmsg);
2304         }
2305         b43_print_fw_helptext(dev->wl, 1);
2306         err = -ENOENT;
2307
2308 out:
2309         kfree(ctx);
2310         return err;
2311 }
2312
2313 static int b43_upload_microcode(struct b43_wldev *dev)
2314 {
2315         struct wiphy *wiphy = dev->wl->hw->wiphy;
2316         const size_t hdr_len = sizeof(struct b43_fw_header);
2317         const __be32 *data;
2318         unsigned int i, len;
2319         u16 fwrev, fwpatch, fwdate, fwtime;
2320         u32 tmp, macctl;
2321         int err = 0;
2322
2323         /* Jump the microcode PSM to offset 0 */
2324         macctl = b43_read32(dev, B43_MMIO_MACCTL);
2325         B43_WARN_ON(macctl & B43_MACCTL_PSM_RUN);
2326         macctl |= B43_MACCTL_PSM_JMP0;
2327         b43_write32(dev, B43_MMIO_MACCTL, macctl);
2328         /* Zero out all microcode PSM registers and shared memory. */
2329         for (i = 0; i < 64; i++)
2330                 b43_shm_write16(dev, B43_SHM_SCRATCH, i, 0);
2331         for (i = 0; i < 4096; i += 2)
2332                 b43_shm_write16(dev, B43_SHM_SHARED, i, 0);
2333
2334         /* Upload Microcode. */
2335         data = (__be32 *) (dev->fw.ucode.data->data + hdr_len);
2336         len = (dev->fw.ucode.data->size - hdr_len) / sizeof(__be32);
2337         b43_shm_control_word(dev, B43_SHM_UCODE | B43_SHM_AUTOINC_W, 0x0000);
2338         for (i = 0; i < len; i++) {
2339                 b43_write32(dev, B43_MMIO_SHM_DATA, be32_to_cpu(data[i]));
2340                 udelay(10);
2341         }
2342
2343         if (dev->fw.pcm.data) {
2344                 /* Upload PCM data. */
2345                 data = (__be32 *) (dev->fw.pcm.data->data + hdr_len);
2346                 len = (dev->fw.pcm.data->size - hdr_len) / sizeof(__be32);
2347                 b43_shm_control_word(dev, B43_SHM_HW, 0x01EA);
2348                 b43_write32(dev, B43_MMIO_SHM_DATA, 0x00004000);
2349                 /* No need for autoinc bit in SHM_HW */
2350                 b43_shm_control_word(dev, B43_SHM_HW, 0x01EB);
2351                 for (i = 0; i < len; i++) {
2352                         b43_write32(dev, B43_MMIO_SHM_DATA, be32_to_cpu(data[i]));
2353                         udelay(10);
2354                 }
2355         }
2356
2357         b43_write32(dev, B43_MMIO_GEN_IRQ_REASON, B43_IRQ_ALL);
2358
2359         /* Start the microcode PSM */
2360         macctl = b43_read32(dev, B43_MMIO_MACCTL);
2361         macctl &= ~B43_MACCTL_PSM_JMP0;
2362         macctl |= B43_MACCTL_PSM_RUN;
2363         b43_write32(dev, B43_MMIO_MACCTL, macctl);
2364
2365         /* Wait for the microcode to load and respond */
2366         i = 0;
2367         while (1) {
2368                 tmp = b43_read32(dev, B43_MMIO_GEN_IRQ_REASON);
2369                 if (tmp == B43_IRQ_MAC_SUSPENDED)
2370                         break;
2371                 i++;
2372                 if (i >= 20) {
2373                         b43err(dev->wl, "Microcode not responding\n");
2374                         b43_print_fw_helptext(dev->wl, 1);
2375                         err = -ENODEV;
2376                         goto error;
2377                 }
2378                 msleep(50);
2379         }
2380         b43_read32(dev, B43_MMIO_GEN_IRQ_REASON);       /* dummy read */
2381
2382         /* Get and check the revisions. */
2383         fwrev = b43_shm_read16(dev, B43_SHM_SHARED, B43_SHM_SH_UCODEREV);
2384         fwpatch = b43_shm_read16(dev, B43_SHM_SHARED, B43_SHM_SH_UCODEPATCH);
2385         fwdate = b43_shm_read16(dev, B43_SHM_SHARED, B43_SHM_SH_UCODEDATE);
2386         fwtime = b43_shm_read16(dev, B43_SHM_SHARED, B43_SHM_SH_UCODETIME);
2387
2388         if (fwrev <= 0x128) {
2389                 b43err(dev->wl, "YOUR FIRMWARE IS TOO OLD. Firmware from "
2390                        "binary drivers older than version 4.x is unsupported. "
2391                        "You must upgrade your firmware files.\n");
2392                 b43_print_fw_helptext(dev->wl, 1);
2393                 err = -EOPNOTSUPP;
2394                 goto error;
2395         }
2396         dev->fw.rev = fwrev;
2397         dev->fw.patch = fwpatch;
2398         dev->fw.opensource = (fwdate == 0xFFFF);
2399
2400         /* Default to use-all-queues. */
2401         dev->wl->hw->queues = dev->wl->mac80211_initially_registered_queues;
2402         dev->qos_enabled = !!modparam_qos;
2403         /* Default to firmware/hardware crypto acceleration. */
2404         dev->hwcrypto_enabled = 1;
2405
2406         if (dev->fw.opensource) {
2407                 u16 fwcapa;
2408
2409                 /* Patchlevel info is encoded in the "time" field. */
2410                 dev->fw.patch = fwtime;
2411                 b43info(dev->wl, "Loading OpenSource firmware version %u.%u\n",
2412                         dev->fw.rev, dev->fw.patch);
2413
2414                 fwcapa = b43_fwcapa_read(dev);
2415                 if (!(fwcapa & B43_FWCAPA_HWCRYPTO) || dev->fw.pcm_request_failed) {
2416                         b43info(dev->wl, "Hardware crypto acceleration not supported by firmware\n");
2417                         /* Disable hardware crypto and fall back to software crypto. */
2418                         dev->hwcrypto_enabled = 0;
2419                 }
2420                 if (!(fwcapa & B43_FWCAPA_QOS)) {
2421                         b43info(dev->wl, "QoS not supported by firmware\n");
2422                         /* Disable QoS. Tweak hw->queues to 1. It will be restored before
2423                          * ieee80211_unregister to make sure the networking core can
2424                          * properly free possible resources. */
2425                         dev->wl->hw->queues = 1;
2426                         dev->qos_enabled = 0;
2427                 }
2428         } else {
2429                 b43info(dev->wl, "Loading firmware version %u.%u "
2430                         "(20%.2i-%.2i-%.2i %.2i:%.2i:%.2i)\n",
2431                         fwrev, fwpatch,
2432                         (fwdate >> 12) & 0xF, (fwdate >> 8) & 0xF, fwdate & 0xFF,
2433                         (fwtime >> 11) & 0x1F, (fwtime >> 5) & 0x3F, fwtime & 0x1F);
2434                 if (dev->fw.pcm_request_failed) {
2435                         b43warn(dev->wl, "No \"pcm5.fw\" firmware file found. "
2436                                 "Hardware accelerated cryptography is disabled.\n");
2437                         b43_print_fw_helptext(dev->wl, 0);
2438                 }
2439         }
2440
2441         snprintf(wiphy->fw_version, sizeof(wiphy->fw_version), "%u.%u",
2442                         dev->fw.rev, dev->fw.patch);
2443         wiphy->hw_version = dev->dev->id.coreid;
2444
2445         if (b43_is_old_txhdr_format(dev)) {
2446                 /* We're over the deadline, but we keep support for old fw
2447                  * until it turns out to be in major conflict with something new. */
2448                 b43warn(dev->wl, "You are using an old firmware image. "
2449                         "Support for old firmware will be removed soon "
2450                         "(official deadline was July 2008).\n");
2451                 b43_print_fw_helptext(dev->wl, 0);
2452         }
2453
2454         return 0;
2455
2456 error:
2457         macctl = b43_read32(dev, B43_MMIO_MACCTL);
2458         macctl &= ~B43_MACCTL_PSM_RUN;
2459         macctl |= B43_MACCTL_PSM_JMP0;
2460         b43_write32(dev, B43_MMIO_MACCTL, macctl);
2461
2462         return err;
2463 }
2464
2465 static int b43_write_initvals(struct b43_wldev *dev,
2466                               const struct b43_iv *ivals,
2467                               size_t count,
2468                               size_t array_size)
2469 {
2470         const struct b43_iv *iv;
2471         u16 offset;
2472         size_t i;
2473         bool bit32;
2474
2475         BUILD_BUG_ON(sizeof(struct b43_iv) != 6);
2476         iv = ivals;
2477         for (i = 0; i < count; i++) {
2478                 if (array_size < sizeof(iv->offset_size))
2479                         goto err_format;
2480                 array_size -= sizeof(iv->offset_size);
2481                 offset = be16_to_cpu(iv->offset_size);
2482                 bit32 = !!(offset & B43_IV_32BIT);
2483                 offset &= B43_IV_OFFSET_MASK;
2484                 if (offset >= 0x1000)
2485                         goto err_format;
2486                 if (bit32) {
2487                         u32 value;
2488
2489                         if (array_size < sizeof(iv->data.d32))
2490                                 goto err_format;
2491                         array_size -= sizeof(iv->data.d32);
2492
2493                         value = get_unaligned_be32(&iv->data.d32);
2494                         b43_write32(dev, offset, value);
2495
2496                         iv = (const struct b43_iv *)((const uint8_t *)iv +
2497                                                         sizeof(__be16) +
2498                                                         sizeof(__be32));
2499                 } else {
2500                         u16 value;
2501
2502                         if (array_size < sizeof(iv->data.d16))
2503                                 goto err_format;
2504                         array_size -= sizeof(iv->data.d16);
2505
2506                         value = be16_to_cpu(iv->data.d16);
2507                         b43_write16(dev, offset, value);
2508
2509                         iv = (const struct b43_iv *)((const uint8_t *)iv +
2510                                                         sizeof(__be16) +
2511                                                         sizeof(__be16));
2512                 }
2513         }
2514         if (array_size)
2515                 goto err_format;
2516
2517         return 0;
2518
2519 err_format:
2520         b43err(dev->wl, "Initial Values Firmware file-format error.\n");
2521         b43_print_fw_helptext(dev->wl, 1);
2522
2523         return -EPROTO;
2524 }
2525
2526 static int b43_upload_initvals(struct b43_wldev *dev)
2527 {
2528         const size_t hdr_len = sizeof(struct b43_fw_header);
2529         const struct b43_fw_header *hdr;
2530         struct b43_firmware *fw = &dev->fw;
2531         const struct b43_iv *ivals;
2532         size_t count;
2533         int err;
2534
2535         hdr = (const struct b43_fw_header *)(fw->initvals.data->data);
2536         ivals = (const struct b43_iv *)(fw->initvals.data->data + hdr_len);
2537         count = be32_to_cpu(hdr->size);
2538         err = b43_write_initvals(dev, ivals, count,
2539                                  fw->initvals.data->size - hdr_len);
2540         if (err)
2541                 goto out;
2542         if (fw->initvals_band.data) {
2543                 hdr = (const struct b43_fw_header *)(fw->initvals_band.data->data);
2544                 ivals = (const struct b43_iv *)(fw->initvals_band.data->data + hdr_len);
2545                 count = be32_to_cpu(hdr->size);
2546                 err = b43_write_initvals(dev, ivals, count,
2547                                          fw->initvals_band.data->size - hdr_len);
2548                 if (err)
2549                         goto out;
2550         }
2551 out:
2552
2553         return err;
2554 }
2555
2556 /* Initialize the GPIOs
2557  * http://bcm-specs.sipsolutions.net/GPIO
2558  */
2559 static int b43_gpio_init(struct b43_wldev *dev)
2560 {
2561         struct ssb_bus *bus = dev->dev->bus;
2562         struct ssb_device *gpiodev, *pcidev = NULL;
2563         u32 mask, set;
2564
2565         b43_write32(dev, B43_MMIO_MACCTL, b43_read32(dev, B43_MMIO_MACCTL)
2566                     & ~B43_MACCTL_GPOUTSMSK);
2567
2568         b43_write16(dev, B43_MMIO_GPIO_MASK, b43_read16(dev, B43_MMIO_GPIO_MASK)
2569                     | 0x000F);
2570
2571         mask = 0x0000001F;
2572         set = 0x0000000F;
2573         if (dev->dev->bus->chip_id == 0x4301) {
2574                 mask |= 0x0060;
2575                 set |= 0x0060;
2576         }
2577         if (0 /* FIXME: conditional unknown */ ) {
2578                 b43_write16(dev, B43_MMIO_GPIO_MASK,
2579                             b43_read16(dev, B43_MMIO_GPIO_MASK)
2580                             | 0x0100);
2581                 mask |= 0x0180;
2582                 set |= 0x0180;
2583         }
2584         if (dev->dev->bus->sprom.boardflags_lo & B43_BFL_PACTRL) {
2585                 b43_write16(dev, B43_MMIO_GPIO_MASK,
2586                             b43_read16(dev, B43_MMIO_GPIO_MASK)
2587                             | 0x0200);
2588                 mask |= 0x0200;
2589                 set |= 0x0200;
2590         }
2591         if (dev->dev->id.revision >= 2)
2592                 mask |= 0x0010; /* FIXME: This is redundant. */
2593
2594 #ifdef CONFIG_SSB_DRIVER_PCICORE
2595         pcidev = bus->pcicore.dev;
2596 #endif
2597         gpiodev = bus->chipco.dev ? : pcidev;
2598         if (!gpiodev)
2599                 return 0;
2600         ssb_write32(gpiodev, B43_GPIO_CONTROL,
2601                     (ssb_read32(gpiodev, B43_GPIO_CONTROL)
2602                      & mask) | set);
2603
2604         return 0;
2605 }
2606
2607 /* Turn off all GPIO stuff. Call this on module unload, for example. */
2608 static void b43_gpio_cleanup(struct b43_wldev *dev)
2609 {
2610         struct ssb_bus *bus = dev->dev->bus;
2611         struct ssb_device *gpiodev, *pcidev = NULL;
2612
2613 #ifdef CONFIG_SSB_DRIVER_PCICORE
2614         pcidev = bus->pcicore.dev;
2615 #endif
2616         gpiodev = bus->chipco.dev ? : pcidev;
2617         if (!gpiodev)
2618                 return;
2619         ssb_write32(gpiodev, B43_GPIO_CONTROL, 0);
2620 }
2621
2622 /* http://bcm-specs.sipsolutions.net/EnableMac */
2623 void b43_mac_enable(struct b43_wldev *dev)
2624 {
2625         if (b43_debug(dev, B43_DBG_FIRMWARE)) {
2626                 u16 fwstate;
2627
2628                 fwstate = b43_shm_read16(dev, B43_SHM_SHARED,
2629                                          B43_SHM_SH_UCODESTAT);
2630                 if ((fwstate != B43_SHM_SH_UCODESTAT_SUSP) &&
2631                     (fwstate != B43_SHM_SH_UCODESTAT_SLEEP)) {
2632                         b43err(dev->wl, "b43_mac_enable(): The firmware "
2633                                "should be suspended, but current state is %u\n",
2634                                fwstate);
2635                 }
2636         }
2637
2638         dev->mac_suspended--;
2639         B43_WARN_ON(dev->mac_suspended < 0);
2640         if (dev->mac_suspended == 0) {
2641                 b43_write32(dev, B43_MMIO_MACCTL,
2642                             b43_read32(dev, B43_MMIO_MACCTL)
2643                             | B43_MACCTL_ENABLED);
2644                 b43_write32(dev, B43_MMIO_GEN_IRQ_REASON,
2645                             B43_IRQ_MAC_SUSPENDED);
2646                 /* Commit writes */
2647                 b43_read32(dev, B43_MMIO_MACCTL);
2648                 b43_read32(dev, B43_MMIO_GEN_IRQ_REASON);
2649                 b43_power_saving_ctl_bits(dev, 0);
2650         }
2651 }
2652
2653 /* http://bcm-specs.sipsolutions.net/SuspendMAC */
2654 void b43_mac_suspend(struct b43_wldev *dev)
2655 {
2656         int i;
2657         u32 tmp;
2658
2659         might_sleep();
2660         B43_WARN_ON(dev->mac_suspended < 0);
2661
2662         if (dev->mac_suspended == 0) {
2663                 b43_power_saving_ctl_bits(dev, B43_PS_AWAKE);
2664                 b43_write32(dev, B43_MMIO_MACCTL,
2665                             b43_read32(dev, B43_MMIO_MACCTL)
2666                             & ~B43_MACCTL_ENABLED);
2667                 /* force pci to flush the write */
2668                 b43_read32(dev, B43_MMIO_MACCTL);
2669                 for (i = 35; i; i--) {
2670                         tmp = b43_read32(dev, B43_MMIO_GEN_IRQ_REASON);
2671                         if (tmp & B43_IRQ_MAC_SUSPENDED)
2672                                 goto out;
2673                         udelay(10);
2674                 }
2675                 /* Hm, it seems this will take some time. Use msleep(). */
2676                 for (i = 40; i; i--) {
2677                         tmp = b43_read32(dev, B43_MMIO_GEN_IRQ_REASON);
2678                         if (tmp & B43_IRQ_MAC_SUSPENDED)
2679                                 goto out;
2680                         msleep(1);
2681                 }
2682                 b43err(dev->wl, "MAC suspend failed\n");
2683         }
2684 out:
2685         dev->mac_suspended++;
2686 }
2687
2688 static void b43_adjust_opmode(struct b43_wldev *dev)
2689 {
2690         struct b43_wl *wl = dev->wl;
2691         u32 ctl;
2692         u16 cfp_pretbtt;
2693
2694         ctl = b43_read32(dev, B43_MMIO_MACCTL);
2695         /* Reset status to STA infrastructure mode. */
2696         ctl &= ~B43_MACCTL_AP;
2697         ctl &= ~B43_MACCTL_KEEP_CTL;
2698         ctl &= ~B43_MACCTL_KEEP_BADPLCP;
2699         ctl &= ~B43_MACCTL_KEEP_BAD;
2700         ctl &= ~B43_MACCTL_PROMISC;
2701         ctl &= ~B43_MACCTL_BEACPROMISC;
2702         ctl |= B43_MACCTL_INFRA;
2703
2704         if (b43_is_mode(wl, NL80211_IFTYPE_AP) ||
2705             b43_is_mode(wl, NL80211_IFTYPE_MESH_POINT))
2706                 ctl |= B43_MACCTL_AP;
2707         else if (b43_is_mode(wl, NL80211_IFTYPE_ADHOC))
2708                 ctl &= ~B43_MACCTL_INFRA;
2709
2710         if (wl->filter_flags & FIF_CONTROL)
2711                 ctl |= B43_MACCTL_KEEP_CTL;
2712         if (wl->filter_flags & FIF_FCSFAIL)
2713                 ctl |= B43_MACCTL_KEEP_BAD;
2714         if (wl->filter_flags & FIF_PLCPFAIL)
2715                 ctl |= B43_MACCTL_KEEP_BADPLCP;
2716         if (wl->filter_flags & FIF_PROMISC_IN_BSS)
2717                 ctl |= B43_MACCTL_PROMISC;
2718         if (wl->filter_flags & FIF_BCN_PRBRESP_PROMISC)
2719                 ctl |= B43_MACCTL_BEACPROMISC;
2720
2721         /* Workaround: On old hardware the HW-MAC-address-filter
2722          * doesn't work properly, so always run promisc in filter
2723          * it in software. */
2724         if (dev->dev->id.revision <= 4)
2725                 ctl |= B43_MACCTL_PROMISC;
2726
2727         b43_write32(dev, B43_MMIO_MACCTL, ctl);
2728
2729         cfp_pretbtt = 2;
2730         if ((ctl & B43_MACCTL_INFRA) && !(ctl & B43_MACCTL_AP)) {
2731                 if (dev->dev->bus->chip_id == 0x4306 &&
2732                     dev->dev->bus->chip_rev == 3)
2733                         cfp_pretbtt = 100;
2734                 else
2735                         cfp_pretbtt = 50;
2736         }
2737         b43_write16(dev, 0x612, cfp_pretbtt);
2738
2739         /* FIXME: We don't currently implement the PMQ mechanism,
2740          *        so always disable it. If we want to implement PMQ,
2741          *        we need to enable it here (clear DISCPMQ) in AP mode.
2742          */
2743         if (0  /* ctl & B43_MACCTL_AP */) {
2744                 b43_write32(dev, B43_MMIO_MACCTL,
2745                             b43_read32(dev, B43_MMIO_MACCTL)
2746                             & ~B43_MACCTL_DISCPMQ);
2747         } else {
2748                 b43_write32(dev, B43_MMIO_MACCTL,
2749                             b43_read32(dev, B43_MMIO_MACCTL)
2750                             | B43_MACCTL_DISCPMQ);
2751         }
2752 }
2753
2754 static void b43_rate_memory_write(struct b43_wldev *dev, u16 rate, int is_ofdm)
2755 {
2756         u16 offset;
2757
2758         if (is_ofdm) {
2759                 offset = 0x480;
2760                 offset += (b43_plcp_get_ratecode_ofdm(rate) & 0x000F) * 2;
2761         } else {
2762                 offset = 0x4C0;
2763                 offset += (b43_plcp_get_ratecode_cck(rate) & 0x000F) * 2;
2764         }
2765         b43_shm_write16(dev, B43_SHM_SHARED, offset + 0x20,
2766                         b43_shm_read16(dev, B43_SHM_SHARED, offset));
2767 }
2768
2769 static void b43_rate_memory_init(struct b43_wldev *dev)
2770 {
2771         switch (dev->phy.type) {
2772         case B43_PHYTYPE_A:
2773         case B43_PHYTYPE_G:
2774         case B43_PHYTYPE_N:
2775         case B43_PHYTYPE_LP:
2776                 b43_rate_memory_write(dev, B43_OFDM_RATE_6MB, 1);
2777                 b43_rate_memory_write(dev, B43_OFDM_RATE_12MB, 1);
2778                 b43_rate_memory_write(dev, B43_OFDM_RATE_18MB, 1);
2779                 b43_rate_memory_write(dev, B43_OFDM_RATE_24MB, 1);
2780                 b43_rate_memory_write(dev, B43_OFDM_RATE_36MB, 1);
2781                 b43_rate_memory_write(dev, B43_OFDM_RATE_48MB, 1);
2782                 b43_rate_memory_write(dev, B43_OFDM_RATE_54MB, 1);
2783                 if (dev->phy.type == B43_PHYTYPE_A)
2784                         break;
2785                 /* fallthrough */
2786         case B43_PHYTYPE_B:
2787                 b43_rate_memory_write(dev, B43_CCK_RATE_1MB, 0);
2788                 b43_rate_memory_write(dev, B43_CCK_RATE_2MB, 0);
2789                 b43_rate_memory_write(dev, B43_CCK_RATE_5MB, 0);
2790                 b43_rate_memory_write(dev, B43_CCK_RATE_11MB, 0);
2791                 break;
2792         default:
2793                 B43_WARN_ON(1);
2794         }
2795 }
2796
2797 /* Set the default values for the PHY TX Control Words. */
2798 static void b43_set_phytxctl_defaults(struct b43_wldev *dev)
2799 {
2800         u16 ctl = 0;
2801
2802         ctl |= B43_TXH_PHY_ENC_CCK;
2803         ctl |= B43_TXH_PHY_ANT01AUTO;
2804         ctl |= B43_TXH_PHY_TXPWR;
2805
2806         b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_BEACPHYCTL, ctl);
2807         b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_ACKCTSPHYCTL, ctl);
2808         b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_PRPHYCTL, ctl);
2809 }
2810
2811 /* Set the TX-Antenna for management frames sent by firmware. */
2812 static void b43_mgmtframe_txantenna(struct b43_wldev *dev, int antenna)
2813 {
2814         u16 ant;
2815         u16 tmp;
2816
2817         ant = b43_antenna_to_phyctl(antenna);
2818
2819         /* For ACK/CTS */
2820         tmp = b43_shm_read16(dev, B43_SHM_SHARED, B43_SHM_SH_ACKCTSPHYCTL);
2821         tmp = (tmp & ~B43_TXH_PHY_ANT) | ant;
2822         b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_ACKCTSPHYCTL, tmp);
2823         /* For Probe Resposes */
2824         tmp = b43_shm_read16(dev, B43_SHM_SHARED, B43_SHM_SH_PRPHYCTL);
2825         tmp = (tmp & ~B43_TXH_PHY_ANT) | ant;
2826         b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_PRPHYCTL, tmp);
2827 }
2828
2829 /* This is the opposite of b43_chip_init() */
2830 static void b43_chip_exit(struct b43_wldev *dev)
2831 {
2832         b43_phy_exit(dev);
2833         b43_gpio_cleanup(dev);
2834         /* firmware is released later */
2835 }
2836
2837 /* Initialize the chip
2838  * http://bcm-specs.sipsolutions.net/ChipInit
2839  */
2840 static int b43_chip_init(struct b43_wldev *dev)
2841 {
2842         struct b43_phy *phy = &dev->phy;
2843         int err;
2844         u32 value32, macctl;
2845         u16 value16;
2846
2847         /* Initialize the MAC control */
2848         macctl = B43_MACCTL_IHR_ENABLED | B43_MACCTL_SHM_ENABLED;
2849         if (dev->phy.gmode)
2850                 macctl |= B43_MACCTL_GMODE;
2851         macctl |= B43_MACCTL_INFRA;
2852         b43_write32(dev, B43_MMIO_MACCTL, macctl);
2853
2854         err = b43_request_firmware(dev);
2855         if (err)
2856                 goto out;
2857         err = b43_upload_microcode(dev);
2858         if (err)
2859                 goto out;       /* firmware is released later */
2860
2861         err = b43_gpio_init(dev);
2862         if (err)
2863                 goto out;       /* firmware is released later */
2864
2865         err = b43_upload_initvals(dev);
2866         if (err)
2867                 goto err_gpio_clean;
2868
2869         /* Turn the Analog on and initialize the PHY. */
2870         phy->ops->switch_analog(dev, 1);
2871         err = b43_phy_init(dev);
2872         if (err)
2873                 goto err_gpio_clean;
2874
2875         /* Disable Interference Mitigation. */
2876         if (phy->ops->interf_mitigation)
2877                 phy->ops->interf_mitigation(dev, B43_INTERFMODE_NONE);
2878
2879         /* Select the antennae */
2880         if (phy->ops->set_rx_antenna)
2881                 phy->ops->set_rx_antenna(dev, B43_ANTENNA_DEFAULT);
2882         b43_mgmtframe_txantenna(dev, B43_ANTENNA_DEFAULT);
2883
2884         if (phy->type == B43_PHYTYPE_B) {
2885                 value16 = b43_read16(dev, 0x005E);
2886                 value16 |= 0x0004;
2887                 b43_write16(dev, 0x005E, value16);
2888         }
2889         b43_write32(dev, 0x0100, 0x01000000);
2890         if (dev->dev->id.revision < 5)
2891                 b43_write32(dev, 0x010C, 0x01000000);
2892
2893         b43_write32(dev, B43_MMIO_MACCTL, b43_read32(dev, B43_MMIO_MACCTL)
2894                     & ~B43_MACCTL_INFRA);
2895         b43_write32(dev, B43_MMIO_MACCTL, b43_read32(dev, B43_MMIO_MACCTL)
2896                     | B43_MACCTL_INFRA);
2897
2898         /* Probe Response Timeout value */
2899         /* FIXME: Default to 0, has to be set by ioctl probably... :-/ */
2900         b43_shm_write16(dev, B43_SHM_SHARED, 0x0074, 0x0000);
2901
2902         /* Initially set the wireless operation mode. */
2903         b43_adjust_opmode(dev);
2904
2905         if (dev->dev->id.revision < 3) {
2906                 b43_write16(dev, 0x060E, 0x0000);
2907                 b43_write16(dev, 0x0610, 0x8000);
2908                 b43_write16(dev, 0x0604, 0x0000);
2909                 b43_write16(dev, 0x0606, 0x0200);
2910         } else {
2911                 b43_write32(dev, 0x0188, 0x80000000);
2912                 b43_write32(dev, 0x018C, 0x02000000);
2913         }
2914         b43_write32(dev, B43_MMIO_GEN_IRQ_REASON, 0x00004000);
2915         b43_write32(dev, B43_MMIO_DMA0_IRQ_MASK, 0x0001DC00);
2916         b43_write32(dev, B43_MMIO_DMA1_IRQ_MASK, 0x0000DC00);
2917         b43_write32(dev, B43_MMIO_DMA2_IRQ_MASK, 0x0000DC00);
2918         b43_write32(dev, B43_MMIO_DMA3_IRQ_MASK, 0x0001DC00);
2919         b43_write32(dev, B43_MMIO_DMA4_IRQ_MASK, 0x0000DC00);
2920         b43_write32(dev, B43_MMIO_DMA5_IRQ_MASK, 0x0000DC00);
2921
2922         value32 = ssb_read32(dev->dev, SSB_TMSLOW);
2923         value32 |= 0x00100000;
2924         ssb_write32(dev->dev, SSB_TMSLOW, value32);
2925
2926         b43_write16(dev, B43_MMIO_POWERUP_DELAY,
2927                     dev->dev->bus->chipco.fast_pwrup_delay);
2928
2929         err = 0;
2930         b43dbg(dev->wl, "Chip initialized\n");
2931 out:
2932         return err;
2933
2934 err_gpio_clean:
2935         b43_gpio_cleanup(dev);
2936         return err;
2937 }
2938
2939 static void b43_periodic_every60sec(struct b43_wldev *dev)
2940 {
2941         const struct b43_phy_operations *ops = dev->phy.ops;
2942
2943         if (ops->pwork_60sec)
2944                 ops->pwork_60sec(dev);
2945
2946         /* Force check the TX power emission now. */
2947         b43_phy_txpower_check(dev, B43_TXPWR_IGNORE_TIME);
2948 }
2949
2950 static void b43_periodic_every30sec(struct b43_wldev *dev)
2951 {
2952         /* Update device statistics. */
2953         b43_calculate_link_quality(dev);
2954 }
2955
2956 static void b43_periodic_every15sec(struct b43_wldev *dev)
2957 {
2958         struct b43_phy *phy = &dev->phy;
2959         u16 wdr;
2960
2961         if (dev->fw.opensource) {
2962                 /* Check if the firmware is still alive.
2963                  * It will reset the watchdog counter to 0 in its idle loop. */
2964                 wdr = b43_shm_read16(dev, B43_SHM_SCRATCH, B43_WATCHDOG_REG);
2965                 if (unlikely(wdr)) {
2966                         b43err(dev->wl, "Firmware watchdog: The firmware died!\n");
2967                         b43_controller_restart(dev, "Firmware watchdog");
2968                         return;
2969                 } else {
2970                         b43_shm_write16(dev, B43_SHM_SCRATCH,
2971                                         B43_WATCHDOG_REG, 1);
2972                 }
2973         }
2974
2975         if (phy->ops->pwork_15sec)
2976                 phy->ops->pwork_15sec(dev);
2977
2978         atomic_set(&phy->txerr_cnt, B43_PHY_TX_BADNESS_LIMIT);
2979         wmb();
2980
2981 #if B43_DEBUG
2982         if (b43_debug(dev, B43_DBG_VERBOSESTATS)) {
2983                 unsigned int i;
2984
2985                 b43dbg(dev->wl, "Stats: %7u IRQs/sec, %7u TX/sec, %7u RX/sec\n",
2986                        dev->irq_count / 15,
2987                        dev->tx_count / 15,
2988                        dev->rx_count / 15);
2989                 dev->irq_count = 0;
2990                 dev->tx_count = 0;
2991                 dev->rx_count = 0;
2992                 for (i = 0; i < ARRAY_SIZE(dev->irq_bit_count); i++) {
2993                         if (dev->irq_bit_count[i]) {
2994                                 b43dbg(dev->wl, "Stats: %7u IRQ-%02u/sec (0x%08X)\n",
2995                                        dev->irq_bit_count[i] / 15, i, (1 << i));
2996                                 dev->irq_bit_count[i] = 0;
2997                         }
2998                 }
2999         }
3000 #endif
3001 }
3002
3003 static void do_periodic_work(struct b43_wldev *dev)
3004 {
3005         unsigned int state;
3006
3007         state = dev->periodic_state;
3008         if (state % 4 == 0)
3009                 b43_periodic_every60sec(dev);
3010         if (state % 2 == 0)
3011                 b43_periodic_every30sec(dev);
3012         b43_periodic_every15sec(dev);
3013 }
3014
3015 /* Periodic work locking policy:
3016  *      The whole periodic work handler is protected by
3017  *      wl->mutex. If another lock is needed somewhere in the
3018  *      pwork callchain, it's acquired in-place, where it's needed.
3019  */
3020 static void b43_periodic_work_handler(struct work_struct *work)
3021 {
3022         struct b43_wldev *dev = container_of(work, struct b43_wldev,
3023                                              periodic_work.work);
3024         struct b43_wl *wl = dev->wl;
3025         unsigned long delay;
3026
3027         mutex_lock(&wl->mutex);
3028
3029         if (unlikely(b43_status(dev) != B43_STAT_STARTED))
3030                 goto out;
3031         if (b43_debug(dev, B43_DBG_PWORK_STOP))
3032                 goto out_requeue;
3033
3034         do_periodic_work(dev);
3035
3036         dev->periodic_state++;
3037 out_requeue:
3038         if (b43_debug(dev, B43_DBG_PWORK_FAST))
3039                 delay = msecs_to_jiffies(50);
3040         else
3041                 delay = round_jiffies_relative(HZ * 15);
3042         ieee80211_queue_delayed_work(wl->hw, &dev->periodic_work, delay);
3043 out:
3044         mutex_unlock(&wl->mutex);
3045 }
3046
3047 static void b43_periodic_tasks_setup(struct b43_wldev *dev)
3048 {
3049         struct delayed_work *work = &dev->periodic_work;
3050
3051         dev->periodic_state = 0;
3052         INIT_DELAYED_WORK(work, b43_periodic_work_handler);
3053         ieee80211_queue_delayed_work(dev->wl->hw, work, 0);
3054 }
3055
3056 /* Check if communication with the device works correctly. */
3057 static int b43_validate_chipaccess(struct b43_wldev *dev)
3058 {
3059         u32 v, backup0, backup4;
3060
3061         backup0 = b43_shm_read32(dev, B43_SHM_SHARED, 0);
3062         backup4 = b43_shm_read32(dev, B43_SHM_SHARED, 4);
3063
3064         /* Check for read/write and endianness problems. */
3065         b43_shm_write32(dev, B43_SHM_SHARED, 0, 0x55AAAA55);
3066         if (b43_shm_read32(dev, B43_SHM_SHARED, 0) != 0x55AAAA55)
3067                 goto error;
3068         b43_shm_write32(dev, B43_SHM_SHARED, 0, 0xAA5555AA);
3069         if (b43_shm_read32(dev, B43_SHM_SHARED, 0) != 0xAA5555AA)
3070                 goto error;
3071
3072         /* Check if unaligned 32bit SHM_SHARED access works properly.
3073          * However, don't bail out on failure, because it's noncritical. */
3074         b43_shm_write16(dev, B43_SHM_SHARED, 0, 0x1122);
3075         b43_shm_write16(dev, B43_SHM_SHARED, 2, 0x3344);
3076         b43_shm_write16(dev, B43_SHM_SHARED, 4, 0x5566);
3077         b43_shm_write16(dev, B43_SHM_SHARED, 6, 0x7788);
3078         if (b43_shm_read32(dev, B43_SHM_SHARED, 2) != 0x55663344)
3079                 b43warn(dev->wl, "Unaligned 32bit SHM read access is broken\n");
3080         b43_shm_write32(dev, B43_SHM_SHARED, 2, 0xAABBCCDD);
3081         if (b43_shm_read16(dev, B43_SHM_SHARED, 0) != 0x1122 ||
3082             b43_shm_read16(dev, B43_SHM_SHARED, 2) != 0xCCDD ||
3083             b43_shm_read16(dev, B43_SHM_SHARED, 4) != 0xAABB ||
3084             b43_shm_read16(dev, B43_SHM_SHARED, 6) != 0x7788)
3085                 b43warn(dev->wl, "Unaligned 32bit SHM write access is broken\n");
3086
3087         b43_shm_write32(dev, B43_SHM_SHARED, 0, backup0);
3088         b43_shm_write32(dev, B43_SHM_SHARED, 4, backup4);
3089
3090         if ((dev->dev->id.revision >= 3) && (dev->dev->id.revision <= 10)) {
3091                 /* The 32bit register shadows the two 16bit registers
3092                  * with update sideeffects. Validate this. */
3093                 b43_write16(dev, B43_MMIO_TSF_CFP_START, 0xAAAA);
3094                 b43_write32(dev, B43_MMIO_TSF_CFP_START, 0xCCCCBBBB);
3095                 if (b43_read16(dev, B43_MMIO_TSF_CFP_START_LOW) != 0xBBBB)
3096                         goto error;
3097                 if (b43_read16(dev, B43_MMIO_TSF_CFP_START_HIGH) != 0xCCCC)
3098                         goto error;
3099         }
3100         b43_write32(dev, B43_MMIO_TSF_CFP_START, 0);
3101
3102         v = b43_read32(dev, B43_MMIO_MACCTL);
3103         v |= B43_MACCTL_GMODE;
3104         if (v != (B43_MACCTL_GMODE | B43_MACCTL_IHR_ENABLED))
3105                 goto error;
3106
3107         return 0;
3108 error:
3109         b43err(dev->wl, "Failed to validate the chipaccess\n");
3110         return -ENODEV;
3111 }
3112
3113 static void b43_security_init(struct b43_wldev *dev)
3114 {
3115         dev->ktp = b43_shm_read16(dev, B43_SHM_SHARED, B43_SHM_SH_KTP);
3116         /* KTP is a word address, but we address SHM bytewise.
3117          * So multiply by two.
3118          */
3119         dev->ktp *= 2;
3120         /* Number of RCMTA address slots */
3121         b43_write16(dev, B43_MMIO_RCMTA_COUNT, B43_NR_PAIRWISE_KEYS);
3122         /* Clear the key memory. */
3123         b43_clear_keys(dev);
3124 }
3125
3126 #ifdef CONFIG_B43_HWRNG
3127 static int b43_rng_read(struct hwrng *rng, u32 *data)
3128 {
3129         struct b43_wl *wl = (struct b43_wl *)rng->priv;
3130         struct b43_wldev *dev;
3131         int count = -ENODEV;
3132
3133         mutex_lock(&wl->mutex);
3134         dev = wl->current_dev;
3135         if (likely(dev && b43_status(dev) >= B43_STAT_INITIALIZED)) {
3136                 *data = b43_read16(dev, B43_MMIO_RNG);
3137                 count = sizeof(u16);
3138         }
3139         mutex_unlock(&wl->mutex);
3140
3141         return count;
3142 }
3143 #endif /* CONFIG_B43_HWRNG */
3144
3145 static void b43_rng_exit(struct b43_wl *wl)
3146 {
3147 #ifdef CONFIG_B43_HWRNG
3148         if (wl->rng_initialized)
3149                 hwrng_unregister(&wl->rng);
3150 #endif /* CONFIG_B43_HWRNG */
3151 }
3152
3153 static int b43_rng_init(struct b43_wl *wl)
3154 {
3155         int err = 0;
3156
3157 #ifdef CONFIG_B43_HWRNG
3158         snprintf(wl->rng_name, ARRAY_SIZE(wl->rng_name),
3159                  "%s_%s", KBUILD_MODNAME, wiphy_name(wl->hw->wiphy));
3160         wl->rng.name = wl->rng_name;
3161         wl->rng.data_read = b43_rng_read;
3162         wl->rng.priv = (unsigned long)wl;
3163         wl->rng_initialized = 1;
3164         err = hwrng_register(&wl->rng);
3165         if (err) {
3166                 wl->rng_initialized = 0;
3167                 b43err(wl, "Failed to register the random "
3168                        "number generator (%d)\n", err);
3169         }
3170 #endif /* CONFIG_B43_HWRNG */
3171
3172         return err;
3173 }
3174
3175 static void b43_tx_work(struct work_struct *work)
3176 {
3177         struct b43_wl *wl = container_of(work, struct b43_wl, tx_work);
3178         struct b43_wldev *dev;
3179         struct sk_buff *skb;
3180         int err = 0;
3181
3182         mutex_lock(&wl->mutex);
3183         dev = wl->current_dev;
3184         if (unlikely(!dev || b43_status(dev) < B43_STAT_STARTED)) {
3185                 mutex_unlock(&wl->mutex);
3186                 return;
3187         }
3188
3189         while (skb_queue_len(&wl->tx_queue)) {
3190                 skb = skb_dequeue(&wl->tx_queue);
3191
3192                 if (b43_using_pio_transfers(dev))
3193                         err = b43_pio_tx(dev, skb);
3194                 else
3195                         err = b43_dma_tx(dev, skb);
3196                 if (unlikely(err))
3197                         dev_kfree_skb(skb); /* Drop it */
3198         }
3199
3200 #if B43_DEBUG
3201         dev->tx_count++;
3202 #endif
3203         mutex_unlock(&wl->mutex);
3204 }
3205
3206 static int b43_op_tx(struct ieee80211_hw *hw,
3207                      struct sk_buff *skb)
3208 {
3209         struct b43_wl *wl = hw_to_b43_wl(hw);
3210
3211         if (unlikely(skb->len < 2 + 2 + 6)) {
3212                 /* Too short, this can't be a valid frame. */
3213                 dev_kfree_skb_any(skb);
3214                 return NETDEV_TX_OK;
3215         }
3216         B43_WARN_ON(skb_shinfo(skb)->nr_frags);
3217
3218         skb_queue_tail(&wl->tx_queue, skb);
3219         ieee80211_queue_work(wl->hw, &wl->tx_work);
3220
3221         return NETDEV_TX_OK;
3222 }
3223
3224 static void b43_qos_params_upload(struct b43_wldev *dev,
3225                                   const struct ieee80211_tx_queue_params *p,
3226                                   u16 shm_offset)
3227 {
3228         u16 params[B43_NR_QOSPARAMS];
3229         int bslots, tmp;
3230         unsigned int i;
3231
3232         if (!dev->qos_enabled)
3233                 return;
3234
3235         bslots = b43_read16(dev, B43_MMIO_RNG) & p->cw_min;
3236
3237         memset(&params, 0, sizeof(params));
3238
3239         params[B43_QOSPARAM_TXOP] = p->txop * 32;
3240         params[B43_QOSPARAM_CWMIN] = p->cw_min;
3241         params[B43_QOSPARAM_CWMAX] = p->cw_max;
3242         params[B43_QOSPARAM_CWCUR] = p->cw_min;
3243         params[B43_QOSPARAM_AIFS] = p->aifs;
3244         params[B43_QOSPARAM_BSLOTS] = bslots;
3245         params[B43_QOSPARAM_REGGAP] = bslots + p->aifs;
3246
3247         for (i = 0; i < ARRAY_SIZE(params); i++) {
3248                 if (i == B43_QOSPARAM_STATUS) {
3249                         tmp = b43_shm_read16(dev, B43_SHM_SHARED,
3250                                              shm_offset + (i * 2));
3251                         /* Mark the parameters as updated. */
3252                         tmp |= 0x100;
3253                         b43_shm_write16(dev, B43_SHM_SHARED,
3254                                         shm_offset + (i * 2),
3255                                         tmp);
3256                 } else {
3257                         b43_shm_write16(dev, B43_SHM_SHARED,
3258                                         shm_offset + (i * 2),
3259                                         params[i]);
3260                 }
3261         }
3262 }
3263
3264 /* Mapping of mac80211 queue numbers to b43 QoS SHM offsets. */
3265 static const u16 b43_qos_shm_offsets[] = {
3266         /* [mac80211-queue-nr] = SHM_OFFSET, */
3267         [0] = B43_QOS_VOICE,
3268         [1] = B43_QOS_VIDEO,
3269         [2] = B43_QOS_BESTEFFORT,
3270         [3] = B43_QOS_BACKGROUND,
3271 };
3272
3273 /* Update all QOS parameters in hardware. */
3274 static void b43_qos_upload_all(struct b43_wldev *dev)
3275 {
3276         struct b43_wl *wl = dev->wl;
3277         struct b43_qos_params *params;
3278         unsigned int i;
3279
3280         if (!dev->qos_enabled)
3281                 return;
3282
3283         BUILD_BUG_ON(ARRAY_SIZE(b43_qos_shm_offsets) !=
3284                      ARRAY_SIZE(wl->qos_params));
3285
3286         b43_mac_suspend(dev);
3287         for (i = 0; i < ARRAY_SIZE(wl->qos_params); i++) {
3288                 params = &(wl->qos_params[i]);
3289                 b43_qos_params_upload(dev, &(params->p),
3290                                       b43_qos_shm_offsets[i]);
3291         }
3292         b43_mac_enable(dev);
3293 }
3294
3295 static void b43_qos_clear(struct b43_wl *wl)
3296 {
3297         struct b43_qos_params *params;
3298         unsigned int i;
3299
3300         /* Initialize QoS parameters to sane defaults. */
3301
3302         BUILD_BUG_ON(ARRAY_SIZE(b43_qos_shm_offsets) !=
3303                      ARRAY_SIZE(wl->qos_params));
3304
3305         for (i = 0; i < ARRAY_SIZE(wl->qos_params); i++) {
3306                 params = &(wl->qos_params[i]);
3307
3308                 switch (b43_qos_shm_offsets[i]) {
3309                 case B43_QOS_VOICE:
3310                         params->p.txop = 0;
3311                         params->p.aifs = 2;
3312                         params->p.cw_min = 0x0001;
3313                         params->p.cw_max = 0x0001;
3314                         break;
3315                 case B43_QOS_VIDEO:
3316                         params->p.txop = 0;
3317                         params->p.aifs = 2;
3318                         params->p.cw_min = 0x0001;
3319                         params->p.cw_max = 0x0001;
3320                         break;
3321                 case B43_QOS_BESTEFFORT:
3322                         params->p.txop = 0;
3323                         params->p.aifs = 3;
3324                         params->p.cw_min = 0x0001;
3325                         params->p.cw_max = 0x03FF;
3326                         break;
3327                 case B43_QOS_BACKGROUND:
3328                         params->p.txop = 0;
3329                         params->p.aifs = 7;
3330                         params->p.cw_min = 0x0001;
3331                         params->p.cw_max = 0x03FF;
3332                         break;
3333                 default:
3334                         B43_WARN_ON(1);
3335                 }
3336         }
3337 }
3338
3339 /* Initialize the core's QOS capabilities */
3340 static void b43_qos_init(struct b43_wldev *dev)
3341 {
3342         if (!dev->qos_enabled) {
3343                 /* Disable QOS support. */
3344                 b43_hf_write(dev, b43_hf_read(dev) & ~B43_HF_EDCF);
3345                 b43_write16(dev, B43_MMIO_IFSCTL,
3346                             b43_read16(dev, B43_MMIO_IFSCTL)
3347                             & ~B43_MMIO_IFSCTL_USE_EDCF);
3348                 b43dbg(dev->wl, "QoS disabled\n");
3349                 return;
3350         }
3351
3352         /* Upload the current QOS parameters. */
3353         b43_qos_upload_all(dev);
3354
3355         /* Enable QOS support. */
3356         b43_hf_write(dev, b43_hf_read(dev) | B43_HF_EDCF);
3357         b43_write16(dev, B43_MMIO_IFSCTL,
3358                     b43_read16(dev, B43_MMIO_IFSCTL)
3359                     | B43_MMIO_IFSCTL_USE_EDCF);
3360         b43dbg(dev->wl, "QoS enabled\n");
3361 }
3362
3363 static int b43_op_conf_tx(struct ieee80211_hw *hw, u16 _queue,
3364                           const struct ieee80211_tx_queue_params *params)
3365 {
3366         struct b43_wl *wl = hw_to_b43_wl(hw);
3367         struct b43_wldev *dev;
3368         unsigned int queue = (unsigned int)_queue;
3369         int err = -ENODEV;
3370
3371         if (queue >= ARRAY_SIZE(wl->qos_params)) {
3372                 /* Queue not available or don't support setting
3373                  * params on this queue. Return success to not
3374                  * confuse mac80211. */
3375                 return 0;
3376         }
3377         BUILD_BUG_ON(ARRAY_SIZE(b43_qos_shm_offsets) !=
3378                      ARRAY_SIZE(wl->qos_params));
3379
3380         mutex_lock(&wl->mutex);
3381         dev = wl->current_dev;
3382         if (unlikely(!dev || (b43_status(dev) < B43_STAT_INITIALIZED)))
3383                 goto out_unlock;
3384
3385         memcpy(&(wl->qos_params[queue].p), params, sizeof(*params));
3386         b43_mac_suspend(dev);
3387         b43_qos_params_upload(dev, &(wl->qos_params[queue].p),
3388                               b43_qos_shm_offsets[queue]);
3389         b43_mac_enable(dev);
3390         err = 0;
3391
3392 out_unlock:
3393         mutex_unlock(&wl->mutex);
3394
3395         return err;
3396 }
3397
3398 static int b43_op_get_stats(struct ieee80211_hw *hw,
3399                             struct ieee80211_low_level_stats *stats)
3400 {
3401         struct b43_wl *wl = hw_to_b43_wl(hw);
3402
3403         mutex_lock(&wl->mutex);
3404         memcpy(stats, &wl->ieee_stats, sizeof(*stats));
3405         mutex_unlock(&wl->mutex);
3406
3407         return 0;
3408 }
3409
3410 static u64 b43_op_get_tsf(struct ieee80211_hw *hw)
3411 {
3412         struct b43_wl *wl = hw_to_b43_wl(hw);
3413         struct b43_wldev *dev;
3414         u64 tsf;
3415
3416         mutex_lock(&wl->mutex);
3417         dev = wl->current_dev;
3418
3419         if (dev && (b43_status(dev) >= B43_STAT_INITIALIZED))
3420                 b43_tsf_read(dev, &tsf);
3421         else
3422                 tsf = 0;
3423
3424         mutex_unlock(&wl->mutex);
3425
3426         return tsf;
3427 }
3428
3429 static void b43_op_set_tsf(struct ieee80211_hw *hw, u64 tsf)
3430 {
3431         struct b43_wl *wl = hw_to_b43_wl(hw);
3432         struct b43_wldev *dev;
3433
3434         mutex_lock(&wl->mutex);
3435         dev = wl->current_dev;
3436
3437         if (dev && (b43_status(dev) >= B43_STAT_INITIALIZED))
3438                 b43_tsf_write(dev, tsf);
3439
3440         mutex_unlock(&wl->mutex);
3441 }
3442
3443 static void b43_put_phy_into_reset(struct b43_wldev *dev)
3444 {
3445         struct ssb_device *sdev = dev->dev;
3446         u32 tmslow;
3447
3448         tmslow = ssb_read32(sdev, SSB_TMSLOW);
3449         tmslow &= ~B43_TMSLOW_GMODE;
3450         tmslow |= B43_TMSLOW_PHYRESET;
3451         tmslow |= SSB_TMSLOW_FGC;
3452         ssb_write32(sdev, SSB_TMSLOW, tmslow);
3453         msleep(1);
3454
3455         tmslow = ssb_read32(sdev, SSB_TMSLOW);
3456         tmslow &= ~SSB_TMSLOW_FGC;
3457         tmslow |= B43_TMSLOW_PHYRESET;
3458         ssb_write32(sdev, SSB_TMSLOW, tmslow);
3459         msleep(1);
3460 }
3461
3462 static const char *band_to_string(enum ieee80211_band band)
3463 {
3464         switch (band) {
3465         case IEEE80211_BAND_5GHZ:
3466                 return "5";
3467         case IEEE80211_BAND_2GHZ:
3468                 return "2.4";
3469         default:
3470                 break;
3471         }
3472         B43_WARN_ON(1);
3473         return "";
3474 }
3475
3476 /* Expects wl->mutex locked */
3477 static int b43_switch_band(struct b43_wl *wl, struct ieee80211_channel *chan)
3478 {
3479         struct b43_wldev *up_dev = NULL;
3480         struct b43_wldev *down_dev;
3481         struct b43_wldev *d;
3482         int err;
3483         bool uninitialized_var(gmode);
3484         int prev_status;
3485
3486         /* Find a device and PHY which supports the band. */
3487         list_for_each_entry(d, &wl->devlist, list) {
3488                 switch (chan->band) {
3489                 case IEEE80211_BAND_5GHZ:
3490                         if (d->phy.supports_5ghz) {
3491                                 up_dev = d;
3492                                 gmode = 0;
3493                         }
3494                         break;
3495                 case IEEE80211_BAND_2GHZ:
3496                         if (d->phy.supports_2ghz) {
3497                                 up_dev = d;
3498                                 gmode = 1;
3499                         }
3500                         break;
3501                 default:
3502                         B43_WARN_ON(1);
3503                         return -EINVAL;
3504                 }
3505                 if (up_dev)
3506                         break;
3507         }
3508         if (!up_dev) {
3509                 b43err(wl, "Could not find a device for %s-GHz band operation\n",
3510                        band_to_string(chan->band));
3511                 return -ENODEV;
3512         }
3513         if ((up_dev == wl->current_dev) &&
3514             (!!wl->current_dev->phy.gmode == !!gmode)) {
3515                 /* This device is already running. */
3516                 return 0;
3517         }
3518         b43dbg(wl, "Switching to %s-GHz band\n",
3519                band_to_string(chan->band));
3520         down_dev = wl->current_dev;
3521
3522         prev_status = b43_status(down_dev);
3523         /* Shutdown the currently running core. */
3524         if (prev_status >= B43_STAT_STARTED)
3525                 down_dev = b43_wireless_core_stop(down_dev);
3526         if (prev_status >= B43_STAT_INITIALIZED)
3527                 b43_wireless_core_exit(down_dev);
3528
3529         if (down_dev != up_dev) {
3530                 /* We switch to a different core, so we put PHY into
3531                  * RESET on the old core. */
3532                 b43_put_phy_into_reset(down_dev);
3533         }
3534
3535         /* Now start the new core. */
3536         up_dev->phy.gmode = gmode;
3537         if (prev_status >= B43_STAT_INITIALIZED) {
3538                 err = b43_wireless_core_init(up_dev);
3539                 if (err) {
3540                         b43err(wl, "Fatal: Could not initialize device for "
3541                                "selected %s-GHz band\n",
3542                                band_to_string(chan->band));
3543                         goto init_failure;
3544                 }
3545         }
3546         if (prev_status >= B43_STAT_STARTED) {
3547                 err = b43_wireless_core_start(up_dev);
3548                 if (err) {
3549                         b43err(wl, "Fatal: Coult not start device for "
3550                                "selected %s-GHz band\n",
3551                                band_to_string(chan->band));
3552                         b43_wireless_core_exit(up_dev);
3553                         goto init_failure;
3554                 }
3555         }
3556         B43_WARN_ON(b43_status(up_dev) != prev_status);
3557
3558         wl->current_dev = up_dev;
3559
3560         return 0;
3561 init_failure:
3562         /* Whoops, failed to init the new core. No core is operating now. */
3563         wl->current_dev = NULL;
3564         return err;
3565 }
3566
3567 /* Write the short and long frame retry limit values. */
3568 static void b43_set_retry_limits(struct b43_wldev *dev,
3569                                  unsigned int short_retry,
3570                                  unsigned int long_retry)
3571 {
3572         /* The retry limit is a 4-bit counter. Enforce this to avoid overflowing
3573          * the chip-internal counter. */
3574         short_retry = min(short_retry, (unsigned int)0xF);
3575         long_retry = min(long_retry, (unsigned int)0xF);
3576
3577         b43_shm_write16(dev, B43_SHM_SCRATCH, B43_SHM_SC_SRLIMIT,
3578                         short_retry);
3579         b43_shm_write16(dev, B43_SHM_SCRATCH, B43_SHM_SC_LRLIMIT,
3580                         long_retry);
3581 }
3582
3583 static int b43_op_config(struct ieee80211_hw *hw, u32 changed)
3584 {
3585         struct b43_wl *wl = hw_to_b43_wl(hw);
3586         struct b43_wldev *dev;
3587         struct b43_phy *phy;
3588         struct ieee80211_conf *conf = &hw->conf;
3589         int antenna;
3590         int err = 0;
3591
3592         mutex_lock(&wl->mutex);
3593
3594         /* Switch the band (if necessary). This might change the active core. */
3595         err = b43_switch_band(wl, conf->channel);
3596         if (err)
3597                 goto out_unlock_mutex;
3598         dev = wl->current_dev;
3599         phy = &dev->phy;
3600
3601         if (conf_is_ht(conf))
3602                 phy->is_40mhz =
3603                         (conf_is_ht40_minus(conf) || conf_is_ht40_plus(conf));
3604         else
3605                 phy->is_40mhz = false;
3606
3607         b43_mac_suspend(dev);
3608
3609         if (changed & IEEE80211_CONF_CHANGE_RETRY_LIMITS)
3610                 b43_set_retry_limits(dev, conf->short_frame_max_tx_count,
3611                                           conf->long_frame_max_tx_count);
3612         changed &= ~IEEE80211_CONF_CHANGE_RETRY_LIMITS;
3613         if (!changed)
3614                 goto out_mac_enable;
3615
3616         /* Switch to the requested channel.
3617          * The firmware takes care of races with the TX handler. */
3618         if (conf->channel->hw_value != phy->channel)
3619                 b43_switch_channel(dev, conf->channel->hw_value);
3620
3621         dev->wl->radiotap_enabled = !!(conf->flags & IEEE80211_CONF_MONITOR);
3622
3623         /* Adjust the desired TX power level. */
3624         if (conf->power_level != 0) {
3625                 if (conf->power_level != phy->desired_txpower) {
3626                         phy->desired_txpower = conf->power_level;
3627                         b43_phy_txpower_check(dev, B43_TXPWR_IGNORE_TIME |
3628                                                    B43_TXPWR_IGNORE_TSSI);
3629                 }
3630         }
3631
3632         /* Antennas for RX and management frame TX. */
3633         antenna = B43_ANTENNA_DEFAULT;
3634         b43_mgmtframe_txantenna(dev, antenna);
3635         antenna = B43_ANTENNA_DEFAULT;
3636         if (phy->ops->set_rx_antenna)
3637                 phy->ops->set_rx_antenna(dev, antenna);
3638
3639         if (wl->radio_enabled != phy->radio_on) {
3640                 if (wl->radio_enabled) {
3641                         b43_software_rfkill(dev, false);
3642                         b43info(dev->wl, "Radio turned on by software\n");
3643                         if (!dev->radio_hw_enable) {
3644                                 b43info(dev->wl, "The hardware RF-kill button "
3645                                         "still turns the radio physically off. "
3646                                         "Press the button to turn it on.\n");
3647                         }
3648                 } else {
3649                         b43_software_rfkill(dev, true);
3650                         b43info(dev->wl, "Radio turned off by software\n");
3651                 }
3652         }
3653
3654 out_mac_enable:
3655         b43_mac_enable(dev);
3656 out_unlock_mutex:
3657         mutex_unlock(&wl->mutex);
3658
3659         return err;
3660 }
3661
3662 static void b43_update_basic_rates(struct b43_wldev *dev, u32 brates)
3663 {
3664         struct ieee80211_supported_band *sband =
3665                 dev->wl->hw->wiphy->bands[b43_current_band(dev->wl)];
3666         struct ieee80211_rate *rate;
3667         int i;
3668         u16 basic, direct, offset, basic_offset, rateptr;
3669
3670         for (i = 0; i < sband->n_bitrates; i++) {
3671                 rate = &sband->bitrates[i];
3672
3673                 if (b43_is_cck_rate(rate->hw_value)) {
3674                         direct = B43_SHM_SH_CCKDIRECT;
3675                         basic = B43_SHM_SH_CCKBASIC;
3676                         offset = b43_plcp_get_ratecode_cck(rate->hw_value);
3677                         offset &= 0xF;
3678                 } else {
3679                         direct = B43_SHM_SH_OFDMDIRECT;
3680                         basic = B43_SHM_SH_OFDMBASIC;
3681                         offset = b43_plcp_get_ratecode_ofdm(rate->hw_value);
3682                         offset &= 0xF;
3683                 }
3684
3685                 rate = ieee80211_get_response_rate(sband, brates, rate->bitrate);
3686
3687                 if (b43_is_cck_rate(rate->hw_value)) {
3688                         basic_offset = b43_plcp_get_ratecode_cck(rate->hw_value);
3689                         basic_offset &= 0xF;
3690                 } else {
3691                         basic_offset = b43_plcp_get_ratecode_ofdm(rate->hw_value);
3692                         basic_offset &= 0xF;
3693                 }
3694
3695                 /*
3696                  * Get the pointer that we need to point to
3697                  * from the direct map
3698                  */
3699                 rateptr = b43_shm_read16(dev, B43_SHM_SHARED,
3700                                          direct + 2 * basic_offset);
3701                 /* and write it to the basic map */
3702                 b43_shm_write16(dev, B43_SHM_SHARED, basic + 2 * offset,
3703                                 rateptr);
3704         }
3705 }
3706
3707 static void b43_op_bss_info_changed(struct ieee80211_hw *hw,
3708                                     struct ieee80211_vif *vif,
3709                                     struct ieee80211_bss_conf *conf,
3710                                     u32 changed)
3711 {
3712         struct b43_wl *wl = hw_to_b43_wl(hw);
3713         struct b43_wldev *dev;
3714
3715         mutex_lock(&wl->mutex);
3716
3717         dev = wl->current_dev;
3718         if (!dev || b43_status(dev) < B43_STAT_STARTED)
3719                 goto out_unlock_mutex;
3720
3721         B43_WARN_ON(wl->vif != vif);
3722
3723         if (changed & BSS_CHANGED_BSSID) {
3724                 if (conf->bssid)
3725                         memcpy(wl->bssid, conf->bssid, ETH_ALEN);
3726                 else
3727                         memset(wl->bssid, 0, ETH_ALEN);
3728         }
3729
3730         if (b43_status(dev) >= B43_STAT_INITIALIZED) {
3731                 if (changed & BSS_CHANGED_BEACON &&
3732                     (b43_is_mode(wl, NL80211_IFTYPE_AP) ||
3733                      b43_is_mode(wl, NL80211_IFTYPE_MESH_POINT) ||
3734                      b43_is_mode(wl, NL80211_IFTYPE_ADHOC)))
3735                         b43_update_templates(wl);
3736
3737                 if (changed & BSS_CHANGED_BSSID)
3738                         b43_write_mac_bssid_templates(dev);
3739         }
3740
3741         b43_mac_suspend(dev);
3742
3743         /* Update templates for AP/mesh mode. */
3744         if (changed & BSS_CHANGED_BEACON_INT &&
3745             (b43_is_mode(wl, NL80211_IFTYPE_AP) ||
3746              b43_is_mode(wl, NL80211_IFTYPE_MESH_POINT) ||
3747              b43_is_mode(wl, NL80211_IFTYPE_ADHOC)))
3748                 b43_set_beacon_int(dev, conf->beacon_int);
3749
3750         if (changed & BSS_CHANGED_BASIC_RATES)
3751                 b43_update_basic_rates(dev, conf->basic_rates);
3752
3753         if (changed & BSS_CHANGED_ERP_SLOT) {
3754                 if (conf->use_short_slot)
3755                         b43_short_slot_timing_enable(dev);
3756                 else
3757                         b43_short_slot_timing_disable(dev);
3758         }
3759
3760         b43_mac_enable(dev);
3761 out_unlock_mutex:
3762         mutex_unlock(&wl->mutex);
3763 }
3764
3765 static int b43_op_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
3766                           struct ieee80211_vif *vif, struct ieee80211_sta *sta,
3767                           struct ieee80211_key_conf *key)
3768 {
3769         struct b43_wl *wl = hw_to_b43_wl(hw);
3770         struct b43_wldev *dev;
3771         u8 algorithm;
3772         u8 index;
3773         int err;
3774         static const u8 bcast_addr[ETH_ALEN] = { 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };
3775
3776         if (modparam_nohwcrypt)
3777                 return -ENOSPC; /* User disabled HW-crypto */
3778
3779         mutex_lock(&wl->mutex);
3780
3781         dev = wl->current_dev;
3782         err = -ENODEV;
3783         if (!dev || b43_status(dev) < B43_STAT_INITIALIZED)
3784                 goto out_unlock;
3785
3786         if (dev->fw.pcm_request_failed || !dev->hwcrypto_enabled) {
3787                 /* We don't have firmware for the crypto engine.
3788                  * Must use software-crypto. */
3789                 err = -EOPNOTSUPP;
3790                 goto out_unlock;
3791         }
3792
3793         err = -EINVAL;
3794         switch (key->cipher) {
3795         case WLAN_CIPHER_SUITE_WEP40:
3796                 algorithm = B43_SEC_ALGO_WEP40;
3797                 break;
3798         case WLAN_CIPHER_SUITE_WEP104:
3799                 algorithm = B43_SEC_ALGO_WEP104;
3800                 break;
3801         case WLAN_CIPHER_SUITE_TKIP:
3802                 algorithm = B43_SEC_ALGO_TKIP;
3803                 break;
3804         case WLAN_CIPHER_SUITE_CCMP:
3805                 algorithm = B43_SEC_ALGO_AES;
3806                 break;
3807         default:
3808                 B43_WARN_ON(1);
3809                 goto out_unlock;
3810         }
3811         index = (u8) (key->keyidx);
3812         if (index > 3)
3813                 goto out_unlock;
3814
3815         switch (cmd) {
3816         case SET_KEY:
3817                 if (algorithm == B43_SEC_ALGO_TKIP &&
3818                     (!(key->flags & IEEE80211_KEY_FLAG_PAIRWISE) ||
3819                     !modparam_hwtkip)) {
3820                         /* We support only pairwise key */
3821                         err = -EOPNOTSUPP;
3822                         goto out_unlock;
3823                 }
3824
3825                 if (key->flags & IEEE80211_KEY_FLAG_PAIRWISE) {
3826                         if (WARN_ON(!sta)) {
3827                                 err = -EOPNOTSUPP;
3828                                 goto out_unlock;
3829                         }
3830                         /* Pairwise key with an assigned MAC address. */
3831                         err = b43_key_write(dev, -1, algorithm,
3832                                             key->key, key->keylen,
3833                                             sta->addr, key);
3834                 } else {
3835                         /* Group key */
3836                         err = b43_key_write(dev, index, algorithm,
3837                                             key->key, key->keylen, NULL, key);
3838                 }
3839                 if (err)
3840                         goto out_unlock;
3841
3842                 if (algorithm == B43_SEC_ALGO_WEP40 ||
3843                     algorithm == B43_SEC_ALGO_WEP104) {
3844                         b43_hf_write(dev, b43_hf_read(dev) | B43_HF_USEDEFKEYS);
3845                 } else {
3846                         b43_hf_write(dev,
3847                                      b43_hf_read(dev) & ~B43_HF_USEDEFKEYS);
3848                 }
3849                 key->flags |= IEEE80211_KEY_FLAG_GENERATE_IV;
3850                 if (algorithm == B43_SEC_ALGO_TKIP)
3851                         key->flags |= IEEE80211_KEY_FLAG_GENERATE_MMIC;
3852                 break;
3853         case DISABLE_KEY: {
3854                 err = b43_key_clear(dev, key->hw_key_idx);
3855                 if (err)
3856                         goto out_unlock;
3857                 break;
3858         }
3859         default:
3860                 B43_WARN_ON(1);
3861         }
3862
3863 out_unlock:
3864         if (!err) {
3865                 b43dbg(wl, "%s hardware based encryption for keyidx: %d, "
3866                        "mac: %pM\n",
3867                        cmd == SET_KEY ? "Using" : "Disabling", key->keyidx,
3868                        sta ? sta->addr : bcast_addr);
3869                 b43_dump_keymemory(dev);
3870         }
3871         mutex_unlock(&wl->mutex);
3872
3873         return err;
3874 }
3875
3876 static void b43_op_configure_filter(struct ieee80211_hw *hw,
3877                                     unsigned int changed, unsigned int *fflags,
3878                                     u64 multicast)
3879 {
3880         struct b43_wl *wl = hw_to_b43_wl(hw);
3881         struct b43_wldev *dev;
3882
3883         mutex_lock(&wl->mutex);
3884         dev = wl->current_dev;
3885         if (!dev) {
3886                 *fflags = 0;
3887                 goto out_unlock;
3888         }
3889
3890         *fflags &= FIF_PROMISC_IN_BSS |
3891                   FIF_ALLMULTI |
3892                   FIF_FCSFAIL |
3893                   FIF_PLCPFAIL |
3894                   FIF_CONTROL |
3895                   FIF_OTHER_BSS |
3896                   FIF_BCN_PRBRESP_PROMISC;
3897
3898         changed &= FIF_PROMISC_IN_BSS |
3899                    FIF_ALLMULTI |
3900                    FIF_FCSFAIL |
3901                    FIF_PLCPFAIL |
3902                    FIF_CONTROL |
3903                    FIF_OTHER_BSS |
3904                    FIF_BCN_PRBRESP_PROMISC;
3905
3906         wl->filter_flags = *fflags;
3907
3908         if (changed && b43_status(dev) >= B43_STAT_INITIALIZED)
3909                 b43_adjust_opmode(dev);
3910
3911 out_unlock:
3912         mutex_unlock(&wl->mutex);
3913 }
3914
3915 /* Locking: wl->mutex
3916  * Returns the current dev. This might be different from the passed in dev,
3917  * because the core might be gone away while we unlocked the mutex. */
3918 static struct b43_wldev * b43_wireless_core_stop(struct b43_wldev *dev)
3919 {
3920         struct b43_wl *wl = dev->wl;
3921         struct b43_wldev *orig_dev;
3922         u32 mask;
3923
3924 redo:
3925         if (!dev || b43_status(dev) < B43_STAT_STARTED)
3926                 return dev;
3927
3928         /* Cancel work. Unlock to avoid deadlocks. */
3929         mutex_unlock(&wl->mutex);
3930         cancel_delayed_work_sync(&dev->periodic_work);
3931         cancel_work_sync(&wl->tx_work);
3932         mutex_lock(&wl->mutex);
3933         dev = wl->current_dev;
3934         if (!dev || b43_status(dev) < B43_STAT_STARTED) {
3935                 /* Whoops, aliens ate up the device while we were unlocked. */
3936                 return dev;
3937         }
3938
3939         /* Disable interrupts on the device. */
3940         b43_set_status(dev, B43_STAT_INITIALIZED);
3941         if (dev->dev->bus->bustype == SSB_BUSTYPE_SDIO) {
3942                 /* wl->mutex is locked. That is enough. */
3943                 b43_write32(dev, B43_MMIO_GEN_IRQ_MASK, 0);
3944                 b43_read32(dev, B43_MMIO_GEN_IRQ_MASK); /* Flush */
3945         } else {
3946                 spin_lock_irq(&wl->hardirq_lock);
3947                 b43_write32(dev, B43_MMIO_GEN_IRQ_MASK, 0);
3948                 b43_read32(dev, B43_MMIO_GEN_IRQ_MASK); /* Flush */
3949                 spin_unlock_irq(&wl->hardirq_lock);
3950         }
3951         /* Synchronize and free the interrupt handlers. Unlock to avoid deadlocks. */
3952         orig_dev = dev;
3953         mutex_unlock(&wl->mutex);
3954         if (dev->dev->bus->bustype == SSB_BUSTYPE_SDIO) {
3955                 b43_sdio_free_irq(dev);
3956         } else {
3957                 synchronize_irq(dev->dev->irq);
3958                 free_irq(dev->dev->irq, dev);
3959         }
3960         mutex_lock(&wl->mutex);
3961         dev = wl->current_dev;
3962         if (!dev)
3963                 return dev;
3964         if (dev != orig_dev) {
3965                 if (b43_status(dev) >= B43_STAT_STARTED)
3966                         goto redo;
3967                 return dev;
3968         }
3969         mask = b43_read32(dev, B43_MMIO_GEN_IRQ_MASK);
3970         B43_WARN_ON(mask != 0xFFFFFFFF && mask);
3971
3972         /* Drain the TX queue */
3973         while (skb_queue_len(&wl->tx_queue))
3974                 dev_kfree_skb(skb_dequeue(&wl->tx_queue));
3975
3976         b43_mac_suspend(dev);
3977         b43_leds_exit(dev);
3978         b43dbg(wl, "Wireless interface stopped\n");
3979
3980         return dev;
3981 }
3982
3983 /* Locking: wl->mutex */
3984 static int b43_wireless_core_start(struct b43_wldev *dev)
3985 {
3986         int err;
3987
3988         B43_WARN_ON(b43_status(dev) != B43_STAT_INITIALIZED);
3989
3990         drain_txstatus_queue(dev);
3991         if (dev->dev->bus->bustype == SSB_BUSTYPE_SDIO) {
3992                 err = b43_sdio_request_irq(dev, b43_sdio_interrupt_handler);
3993                 if (err) {
3994                         b43err(dev->wl, "Cannot request SDIO IRQ\n");
3995                         goto out;
3996                 }
3997         } else {
3998                 err = request_threaded_irq(dev->dev->irq, b43_interrupt_handler,
3999                                            b43_interrupt_thread_handler,
4000                                            IRQF_SHARED, KBUILD_MODNAME, dev);
4001                 if (err) {
4002                         b43err(dev->wl, "Cannot request IRQ-%d\n", dev->dev->irq);
4003                         goto out;
4004                 }
4005         }
4006
4007         /* We are ready to run. */
4008         ieee80211_wake_queues(dev->wl->hw);
4009         b43_set_status(dev, B43_STAT_STARTED);
4010
4011         /* Start data flow (TX/RX). */
4012         b43_mac_enable(dev);
4013         b43_write32(dev, B43_MMIO_GEN_IRQ_MASK, dev->irq_mask);
4014
4015         /* Start maintainance work */
4016         b43_periodic_tasks_setup(dev);
4017
4018         b43_leds_init(dev);
4019
4020         b43dbg(dev->wl, "Wireless interface started\n");
4021 out:
4022         return err;
4023 }
4024
4025 /* Get PHY and RADIO versioning numbers */
4026 static int b43_phy_versioning(struct b43_wldev *dev)
4027 {
4028         struct b43_phy *phy = &dev->phy;
4029         u32 tmp;
4030         u8 analog_type;
4031         u8 phy_type;
4032         u8 phy_rev;
4033         u16 radio_manuf;
4034         u16 radio_ver;
4035         u16 radio_rev;
4036         int unsupported = 0;
4037
4038         /* Get PHY versioning */
4039         tmp = b43_read16(dev, B43_MMIO_PHY_VER);
4040         analog_type = (tmp & B43_PHYVER_ANALOG) >> B43_PHYVER_ANALOG_SHIFT;
4041         phy_type = (tmp & B43_PHYVER_TYPE) >> B43_PHYVER_TYPE_SHIFT;
4042         phy_rev = (tmp & B43_PHYVER_VERSION);
4043         switch (phy_type) {
4044         case B43_PHYTYPE_A:
4045                 if (phy_rev >= 4)
4046                         unsupported = 1;
4047                 break;
4048         case B43_PHYTYPE_B:
4049                 if (phy_rev != 2 && phy_rev != 4 && phy_rev != 6
4050                     && phy_rev != 7)
4051                         unsupported = 1;
4052                 break;
4053         case B43_PHYTYPE_G:
4054                 if (phy_rev > 9)
4055                         unsupported = 1;
4056                 break;
4057 #ifdef CONFIG_B43_PHY_N
4058         case B43_PHYTYPE_N:
4059                 if (phy_rev > 9)
4060                         unsupported = 1;
4061                 break;
4062 #endif
4063 #ifdef CONFIG_B43_PHY_LP
4064         case B43_PHYTYPE_LP:
4065                 if (phy_rev > 2)
4066                         unsupported = 1;
4067                 break;
4068 #endif
4069         default:
4070                 unsupported = 1;
4071         };
4072         if (unsupported) {
4073                 b43err(dev->wl, "FOUND UNSUPPORTED PHY "
4074                        "(Analog %u, Type %u, Revision %u)\n",
4075                        analog_type, phy_type, phy_rev);
4076                 return -EOPNOTSUPP;
4077         }
4078         b43dbg(dev->wl, "Found PHY: Analog %u, Type %u, Revision %u\n",
4079                analog_type, phy_type, phy_rev);
4080
4081         /* Get RADIO versioning */
4082         if (dev->dev->bus->chip_id == 0x4317) {
4083                 if (dev->dev->bus->chip_rev == 0)
4084                         tmp = 0x3205017F;
4085                 else if (dev->dev->bus->chip_rev == 1)
4086                         tmp = 0x4205017F;
4087                 else
4088                         tmp = 0x5205017F;
4089         } else {
4090                 b43_write16(dev, B43_MMIO_RADIO_CONTROL, B43_RADIOCTL_ID);
4091                 tmp = b43_read16(dev, B43_MMIO_RADIO_DATA_LOW);
4092                 b43_write16(dev, B43_MMIO_RADIO_CONTROL, B43_RADIOCTL_ID);
4093                 tmp |= (u32)b43_read16(dev, B43_MMIO_RADIO_DATA_HIGH) << 16;
4094         }
4095         radio_manuf = (tmp & 0x00000FFF);
4096         radio_ver = (tmp & 0x0FFFF000) >> 12;
4097         radio_rev = (tmp & 0xF0000000) >> 28;
4098         if (radio_manuf != 0x17F /* Broadcom */)
4099                 unsupported = 1;
4100         switch (phy_type) {
4101         case B43_PHYTYPE_A:
4102                 if (radio_ver != 0x2060)
4103                         unsupported = 1;
4104                 if (radio_rev != 1)
4105                         unsupported = 1;
4106                 if (radio_manuf != 0x17F)
4107                         unsupported = 1;
4108                 break;
4109         case B43_PHYTYPE_B:
4110                 if ((radio_ver & 0xFFF0) != 0x2050)
4111                         unsupported = 1;
4112                 break;
4113         case B43_PHYTYPE_G:
4114                 if (radio_ver != 0x2050)
4115                         unsupported = 1;
4116                 break;
4117         case B43_PHYTYPE_N:
4118                 if (radio_ver != 0x2055 && radio_ver != 0x2056)
4119                         unsupported = 1;
4120                 break;
4121         case B43_PHYTYPE_LP:
4122                 if (radio_ver != 0x2062 && radio_ver != 0x2063)
4123                         unsupported = 1;
4124                 break;
4125         default:
4126                 B43_WARN_ON(1);
4127         }
4128         if (unsupported) {
4129                 b43err(dev->wl, "FOUND UNSUPPORTED RADIO "
4130                        "(Manuf 0x%X, Version 0x%X, Revision %u)\n",
4131                        radio_manuf, radio_ver, radio_rev);
4132                 return -EOPNOTSUPP;
4133         }
4134         b43dbg(dev->wl, "Found Radio: Manuf 0x%X, Version 0x%X, Revision %u\n",
4135                radio_manuf, radio_ver, radio_rev);
4136
4137         phy->radio_manuf = radio_manuf;
4138         phy->radio_ver = radio_ver;
4139         phy->radio_rev = radio_rev;
4140
4141         phy->analog = analog_type;
4142         phy->type = phy_type;
4143         phy->rev = phy_rev;
4144
4145         return 0;
4146 }
4147
4148 static void setup_struct_phy_for_init(struct b43_wldev *dev,
4149                                       struct b43_phy *phy)
4150 {
4151         phy->hardware_power_control = !!modparam_hwpctl;
4152         phy->next_txpwr_check_time = jiffies;
4153         /* PHY TX errors counter. */
4154         atomic_set(&phy->txerr_cnt, B43_PHY_TX_BADNESS_LIMIT);
4155
4156 #if B43_DEBUG
4157         phy->phy_locked = 0;
4158         phy->radio_locked = 0;
4159 #endif
4160 }
4161
4162 static void setup_struct_wldev_for_init(struct b43_wldev *dev)
4163 {
4164         dev->dfq_valid = 0;
4165
4166         /* Assume the radio is enabled. If it's not enabled, the state will
4167          * immediately get fixed on the first periodic work run. */
4168         dev->radio_hw_enable = 1;
4169
4170         /* Stats */
4171         memset(&dev->stats, 0, sizeof(dev->stats));
4172
4173         setup_struct_phy_for_init(dev, &dev->phy);
4174
4175         /* IRQ related flags */
4176         dev->irq_reason = 0;
4177         memset(dev->dma_reason, 0, sizeof(dev->dma_reason));
4178         dev->irq_mask = B43_IRQ_MASKTEMPLATE;
4179         if (b43_modparam_verbose < B43_VERBOSITY_DEBUG)
4180                 dev->irq_mask &= ~B43_IRQ_PHY_TXERR;
4181
4182         dev->mac_suspended = 1;
4183
4184         /* Noise calculation context */
4185         memset(&dev->noisecalc, 0, sizeof(dev->noisecalc));
4186 }
4187
4188 static void b43_bluetooth_coext_enable(struct b43_wldev *dev)
4189 {
4190         struct ssb_sprom *sprom = &dev->dev->bus->sprom;
4191         u64 hf;
4192
4193         if (!modparam_btcoex)
4194                 return;
4195         if (!(sprom->boardflags_lo & B43_BFL_BTCOEXIST))
4196                 return;
4197         if (dev->phy.type != B43_PHYTYPE_B && !dev->phy.gmode)
4198                 return;
4199
4200         hf = b43_hf_read(dev);
4201         if (sprom->boardflags_lo & B43_BFL_BTCMOD)
4202                 hf |= B43_HF_BTCOEXALT;
4203         else
4204                 hf |= B43_HF_BTCOEX;
4205         b43_hf_write(dev, hf);
4206 }
4207
4208 static void b43_bluetooth_coext_disable(struct b43_wldev *dev)
4209 {
4210         if (!modparam_btcoex)
4211                 return;
4212         //TODO
4213 }
4214
4215 static void b43_imcfglo_timeouts_workaround(struct b43_wldev *dev)
4216 {
4217 #ifdef CONFIG_SSB_DRIVER_PCICORE
4218         struct ssb_bus *bus = dev->dev->bus;
4219         u32 tmp;
4220
4221         if (bus->pcicore.dev &&
4222             bus->pcicore.dev->id.coreid == SSB_DEV_PCI &&
4223             bus->pcicore.dev->id.revision <= 5) {
4224                 /* IMCFGLO timeouts workaround. */
4225                 tmp = ssb_read32(dev->dev, SSB_IMCFGLO);
4226                 switch (bus->bustype) {
4227                 case SSB_BUSTYPE_PCI:
4228                 case SSB_BUSTYPE_PCMCIA:
4229                         tmp &= ~SSB_IMCFGLO_REQTO;
4230                         tmp &= ~SSB_IMCFGLO_SERTO;
4231                         tmp |= 0x32;
4232                         break;
4233                 case SSB_BUSTYPE_SSB:
4234                         tmp &= ~SSB_IMCFGLO_REQTO;
4235                         tmp &= ~SSB_IMCFGLO_SERTO;
4236                         tmp |= 0x53;
4237                         break;
4238                 default:
4239                         break;
4240                 }
4241                 ssb_write32(dev->dev, SSB_IMCFGLO, tmp);
4242         }
4243 #endif /* CONFIG_SSB_DRIVER_PCICORE */
4244 }
4245
4246 static void b43_set_synth_pu_delay(struct b43_wldev *dev, bool idle)
4247 {
4248         u16 pu_delay;
4249
4250         /* The time value is in microseconds. */
4251         if (dev->phy.type == B43_PHYTYPE_A)
4252                 pu_delay = 3700;
4253         else
4254                 pu_delay = 1050;
4255         if (b43_is_mode(dev->wl, NL80211_IFTYPE_ADHOC) || idle)
4256                 pu_delay = 500;
4257         if ((dev->phy.radio_ver == 0x2050) && (dev->phy.radio_rev == 8))
4258                 pu_delay = max(pu_delay, (u16)2400);
4259
4260         b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_SPUWKUP, pu_delay);
4261 }
4262
4263 /* Set the TSF CFP pre-TargetBeaconTransmissionTime. */
4264 static void b43_set_pretbtt(struct b43_wldev *dev)
4265 {
4266         u16 pretbtt;
4267
4268         /* The time value is in microseconds. */
4269         if (b43_is_mode(dev->wl, NL80211_IFTYPE_ADHOC)) {
4270                 pretbtt = 2;
4271         } else {
4272                 if (dev->phy.type == B43_PHYTYPE_A)
4273                         pretbtt = 120;
4274                 else
4275                         pretbtt = 250;
4276         }
4277         b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_PRETBTT, pretbtt);
4278         b43_write16(dev, B43_MMIO_TSF_CFP_PRETBTT, pretbtt);
4279 }
4280
4281 /* Shutdown a wireless core */
4282 /* Locking: wl->mutex */
4283 static void b43_wireless_core_exit(struct b43_wldev *dev)
4284 {
4285         u32 macctl;
4286
4287         B43_WARN_ON(dev && b43_status(dev) > B43_STAT_INITIALIZED);
4288         if (!dev || b43_status(dev) != B43_STAT_INITIALIZED)
4289                 return;
4290
4291         /* Unregister HW RNG driver */
4292         b43_rng_exit(dev->wl);
4293
4294         b43_set_status(dev, B43_STAT_UNINIT);
4295
4296         /* Stop the microcode PSM. */
4297         macctl = b43_read32(dev, B43_MMIO_MACCTL);
4298         macctl &= ~B43_MACCTL_PSM_RUN;
4299         macctl |= B43_MACCTL_PSM_JMP0;
4300         b43_write32(dev, B43_MMIO_MACCTL, macctl);
4301
4302         b43_dma_free(dev);
4303         b43_pio_free(dev);
4304         b43_chip_exit(dev);
4305         dev->phy.ops->switch_analog(dev, 0);
4306         if (dev->wl->current_beacon) {
4307                 dev_kfree_skb_any(dev->wl->current_beacon);
4308                 dev->wl->current_beacon = NULL;
4309         }
4310
4311         ssb_device_disable(dev->dev, 0);
4312         ssb_bus_may_powerdown(dev->dev->bus);
4313 }
4314
4315 /* Initialize a wireless core */
4316 static int b43_wireless_core_init(struct b43_wldev *dev)
4317 {
4318         struct ssb_bus *bus = dev->dev->bus;
4319         struct ssb_sprom *sprom = &bus->sprom;
4320         struct b43_phy *phy = &dev->phy;
4321         int err;
4322         u64 hf;
4323         u32 tmp;
4324
4325         B43_WARN_ON(b43_status(dev) != B43_STAT_UNINIT);
4326
4327         err = ssb_bus_powerup(bus, 0);
4328         if (err)
4329                 goto out;
4330         if (!ssb_device_is_enabled(dev->dev)) {
4331                 tmp = phy->gmode ? B43_TMSLOW_GMODE : 0;
4332                 b43_wireless_core_reset(dev, tmp);
4333         }
4334
4335         /* Reset all data structures. */
4336         setup_struct_wldev_for_init(dev);
4337         phy->ops->prepare_structs(dev);
4338
4339         /* Enable IRQ routing to this device. */
4340         ssb_pcicore_dev_irqvecs_enable(&bus->pcicore, dev->dev);
4341
4342         b43_imcfglo_timeouts_workaround(dev);
4343         b43_bluetooth_coext_disable(dev);
4344         if (phy->ops->prepare_hardware) {
4345                 err = phy->ops->prepare_hardware(dev);
4346                 if (err)
4347                         goto err_busdown;
4348         }
4349         err = b43_chip_init(dev);
4350         if (err)
4351                 goto err_busdown;
4352         b43_shm_write16(dev, B43_SHM_SHARED,
4353                         B43_SHM_SH_WLCOREREV, dev->dev->id.revision);
4354         hf = b43_hf_read(dev);
4355         if (phy->type == B43_PHYTYPE_G) {
4356                 hf |= B43_HF_SYMW;
4357                 if (phy->rev == 1)
4358                         hf |= B43_HF_GDCW;
4359                 if (sprom->boardflags_lo & B43_BFL_PACTRL)
4360                         hf |= B43_HF_OFDMPABOOST;
4361         }
4362         if (phy->radio_ver == 0x2050) {
4363                 if (phy->radio_rev == 6)
4364                         hf |= B43_HF_4318TSSI;
4365                 if (phy->radio_rev < 6)
4366                         hf |= B43_HF_VCORECALC;
4367         }
4368         if (sprom->boardflags_lo & B43_BFL_XTAL_NOSLOW)
4369                 hf |= B43_HF_DSCRQ; /* Disable slowclock requests from ucode. */
4370 #ifdef CONFIG_SSB_DRIVER_PCICORE
4371         if ((bus->bustype == SSB_BUSTYPE_PCI) &&
4372             (bus->pcicore.dev->id.revision <= 10))
4373                 hf |= B43_HF_PCISCW; /* PCI slow clock workaround. */
4374 #endif
4375         hf &= ~B43_HF_SKCFPUP;
4376         b43_hf_write(dev, hf);
4377
4378         b43_set_retry_limits(dev, B43_DEFAULT_SHORT_RETRY_LIMIT,
4379                              B43_DEFAULT_LONG_RETRY_LIMIT);
4380         b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_SFFBLIM, 3);
4381         b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_LFFBLIM, 2);
4382
4383         /* Disable sending probe responses from firmware.
4384          * Setting the MaxTime to one usec will always trigger
4385          * a timeout, so we never send any probe resp.
4386          * A timeout of zero is infinite. */
4387         b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_PRMAXTIME, 1);
4388
4389         b43_rate_memory_init(dev);
4390         b43_set_phytxctl_defaults(dev);
4391
4392         /* Minimum Contention Window */
4393         if (phy->type == B43_PHYTYPE_B)
4394                 b43_shm_write16(dev, B43_SHM_SCRATCH, B43_SHM_SC_MINCONT, 0x1F);
4395         else
4396                 b43_shm_write16(dev, B43_SHM_SCRATCH, B43_SHM_SC_MINCONT, 0xF);
4397         /* Maximum Contention Window */
4398         b43_shm_write16(dev, B43_SHM_SCRATCH, B43_SHM_SC_MAXCONT, 0x3FF);
4399
4400         if ((dev->dev->bus->bustype == SSB_BUSTYPE_PCMCIA) ||
4401             (dev->dev->bus->bustype == SSB_BUSTYPE_SDIO) ||
4402             dev->use_pio) {
4403                 dev->__using_pio_transfers = 1;
4404                 err = b43_pio_init(dev);
4405         } else {
4406                 dev->__using_pio_transfers = 0;
4407                 err = b43_dma_init(dev);
4408         }
4409         if (err)
4410                 goto err_chip_exit;
4411         b43_qos_init(dev);
4412         b43_set_synth_pu_delay(dev, 1);
4413         b43_bluetooth_coext_enable(dev);
4414
4415         ssb_bus_powerup(bus, !(sprom->boardflags_lo & B43_BFL_XTAL_NOSLOW));
4416         b43_upload_card_macaddress(dev);
4417         b43_security_init(dev);
4418
4419         ieee80211_wake_queues(dev->wl->hw);
4420
4421         b43_set_status(dev, B43_STAT_INITIALIZED);
4422
4423         /* Register HW RNG driver */
4424         b43_rng_init(dev->wl);
4425
4426 out:
4427         return err;
4428
4429 err_chip_exit:
4430         b43_chip_exit(dev);
4431 err_busdown:
4432         ssb_bus_may_powerdown(bus);
4433         B43_WARN_ON(b43_status(dev) != B43_STAT_UNINIT);
4434         return err;
4435 }
4436
4437 static int b43_op_add_interface(struct ieee80211_hw *hw,
4438                                 struct ieee80211_vif *vif)
4439 {
4440         struct b43_wl *wl = hw_to_b43_wl(hw);
4441         struct b43_wldev *dev;
4442         int err = -EOPNOTSUPP;
4443
4444         /* TODO: allow WDS/AP devices to coexist */
4445
4446         if (vif->type != NL80211_IFTYPE_AP &&
4447             vif->type != NL80211_IFTYPE_MESH_POINT &&
4448             vif->type != NL80211_IFTYPE_STATION &&
4449             vif->type != NL80211_IFTYPE_WDS &&
4450             vif->type != NL80211_IFTYPE_ADHOC)
4451                 return -EOPNOTSUPP;
4452
4453         mutex_lock(&wl->mutex);
4454         if (wl->operating)
4455                 goto out_mutex_unlock;
4456
4457         b43dbg(wl, "Adding Interface type %d\n", vif->type);
4458
4459         dev = wl->current_dev;
4460         wl->operating = 1;
4461         wl->vif = vif;
4462         wl->if_type = vif->type;
4463         memcpy(wl->mac_addr, vif->addr, ETH_ALEN);
4464
4465         b43_adjust_opmode(dev);
4466         b43_set_pretbtt(dev);
4467         b43_set_synth_pu_delay(dev, 0);
4468         b43_upload_card_macaddress(dev);
4469
4470         err = 0;
4471  out_mutex_unlock:
4472         mutex_unlock(&wl->mutex);
4473
4474         return err;
4475 }
4476
4477 static void b43_op_remove_interface(struct ieee80211_hw *hw,
4478                                     struct ieee80211_vif *vif)
4479 {
4480         struct b43_wl *wl = hw_to_b43_wl(hw);
4481         struct b43_wldev *dev = wl->current_dev;
4482
4483         b43dbg(wl, "Removing Interface type %d\n", vif->type);
4484
4485         mutex_lock(&wl->mutex);
4486
4487         B43_WARN_ON(!wl->operating);
4488         B43_WARN_ON(wl->vif != vif);
4489         wl->vif = NULL;
4490
4491         wl->operating = 0;
4492
4493         b43_adjust_opmode(dev);
4494         memset(wl->mac_addr, 0, ETH_ALEN);
4495         b43_upload_card_macaddress(dev);
4496
4497         mutex_unlock(&wl->mutex);
4498 }
4499
4500 static int b43_op_start(struct ieee80211_hw *hw)
4501 {
4502         struct b43_wl *wl = hw_to_b43_wl(hw);
4503         struct b43_wldev *dev = wl->current_dev;
4504         int did_init = 0;
4505         int err = 0;
4506
4507         /* Kill all old instance specific information to make sure
4508          * the card won't use it in the short timeframe between start
4509          * and mac80211 reconfiguring it. */
4510         memset(wl->bssid, 0, ETH_ALEN);
4511         memset(wl->mac_addr, 0, ETH_ALEN);
4512         wl->filter_flags = 0;
4513         wl->radiotap_enabled = 0;
4514         b43_qos_clear(wl);
4515         wl->beacon0_uploaded = 0;
4516         wl->beacon1_uploaded = 0;
4517         wl->beacon_templates_virgin = 1;
4518         wl->radio_enabled = 1;
4519
4520         mutex_lock(&wl->mutex);
4521
4522         if (b43_status(dev) < B43_STAT_INITIALIZED) {
4523                 err = b43_wireless_core_init(dev);
4524                 if (err)
4525                         goto out_mutex_unlock;
4526                 did_init = 1;
4527         }
4528
4529         if (b43_status(dev) < B43_STAT_STARTED) {
4530                 err = b43_wireless_core_start(dev);
4531                 if (err) {
4532                         if (did_init)
4533                                 b43_wireless_core_exit(dev);
4534                         goto out_mutex_unlock;
4535                 }
4536         }
4537
4538         /* XXX: only do if device doesn't support rfkill irq */
4539         wiphy_rfkill_start_polling(hw->wiphy);
4540
4541  out_mutex_unlock:
4542         mutex_unlock(&wl->mutex);
4543
4544         return err;
4545 }
4546
4547 static void b43_op_stop(struct ieee80211_hw *hw)
4548 {
4549         struct b43_wl *wl = hw_to_b43_wl(hw);
4550         struct b43_wldev *dev = wl->current_dev;
4551
4552         cancel_work_sync(&(wl->beacon_update_trigger));
4553
4554         mutex_lock(&wl->mutex);
4555         if (b43_status(dev) >= B43_STAT_STARTED) {
4556                 dev = b43_wireless_core_stop(dev);
4557                 if (!dev)
4558                         goto out_unlock;
4559         }
4560         b43_wireless_core_exit(dev);
4561         wl->radio_enabled = 0;
4562
4563 out_unlock:
4564         mutex_unlock(&wl->mutex);
4565
4566         cancel_work_sync(&(wl->txpower_adjust_work));
4567 }
4568
4569 static int b43_op_beacon_set_tim(struct ieee80211_hw *hw,
4570                                  struct ieee80211_sta *sta, bool set)
4571 {
4572         struct b43_wl *wl = hw_to_b43_wl(hw);
4573
4574         /* FIXME: add locking */
4575         b43_update_templates(wl);
4576
4577         return 0;
4578 }
4579
4580 static void b43_op_sta_notify(struct ieee80211_hw *hw,
4581                               struct ieee80211_vif *vif,
4582                               enum sta_notify_cmd notify_cmd,
4583                               struct ieee80211_sta *sta)
4584 {
4585         struct b43_wl *wl = hw_to_b43_wl(hw);
4586
4587         B43_WARN_ON(!vif || wl->vif != vif);
4588 }
4589
4590 static void b43_op_sw_scan_start_notifier(struct ieee80211_hw *hw)
4591 {
4592         struct b43_wl *wl = hw_to_b43_wl(hw);
4593         struct b43_wldev *dev;
4594
4595         mutex_lock(&wl->mutex);
4596         dev = wl->current_dev;
4597         if (dev && (b43_status(dev) >= B43_STAT_INITIALIZED)) {
4598                 /* Disable CFP update during scan on other channels. */
4599                 b43_hf_write(dev, b43_hf_read(dev) | B43_HF_SKCFPUP);
4600         }
4601         mutex_unlock(&wl->mutex);
4602 }
4603
4604 static void b43_op_sw_scan_complete_notifier(struct ieee80211_hw *hw)
4605 {
4606         struct b43_wl *wl = hw_to_b43_wl(hw);
4607         struct b43_wldev *dev;
4608
4609         mutex_lock(&wl->mutex);
4610         dev = wl->current_dev;
4611         if (dev && (b43_status(dev) >= B43_STAT_INITIALIZED)) {
4612                 /* Re-enable CFP update. */
4613                 b43_hf_write(dev, b43_hf_read(dev) & ~B43_HF_SKCFPUP);
4614         }
4615         mutex_unlock(&wl->mutex);
4616 }
4617
4618 static int b43_op_get_survey(struct ieee80211_hw *hw, int idx,
4619                              struct survey_info *survey)
4620 {
4621         struct b43_wl *wl = hw_to_b43_wl(hw);
4622         struct b43_wldev *dev = wl->current_dev;
4623         struct ieee80211_conf *conf = &hw->conf;
4624
4625         if (idx != 0)
4626                 return -ENOENT;
4627
4628         survey->channel = conf->channel;
4629         survey->filled = SURVEY_INFO_NOISE_DBM;
4630         survey->noise = dev->stats.link_noise;
4631
4632         return 0;
4633 }
4634
4635 static const struct ieee80211_ops b43_hw_ops = {
4636         .tx                     = b43_op_tx,
4637         .conf_tx                = b43_op_conf_tx,
4638         .add_interface          = b43_op_add_interface,
4639         .remove_interface       = b43_op_remove_interface,
4640         .config                 = b43_op_config,
4641         .bss_info_changed       = b43_op_bss_info_changed,
4642         .configure_filter       = b43_op_configure_filter,
4643         .set_key                = b43_op_set_key,
4644         .update_tkip_key        = b43_op_update_tkip_key,
4645         .get_stats              = b43_op_get_stats,
4646         .get_tsf                = b43_op_get_tsf,
4647         .set_tsf                = b43_op_set_tsf,
4648         .start                  = b43_op_start,
4649         .stop                   = b43_op_stop,
4650         .set_tim                = b43_op_beacon_set_tim,
4651         .sta_notify             = b43_op_sta_notify,
4652         .sw_scan_start          = b43_op_sw_scan_start_notifier,
4653         .sw_scan_complete       = b43_op_sw_scan_complete_notifier,
4654         .get_survey             = b43_op_get_survey,
4655         .rfkill_poll            = b43_rfkill_poll,
4656 };
4657
4658 /* Hard-reset the chip. Do not call this directly.
4659  * Use b43_controller_restart()
4660  */
4661 static void b43_chip_reset(struct work_struct *work)
4662 {
4663         struct b43_wldev *dev =
4664             container_of(work, struct b43_wldev, restart_work);
4665         struct b43_wl *wl = dev->wl;
4666         int err = 0;
4667         int prev_status;
4668
4669         mutex_lock(&wl->mutex);
4670
4671         prev_status = b43_status(dev);
4672         /* Bring the device down... */
4673         if (prev_status >= B43_STAT_STARTED) {
4674                 dev = b43_wireless_core_stop(dev);
4675                 if (!dev) {
4676                         err = -ENODEV;
4677                         goto out;
4678                 }
4679         }
4680         if (prev_status >= B43_STAT_INITIALIZED)
4681                 b43_wireless_core_exit(dev);
4682
4683         /* ...and up again. */
4684         if (prev_status >= B43_STAT_INITIALIZED) {
4685                 err = b43_wireless_core_init(dev);
4686                 if (err)
4687                         goto out;
4688         }
4689         if (prev_status >= B43_STAT_STARTED) {
4690                 err = b43_wireless_core_start(dev);
4691                 if (err) {
4692                         b43_wireless_core_exit(dev);
4693                         goto out;
4694                 }
4695         }
4696 out:
4697         if (err)
4698                 wl->current_dev = NULL; /* Failed to init the dev. */
4699         mutex_unlock(&wl->mutex);
4700         if (err)
4701                 b43err(wl, "Controller restart FAILED\n");
4702         else
4703                 b43info(wl, "Controller restarted\n");
4704 }
4705
4706 static int b43_setup_bands(struct b43_wldev *dev,
4707                            bool have_2ghz_phy, bool have_5ghz_phy)
4708 {
4709         struct ieee80211_hw *hw = dev->wl->hw;
4710
4711         if (have_2ghz_phy)
4712                 hw->wiphy->bands[IEEE80211_BAND_2GHZ] = &b43_band_2GHz;
4713         if (dev->phy.type == B43_PHYTYPE_N) {
4714                 if (have_5ghz_phy)
4715                         hw->wiphy->bands[IEEE80211_BAND_5GHZ] = &b43_band_5GHz_nphy;
4716         } else {
4717                 if (have_5ghz_phy)
4718                         hw->wiphy->bands[IEEE80211_BAND_5GHZ] = &b43_band_5GHz_aphy;
4719         }
4720
4721         dev->phy.supports_2ghz = have_2ghz_phy;
4722         dev->phy.supports_5ghz = have_5ghz_phy;
4723
4724         return 0;
4725 }
4726
4727 static void b43_wireless_core_detach(struct b43_wldev *dev)
4728 {
4729         /* We release firmware that late to not be required to re-request
4730          * is all the time when we reinit the core. */
4731         b43_release_firmware(dev);
4732         b43_phy_free(dev);
4733 }
4734
4735 static int b43_wireless_core_attach(struct b43_wldev *dev)
4736 {
4737         struct b43_wl *wl = dev->wl;
4738         struct ssb_bus *bus = dev->dev->bus;
4739         struct pci_dev *pdev = (bus->bustype == SSB_BUSTYPE_PCI) ? bus->host_pci : NULL;
4740         int err;
4741         bool have_2ghz_phy = 0, have_5ghz_phy = 0;
4742         u32 tmp;
4743
4744         /* Do NOT do any device initialization here.
4745          * Do it in wireless_core_init() instead.
4746          * This function is for gathering basic information about the HW, only.
4747          * Also some structs may be set up here. But most likely you want to have
4748          * that in core_init(), too.
4749          */
4750
4751         err = ssb_bus_powerup(bus, 0);
4752         if (err) {
4753                 b43err(wl, "Bus powerup failed\n");
4754                 goto out;
4755         }
4756         /* Get the PHY type. */
4757         if (dev->dev->id.revision >= 5) {
4758                 u32 tmshigh;
4759
4760                 tmshigh = ssb_read32(dev->dev, SSB_TMSHIGH);
4761                 have_2ghz_phy = !!(tmshigh & B43_TMSHIGH_HAVE_2GHZ_PHY);
4762                 have_5ghz_phy = !!(tmshigh & B43_TMSHIGH_HAVE_5GHZ_PHY);
4763         } else
4764                 B43_WARN_ON(1);
4765
4766         dev->phy.gmode = have_2ghz_phy;
4767         dev->phy.radio_on = 1;
4768         tmp = dev->phy.gmode ? B43_TMSLOW_GMODE : 0;
4769         b43_wireless_core_reset(dev, tmp);
4770
4771         err = b43_phy_versioning(dev);
4772         if (err)
4773                 goto err_powerdown;
4774         /* Check if this device supports multiband. */
4775         if (!pdev ||
4776             (pdev->device != 0x4312 &&
4777              pdev->device != 0x4319 && pdev->device != 0x4324)) {
4778                 /* No multiband support. */
4779                 have_2ghz_phy = 0;
4780                 have_5ghz_phy = 0;
4781                 switch (dev->phy.type) {
4782                 case B43_PHYTYPE_A:
4783                         have_5ghz_phy = 1;
4784                         break;
4785                 case B43_PHYTYPE_LP: //FIXME not always!
4786 #if 0 //FIXME enabling 5GHz causes a NULL pointer dereference
4787                         have_5ghz_phy = 1;
4788 #endif
4789                 case B43_PHYTYPE_G:
4790                 case B43_PHYTYPE_N:
4791                         have_2ghz_phy = 1;
4792                         break;
4793                 default:
4794                         B43_WARN_ON(1);
4795                 }
4796         }
4797         if (dev->phy.type == B43_PHYTYPE_A) {
4798                 /* FIXME */
4799                 b43err(wl, "IEEE 802.11a devices are unsupported\n");
4800                 err = -EOPNOTSUPP;
4801                 goto err_powerdown;
4802         }
4803         if (1 /* disable A-PHY */) {
4804                 /* FIXME: For now we disable the A-PHY on multi-PHY devices. */
4805                 if (dev->phy.type != B43_PHYTYPE_N &&
4806                     dev->phy.type != B43_PHYTYPE_LP) {
4807                         have_2ghz_phy = 1;
4808                         have_5ghz_phy = 0;
4809                 }
4810         }
4811
4812         err = b43_phy_allocate(dev);
4813         if (err)
4814                 goto err_powerdown;
4815
4816         dev->phy.gmode = have_2ghz_phy;
4817         tmp = dev->phy.gmode ? B43_TMSLOW_GMODE : 0;
4818         b43_wireless_core_reset(dev, tmp);
4819
4820         err = b43_validate_chipaccess(dev);
4821         if (err)
4822                 goto err_phy_free;
4823         err = b43_setup_bands(dev, have_2ghz_phy, have_5ghz_phy);
4824         if (err)
4825                 goto err_phy_free;
4826
4827         /* Now set some default "current_dev" */
4828         if (!wl->current_dev)
4829                 wl->current_dev = dev;
4830         INIT_WORK(&dev->restart_work, b43_chip_reset);
4831
4832         dev->phy.ops->switch_analog(dev, 0);
4833         ssb_device_disable(dev->dev, 0);
4834         ssb_bus_may_powerdown(bus);
4835
4836 out:
4837         return err;
4838
4839 err_phy_free:
4840         b43_phy_free(dev);
4841 err_powerdown:
4842         ssb_bus_may_powerdown(bus);
4843         return err;
4844 }
4845
4846 static void b43_one_core_detach(struct ssb_device *dev)
4847 {
4848         struct b43_wldev *wldev;
4849         struct b43_wl *wl;
4850
4851         /* Do not cancel ieee80211-workqueue based work here.
4852          * See comment in b43_remove(). */
4853
4854         wldev = ssb_get_drvdata(dev);
4855         wl = wldev->wl;
4856         b43_debugfs_remove_device(wldev);
4857         b43_wireless_core_detach(wldev);
4858         list_del(&wldev->list);
4859         wl->nr_devs--;
4860         ssb_set_drvdata(dev, NULL);
4861         kfree(wldev);
4862 }
4863
4864 static int b43_one_core_attach(struct ssb_device *dev, struct b43_wl *wl)
4865 {
4866         struct b43_wldev *wldev;
4867         struct pci_dev *pdev;
4868         int err = -ENOMEM;
4869
4870         if (!list_empty(&wl->devlist)) {
4871                 /* We are not the first core on this chip. */
4872                 pdev = (dev->bus->bustype == SSB_BUSTYPE_PCI) ? dev->bus->host_pci : NULL;
4873                 /* Only special chips support more than one wireless
4874                  * core, although some of the other chips have more than
4875                  * one wireless core as well. Check for this and
4876                  * bail out early.
4877                  */
4878                 if (!pdev ||
4879                     ((pdev->device != 0x4321) &&
4880                      (pdev->device != 0x4313) && (pdev->device != 0x431A))) {
4881                         b43dbg(wl, "Ignoring unconnected 802.11 core\n");
4882                         return -ENODEV;
4883                 }
4884         }
4885
4886         wldev = kzalloc(sizeof(*wldev), GFP_KERNEL);
4887         if (!wldev)
4888                 goto out;
4889
4890         wldev->use_pio = b43_modparam_pio;
4891         wldev->dev = dev;
4892         wldev->wl = wl;
4893         b43_set_status(wldev, B43_STAT_UNINIT);
4894         wldev->bad_frames_preempt = modparam_bad_frames_preempt;
4895         INIT_LIST_HEAD(&wldev->list);
4896
4897         err = b43_wireless_core_attach(wldev);
4898         if (err)
4899                 goto err_kfree_wldev;
4900
4901         list_add(&wldev->list, &wl->devlist);
4902         wl->nr_devs++;
4903         ssb_set_drvdata(dev, wldev);
4904         b43_debugfs_add_device(wldev);
4905
4906       out:
4907         return err;
4908
4909       err_kfree_wldev:
4910         kfree(wldev);
4911         return err;
4912 }
4913
4914 #define IS_PDEV(pdev, _vendor, _device, _subvendor, _subdevice)         ( \
4915         (pdev->vendor == PCI_VENDOR_ID_##_vendor) &&                    \
4916         (pdev->device == _device) &&                                    \
4917         (pdev->subsystem_vendor == PCI_VENDOR_ID_##_subvendor) &&       \
4918         (pdev->subsystem_device == _subdevice)                          )
4919
4920 static void b43_sprom_fixup(struct ssb_bus *bus)
4921 {
4922         struct pci_dev *pdev;
4923
4924         /* boardflags workarounds */
4925         if (bus->boardinfo.vendor == SSB_BOARDVENDOR_DELL &&
4926             bus->chip_id == 0x4301 && bus->boardinfo.rev == 0x74)
4927                 bus->sprom.boardflags_lo |= B43_BFL_BTCOEXIST;
4928         if (bus->boardinfo.vendor == PCI_VENDOR_ID_APPLE &&
4929             bus->boardinfo.type == 0x4E && bus->boardinfo.rev > 0x40)
4930                 bus->sprom.boardflags_lo |= B43_BFL_PACTRL;
4931         if (bus->bustype == SSB_BUSTYPE_PCI) {
4932                 pdev = bus->host_pci;
4933                 if (IS_PDEV(pdev, BROADCOM, 0x4318, ASUSTEK, 0x100F) ||
4934                     IS_PDEV(pdev, BROADCOM, 0x4320,    DELL, 0x0003) ||
4935                     IS_PDEV(pdev, BROADCOM, 0x4320,      HP, 0x12f8) ||
4936                     IS_PDEV(pdev, BROADCOM, 0x4320, LINKSYS, 0x0015) ||
4937                     IS_PDEV(pdev, BROADCOM, 0x4320, LINKSYS, 0x0014) ||
4938                     IS_PDEV(pdev, BROADCOM, 0x4320, LINKSYS, 0x0013) ||
4939                     IS_PDEV(pdev, BROADCOM, 0x4320, MOTOROLA, 0x7010))
4940                         bus->sprom.boardflags_lo &= ~B43_BFL_BTCOEXIST;
4941         }
4942 }
4943
4944 static void b43_wireless_exit(struct ssb_device *dev, struct b43_wl *wl)
4945 {
4946         struct ieee80211_hw *hw = wl->hw;
4947
4948         ssb_set_devtypedata(dev, NULL);
4949         ieee80211_free_hw(hw);
4950 }
4951
4952 static int b43_wireless_init(struct ssb_device *dev)
4953 {
4954         struct ssb_sprom *sprom = &dev->bus->sprom;
4955         struct ieee80211_hw *hw;
4956         struct b43_wl *wl;
4957         int err = -ENOMEM;
4958
4959         b43_sprom_fixup(dev->bus);
4960
4961         hw = ieee80211_alloc_hw(sizeof(*wl), &b43_hw_ops);
4962         if (!hw) {
4963                 b43err(NULL, "Could not allocate ieee80211 device\n");
4964                 goto out;
4965         }
4966         wl = hw_to_b43_wl(hw);
4967
4968         /* fill hw info */
4969         hw->flags = IEEE80211_HW_RX_INCLUDES_FCS |
4970                     IEEE80211_HW_SIGNAL_DBM;
4971
4972         hw->wiphy->interface_modes =
4973                 BIT(NL80211_IFTYPE_AP) |
4974                 BIT(NL80211_IFTYPE_MESH_POINT) |
4975                 BIT(NL80211_IFTYPE_STATION) |
4976                 BIT(NL80211_IFTYPE_WDS) |
4977                 BIT(NL80211_IFTYPE_ADHOC);
4978
4979         hw->queues = modparam_qos ? 4 : 1;
4980         wl->mac80211_initially_registered_queues = hw->queues;
4981         hw->max_rates = 2;
4982         SET_IEEE80211_DEV(hw, dev->dev);
4983         if (is_valid_ether_addr(sprom->et1mac))
4984                 SET_IEEE80211_PERM_ADDR(hw, sprom->et1mac);
4985         else
4986                 SET_IEEE80211_PERM_ADDR(hw, sprom->il0mac);
4987
4988         /* Initialize struct b43_wl */
4989         wl->hw = hw;
4990         mutex_init(&wl->mutex);
4991         spin_lock_init(&wl->hardirq_lock);
4992         INIT_LIST_HEAD(&wl->devlist);
4993         INIT_WORK(&wl->beacon_update_trigger, b43_beacon_update_trigger_work);
4994         INIT_WORK(&wl->txpower_adjust_work, b43_phy_txpower_adjust_work);
4995         INIT_WORK(&wl->tx_work, b43_tx_work);
4996         skb_queue_head_init(&wl->tx_queue);
4997
4998         ssb_set_devtypedata(dev, wl);
4999         b43info(wl, "Broadcom %04X WLAN found (core revision %u)\n",
5000                 dev->bus->chip_id, dev->id.revision);
5001         err = 0;
5002 out:
5003         return err;
5004 }
5005
5006 static int b43_probe(struct ssb_device *dev, const struct ssb_device_id *id)
5007 {
5008         struct b43_wl *wl;
5009         int err;
5010         int first = 0;
5011
5012         wl = ssb_get_devtypedata(dev);
5013         if (!wl) {
5014                 /* Probing the first core. Must setup common struct b43_wl */
5015                 first = 1;
5016                 err = b43_wireless_init(dev);
5017                 if (err)
5018                         goto out;
5019                 wl = ssb_get_devtypedata(dev);
5020                 B43_WARN_ON(!wl);
5021         }
5022         err = b43_one_core_attach(dev, wl);
5023         if (err)
5024                 goto err_wireless_exit;
5025
5026         if (first) {
5027                 err = ieee80211_register_hw(wl->hw);
5028                 if (err)
5029                         goto err_one_core_detach;
5030                 b43_leds_register(wl->current_dev);
5031         }
5032
5033       out:
5034         return err;
5035
5036       err_one_core_detach:
5037         b43_one_core_detach(dev);
5038       err_wireless_exit:
5039         if (first)
5040                 b43_wireless_exit(dev, wl);
5041         return err;
5042 }
5043
5044 static void b43_remove(struct ssb_device *dev)
5045 {
5046         struct b43_wl *wl = ssb_get_devtypedata(dev);
5047         struct b43_wldev *wldev = ssb_get_drvdata(dev);
5048
5049         /* We must cancel any work here before unregistering from ieee80211,
5050          * as the ieee80211 unreg will destroy the workqueue. */
5051         cancel_work_sync(&wldev->restart_work);
5052
5053         B43_WARN_ON(!wl);
5054         if (wl->current_dev == wldev) {
5055                 /* Restore the queues count before unregistering, because firmware detect
5056                  * might have modified it. Restoring is important, so the networking
5057                  * stack can properly free resources. */
5058                 wl->hw->queues = wl->mac80211_initially_registered_queues;
5059                 b43_leds_stop(wldev);
5060                 ieee80211_unregister_hw(wl->hw);
5061         }
5062
5063         b43_one_core_detach(dev);
5064
5065         if (list_empty(&wl->devlist)) {
5066                 b43_leds_unregister(wl);
5067                 /* Last core on the chip unregistered.
5068                  * We can destroy common struct b43_wl.
5069                  */
5070                 b43_wireless_exit(dev, wl);
5071         }
5072 }
5073
5074 /* Perform a hardware reset. This can be called from any context. */
5075 void b43_controller_restart(struct b43_wldev *dev, const char *reason)
5076 {
5077         /* Must avoid requeueing, if we are in shutdown. */
5078         if (b43_status(dev) < B43_STAT_INITIALIZED)
5079                 return;
5080         b43info(dev->wl, "Controller RESET (%s) ...\n", reason);
5081         ieee80211_queue_work(dev->wl->hw, &dev->restart_work);
5082 }
5083
5084 static struct ssb_driver b43_ssb_driver = {
5085         .name           = KBUILD_MODNAME,
5086         .id_table       = b43_ssb_tbl,
5087         .probe          = b43_probe,
5088         .remove         = b43_remove,
5089 };
5090
5091 static void b43_print_driverinfo(void)
5092 {
5093         const char *feat_pci = "", *feat_pcmcia = "", *feat_nphy = "",
5094                    *feat_leds = "", *feat_sdio = "";
5095
5096 #ifdef CONFIG_B43_PCI_AUTOSELECT
5097         feat_pci = "P";
5098 #endif
5099 #ifdef CONFIG_B43_PCMCIA
5100         feat_pcmcia = "M";
5101 #endif
5102 #ifdef CONFIG_B43_PHY_N
5103         feat_nphy = "N";
5104 #endif
5105 #ifdef CONFIG_B43_LEDS
5106         feat_leds = "L";
5107 #endif
5108 #ifdef CONFIG_B43_SDIO
5109         feat_sdio = "S";
5110 #endif
5111         printk(KERN_INFO "Broadcom 43xx driver loaded "
5112                "[ Features: %s%s%s%s%s, Firmware-ID: "
5113                B43_SUPPORTED_FIRMWARE_ID " ]\n",
5114                feat_pci, feat_pcmcia, feat_nphy,
5115                feat_leds, feat_sdio);
5116 }
5117
5118 static int __init b43_init(void)
5119 {
5120         int err;
5121
5122         b43_debugfs_init();
5123         err = b43_pcmcia_init();
5124         if (err)
5125                 goto err_dfs_exit;
5126         err = b43_sdio_init();
5127         if (err)
5128                 goto err_pcmcia_exit;
5129         err = ssb_driver_register(&b43_ssb_driver);
5130         if (err)
5131                 goto err_sdio_exit;
5132         b43_print_driverinfo();
5133
5134         return err;
5135
5136 err_sdio_exit:
5137         b43_sdio_exit();
5138 err_pcmcia_exit:
5139         b43_pcmcia_exit();
5140 err_dfs_exit:
5141         b43_debugfs_exit();
5142         return err;
5143 }
5144
5145 static void __exit b43_exit(void)
5146 {
5147         ssb_driver_unregister(&b43_ssb_driver);
5148         b43_sdio_exit();
5149         b43_pcmcia_exit();
5150         b43_debugfs_exit();
5151 }
5152
5153 module_init(b43_init)
5154 module_exit(b43_exit)