drivers/net/wireless/b43/main.c

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