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