SAFE public projects git trees. - safe/jmp/linux-2.6/blob - drivers/net/wireless/b43/main.c

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