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