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