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