git.oblomov.eu Git - linux-2.6/blob - drivers/net/wireless/b43/main.c

   1 /*
   2
   3   Broadcom B43 wireless driver
   4
   5   Copyright (c) 2005 Martin Langer <martin-langer@gmx.de>
   6   Copyright (c) 2005 Stefano Brivio <stefano.brivio@polimi.it>
   7   Copyright (c) 2005, 2006 Michael Buesch <mb@bu3sch.de>
   8   Copyright (c) 2005 Danny van Dyk <kugelfang@gentoo.org>
   9   Copyright (c) 2005 Andreas Jaggi <andreas.jaggi@waterwave.ch>
  10
  11   Some parts of the code in this file are derived from the ipw2200
  12   driver  Copyright(c) 2003 - 2004 Intel Corporation.
  13
  14   This program is free software; you can redistribute it and/or modify
  15   it under the terms of the GNU General Public License as published by
  16   the Free Software Foundation; either version 2 of the License, or
  17   (at your option) any later version.
  18
  19   This program is distributed in the hope that it will be useful,
  20   but WITHOUT ANY WARRANTY; without even the implied warranty of
  21   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  22   GNU General Public License for more details.
  23
  24   You should have received a copy of the GNU General Public License
  25   along with this program; see the file COPYING.  If not, write to
  26   the Free Software Foundation, Inc., 51 Franklin Steet, Fifth Floor,
  27   Boston, MA 02110-1301, USA.
  28
  29 */
  30
  31 #include <linux/delay.h>
  32 #include <linux/init.h>
  33 #include <linux/moduleparam.h>
  34 #include <linux/if_arp.h>
  35 #include <linux/etherdevice.h>
  36 #include <linux/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         }
1341 }
1342
1343 static void b43_write_probe_resp_plcp(struct b43_wldev *dev,
1344                                       u16 shm_offset, u16 size,
1345                                       struct ieee80211_rate *rate)
1346 {
1347         struct b43_plcp_hdr4 plcp;
1348         u32 tmp;
1349         __le16 dur;
1350
1351         plcp.data = 0;
1352         b43_generate_plcp_hdr(&plcp, size + FCS_LEN, rate->hw_value);
1353         dur = ieee80211_generic_frame_duration(dev->wl->hw,
1354                                                dev->wl->vif, size,
1355                                                rate);
1356         /* Write PLCP in two parts and timing for packet transfer */
1357         tmp = le32_to_cpu(plcp.data);
1358         b43_shm_write16(dev, B43_SHM_SHARED, shm_offset, tmp & 0xFFFF);
1359         b43_shm_write16(dev, B43_SHM_SHARED, shm_offset + 2, tmp >> 16);
1360         b43_shm_write16(dev, B43_SHM_SHARED, shm_offset + 6, le16_to_cpu(dur));
1361 }
1362
1363 /* Instead of using custom probe response template, this function
1364  * just patches custom beacon template by:
1365  * 1) Changing packet type
1366  * 2) Patching duration field
1367  * 3) Stripping TIM
1368  */
1369 static const u8 * b43_generate_probe_resp(struct b43_wldev *dev,
1370                                           u16 *dest_size,
1371                                           struct ieee80211_rate *rate)
1372 {
1373         const u8 *src_data;
1374         u8 *dest_data;
1375         u16 src_size, elem_size, src_pos, dest_pos;
1376         __le16 dur;
1377         struct ieee80211_hdr *hdr;
1378         size_t ie_start;
1379
1380         src_size = dev->wl->current_beacon->len;
1381         src_data = (const u8 *)dev->wl->current_beacon->data;
1382
1383         /* Get the start offset of the variable IEs in the packet. */
1384         ie_start = offsetof(struct ieee80211_mgmt, u.probe_resp.variable);
1385         B43_WARN_ON(ie_start != offsetof(struct ieee80211_mgmt, u.beacon.variable));
1386
1387         if (B43_WARN_ON(src_size < ie_start))
1388                 return NULL;
1389
1390         dest_data = kmalloc(src_size, GFP_ATOMIC);
1391         if (unlikely(!dest_data))
1392                 return NULL;
1393
1394         /* Copy the static data and all Information Elements, except the TIM. */
1395         memcpy(dest_data, src_data, ie_start);
1396         src_pos = ie_start;
1397         dest_pos = ie_start;
1398         for ( ; src_pos < src_size - 2; src_pos += elem_size) {
1399                 elem_size = src_data[src_pos + 1] + 2;
1400                 if (src_data[src_pos] == 5) {
1401                         /* This is the TIM. */
1402                         continue;
1403                 }
1404                 memcpy(dest_data + dest_pos, src_data + src_pos,
1405                        elem_size);
1406                 dest_pos += elem_size;
1407         }
1408         *dest_size = dest_pos;
1409         hdr = (struct ieee80211_hdr *)dest_data;
1410
1411         /* Set the frame control. */
1412         hdr->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT |
1413                                          IEEE80211_STYPE_PROBE_RESP);
1414         dur = ieee80211_generic_frame_duration(dev->wl->hw,
1415                                                dev->wl->vif, *dest_size,
1416                                                rate);
1417         hdr->duration_id = dur;
1418
1419         return dest_data;
1420 }
1421
1422 static void b43_write_probe_resp_template(struct b43_wldev *dev,
1423                                           u16 ram_offset,
1424                                           u16 shm_size_offset,
1425                                           struct ieee80211_rate *rate)
1426 {
1427         const u8 *probe_resp_data;
1428         u16 size;
1429
1430         size = dev->wl->current_beacon->len;
1431         probe_resp_data = b43_generate_probe_resp(dev, &size, rate);
1432         if (unlikely(!probe_resp_data))
1433                 return;
1434
1435         /* Looks like PLCP headers plus packet timings are stored for
1436          * all possible basic rates
1437          */
1438         b43_write_probe_resp_plcp(dev, 0x31A, size, &b43_b_ratetable[0]);
1439         b43_write_probe_resp_plcp(dev, 0x32C, size, &b43_b_ratetable[1]);
1440         b43_write_probe_resp_plcp(dev, 0x33E, size, &b43_b_ratetable[2]);
1441         b43_write_probe_resp_plcp(dev, 0x350, size, &b43_b_ratetable[3]);
1442
1443         size = min((size_t) size, 0x200 - sizeof(struct b43_plcp_hdr6));
1444         b43_write_template_common(dev, probe_resp_data,
1445                                   size, ram_offset, shm_size_offset,
1446                                   rate->hw_value);
1447         kfree(probe_resp_data);
1448 }
1449
1450 /* Asynchronously update the packet templates in template RAM.
1451  * Locking: Requires wl->irq_lock to be locked. */
1452 static void b43_update_templates(struct b43_wl *wl, struct sk_buff *beacon)
1453 {
1454         /* This is the top half of the ansynchronous beacon update.
1455          * The bottom half is the beacon IRQ.
1456          * Beacon update must be asynchronous to avoid sending an
1457          * invalid beacon. This can happen for example, if the firmware
1458          * transmits a beacon while we are updating it. */
1459
1460         if (wl->current_beacon)
1461                 dev_kfree_skb_any(wl->current_beacon);
1462         wl->current_beacon = beacon;
1463         wl->beacon0_uploaded = 0;
1464         wl->beacon1_uploaded = 0;
1465 }
1466
1467 static void b43_set_ssid(struct b43_wldev *dev, const u8 * ssid, u8 ssid_len)
1468 {
1469         u32 tmp;
1470         u16 i, len;
1471
1472         len = min((u16) ssid_len, (u16) 0x100);
1473         for (i = 0; i < len; i += sizeof(u32)) {
1474                 tmp = (u32) (ssid[i + 0]);
1475                 if (i + 1 < len)
1476                         tmp |= (u32) (ssid[i + 1]) << 8;
1477                 if (i + 2 < len)
1478                         tmp |= (u32) (ssid[i + 2]) << 16;
1479                 if (i + 3 < len)
1480                         tmp |= (u32) (ssid[i + 3]) << 24;
1481                 b43_shm_write32(dev, B43_SHM_SHARED, 0x380 + i, tmp);
1482         }
1483         b43_shm_write16(dev, B43_SHM_SHARED, 0x48, len);
1484 }
1485
1486 static void b43_set_beacon_int(struct b43_wldev *dev, u16 beacon_int)
1487 {
1488         b43_time_lock(dev);
1489         if (dev->dev->id.revision >= 3) {
1490                 b43_write32(dev, 0x188, (beacon_int << 16));
1491         } else {
1492                 b43_write16(dev, 0x606, (beacon_int >> 6));
1493                 b43_write16(dev, 0x610, beacon_int);
1494         }
1495         b43_time_unlock(dev);
1496 }
1497
1498 static void handle_irq_beacon(struct b43_wldev *dev)
1499 {
1500         struct b43_wl *wl = dev->wl;
1501         u32 cmd;
1502
1503         if (!b43_is_mode(wl, IEEE80211_IF_TYPE_AP))
1504                 return;
1505
1506         /* This is the bottom half of the asynchronous beacon update. */
1507
1508         cmd = b43_read32(dev, B43_MMIO_MACCMD);
1509         if (!(cmd & B43_MACCMD_BEACON0_VALID)) {
1510                 if (!wl->beacon0_uploaded) {
1511                         b43_write_beacon_template(dev, 0x68, 0x18,
1512                                                   B43_CCK_RATE_1MB);
1513                         b43_write_probe_resp_template(dev, 0x268, 0x4A,
1514                                                       &__b43_ratetable[3]);
1515                         wl->beacon0_uploaded = 1;
1516                 }
1517                 cmd |= B43_MACCMD_BEACON0_VALID;
1518         }
1519         if (!(cmd & B43_MACCMD_BEACON1_VALID)) {
1520                 if (!wl->beacon1_uploaded) {
1521                         b43_write_beacon_template(dev, 0x468, 0x1A,
1522                                                   B43_CCK_RATE_1MB);
1523                         wl->beacon1_uploaded = 1;
1524                 }
1525                 cmd |= B43_MACCMD_BEACON1_VALID;
1526         }
1527         b43_write32(dev, B43_MMIO_MACCMD, cmd);
1528 }
1529
1530 static void handle_irq_ucode_debug(struct b43_wldev *dev)
1531 {
1532         //TODO
1533 }
1534
1535 /* Interrupt handler bottom-half */
1536 static void b43_interrupt_tasklet(struct b43_wldev *dev)
1537 {
1538         u32 reason;
1539         u32 dma_reason[ARRAY_SIZE(dev->dma_reason)];
1540         u32 merged_dma_reason = 0;
1541         int i;
1542         unsigned long flags;
1543
1544         spin_lock_irqsave(&dev->wl->irq_lock, flags);
1545
1546         B43_WARN_ON(b43_status(dev) != B43_STAT_STARTED);
1547
1548         reason = dev->irq_reason;
1549         for (i = 0; i < ARRAY_SIZE(dma_reason); i++) {
1550                 dma_reason[i] = dev->dma_reason[i];
1551                 merged_dma_reason |= dma_reason[i];
1552         }
1553
1554         if (unlikely(reason & B43_IRQ_MAC_TXERR))
1555                 b43err(dev->wl, "MAC transmission error\n");
1556
1557         if (unlikely(reason & B43_IRQ_PHY_TXERR)) {
1558                 b43err(dev->wl, "PHY transmission error\n");
1559                 rmb();
1560                 if (unlikely(atomic_dec_and_test(&dev->phy.txerr_cnt))) {
1561                         atomic_set(&dev->phy.txerr_cnt,
1562                                    B43_PHY_TX_BADNESS_LIMIT);
1563                         b43err(dev->wl, "Too many PHY TX errors, "
1564                                         "restarting the controller\n");
1565                         b43_controller_restart(dev, "PHY TX errors");
1566                 }
1567         }
1568
1569         if (unlikely(merged_dma_reason & (B43_DMAIRQ_FATALMASK |
1570                                           B43_DMAIRQ_NONFATALMASK))) {
1571                 if (merged_dma_reason & B43_DMAIRQ_FATALMASK) {
1572                         b43err(dev->wl, "Fatal DMA error: "
1573                                "0x%08X, 0x%08X, 0x%08X, "
1574                                "0x%08X, 0x%08X, 0x%08X\n",
1575                                dma_reason[0], dma_reason[1],
1576                                dma_reason[2], dma_reason[3],
1577                                dma_reason[4], dma_reason[5]);
1578                         b43_controller_restart(dev, "DMA error");
1579                         mmiowb();
1580                         spin_unlock_irqrestore(&dev->wl->irq_lock, flags);
1581                         return;
1582                 }
1583                 if (merged_dma_reason & B43_DMAIRQ_NONFATALMASK) {
1584                         b43err(dev->wl, "DMA error: "
1585                                "0x%08X, 0x%08X, 0x%08X, "
1586                                "0x%08X, 0x%08X, 0x%08X\n",
1587                                dma_reason[0], dma_reason[1],
1588                                dma_reason[2], dma_reason[3],
1589                                dma_reason[4], dma_reason[5]);
1590                 }
1591         }
1592
1593         if (unlikely(reason & B43_IRQ_UCODE_DEBUG))
1594                 handle_irq_ucode_debug(dev);
1595         if (reason & B43_IRQ_TBTT_INDI)
1596                 handle_irq_tbtt_indication(dev);
1597         if (reason & B43_IRQ_ATIM_END)
1598                 handle_irq_atim_end(dev);
1599         if (reason & B43_IRQ_BEACON)
1600                 handle_irq_beacon(dev);
1601         if (reason & B43_IRQ_PMQ)
1602                 handle_irq_pmq(dev);
1603         if (reason & B43_IRQ_TXFIFO_FLUSH_OK)
1604                 ;/* TODO */
1605         if (reason & B43_IRQ_NOISESAMPLE_OK)
1606                 handle_irq_noise(dev);
1607
1608         /* Check the DMA reason registers for received data. */
1609         if (dma_reason[0] & B43_DMAIRQ_RX_DONE) {
1610                 if (b43_using_pio_transfers(dev))
1611                         b43_pio_rx(dev->pio.rx_queue);
1612                 else
1613                         b43_dma_rx(dev->dma.rx_ring);
1614         }
1615         B43_WARN_ON(dma_reason[1] & B43_DMAIRQ_RX_DONE);
1616         B43_WARN_ON(dma_reason[2] & B43_DMAIRQ_RX_DONE);
1617         B43_WARN_ON(dma_reason[3] & B43_DMAIRQ_RX_DONE);
1618         B43_WARN_ON(dma_reason[4] & B43_DMAIRQ_RX_DONE);
1619         B43_WARN_ON(dma_reason[5] & B43_DMAIRQ_RX_DONE);
1620
1621         if (reason & B43_IRQ_TX_OK)
1622                 handle_irq_transmit_status(dev);
1623
1624         b43_interrupt_enable(dev, dev->irq_savedstate);
1625         mmiowb();
1626         spin_unlock_irqrestore(&dev->wl->irq_lock, flags);
1627 }
1628
1629 static void b43_interrupt_ack(struct b43_wldev *dev, u32 reason)
1630 {
1631         b43_write32(dev, B43_MMIO_GEN_IRQ_REASON, reason);
1632
1633         b43_write32(dev, B43_MMIO_DMA0_REASON, dev->dma_reason[0]);
1634         b43_write32(dev, B43_MMIO_DMA1_REASON, dev->dma_reason[1]);
1635         b43_write32(dev, B43_MMIO_DMA2_REASON, dev->dma_reason[2]);
1636         b43_write32(dev, B43_MMIO_DMA3_REASON, dev->dma_reason[3]);
1637         b43_write32(dev, B43_MMIO_DMA4_REASON, dev->dma_reason[4]);
1638         b43_write32(dev, B43_MMIO_DMA5_REASON, dev->dma_reason[5]);
1639 }
1640
1641 /* Interrupt handler top-half */
1642 static irqreturn_t b43_interrupt_handler(int irq, void *dev_id)
1643 {
1644         irqreturn_t ret = IRQ_NONE;
1645         struct b43_wldev *dev = dev_id;
1646         u32 reason;
1647
1648         if (!dev)
1649                 return IRQ_NONE;
1650
1651         spin_lock(&dev->wl->irq_lock);
1652
1653         if (b43_status(dev) < B43_STAT_STARTED)
1654                 goto out;
1655         reason = b43_read32(dev, B43_MMIO_GEN_IRQ_REASON);
1656         if (reason == 0xffffffff)       /* shared IRQ */
1657                 goto out;
1658         ret = IRQ_HANDLED;
1659         reason &= b43_read32(dev, B43_MMIO_GEN_IRQ_MASK);
1660         if (!reason)
1661                 goto out;
1662
1663         dev->dma_reason[0] = b43_read32(dev, B43_MMIO_DMA0_REASON)
1664             & 0x0001DC00;
1665         dev->dma_reason[1] = b43_read32(dev, B43_MMIO_DMA1_REASON)
1666             & 0x0000DC00;
1667         dev->dma_reason[2] = b43_read32(dev, B43_MMIO_DMA2_REASON)
1668             & 0x0000DC00;
1669         dev->dma_reason[3] = b43_read32(dev, B43_MMIO_DMA3_REASON)
1670             & 0x0001DC00;
1671         dev->dma_reason[4] = b43_read32(dev, B43_MMIO_DMA4_REASON)
1672             & 0x0000DC00;
1673         dev->dma_reason[5] = b43_read32(dev, B43_MMIO_DMA5_REASON)
1674             & 0x0000DC00;
1675
1676         b43_interrupt_ack(dev, reason);
1677         /* disable all IRQs. They are enabled again in the bottom half. */
1678         dev->irq_savedstate = b43_interrupt_disable(dev, B43_IRQ_ALL);
1679         /* save the reason code and call our bottom half. */
1680         dev->irq_reason = reason;
1681         tasklet_schedule(&dev->isr_tasklet);
1682       out:
1683         mmiowb();
1684         spin_unlock(&dev->wl->irq_lock);
1685
1686         return ret;
1687 }
1688
1689 static void do_release_fw(struct b43_firmware_file *fw)
1690 {
1691         release_firmware(fw->data);
1692         fw->data = NULL;
1693         fw->filename = NULL;
1694 }
1695
1696 static void b43_release_firmware(struct b43_wldev *dev)
1697 {
1698         do_release_fw(&dev->fw.ucode);
1699         do_release_fw(&dev->fw.pcm);
1700         do_release_fw(&dev->fw.initvals);
1701         do_release_fw(&dev->fw.initvals_band);
1702 }
1703
1704 static void b43_print_fw_helptext(struct b43_wl *wl, bool error)
1705 {
1706         const char *text;
1707
1708         text = "You must go to "
1709                "http://linuxwireless.org/en/users/Drivers/b43#devicefirmware "
1710                "and download the latest firmware (version 4).\n";
1711         if (error)
1712                 b43err(wl, text);
1713         else
1714                 b43warn(wl, text);
1715 }
1716
1717 static int do_request_fw(struct b43_wldev *dev,
1718                          const char *name,
1719                          struct b43_firmware_file *fw)
1720 {
1721         char path[sizeof(modparam_fwpostfix) + 32];
1722         const struct firmware *blob;
1723         struct b43_fw_header *hdr;
1724         u32 size;
1725         int err;
1726
1727         if (!name) {
1728                 /* Don't fetch anything. Free possibly cached firmware. */
1729                 do_release_fw(fw);
1730                 return 0;
1731         }
1732         if (fw->filename) {
1733                 if (strcmp(fw->filename, name) == 0)
1734                         return 0; /* Already have this fw. */
1735                 /* Free the cached firmware first. */
1736                 do_release_fw(fw);
1737         }
1738
1739         snprintf(path, ARRAY_SIZE(path),
1740                  "b43%s/%s.fw",
1741                  modparam_fwpostfix, name);
1742         err = request_firmware(&blob, path, dev->dev->dev);
1743         if (err) {
1744                 b43err(dev->wl, "Firmware file \"%s\" not found "
1745                        "or load failed.\n", path);
1746                 return err;
1747         }
1748         if (blob->size < sizeof(struct b43_fw_header))
1749                 goto err_format;
1750         hdr = (struct b43_fw_header *)(blob->data);
1751         switch (hdr->type) {
1752         case B43_FW_TYPE_UCODE:
1753         case B43_FW_TYPE_PCM:
1754                 size = be32_to_cpu(hdr->size);
1755                 if (size != blob->size - sizeof(struct b43_fw_header))
1756                         goto err_format;
1757                 /* fallthrough */
1758         case B43_FW_TYPE_IV:
1759                 if (hdr->ver != 1)
1760                         goto err_format;
1761                 break;
1762         default:
1763                 goto err_format;
1764         }
1765
1766         fw->data = blob;
1767         fw->filename = name;
1768
1769         return 0;
1770
1771 err_format:
1772         b43err(dev->wl, "Firmware file \"%s\" format error.\n", path);
1773         release_firmware(blob);
1774
1775         return -EPROTO;
1776 }
1777
1778 static int b43_request_firmware(struct b43_wldev *dev)
1779 {
1780         struct b43_firmware *fw = &dev->fw;
1781         const u8 rev = dev->dev->id.revision;
1782         const char *filename;
1783         u32 tmshigh;
1784         int err;
1785
1786         /* Get microcode */
1787         tmshigh = ssb_read32(dev->dev, SSB_TMSHIGH);
1788         if ((rev >= 5) && (rev <= 10))
1789                 filename = "ucode5";
1790         else if ((rev >= 11) && (rev <= 12))
1791                 filename = "ucode11";
1792         else if (rev >= 13)
1793                 filename = "ucode13";
1794         else
1795                 goto err_no_ucode;
1796         err = do_request_fw(dev, filename, &fw->ucode);
1797         if (err)
1798                 goto err_load;
1799
1800         /* Get PCM code */
1801         if ((rev >= 5) && (rev <= 10))
1802                 filename = "pcm5";
1803         else if (rev >= 11)
1804                 filename = NULL;
1805         else
1806                 goto err_no_pcm;
1807         err = do_request_fw(dev, filename, &fw->pcm);
1808         if (err)
1809                 goto err_load;
1810
1811         /* Get initvals */
1812         switch (dev->phy.type) {
1813         case B43_PHYTYPE_A:
1814                 if ((rev >= 5) && (rev <= 10)) {
1815                         if (tmshigh & B43_TMSHIGH_HAVE_2GHZ_PHY)
1816                                 filename = "a0g1initvals5";
1817                         else
1818                                 filename = "a0g0initvals5";
1819                 } else
1820                         goto err_no_initvals;
1821                 break;
1822         case B43_PHYTYPE_G:
1823                 if ((rev >= 5) && (rev <= 10))
1824                         filename = "b0g0initvals5";
1825                 else if (rev >= 13)
1826                         filename = "lp0initvals13";
1827                 else
1828                         goto err_no_initvals;
1829                 break;
1830         case B43_PHYTYPE_N:
1831                 if ((rev >= 11) && (rev <= 12))
1832                         filename = "n0initvals11";
1833                 else
1834                         goto err_no_initvals;
1835                 break;
1836         default:
1837                 goto err_no_initvals;
1838         }
1839         err = do_request_fw(dev, filename, &fw->initvals);
1840         if (err)
1841                 goto err_load;
1842
1843         /* Get bandswitch initvals */
1844         switch (dev->phy.type) {
1845         case B43_PHYTYPE_A:
1846                 if ((rev >= 5) && (rev <= 10)) {
1847                         if (tmshigh & B43_TMSHIGH_HAVE_2GHZ_PHY)
1848                                 filename = "a0g1bsinitvals5";
1849                         else
1850                                 filename = "a0g0bsinitvals5";
1851                 } else if (rev >= 11)
1852                         filename = NULL;
1853                 else
1854                         goto err_no_initvals;
1855                 break;
1856         case B43_PHYTYPE_G:
1857                 if ((rev >= 5) && (rev <= 10))
1858                         filename = "b0g0bsinitvals5";
1859                 else if (rev >= 11)
1860                         filename = NULL;
1861                 else
1862                         goto err_no_initvals;
1863                 break;
1864         case B43_PHYTYPE_N:
1865                 if ((rev >= 11) && (rev <= 12))
1866                         filename = "n0bsinitvals11";
1867                 else
1868                         goto err_no_initvals;
1869                 break;
1870         default:
1871                 goto err_no_initvals;
1872         }
1873         err = do_request_fw(dev, filename, &fw->initvals_band);
1874         if (err)
1875                 goto err_load;
1876
1877         return 0;
1878
1879 err_load:
1880         b43_print_fw_helptext(dev->wl, 1);
1881         goto error;
1882
1883 err_no_ucode:
1884         err = -ENODEV;
1885         b43err(dev->wl, "No microcode available for core rev %u\n", rev);
1886         goto error;
1887
1888 err_no_pcm:
1889         err = -ENODEV;
1890         b43err(dev->wl, "No PCM available for core rev %u\n", rev);
1891         goto error;
1892
1893 err_no_initvals:
1894         err = -ENODEV;
1895         b43err(dev->wl, "No Initial Values firmware file for PHY %u, "
1896                "core rev %u\n", dev->phy.type, rev);
1897         goto error;
1898
1899 error:
1900         b43_release_firmware(dev);
1901         return err;
1902 }
1903
1904 static int b43_upload_microcode(struct b43_wldev *dev)
1905 {
1906         const size_t hdr_len = sizeof(struct b43_fw_header);
1907         const __be32 *data;
1908         unsigned int i, len;
1909         u16 fwrev, fwpatch, fwdate, fwtime;
1910         u32 tmp, macctl;
1911         int err = 0;
1912
1913         /* Jump the microcode PSM to offset 0 */
1914         macctl = b43_read32(dev, B43_MMIO_MACCTL);
1915         B43_WARN_ON(macctl & B43_MACCTL_PSM_RUN);
1916         macctl |= B43_MACCTL_PSM_JMP0;
1917         b43_write32(dev, B43_MMIO_MACCTL, macctl);
1918         /* Zero out all microcode PSM registers and shared memory. */
1919         for (i = 0; i < 64; i++)
1920                 b43_shm_write16(dev, B43_SHM_SCRATCH, i, 0);
1921         for (i = 0; i < 4096; i += 2)
1922                 b43_shm_write16(dev, B43_SHM_SHARED, i, 0);
1923
1924         /* Upload Microcode. */
1925         data = (__be32 *) (dev->fw.ucode.data->data + hdr_len);
1926         len = (dev->fw.ucode.data->size - hdr_len) / sizeof(__be32);
1927         b43_shm_control_word(dev, B43_SHM_UCODE | B43_SHM_AUTOINC_W, 0x0000);
1928         for (i = 0; i < len; i++) {
1929                 b43_write32(dev, B43_MMIO_SHM_DATA, be32_to_cpu(data[i]));
1930                 udelay(10);
1931         }
1932
1933         if (dev->fw.pcm.data) {
1934                 /* Upload PCM data. */
1935                 data = (__be32 *) (dev->fw.pcm.data->data + hdr_len);
1936                 len = (dev->fw.pcm.data->size - hdr_len) / sizeof(__be32);
1937                 b43_shm_control_word(dev, B43_SHM_HW, 0x01EA);
1938                 b43_write32(dev, B43_MMIO_SHM_DATA, 0x00004000);
1939                 /* No need for autoinc bit in SHM_HW */
1940                 b43_shm_control_word(dev, B43_SHM_HW, 0x01EB);
1941                 for (i = 0; i < len; i++) {
1942                         b43_write32(dev, B43_MMIO_SHM_DATA, be32_to_cpu(data[i]));
1943                         udelay(10);
1944                 }
1945         }
1946
1947         b43_write32(dev, B43_MMIO_GEN_IRQ_REASON, B43_IRQ_ALL);
1948
1949         /* Start the microcode PSM */
1950         macctl = b43_read32(dev, B43_MMIO_MACCTL);
1951         macctl &= ~B43_MACCTL_PSM_JMP0;
1952         macctl |= B43_MACCTL_PSM_RUN;
1953         b43_write32(dev, B43_MMIO_MACCTL, macctl);
1954
1955         /* Wait for the microcode to load and respond */
1956         i = 0;
1957         while (1) {
1958                 tmp = b43_read32(dev, B43_MMIO_GEN_IRQ_REASON);
1959                 if (tmp == B43_IRQ_MAC_SUSPENDED)
1960                         break;
1961                 i++;
1962                 if (i >= 20) {
1963                         b43err(dev->wl, "Microcode not responding\n");
1964                         b43_print_fw_helptext(dev->wl, 1);
1965                         err = -ENODEV;
1966                         goto error;
1967                 }
1968                 msleep_interruptible(50);
1969                 if (signal_pending(current)) {
1970                         err = -EINTR;
1971                         goto error;
1972                 }
1973         }
1974         b43_read32(dev, B43_MMIO_GEN_IRQ_REASON);       /* dummy read */
1975
1976         /* Get and check the revisions. */
1977         fwrev = b43_shm_read16(dev, B43_SHM_SHARED, B43_SHM_SH_UCODEREV);
1978         fwpatch = b43_shm_read16(dev, B43_SHM_SHARED, B43_SHM_SH_UCODEPATCH);
1979         fwdate = b43_shm_read16(dev, B43_SHM_SHARED, B43_SHM_SH_UCODEDATE);
1980         fwtime = b43_shm_read16(dev, B43_SHM_SHARED, B43_SHM_SH_UCODETIME);
1981
1982         if (fwrev <= 0x128) {
1983                 b43err(dev->wl, "YOUR FIRMWARE IS TOO OLD. Firmware from "
1984                        "binary drivers older than version 4.x is unsupported. "
1985                        "You must upgrade your firmware files.\n");
1986                 b43_print_fw_helptext(dev->wl, 1);
1987                 err = -EOPNOTSUPP;
1988                 goto error;
1989         }
1990         b43info(dev->wl, "Loading firmware version %u.%u "
1991                 "(20%.2i-%.2i-%.2i %.2i:%.2i:%.2i)\n",
1992                 fwrev, fwpatch,
1993                 (fwdate >> 12) & 0xF, (fwdate >> 8) & 0xF, fwdate & 0xFF,
1994                 (fwtime >> 11) & 0x1F, (fwtime >> 5) & 0x3F, fwtime & 0x1F);
1995
1996         dev->fw.rev = fwrev;
1997         dev->fw.patch = fwpatch;
1998
1999         if (b43_is_old_txhdr_format(dev)) {
2000                 b43warn(dev->wl, "You are using an old firmware image. "
2001                         "Support for old firmware will be removed in July 2008.\n");
2002                 b43_print_fw_helptext(dev->wl, 0);
2003         }
2004
2005         return 0;
2006
2007 error:
2008         macctl = b43_read32(dev, B43_MMIO_MACCTL);
2009         macctl &= ~B43_MACCTL_PSM_RUN;
2010         macctl |= B43_MACCTL_PSM_JMP0;
2011         b43_write32(dev, B43_MMIO_MACCTL, macctl);
2012
2013         return err;
2014 }
2015
2016 static int b43_write_initvals(struct b43_wldev *dev,
2017                               const struct b43_iv *ivals,
2018                               size_t count,
2019                               size_t array_size)
2020 {
2021         const struct b43_iv *iv;
2022         u16 offset;
2023         size_t i;
2024         bool bit32;
2025
2026         BUILD_BUG_ON(sizeof(struct b43_iv) != 6);
2027         iv = ivals;
2028         for (i = 0; i < count; i++) {
2029                 if (array_size < sizeof(iv->offset_size))
2030                         goto err_format;
2031                 array_size -= sizeof(iv->offset_size);
2032                 offset = be16_to_cpu(iv->offset_size);
2033                 bit32 = !!(offset & B43_IV_32BIT);
2034                 offset &= B43_IV_OFFSET_MASK;
2035                 if (offset >= 0x1000)
2036                         goto err_format;
2037                 if (bit32) {
2038                         u32 value;
2039
2040                         if (array_size < sizeof(iv->data.d32))
2041                                 goto err_format;
2042                         array_size -= sizeof(iv->data.d32);
2043
2044                         value = be32_to_cpu(get_unaligned(&iv->data.d32));
2045                         b43_write32(dev, offset, value);
2046
2047                         iv = (const struct b43_iv *)((const uint8_t *)iv +
2048                                                         sizeof(__be16) +
2049                                                         sizeof(__be32));
2050                 } else {
2051                         u16 value;
2052
2053                         if (array_size < sizeof(iv->data.d16))
2054                                 goto err_format;
2055                         array_size -= sizeof(iv->data.d16);
2056
2057                         value = be16_to_cpu(iv->data.d16);
2058                         b43_write16(dev, offset, value);
2059
2060                         iv = (const struct b43_iv *)((const uint8_t *)iv +
2061                                                         sizeof(__be16) +
2062                                                         sizeof(__be16));
2063                 }
2064         }
2065         if (array_size)
2066                 goto err_format;
2067
2068         return 0;
2069
2070 err_format:
2071         b43err(dev->wl, "Initial Values Firmware file-format error.\n");
2072         b43_print_fw_helptext(dev->wl, 1);
2073
2074         return -EPROTO;
2075 }
2076
2077 static int b43_upload_initvals(struct b43_wldev *dev)
2078 {
2079         const size_t hdr_len = sizeof(struct b43_fw_header);
2080         const struct b43_fw_header *hdr;
2081         struct b43_firmware *fw = &dev->fw;
2082         const struct b43_iv *ivals;
2083         size_t count;
2084         int err;
2085
2086         hdr = (const struct b43_fw_header *)(fw->initvals.data->data);
2087         ivals = (const struct b43_iv *)(fw->initvals.data->data + hdr_len);
2088         count = be32_to_cpu(hdr->size);
2089         err = b43_write_initvals(dev, ivals, count,
2090                                  fw->initvals.data->size - hdr_len);
2091         if (err)
2092                 goto out;
2093         if (fw->initvals_band.data) {
2094                 hdr = (const struct b43_fw_header *)(fw->initvals_band.data->data);
2095                 ivals = (const struct b43_iv *)(fw->initvals_band.data->data + hdr_len);
2096                 count = be32_to_cpu(hdr->size);
2097                 err = b43_write_initvals(dev, ivals, count,
2098                                          fw->initvals_band.data->size - hdr_len);
2099                 if (err)
2100                         goto out;
2101         }
2102 out:
2103
2104         return err;
2105 }
2106
2107 /* Initialize the GPIOs
2108  * http://bcm-specs.sipsolutions.net/GPIO
2109  */
2110 static int b43_gpio_init(struct b43_wldev *dev)
2111 {
2112         struct ssb_bus *bus = dev->dev->bus;
2113         struct ssb_device *gpiodev, *pcidev = NULL;
2114         u32 mask, set;
2115
2116         b43_write32(dev, B43_MMIO_MACCTL, b43_read32(dev, B43_MMIO_MACCTL)
2117                     & ~B43_MACCTL_GPOUTSMSK);
2118
2119         b43_write16(dev, B43_MMIO_GPIO_MASK, b43_read16(dev, B43_MMIO_GPIO_MASK)
2120                     | 0x000F);
2121
2122         mask = 0x0000001F;
2123         set = 0x0000000F;
2124         if (dev->dev->bus->chip_id == 0x4301) {
2125                 mask |= 0x0060;
2126                 set |= 0x0060;
2127         }
2128         if (0 /* FIXME: conditional unknown */ ) {
2129                 b43_write16(dev, B43_MMIO_GPIO_MASK,
2130                             b43_read16(dev, B43_MMIO_GPIO_MASK)
2131                             | 0x0100);
2132                 mask |= 0x0180;
2133                 set |= 0x0180;
2134         }
2135         if (dev->dev->bus->sprom.boardflags_lo & B43_BFL_PACTRL) {
2136                 b43_write16(dev, B43_MMIO_GPIO_MASK,
2137                             b43_read16(dev, B43_MMIO_GPIO_MASK)
2138                             | 0x0200);
2139                 mask |= 0x0200;
2140                 set |= 0x0200;
2141         }
2142         if (dev->dev->id.revision >= 2)
2143                 mask |= 0x0010; /* FIXME: This is redundant. */
2144
2145 #ifdef CONFIG_SSB_DRIVER_PCICORE
2146         pcidev = bus->pcicore.dev;
2147 #endif
2148         gpiodev = bus->chipco.dev ? : pcidev;
2149         if (!gpiodev)
2150                 return 0;
2151         ssb_write32(gpiodev, B43_GPIO_CONTROL,
2152                     (ssb_read32(gpiodev, B43_GPIO_CONTROL)
2153                      & mask) | set);
2154
2155         return 0;
2156 }
2157
2158 /* Turn off all GPIO stuff. Call this on module unload, for example. */
2159 static void b43_gpio_cleanup(struct b43_wldev *dev)
2160 {
2161         struct ssb_bus *bus = dev->dev->bus;
2162         struct ssb_device *gpiodev, *pcidev = NULL;
2163
2164 #ifdef CONFIG_SSB_DRIVER_PCICORE
2165         pcidev = bus->pcicore.dev;
2166 #endif
2167         gpiodev = bus->chipco.dev ? : pcidev;
2168         if (!gpiodev)
2169                 return;
2170         ssb_write32(gpiodev, B43_GPIO_CONTROL, 0);
2171 }
2172
2173 /* http://bcm-specs.sipsolutions.net/EnableMac */
2174 static void b43_mac_enable(struct b43_wldev *dev)
2175 {
2176         dev->mac_suspended--;
2177         B43_WARN_ON(dev->mac_suspended < 0);
2178         if (dev->mac_suspended == 0) {
2179                 b43_write32(dev, B43_MMIO_MACCTL,
2180                             b43_read32(dev, B43_MMIO_MACCTL)
2181                             | B43_MACCTL_ENABLED);
2182                 b43_write32(dev, B43_MMIO_GEN_IRQ_REASON,
2183                             B43_IRQ_MAC_SUSPENDED);
2184                 /* Commit writes */
2185                 b43_read32(dev, B43_MMIO_MACCTL);
2186                 b43_read32(dev, B43_MMIO_GEN_IRQ_REASON);
2187                 b43_power_saving_ctl_bits(dev, 0);
2188
2189                 /* Re-enable IRQs. */
2190                 spin_lock_irq(&dev->wl->irq_lock);
2191                 b43_interrupt_enable(dev, dev->irq_savedstate);
2192                 spin_unlock_irq(&dev->wl->irq_lock);
2193         }
2194 }
2195
2196 /* http://bcm-specs.sipsolutions.net/SuspendMAC */
2197 static void b43_mac_suspend(struct b43_wldev *dev)
2198 {
2199         int i;
2200         u32 tmp;
2201
2202         might_sleep();
2203         B43_WARN_ON(dev->mac_suspended < 0);
2204
2205         if (dev->mac_suspended == 0) {
2206                 /* Mask IRQs before suspending MAC. Otherwise
2207                  * the MAC stays busy and won't suspend. */
2208                 spin_lock_irq(&dev->wl->irq_lock);
2209                 tmp = b43_interrupt_disable(dev, B43_IRQ_ALL);
2210                 spin_unlock_irq(&dev->wl->irq_lock);
2211                 b43_synchronize_irq(dev);
2212                 dev->irq_savedstate = tmp;
2213
2214                 b43_power_saving_ctl_bits(dev, B43_PS_AWAKE);
2215                 b43_write32(dev, B43_MMIO_MACCTL,
2216                             b43_read32(dev, B43_MMIO_MACCTL)
2217                             & ~B43_MACCTL_ENABLED);
2218                 /* force pci to flush the write */
2219                 b43_read32(dev, B43_MMIO_MACCTL);
2220                 for (i = 40; i; i--) {
2221                         tmp = b43_read32(dev, B43_MMIO_GEN_IRQ_REASON);
2222                         if (tmp & B43_IRQ_MAC_SUSPENDED)
2223                                 goto out;
2224                         msleep(1);
2225                 }
2226                 b43err(dev->wl, "MAC suspend failed\n");
2227         }
2228 out:
2229         dev->mac_suspended++;
2230 }
2231
2232 static void b43_adjust_opmode(struct b43_wldev *dev)
2233 {
2234         struct b43_wl *wl = dev->wl;
2235         u32 ctl;
2236         u16 cfp_pretbtt;
2237
2238         ctl = b43_read32(dev, B43_MMIO_MACCTL);
2239         /* Reset status to STA infrastructure mode. */
2240         ctl &= ~B43_MACCTL_AP;
2241         ctl &= ~B43_MACCTL_KEEP_CTL;
2242         ctl &= ~B43_MACCTL_KEEP_BADPLCP;
2243         ctl &= ~B43_MACCTL_KEEP_BAD;
2244         ctl &= ~B43_MACCTL_PROMISC;
2245         ctl &= ~B43_MACCTL_BEACPROMISC;
2246         ctl |= B43_MACCTL_INFRA;
2247
2248         if (b43_is_mode(wl, IEEE80211_IF_TYPE_AP))
2249                 ctl |= B43_MACCTL_AP;
2250         else if (b43_is_mode(wl, IEEE80211_IF_TYPE_IBSS))
2251                 ctl &= ~B43_MACCTL_INFRA;
2252
2253         if (wl->filter_flags & FIF_CONTROL)
2254                 ctl |= B43_MACCTL_KEEP_CTL;
2255         if (wl->filter_flags & FIF_FCSFAIL)
2256                 ctl |= B43_MACCTL_KEEP_BAD;
2257         if (wl->filter_flags & FIF_PLCPFAIL)
2258                 ctl |= B43_MACCTL_KEEP_BADPLCP;
2259         if (wl->filter_flags & FIF_PROMISC_IN_BSS)
2260                 ctl |= B43_MACCTL_PROMISC;
2261         if (wl->filter_flags & FIF_BCN_PRBRESP_PROMISC)
2262                 ctl |= B43_MACCTL_BEACPROMISC;
2263
2264         /* Workaround: On old hardware the HW-MAC-address-filter
2265          * doesn't work properly, so always run promisc in filter
2266          * it in software. */
2267         if (dev->dev->id.revision <= 4)
2268                 ctl |= B43_MACCTL_PROMISC;
2269
2270         b43_write32(dev, B43_MMIO_MACCTL, ctl);
2271
2272         cfp_pretbtt = 2;
2273         if ((ctl & B43_MACCTL_INFRA) && !(ctl & B43_MACCTL_AP)) {
2274                 if (dev->dev->bus->chip_id == 0x4306 &&
2275                     dev->dev->bus->chip_rev == 3)
2276                         cfp_pretbtt = 100;
2277                 else
2278                         cfp_pretbtt = 50;
2279         }
2280         b43_write16(dev, 0x612, cfp_pretbtt);
2281 }
2282
2283 static void b43_rate_memory_write(struct b43_wldev *dev, u16 rate, int is_ofdm)
2284 {
2285         u16 offset;
2286
2287         if (is_ofdm) {
2288                 offset = 0x480;
2289                 offset += (b43_plcp_get_ratecode_ofdm(rate) & 0x000F) * 2;
2290         } else {
2291                 offset = 0x4C0;
2292                 offset += (b43_plcp_get_ratecode_cck(rate) & 0x000F) * 2;
2293         }
2294         b43_shm_write16(dev, B43_SHM_SHARED, offset + 0x20,
2295                         b43_shm_read16(dev, B43_SHM_SHARED, offset));
2296 }
2297
2298 static void b43_rate_memory_init(struct b43_wldev *dev)
2299 {
2300         switch (dev->phy.type) {
2301         case B43_PHYTYPE_A:
2302         case B43_PHYTYPE_G:
2303         case B43_PHYTYPE_N:
2304                 b43_rate_memory_write(dev, B43_OFDM_RATE_6MB, 1);
2305                 b43_rate_memory_write(dev, B43_OFDM_RATE_12MB, 1);
2306                 b43_rate_memory_write(dev, B43_OFDM_RATE_18MB, 1);
2307                 b43_rate_memory_write(dev, B43_OFDM_RATE_24MB, 1);
2308                 b43_rate_memory_write(dev, B43_OFDM_RATE_36MB, 1);
2309                 b43_rate_memory_write(dev, B43_OFDM_RATE_48MB, 1);
2310                 b43_rate_memory_write(dev, B43_OFDM_RATE_54MB, 1);
2311                 if (dev->phy.type == B43_PHYTYPE_A)
2312                         break;
2313                 /* fallthrough */
2314         case B43_PHYTYPE_B:
2315                 b43_rate_memory_write(dev, B43_CCK_RATE_1MB, 0);
2316                 b43_rate_memory_write(dev, B43_CCK_RATE_2MB, 0);
2317                 b43_rate_memory_write(dev, B43_CCK_RATE_5MB, 0);
2318                 b43_rate_memory_write(dev, B43_CCK_RATE_11MB, 0);
2319                 break;
2320         default:
2321                 B43_WARN_ON(1);
2322         }
2323 }
2324
2325 /* Set the TX-Antenna for management frames sent by firmware. */
2326 static void b43_mgmtframe_txantenna(struct b43_wldev *dev, int antenna)
2327 {
2328         u16 ant = 0;
2329         u16 tmp;
2330
2331         switch (antenna) {
2332         case B43_ANTENNA0:
2333                 ant |= B43_TXH_PHY_ANT0;
2334                 break;
2335         case B43_ANTENNA1:
2336                 ant |= B43_TXH_PHY_ANT1;
2337                 break;
2338         case B43_ANTENNA2:
2339                 ant |= B43_TXH_PHY_ANT2;
2340                 break;
2341         case B43_ANTENNA3:
2342                 ant |= B43_TXH_PHY_ANT3;
2343                 break;
2344         case B43_ANTENNA_AUTO:
2345                 ant |= B43_TXH_PHY_ANT01AUTO;
2346                 break;
2347         default:
2348                 B43_WARN_ON(1);
2349         }
2350
2351         /* FIXME We also need to set the other flags of the PHY control field somewhere. */
2352
2353         /* For Beacons */
2354         tmp = b43_shm_read16(dev, B43_SHM_SHARED, B43_SHM_SH_BEACPHYCTL);
2355         tmp = (tmp & ~B43_TXH_PHY_ANT) | ant;
2356         b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_BEACPHYCTL, tmp);
2357         /* For ACK/CTS */
2358         tmp = b43_shm_read16(dev, B43_SHM_SHARED, B43_SHM_SH_ACKCTSPHYCTL);
2359         tmp = (tmp & ~B43_TXH_PHY_ANT) | ant;
2360         b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_ACKCTSPHYCTL, tmp);
2361         /* For Probe Resposes */
2362         tmp = b43_shm_read16(dev, B43_SHM_SHARED, B43_SHM_SH_PRPHYCTL);
2363         tmp = (tmp & ~B43_TXH_PHY_ANT) | ant;
2364         b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_PRPHYCTL, tmp);
2365 }
2366
2367 /* This is the opposite of b43_chip_init() */
2368 static void b43_chip_exit(struct b43_wldev *dev)
2369 {
2370         b43_radio_turn_off(dev, 1);
2371         b43_gpio_cleanup(dev);
2372         /* firmware is released later */
2373 }
2374
2375 /* Initialize the chip
2376  * http://bcm-specs.sipsolutions.net/ChipInit
2377  */
2378 static int b43_chip_init(struct b43_wldev *dev)
2379 {
2380         struct b43_phy *phy = &dev->phy;
2381         int err, tmp;
2382         u32 value32, macctl;
2383         u16 value16;
2384
2385         /* Initialize the MAC control */
2386         macctl = B43_MACCTL_IHR_ENABLED | B43_MACCTL_SHM_ENABLED;
2387         if (dev->phy.gmode)
2388                 macctl |= B43_MACCTL_GMODE;
2389         macctl |= B43_MACCTL_INFRA;
2390         b43_write32(dev, B43_MMIO_MACCTL, macctl);
2391
2392         err = b43_request_firmware(dev);
2393         if (err)
2394                 goto out;
2395         err = b43_upload_microcode(dev);
2396         if (err)
2397                 goto out;       /* firmware is released later */
2398
2399         err = b43_gpio_init(dev);
2400         if (err)
2401                 goto out;       /* firmware is released later */
2402
2403         err = b43_upload_initvals(dev);
2404         if (err)
2405                 goto err_gpio_clean;
2406         b43_radio_turn_on(dev);
2407
2408         b43_write16(dev, 0x03E6, 0x0000);
2409         err = b43_phy_init(dev);
2410         if (err)
2411                 goto err_radio_off;
2412
2413         /* Select initial Interference Mitigation. */
2414         tmp = phy->interfmode;
2415         phy->interfmode = B43_INTERFMODE_NONE;
2416         b43_radio_set_interference_mitigation(dev, tmp);
2417
2418         b43_set_rx_antenna(dev, B43_ANTENNA_DEFAULT);
2419         b43_mgmtframe_txantenna(dev, B43_ANTENNA_DEFAULT);
2420
2421         if (phy->type == B43_PHYTYPE_B) {
2422                 value16 = b43_read16(dev, 0x005E);
2423                 value16 |= 0x0004;
2424                 b43_write16(dev, 0x005E, value16);
2425         }
2426         b43_write32(dev, 0x0100, 0x01000000);
2427         if (dev->dev->id.revision < 5)
2428                 b43_write32(dev, 0x010C, 0x01000000);
2429
2430         b43_write32(dev, B43_MMIO_MACCTL, b43_read32(dev, B43_MMIO_MACCTL)
2431                     & ~B43_MACCTL_INFRA);
2432         b43_write32(dev, B43_MMIO_MACCTL, b43_read32(dev, B43_MMIO_MACCTL)
2433                     | B43_MACCTL_INFRA);
2434
2435         /* Probe Response Timeout value */
2436         /* FIXME: Default to 0, has to be set by ioctl probably... :-/ */
2437         b43_shm_write16(dev, B43_SHM_SHARED, 0x0074, 0x0000);
2438
2439         /* Initially set the wireless operation mode. */
2440         b43_adjust_opmode(dev);
2441
2442         if (dev->dev->id.revision < 3) {
2443                 b43_write16(dev, 0x060E, 0x0000);
2444                 b43_write16(dev, 0x0610, 0x8000);
2445                 b43_write16(dev, 0x0604, 0x0000);
2446                 b43_write16(dev, 0x0606, 0x0200);
2447         } else {
2448                 b43_write32(dev, 0x0188, 0x80000000);
2449                 b43_write32(dev, 0x018C, 0x02000000);
2450         }
2451         b43_write32(dev, B43_MMIO_GEN_IRQ_REASON, 0x00004000);
2452         b43_write32(dev, B43_MMIO_DMA0_IRQ_MASK, 0x0001DC00);
2453         b43_write32(dev, B43_MMIO_DMA1_IRQ_MASK, 0x0000DC00);
2454         b43_write32(dev, B43_MMIO_DMA2_IRQ_MASK, 0x0000DC00);
2455         b43_write32(dev, B43_MMIO_DMA3_IRQ_MASK, 0x0001DC00);
2456         b43_write32(dev, B43_MMIO_DMA4_IRQ_MASK, 0x0000DC00);
2457         b43_write32(dev, B43_MMIO_DMA5_IRQ_MASK, 0x0000DC00);
2458
2459         value32 = ssb_read32(dev->dev, SSB_TMSLOW);
2460         value32 |= 0x00100000;
2461         ssb_write32(dev->dev, SSB_TMSLOW, value32);
2462
2463         b43_write16(dev, B43_MMIO_POWERUP_DELAY,
2464                     dev->dev->bus->chipco.fast_pwrup_delay);
2465
2466         err = 0;
2467         b43dbg(dev->wl, "Chip initialized\n");
2468 out:
2469         return err;
2470
2471 err_radio_off:
2472         b43_radio_turn_off(dev, 1);
2473 err_gpio_clean:
2474         b43_gpio_cleanup(dev);
2475         return err;
2476 }
2477
2478 static void b43_periodic_every120sec(struct b43_wldev *dev)
2479 {
2480         struct b43_phy *phy = &dev->phy;
2481
2482         if (phy->type != B43_PHYTYPE_G || phy->rev < 2)
2483                 return;
2484
2485         b43_mac_suspend(dev);
2486         b43_lo_g_measure(dev);
2487         b43_mac_enable(dev);
2488         if (b43_has_hardware_pctl(phy))
2489                 b43_lo_g_ctl_mark_all_unused(dev);
2490 }
2491
2492 static void b43_periodic_every60sec(struct b43_wldev *dev)
2493 {
2494         struct b43_phy *phy = &dev->phy;
2495
2496         if (phy->type != B43_PHYTYPE_G)
2497                 return;
2498         if (!b43_has_hardware_pctl(phy))
2499                 b43_lo_g_ctl_mark_all_unused(dev);
2500         if (dev->dev->bus->sprom.boardflags_lo & B43_BFL_RSSI) {
2501                 b43_mac_suspend(dev);
2502                 b43_calc_nrssi_slope(dev);
2503                 if ((phy->radio_ver == 0x2050) && (phy->radio_rev == 8)) {
2504                         u8 old_chan = phy->channel;
2505
2506                         /* VCO Calibration */
2507                         if (old_chan >= 8)
2508                                 b43_radio_selectchannel(dev, 1, 0);
2509                         else
2510                                 b43_radio_selectchannel(dev, 13, 0);
2511                         b43_radio_selectchannel(dev, old_chan, 0);
2512                 }
2513                 b43_mac_enable(dev);
2514         }
2515 }
2516
2517 static void b43_periodic_every30sec(struct b43_wldev *dev)
2518 {
2519         /* Update device statistics. */
2520         b43_calculate_link_quality(dev);
2521 }
2522
2523 static void b43_periodic_every15sec(struct b43_wldev *dev)
2524 {
2525         struct b43_phy *phy = &dev->phy;
2526
2527         if (phy->type == B43_PHYTYPE_G) {
2528                 //TODO: update_aci_moving_average
2529                 if (phy->aci_enable && phy->aci_wlan_automatic) {
2530                         b43_mac_suspend(dev);
2531                         if (!phy->aci_enable && 1 /*TODO: not scanning? */ ) {
2532                                 if (0 /*TODO: bunch of conditions */ ) {
2533                                         b43_radio_set_interference_mitigation
2534                                             (dev, B43_INTERFMODE_MANUALWLAN);
2535                                 }
2536                         } else if (1 /*TODO*/) {
2537                                 /*
2538                                    if ((aci_average > 1000) && !(b43_radio_aci_scan(dev))) {
2539                                    b43_radio_set_interference_mitigation(dev,
2540                                    B43_INTERFMODE_NONE);
2541                                    }
2542                                  */
2543                         }
2544                         b43_mac_enable(dev);
2545                 } else if (phy->interfmode == B43_INTERFMODE_NONWLAN &&
2546                            phy->rev == 1) {
2547                         //TODO: implement rev1 workaround
2548                 }
2549         }
2550         b43_phy_xmitpower(dev); //FIXME: unless scanning?
2551         //TODO for APHY (temperature?)
2552
2553         atomic_set(&phy->txerr_cnt, B43_PHY_TX_BADNESS_LIMIT);
2554         wmb();
2555 }
2556
2557 static void do_periodic_work(struct b43_wldev *dev)
2558 {
2559         unsigned int state;
2560
2561         state = dev->periodic_state;
2562         if (state % 8 == 0)
2563                 b43_periodic_every120sec(dev);
2564         if (state % 4 == 0)
2565                 b43_periodic_every60sec(dev);
2566         if (state % 2 == 0)
2567                 b43_periodic_every30sec(dev);
2568         b43_periodic_every15sec(dev);
2569 }
2570
2571 /* Periodic work locking policy:
2572  *      The whole periodic work handler is protected by
2573  *      wl->mutex. If another lock is needed somewhere in the
2574  *      pwork callchain, it's aquired in-place, where it's needed.
2575  */
2576 static void b43_periodic_work_handler(struct work_struct *work)
2577 {
2578         struct b43_wldev *dev = container_of(work, struct b43_wldev,
2579                                              periodic_work.work);
2580         struct b43_wl *wl = dev->wl;
2581         unsigned long delay;
2582
2583         mutex_lock(&wl->mutex);
2584
2585         if (unlikely(b43_status(dev) != B43_STAT_STARTED))
2586                 goto out;
2587         if (b43_debug(dev, B43_DBG_PWORK_STOP))
2588                 goto out_requeue;
2589
2590         do_periodic_work(dev);
2591
2592         dev->periodic_state++;
2593 out_requeue:
2594         if (b43_debug(dev, B43_DBG_PWORK_FAST))
2595                 delay = msecs_to_jiffies(50);
2596         else
2597                 delay = round_jiffies_relative(HZ * 15);
2598         queue_delayed_work(wl->hw->workqueue, &dev->periodic_work, delay);
2599 out:
2600         mutex_unlock(&wl->mutex);
2601 }
2602
2603 static void b43_periodic_tasks_setup(struct b43_wldev *dev)
2604 {
2605         struct delayed_work *work = &dev->periodic_work;
2606
2607         dev->periodic_state = 0;
2608         INIT_DELAYED_WORK(work, b43_periodic_work_handler);
2609         queue_delayed_work(dev->wl->hw->workqueue, work, 0);
2610 }
2611
2612 /* Check if communication with the device works correctly. */
2613 static int b43_validate_chipaccess(struct b43_wldev *dev)
2614 {
2615         u32 v, backup;
2616
2617         backup = b43_shm_read32(dev, B43_SHM_SHARED, 0);
2618
2619         /* Check for read/write and endianness problems. */
2620         b43_shm_write32(dev, B43_SHM_SHARED, 0, 0x55AAAA55);
2621         if (b43_shm_read32(dev, B43_SHM_SHARED, 0) != 0x55AAAA55)
2622                 goto error;
2623         b43_shm_write32(dev, B43_SHM_SHARED, 0, 0xAA5555AA);
2624         if (b43_shm_read32(dev, B43_SHM_SHARED, 0) != 0xAA5555AA)
2625                 goto error;
2626
2627         b43_shm_write32(dev, B43_SHM_SHARED, 0, backup);
2628
2629         if ((dev->dev->id.revision >= 3) && (dev->dev->id.revision <= 10)) {
2630                 /* The 32bit register shadows the two 16bit registers
2631                  * with update sideeffects. Validate this. */
2632                 b43_write16(dev, B43_MMIO_TSF_CFP_START, 0xAAAA);
2633                 b43_write32(dev, B43_MMIO_TSF_CFP_START, 0xCCCCBBBB);
2634                 if (b43_read16(dev, B43_MMIO_TSF_CFP_START_LOW) != 0xBBBB)
2635                         goto error;
2636                 if (b43_read16(dev, B43_MMIO_TSF_CFP_START_HIGH) != 0xCCCC)
2637                         goto error;
2638         }
2639         b43_write32(dev, B43_MMIO_TSF_CFP_START, 0);
2640
2641         v = b43_read32(dev, B43_MMIO_MACCTL);
2642         v |= B43_MACCTL_GMODE;
2643         if (v != (B43_MACCTL_GMODE | B43_MACCTL_IHR_ENABLED))
2644                 goto error;
2645
2646         return 0;
2647 error:
2648         b43err(dev->wl, "Failed to validate the chipaccess\n");
2649         return -ENODEV;
2650 }
2651
2652 static void b43_security_init(struct b43_wldev *dev)
2653 {
2654         dev->max_nr_keys = (dev->dev->id.revision >= 5) ? 58 : 20;
2655         B43_WARN_ON(dev->max_nr_keys > ARRAY_SIZE(dev->key));
2656         dev->ktp = b43_shm_read16(dev, B43_SHM_SHARED, B43_SHM_SH_KTP);
2657         /* KTP is a word address, but we address SHM bytewise.
2658          * So multiply by two.
2659          */
2660         dev->ktp *= 2;
2661         if (dev->dev->id.revision >= 5) {
2662                 /* Number of RCMTA address slots */
2663                 b43_write16(dev, B43_MMIO_RCMTA_COUNT, dev->max_nr_keys - 8);
2664         }
2665         b43_clear_keys(dev);
2666 }
2667
2668 static int b43_rng_read(struct hwrng *rng, u32 * data)
2669 {
2670         struct b43_wl *wl = (struct b43_wl *)rng->priv;
2671         unsigned long flags;
2672
2673         /* Don't take wl->mutex here, as it could deadlock with
2674          * hwrng internal locking. It's not needed to take
2675          * wl->mutex here, anyway. */
2676
2677         spin_lock_irqsave(&wl->irq_lock, flags);
2678         *data = b43_read16(wl->current_dev, B43_MMIO_RNG);
2679         spin_unlock_irqrestore(&wl->irq_lock, flags);
2680
2681         return (sizeof(u16));
2682 }
2683
2684 static void b43_rng_exit(struct b43_wl *wl, bool suspended)
2685 {
2686         if (wl->rng_initialized)
2687                 __hwrng_unregister(&wl->rng, suspended);
2688 }
2689
2690 static int b43_rng_init(struct b43_wl *wl)
2691 {
2692         int err;
2693
2694         snprintf(wl->rng_name, ARRAY_SIZE(wl->rng_name),
2695                  "%s_%s", KBUILD_MODNAME, wiphy_name(wl->hw->wiphy));
2696         wl->rng.name = wl->rng_name;
2697         wl->rng.data_read = b43_rng_read;
2698         wl->rng.priv = (unsigned long)wl;
2699         wl->rng_initialized = 1;
2700         err = hwrng_register(&wl->rng);
2701         if (err) {
2702                 wl->rng_initialized = 0;
2703                 b43err(wl, "Failed to register the random "
2704                        "number generator (%d)\n", err);
2705         }
2706
2707         return err;
2708 }
2709
2710 static int b43_op_tx(struct ieee80211_hw *hw,
2711                      struct sk_buff *skb,
2712                      struct ieee80211_tx_control *ctl)
2713 {
2714         struct b43_wl *wl = hw_to_b43_wl(hw);
2715         struct b43_wldev *dev = wl->current_dev;
2716         int err = -ENODEV;
2717
2718         if (unlikely(skb->len < 2 + 2 + 6)) {
2719                 /* Too short, this can't be a valid frame. */
2720                 return -EINVAL;
2721         }
2722         B43_WARN_ON(skb_shinfo(skb)->nr_frags);
2723
2724         if (unlikely(!dev))
2725                 goto out;
2726         if (unlikely(b43_status(dev) < B43_STAT_STARTED))
2727                 goto out;
2728         /* TX is done without a global lock. */
2729         if (b43_using_pio_transfers(dev))
2730                 err = b43_pio_tx(dev, skb, ctl);
2731         else
2732                 err = b43_dma_tx(dev, skb, ctl);
2733 out:
2734         if (unlikely(err))
2735                 return NETDEV_TX_BUSY;
2736         return NETDEV_TX_OK;
2737 }
2738
2739 /* Locking: wl->irq_lock */
2740 static void b43_qos_params_upload(struct b43_wldev *dev,
2741                                   const struct ieee80211_tx_queue_params *p,
2742                                   u16 shm_offset)
2743 {
2744         u16 params[B43_NR_QOSPARAMS];
2745         int cw_min, cw_max, aifs, bslots, tmp;
2746         unsigned int i;
2747
2748         const u16 aCWmin = 0x0001;
2749         const u16 aCWmax = 0x03FF;
2750
2751         /* Calculate the default values for the parameters, if needed. */
2752         switch (shm_offset) {
2753         case B43_QOS_VOICE:
2754                 aifs = (p->aifs == -1) ? 2 : p->aifs;
2755                 cw_min = (p->cw_min == 0) ? ((aCWmin + 1) / 4 - 1) : p->cw_min;
2756                 cw_max = (p->cw_max == 0) ? ((aCWmin + 1) / 2 - 1) : p->cw_max;
2757                 break;
2758         case B43_QOS_VIDEO:
2759                 aifs = (p->aifs == -1) ? 2 : p->aifs;
2760                 cw_min = (p->cw_min == 0) ? ((aCWmin + 1) / 2 - 1) : p->cw_min;
2761                 cw_max = (p->cw_max == 0) ? aCWmin : p->cw_max;
2762                 break;
2763         case B43_QOS_BESTEFFORT:
2764                 aifs = (p->aifs == -1) ? 3 : p->aifs;
2765                 cw_min = (p->cw_min == 0) ? aCWmin : p->cw_min;
2766                 cw_max = (p->cw_max == 0) ? aCWmax : p->cw_max;
2767                 break;
2768         case B43_QOS_BACKGROUND:
2769                 aifs = (p->aifs == -1) ? 7 : p->aifs;
2770                 cw_min = (p->cw_min == 0) ? aCWmin : p->cw_min;
2771                 cw_max = (p->cw_max == 0) ? aCWmax : p->cw_max;
2772                 break;
2773         default:
2774                 B43_WARN_ON(1);
2775                 return;
2776         }
2777         if (cw_min <= 0)
2778                 cw_min = aCWmin;
2779         if (cw_max <= 0)
2780                 cw_max = aCWmin;
2781         bslots = b43_read16(dev, B43_MMIO_RNG) % cw_min;
2782
2783         memset(&params, 0, sizeof(params));
2784
2785         params[B43_QOSPARAM_TXOP] = p->txop * 32;
2786         params[B43_QOSPARAM_CWMIN] = cw_min;
2787         params[B43_QOSPARAM_CWMAX] = cw_max;
2788         params[B43_QOSPARAM_CWCUR] = cw_min;
2789         params[B43_QOSPARAM_AIFS] = aifs;
2790         params[B43_QOSPARAM_BSLOTS] = bslots;
2791         params[B43_QOSPARAM_REGGAP] = bslots + aifs;
2792
2793         for (i = 0; i < ARRAY_SIZE(params); i++) {
2794                 if (i == B43_QOSPARAM_STATUS) {
2795                         tmp = b43_shm_read16(dev, B43_SHM_SHARED,
2796                                              shm_offset + (i * 2));
2797                         /* Mark the parameters as updated. */
2798                         tmp |= 0x100;
2799                         b43_shm_write16(dev, B43_SHM_SHARED,
2800                                         shm_offset + (i * 2),
2801                                         tmp);
2802                 } else {
2803                         b43_shm_write16(dev, B43_SHM_SHARED,
2804                                         shm_offset + (i * 2),
2805                                         params[i]);
2806                 }
2807         }
2808 }
2809
2810 /* Update the QOS parameters in hardware. */
2811 static void b43_qos_update(struct b43_wldev *dev)
2812 {
2813         struct b43_wl *wl = dev->wl;
2814         struct b43_qos_params *params;
2815         unsigned long flags;
2816         unsigned int i;
2817
2818         /* Mapping of mac80211 queues to b43 SHM offsets. */
2819         static const u16 qos_shm_offsets[] = {
2820                 [0] = B43_QOS_VOICE,
2821                 [1] = B43_QOS_VIDEO,
2822                 [2] = B43_QOS_BESTEFFORT,
2823                 [3] = B43_QOS_BACKGROUND,
2824         };
2825         BUILD_BUG_ON(ARRAY_SIZE(qos_shm_offsets) != ARRAY_SIZE(wl->qos_params));
2826
2827         b43_mac_suspend(dev);
2828         spin_lock_irqsave(&wl->irq_lock, flags);
2829
2830         for (i = 0; i < ARRAY_SIZE(wl->qos_params); i++) {
2831                 params = &(wl->qos_params[i]);
2832                 if (params->need_hw_update) {
2833                         b43_qos_params_upload(dev, &(params->p),
2834                                               qos_shm_offsets[i]);
2835                         params->need_hw_update = 0;
2836                 }
2837         }
2838
2839         spin_unlock_irqrestore(&wl->irq_lock, flags);
2840         b43_mac_enable(dev);
2841 }
2842
2843 static void b43_qos_clear(struct b43_wl *wl)
2844 {
2845         struct b43_qos_params *params;
2846         unsigned int i;
2847
2848         for (i = 0; i < ARRAY_SIZE(wl->qos_params); i++) {
2849                 params = &(wl->qos_params[i]);
2850
2851                 memset(&(params->p), 0, sizeof(params->p));
2852                 params->p.aifs = -1;
2853                 params->need_hw_update = 1;
2854         }
2855 }
2856
2857 /* Initialize the core's QOS capabilities */
2858 static void b43_qos_init(struct b43_wldev *dev)
2859 {
2860         struct b43_wl *wl = dev->wl;
2861         unsigned int i;
2862
2863         /* Upload the current QOS parameters. */
2864         for (i = 0; i < ARRAY_SIZE(wl->qos_params); i++)
2865                 wl->qos_params[i].need_hw_update = 1;
2866         b43_qos_update(dev);
2867
2868         /* Enable QOS support. */
2869         b43_hf_write(dev, b43_hf_read(dev) | B43_HF_EDCF);
2870         b43_write16(dev, B43_MMIO_IFSCTL,
2871                     b43_read16(dev, B43_MMIO_IFSCTL)
2872                     | B43_MMIO_IFSCTL_USE_EDCF);
2873 }
2874
2875 static void b43_qos_update_work(struct work_struct *work)
2876 {
2877         struct b43_wl *wl = container_of(work, struct b43_wl, qos_update_work);
2878         struct b43_wldev *dev;
2879
2880         mutex_lock(&wl->mutex);
2881         dev = wl->current_dev;
2882         if (likely(dev && (b43_status(dev) >= B43_STAT_INITIALIZED)))
2883                 b43_qos_update(dev);
2884         mutex_unlock(&wl->mutex);
2885 }
2886
2887 static int b43_op_conf_tx(struct ieee80211_hw *hw,
2888                           int _queue,
2889                           const struct ieee80211_tx_queue_params *params)
2890 {
2891         struct b43_wl *wl = hw_to_b43_wl(hw);
2892         unsigned long flags;
2893         unsigned int queue = (unsigned int)_queue;
2894         struct b43_qos_params *p;
2895
2896         if (queue >= ARRAY_SIZE(wl->qos_params)) {
2897                 /* Queue not available or don't support setting
2898                  * params on this queue. Return success to not
2899                  * confuse mac80211. */
2900                 return 0;
2901         }
2902
2903         spin_lock_irqsave(&wl->irq_lock, flags);
2904         p = &(wl->qos_params[queue]);
2905         memcpy(&(p->p), params, sizeof(p->p));
2906         p->need_hw_update = 1;
2907         spin_unlock_irqrestore(&wl->irq_lock, flags);
2908
2909         queue_work(hw->workqueue, &wl->qos_update_work);
2910
2911         return 0;
2912 }
2913
2914 static int b43_op_get_tx_stats(struct ieee80211_hw *hw,
2915                                struct ieee80211_tx_queue_stats *stats)
2916 {
2917         struct b43_wl *wl = hw_to_b43_wl(hw);
2918         struct b43_wldev *dev = wl->current_dev;
2919         unsigned long flags;
2920         int err = -ENODEV;
2921
2922         if (!dev)
2923                 goto out;
2924         spin_lock_irqsave(&wl->irq_lock, flags);
2925         if (likely(b43_status(dev) >= B43_STAT_STARTED)) {
2926                 if (b43_using_pio_transfers(dev))
2927                         b43_pio_get_tx_stats(dev, stats);
2928                 else
2929                         b43_dma_get_tx_stats(dev, stats);
2930                 err = 0;
2931         }
2932         spin_unlock_irqrestore(&wl->irq_lock, flags);
2933 out:
2934         return err;
2935 }
2936
2937 static int b43_op_get_stats(struct ieee80211_hw *hw,
2938                             struct ieee80211_low_level_stats *stats)
2939 {
2940         struct b43_wl *wl = hw_to_b43_wl(hw);
2941         unsigned long flags;
2942
2943         spin_lock_irqsave(&wl->irq_lock, flags);
2944         memcpy(stats, &wl->ieee_stats, sizeof(*stats));
2945         spin_unlock_irqrestore(&wl->irq_lock, flags);
2946
2947         return 0;
2948 }
2949
2950 static void b43_put_phy_into_reset(struct b43_wldev *dev)
2951 {
2952         struct ssb_device *sdev = dev->dev;
2953         u32 tmslow;
2954
2955         tmslow = ssb_read32(sdev, SSB_TMSLOW);
2956         tmslow &= ~B43_TMSLOW_GMODE;
2957         tmslow |= B43_TMSLOW_PHYRESET;
2958         tmslow |= SSB_TMSLOW_FGC;
2959         ssb_write32(sdev, SSB_TMSLOW, tmslow);
2960         msleep(1);
2961
2962         tmslow = ssb_read32(sdev, SSB_TMSLOW);
2963         tmslow &= ~SSB_TMSLOW_FGC;
2964         tmslow |= B43_TMSLOW_PHYRESET;
2965         ssb_write32(sdev, SSB_TMSLOW, tmslow);
2966         msleep(1);
2967 }
2968
2969 static const char * band_to_string(enum ieee80211_band band)
2970 {
2971         switch (band) {
2972         case IEEE80211_BAND_5GHZ:
2973                 return "5";
2974         case IEEE80211_BAND_2GHZ:
2975                 return "2.4";
2976         default:
2977                 break;
2978         }
2979         B43_WARN_ON(1);
2980         return "";
2981 }
2982
2983 /* Expects wl->mutex locked */
2984 static int b43_switch_band(struct b43_wl *wl, struct ieee80211_channel *chan)
2985 {
2986         struct b43_wldev *up_dev = NULL;
2987         struct b43_wldev *down_dev;
2988         struct b43_wldev *d;
2989         int err;
2990         bool gmode;
2991         int prev_status;
2992
2993         /* Find a device and PHY which supports the band. */
2994         list_for_each_entry(d, &wl->devlist, list) {
2995                 switch (chan->band) {
2996                 case IEEE80211_BAND_5GHZ:
2997                         if (d->phy.supports_5ghz) {
2998                                 up_dev = d;
2999                                 gmode = 0;
3000                         }
3001                         break;
3002                 case IEEE80211_BAND_2GHZ:
3003                         if (d->phy.supports_2ghz) {
3004                                 up_dev = d;
3005                                 gmode = 1;
3006                         }
3007                         break;
3008                 default:
3009                         B43_WARN_ON(1);
3010                         return -EINVAL;
3011                 }
3012                 if (up_dev)
3013                         break;
3014         }
3015         if (!up_dev) {
3016                 b43err(wl, "Could not find a device for %s-GHz band operation\n",
3017                        band_to_string(chan->band));
3018                 return -ENODEV;
3019         }
3020         if ((up_dev == wl->current_dev) &&
3021             (!!wl->current_dev->phy.gmode == !!gmode)) {
3022                 /* This device is already running. */
3023                 return 0;
3024         }
3025         b43dbg(wl, "Switching to %s-GHz band\n",
3026                band_to_string(chan->band));
3027         down_dev = wl->current_dev;
3028
3029         prev_status = b43_status(down_dev);
3030         /* Shutdown the currently running core. */
3031         if (prev_status >= B43_STAT_STARTED)
3032                 b43_wireless_core_stop(down_dev);
3033         if (prev_status >= B43_STAT_INITIALIZED)
3034                 b43_wireless_core_exit(down_dev);
3035
3036         if (down_dev != up_dev) {
3037                 /* We switch to a different core, so we put PHY into
3038                  * RESET on the old core. */
3039                 b43_put_phy_into_reset(down_dev);
3040         }
3041
3042         /* Now start the new core. */
3043         up_dev->phy.gmode = gmode;
3044         if (prev_status >= B43_STAT_INITIALIZED) {
3045                 err = b43_wireless_core_init(up_dev);
3046                 if (err) {
3047                         b43err(wl, "Fatal: Could not initialize device for "
3048                                "selected %s-GHz band\n",
3049                                band_to_string(chan->band));
3050                         goto init_failure;
3051                 }
3052         }
3053         if (prev_status >= B43_STAT_STARTED) {
3054                 err = b43_wireless_core_start(up_dev);
3055                 if (err) {
3056                         b43err(wl, "Fatal: Coult not start device for "
3057                                "selected %s-GHz band\n",
3058                                band_to_string(chan->band));
3059                         b43_wireless_core_exit(up_dev);
3060                         goto init_failure;
3061                 }
3062         }
3063         B43_WARN_ON(b43_status(up_dev) != prev_status);
3064
3065         wl->current_dev = up_dev;
3066
3067         return 0;
3068 init_failure:
3069         /* Whoops, failed to init the new core. No core is operating now. */
3070         wl->current_dev = NULL;
3071         return err;
3072 }
3073
3074 /* Check if the use of the antenna that ieee80211 told us to
3075  * use is possible. This will fall back to DEFAULT.
3076  * "antenna_nr" is the antenna identifier we got from ieee80211. */
3077 u8 b43_ieee80211_antenna_sanitize(struct b43_wldev *dev,
3078                                   u8 antenna_nr)
3079 {
3080         u8 antenna_mask;
3081
3082         if (antenna_nr == 0) {
3083                 /* Zero means "use default antenna". That's always OK. */
3084                 return 0;
3085         }
3086
3087         /* Get the mask of available antennas. */
3088         if (dev->phy.gmode)
3089                 antenna_mask = dev->dev->bus->sprom.ant_available_bg;
3090         else
3091                 antenna_mask = dev->dev->bus->sprom.ant_available_a;
3092
3093         if (!(antenna_mask & (1 << (antenna_nr - 1)))) {
3094                 /* This antenna is not available. Fall back to default. */
3095                 return 0;
3096         }
3097
3098         return antenna_nr;
3099 }
3100
3101 static int b43_antenna_from_ieee80211(struct b43_wldev *dev, u8 antenna)
3102 {
3103         antenna = b43_ieee80211_antenna_sanitize(dev, antenna);
3104         switch (antenna) {
3105         case 0:         /* default/diversity */
3106                 return B43_ANTENNA_DEFAULT;
3107         case 1:         /* Antenna 0 */
3108                 return B43_ANTENNA0;
3109         case 2:         /* Antenna 1 */
3110                 return B43_ANTENNA1;
3111         case 3:         /* Antenna 2 */
3112                 return B43_ANTENNA2;
3113         case 4:         /* Antenna 3 */
3114                 return B43_ANTENNA3;
3115         default:
3116                 return B43_ANTENNA_DEFAULT;
3117         }
3118 }
3119
3120 static int b43_op_config(struct ieee80211_hw *hw, struct ieee80211_conf *conf)
3121 {
3122         struct b43_wl *wl = hw_to_b43_wl(hw);
3123         struct b43_wldev *dev;
3124         struct b43_phy *phy;
3125         unsigned long flags;
3126         int antenna;
3127         int err = 0;
3128         u32 savedirqs;
3129
3130         mutex_lock(&wl->mutex);
3131
3132         /* Switch the band (if necessary). This might change the active core. */
3133         err = b43_switch_band(wl, conf->channel);
3134         if (err)
3135                 goto out_unlock_mutex;
3136         dev = wl->current_dev;
3137         phy = &dev->phy;
3138
3139         /* Disable IRQs while reconfiguring the device.
3140          * This makes it possible to drop the spinlock throughout
3141          * the reconfiguration process. */
3142         spin_lock_irqsave(&wl->irq_lock, flags);
3143         if (b43_status(dev) < B43_STAT_STARTED) {
3144                 spin_unlock_irqrestore(&wl->irq_lock, flags);
3145                 goto out_unlock_mutex;
3146         }
3147         savedirqs = b43_interrupt_disable(dev, B43_IRQ_ALL);
3148         spin_unlock_irqrestore(&wl->irq_lock, flags);
3149         b43_synchronize_irq(dev);
3150
3151         /* Switch to the requested channel.
3152          * The firmware takes care of races with the TX handler. */
3153         if (conf->channel->hw_value != phy->channel)
3154                 b43_radio_selectchannel(dev, conf->channel->hw_value, 0);
3155
3156         /* Enable/Disable ShortSlot timing. */
3157         if ((!!(conf->flags & IEEE80211_CONF_SHORT_SLOT_TIME)) !=
3158             dev->short_slot) {
3159                 B43_WARN_ON(phy->type != B43_PHYTYPE_G);
3160                 if (conf->flags & IEEE80211_CONF_SHORT_SLOT_TIME)
3161                         b43_short_slot_timing_enable(dev);
3162                 else
3163                         b43_short_slot_timing_disable(dev);
3164         }
3165
3166         dev->wl->radiotap_enabled = !!(conf->flags & IEEE80211_CONF_RADIOTAP);
3167
3168         /* Adjust the desired TX power level. */
3169         if (conf->power_level != 0) {
3170                 if (conf->power_level != phy->power_level) {
3171                         phy->power_level = conf->power_level;
3172                         b43_phy_xmitpower(dev);
3173                 }
3174         }
3175
3176         /* Antennas for RX and management frame TX. */
3177         antenna = b43_antenna_from_ieee80211(dev, conf->antenna_sel_tx);
3178         b43_mgmtframe_txantenna(dev, antenna);
3179         antenna = b43_antenna_from_ieee80211(dev, conf->antenna_sel_rx);
3180         b43_set_rx_antenna(dev, antenna);
3181
3182         /* Update templates for AP mode. */
3183         if (b43_is_mode(wl, IEEE80211_IF_TYPE_AP))
3184                 b43_set_beacon_int(dev, conf->beacon_int);
3185
3186         if (!!conf->radio_enabled != phy->radio_on) {
3187                 if (conf->radio_enabled) {
3188                         b43_radio_turn_on(dev);
3189                         b43info(dev->wl, "Radio turned on by software\n");
3190                         if (!dev->radio_hw_enable) {
3191                                 b43info(dev->wl, "The hardware RF-kill button "
3192                                         "still turns the radio physically off. "
3193                                         "Press the button to turn it on.\n");
3194                         }
3195                 } else {
3196                         b43_radio_turn_off(dev, 0);
3197                         b43info(dev->wl, "Radio turned off by software\n");
3198                 }
3199         }
3200
3201         spin_lock_irqsave(&wl->irq_lock, flags);
3202         b43_interrupt_enable(dev, savedirqs);
3203         mmiowb();
3204         spin_unlock_irqrestore(&wl->irq_lock, flags);
3205       out_unlock_mutex:
3206         mutex_unlock(&wl->mutex);
3207
3208         return err;
3209 }
3210
3211 static int b43_op_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
3212                            const u8 *local_addr, const u8 *addr,
3213                            struct ieee80211_key_conf *key)
3214 {
3215         struct b43_wl *wl = hw_to_b43_wl(hw);
3216         struct b43_wldev *dev;
3217         unsigned long flags;
3218         u8 algorithm;
3219         u8 index;
3220         int err;
3221         DECLARE_MAC_BUF(mac);
3222
3223         if (modparam_nohwcrypt)
3224                 return -ENOSPC; /* User disabled HW-crypto */
3225
3226         mutex_lock(&wl->mutex);
3227         spin_lock_irqsave(&wl->irq_lock, flags);
3228
3229         dev = wl->current_dev;
3230         err = -ENODEV;
3231         if (!dev || b43_status(dev) < B43_STAT_INITIALIZED)
3232                 goto out_unlock;
3233
3234         err = -EINVAL;
3235         switch (key->alg) {
3236         case ALG_WEP:
3237                 if (key->keylen == 5)
3238                         algorithm = B43_SEC_ALGO_WEP40;
3239                 else
3240                         algorithm = B43_SEC_ALGO_WEP104;
3241                 break;
3242         case ALG_TKIP:
3243                 algorithm = B43_SEC_ALGO_TKIP;
3244                 break;
3245         case ALG_CCMP:
3246                 algorithm = B43_SEC_ALGO_AES;
3247                 break;
3248         default:
3249                 B43_WARN_ON(1);
3250                 goto out_unlock;
3251         }
3252         index = (u8) (key->keyidx);
3253         if (index > 3)
3254                 goto out_unlock;
3255
3256         switch (cmd) {
3257         case SET_KEY:
3258                 if (algorithm == B43_SEC_ALGO_TKIP) {
3259                         /* FIXME: No TKIP hardware encryption for now. */
3260                         err = -EOPNOTSUPP;
3261                         goto out_unlock;
3262                 }
3263
3264                 if (is_broadcast_ether_addr(addr)) {
3265                         /* addr is FF:FF:FF:FF:FF:FF for default keys */
3266                         err = b43_key_write(dev, index, algorithm,
3267                                             key->key, key->keylen, NULL, key);
3268                 } else {
3269                         /*
3270                          * either pairwise key or address is 00:00:00:00:00:00
3271                          * for transmit-only keys
3272                          */
3273                         err = b43_key_write(dev, -1, algorithm,
3274                                             key->key, key->keylen, addr, key);
3275                 }
3276                 if (err)
3277                         goto out_unlock;
3278
3279                 if (algorithm == B43_SEC_ALGO_WEP40 ||
3280                     algorithm == B43_SEC_ALGO_WEP104) {
3281                         b43_hf_write(dev, b43_hf_read(dev) | B43_HF_USEDEFKEYS);
3282                 } else {
3283                         b43_hf_write(dev,
3284                                      b43_hf_read(dev) & ~B43_HF_USEDEFKEYS);
3285                 }
3286                 key->flags |= IEEE80211_KEY_FLAG_GENERATE_IV;
3287                 break;
3288         case DISABLE_KEY: {
3289                 err = b43_key_clear(dev, key->hw_key_idx);
3290                 if (err)
3291                         goto out_unlock;
3292                 break;
3293         }
3294         default:
3295                 B43_WARN_ON(1);
3296         }
3297 out_unlock:
3298         spin_unlock_irqrestore(&wl->irq_lock, flags);
3299         mutex_unlock(&wl->mutex);
3300         if (!err) {
3301                 b43dbg(wl, "%s hardware based encryption for keyidx: %d, "
3302                        "mac: %s\n",
3303                        cmd == SET_KEY ? "Using" : "Disabling", key->keyidx,
3304                        print_mac(mac, addr));
3305         }
3306         return err;
3307 }
3308
3309 static void b43_op_configure_filter(struct ieee80211_hw *hw,
3310                                     unsigned int changed, unsigned int *fflags,
3311                                     int mc_count, struct dev_addr_list *mc_list)
3312 {
3313         struct b43_wl *wl = hw_to_b43_wl(hw);
3314         struct b43_wldev *dev = wl->current_dev;
3315         unsigned long flags;
3316
3317         if (!dev) {
3318                 *fflags = 0;
3319                 return;
3320         }
3321
3322         spin_lock_irqsave(&wl->irq_lock, flags);
3323         *fflags &= FIF_PROMISC_IN_BSS |
3324                   FIF_ALLMULTI |
3325                   FIF_FCSFAIL |
3326                   FIF_PLCPFAIL |
3327                   FIF_CONTROL |
3328                   FIF_OTHER_BSS |
3329                   FIF_BCN_PRBRESP_PROMISC;
3330
3331         changed &= FIF_PROMISC_IN_BSS |
3332                    FIF_ALLMULTI |
3333                    FIF_FCSFAIL |
3334                    FIF_PLCPFAIL |
3335                    FIF_CONTROL |
3336                    FIF_OTHER_BSS |
3337                    FIF_BCN_PRBRESP_PROMISC;
3338
3339         wl->filter_flags = *fflags;
3340
3341         if (changed && b43_status(dev) >= B43_STAT_INITIALIZED)
3342                 b43_adjust_opmode(dev);
3343         spin_unlock_irqrestore(&wl->irq_lock, flags);
3344 }
3345
3346 static int b43_op_config_interface(struct ieee80211_hw *hw,
3347                                    struct ieee80211_vif *vif,
3348                                    struct ieee80211_if_conf *conf)
3349 {
3350         struct b43_wl *wl = hw_to_b43_wl(hw);
3351         struct b43_wldev *dev = wl->current_dev;
3352         unsigned long flags;
3353
3354         if (!dev)
3355                 return -ENODEV;
3356         mutex_lock(&wl->mutex);
3357         spin_lock_irqsave(&wl->irq_lock, flags);
3358         B43_WARN_ON(wl->vif != vif);
3359         if (conf->bssid)
3360                 memcpy(wl->bssid, conf->bssid, ETH_ALEN);
3361         else
3362                 memset(wl->bssid, 0, ETH_ALEN);
3363         if (b43_status(dev) >= B43_STAT_INITIALIZED) {
3364                 if (b43_is_mode(wl, IEEE80211_IF_TYPE_AP)) {
3365                         B43_WARN_ON(conf->type != IEEE80211_IF_TYPE_AP);
3366                         b43_set_ssid(dev, conf->ssid, conf->ssid_len);
3367                         if (conf->beacon)
3368                                 b43_update_templates(wl, conf->beacon);
3369                 }
3370                 b43_write_mac_bssid_templates(dev);
3371         }
3372         spin_unlock_irqrestore(&wl->irq_lock, flags);
3373         mutex_unlock(&wl->mutex);
3374
3375         return 0;
3376 }
3377
3378 /* Locking: wl->mutex */
3379 static void b43_wireless_core_stop(struct b43_wldev *dev)
3380 {
3381         struct b43_wl *wl = dev->wl;
3382         unsigned long flags;
3383
3384         if (b43_status(dev) < B43_STAT_STARTED)
3385                 return;
3386
3387         /* Disable and sync interrupts. We must do this before than
3388          * setting the status to INITIALIZED, as the interrupt handler
3389          * won't care about IRQs then. */
3390         spin_lock_irqsave(&wl->irq_lock, flags);
3391         dev->irq_savedstate = b43_interrupt_disable(dev, B43_IRQ_ALL);
3392         b43_read32(dev, B43_MMIO_GEN_IRQ_MASK); /* flush */
3393         spin_unlock_irqrestore(&wl->irq_lock, flags);
3394         b43_synchronize_irq(dev);
3395
3396         b43_set_status(dev, B43_STAT_INITIALIZED);
3397
3398         b43_pio_stop(dev);
3399         mutex_unlock(&wl->mutex);
3400         /* Must unlock as it would otherwise deadlock. No races here.
3401          * Cancel the possibly running self-rearming periodic work. */
3402         cancel_delayed_work_sync(&dev->periodic_work);
3403         mutex_lock(&wl->mutex);
3404
3405         ieee80211_stop_queues(wl->hw);  //FIXME this could cause a deadlock, as mac80211 seems buggy.
3406
3407         b43_mac_suspend(dev);
3408         free_irq(dev->dev->irq, dev);
3409         b43dbg(wl, "Wireless interface stopped\n");
3410 }
3411
3412 /* Locking: wl->mutex */
3413 static int b43_wireless_core_start(struct b43_wldev *dev)
3414 {
3415         int err;
3416
3417         B43_WARN_ON(b43_status(dev) != B43_STAT_INITIALIZED);
3418
3419         drain_txstatus_queue(dev);
3420         err = request_irq(dev->dev->irq, b43_interrupt_handler,
3421                           IRQF_SHARED, KBUILD_MODNAME, dev);
3422         if (err) {
3423                 b43err(dev->wl, "Cannot request IRQ-%d\n", dev->dev->irq);
3424                 goto out;
3425         }
3426
3427         /* We are ready to run. */
3428         b43_set_status(dev, B43_STAT_STARTED);
3429
3430         /* Start data flow (TX/RX). */
3431         b43_mac_enable(dev);
3432         b43_interrupt_enable(dev, dev->irq_savedstate);
3433         ieee80211_start_queues(dev->wl->hw);
3434
3435         /* Start maintainance work */
3436         b43_periodic_tasks_setup(dev);
3437
3438         b43dbg(dev->wl, "Wireless interface started\n");
3439       out:
3440         return err;
3441 }
3442
3443 /* Get PHY and RADIO versioning numbers */
3444 static int b43_phy_versioning(struct b43_wldev *dev)
3445 {
3446         struct b43_phy *phy = &dev->phy;
3447         u32 tmp;
3448         u8 analog_type;
3449         u8 phy_type;
3450         u8 phy_rev;
3451         u16 radio_manuf;
3452         u16 radio_ver;
3453         u16 radio_rev;
3454         int unsupported = 0;
3455
3456         /* Get PHY versioning */
3457         tmp = b43_read16(dev, B43_MMIO_PHY_VER);
3458         analog_type = (tmp & B43_PHYVER_ANALOG) >> B43_PHYVER_ANALOG_SHIFT;
3459         phy_type = (tmp & B43_PHYVER_TYPE) >> B43_PHYVER_TYPE_SHIFT;
3460         phy_rev = (tmp & B43_PHYVER_VERSION);
3461         switch (phy_type) {
3462         case B43_PHYTYPE_A:
3463                 if (phy_rev >= 4)
3464                         unsupported = 1;
3465                 break;
3466         case B43_PHYTYPE_B:
3467                 if (phy_rev != 2 && phy_rev != 4 && phy_rev != 6
3468                     && phy_rev != 7)
3469                         unsupported = 1;
3470                 break;
3471         case B43_PHYTYPE_G:
3472                 if (phy_rev > 9)
3473                         unsupported = 1;
3474                 break;
3475 #ifdef CONFIG_B43_NPHY
3476         case B43_PHYTYPE_N:
3477                 if (phy_rev > 1)
3478                         unsupported = 1;
3479                 break;
3480 #endif
3481         default:
3482                 unsupported = 1;
3483         };
3484         if (unsupported) {
3485                 b43err(dev->wl, "FOUND UNSUPPORTED PHY "
3486                        "(Analog %u, Type %u, Revision %u)\n",
3487                        analog_type, phy_type, phy_rev);
3488                 return -EOPNOTSUPP;
3489         }
3490         b43dbg(dev->wl, "Found PHY: Analog %u, Type %u, Revision %u\n",
3491                analog_type, phy_type, phy_rev);
3492
3493         /* Get RADIO versioning */
3494         if (dev->dev->bus->chip_id == 0x4317) {
3495                 if (dev->dev->bus->chip_rev == 0)
3496                         tmp = 0x3205017F;
3497                 else if (dev->dev->bus->chip_rev == 1)
3498                         tmp = 0x4205017F;
3499                 else
3500                         tmp = 0x5205017F;
3501         } else {
3502                 b43_write16(dev, B43_MMIO_RADIO_CONTROL, B43_RADIOCTL_ID);
3503                 tmp = b43_read16(dev, B43_MMIO_RADIO_DATA_LOW);
3504                 b43_write16(dev, B43_MMIO_RADIO_CONTROL, B43_RADIOCTL_ID);
3505                 tmp |= (u32)b43_read16(dev, B43_MMIO_RADIO_DATA_HIGH) << 16;
3506         }
3507         radio_manuf = (tmp & 0x00000FFF);
3508         radio_ver = (tmp & 0x0FFFF000) >> 12;
3509         radio_rev = (tmp & 0xF0000000) >> 28;
3510         if (radio_manuf != 0x17F /* Broadcom */)
3511                 unsupported = 1;
3512         switch (phy_type) {
3513         case B43_PHYTYPE_A:
3514                 if (radio_ver != 0x2060)
3515                         unsupported = 1;
3516                 if (radio_rev != 1)
3517                         unsupported = 1;
3518                 if (radio_manuf != 0x17F)
3519                         unsupported = 1;
3520                 break;
3521         case B43_PHYTYPE_B:
3522                 if ((radio_ver & 0xFFF0) != 0x2050)
3523                         unsupported = 1;
3524                 break;
3525         case B43_PHYTYPE_G:
3526                 if (radio_ver != 0x2050)
3527                         unsupported = 1;
3528                 break;
3529         case B43_PHYTYPE_N:
3530                 if (radio_ver != 0x2055)
3531                         unsupported = 1;
3532                 break;
3533         default:
3534                 B43_WARN_ON(1);
3535         }
3536         if (unsupported) {
3537                 b43err(dev->wl, "FOUND UNSUPPORTED RADIO "
3538                        "(Manuf 0x%X, Version 0x%X, Revision %u)\n",
3539                        radio_manuf, radio_ver, radio_rev);
3540                 return -EOPNOTSUPP;
3541         }
3542         b43dbg(dev->wl, "Found Radio: Manuf 0x%X, Version 0x%X, Revision %u\n",
3543                radio_manuf, radio_ver, radio_rev);
3544
3545         phy->radio_manuf = radio_manuf;
3546         phy->radio_ver = radio_ver;
3547         phy->radio_rev = radio_rev;
3548
3549         phy->analog = analog_type;
3550         phy->type = phy_type;
3551         phy->rev = phy_rev;
3552
3553         return 0;
3554 }
3555
3556 static void setup_struct_phy_for_init(struct b43_wldev *dev,
3557                                       struct b43_phy *phy)
3558 {
3559         struct b43_txpower_lo_control *lo;
3560         int i;
3561
3562         memset(phy->minlowsig, 0xFF, sizeof(phy->minlowsig));
3563         memset(phy->minlowsigpos, 0, sizeof(phy->minlowsigpos));
3564
3565         phy->aci_enable = 0;
3566         phy->aci_wlan_automatic = 0;
3567         phy->aci_hw_rssi = 0;
3568
3569         phy->radio_off_context.valid = 0;
3570
3571         lo = phy->lo_control;
3572         if (lo) {
3573                 memset(lo, 0, sizeof(*(phy->lo_control)));
3574                 lo->rebuild = 1;
3575                 lo->tx_bias = 0xFF;
3576         }
3577         phy->max_lb_gain = 0;
3578         phy->trsw_rx_gain = 0;
3579         phy->txpwr_offset = 0;
3580
3581         /* NRSSI */
3582         phy->nrssislope = 0;
3583         for (i = 0; i < ARRAY_SIZE(phy->nrssi); i++)
3584                 phy->nrssi[i] = -1000;
3585         for (i = 0; i < ARRAY_SIZE(phy->nrssi_lt); i++)
3586                 phy->nrssi_lt[i] = i;
3587
3588         phy->lofcal = 0xFFFF;
3589         phy->initval = 0xFFFF;
3590
3591         phy->interfmode = B43_INTERFMODE_NONE;
3592         phy->channel = 0xFF;
3593
3594         phy->hardware_power_control = !!modparam_hwpctl;
3595
3596         /* PHY TX errors counter. */
3597         atomic_set(&phy->txerr_cnt, B43_PHY_TX_BADNESS_LIMIT);
3598
3599         /* OFDM-table address caching. */
3600         phy->ofdmtab_addr_direction = B43_OFDMTAB_DIRECTION_UNKNOWN;
3601 }
3602
3603 static void setup_struct_wldev_for_init(struct b43_wldev *dev)
3604 {
3605         dev->dfq_valid = 0;
3606
3607         /* Assume the radio is enabled. If it's not enabled, the state will
3608          * immediately get fixed on the first periodic work run. */
3609         dev->radio_hw_enable = 1;
3610
3611         /* Stats */
3612         memset(&dev->stats, 0, sizeof(dev->stats));
3613
3614         setup_struct_phy_for_init(dev, &dev->phy);
3615
3616         /* IRQ related flags */
3617         dev->irq_reason = 0;
3618         memset(dev->dma_reason, 0, sizeof(dev->dma_reason));
3619         dev->irq_savedstate = B43_IRQ_MASKTEMPLATE;
3620
3621         dev->mac_suspended = 1;
3622
3623         /* Noise calculation context */
3624         memset(&dev->noisecalc, 0, sizeof(dev->noisecalc));
3625 }
3626
3627 static void b43_bluetooth_coext_enable(struct b43_wldev *dev)
3628 {
3629         struct ssb_sprom *sprom = &dev->dev->bus->sprom;
3630         u32 hf;
3631
3632         if (!(sprom->boardflags_lo & B43_BFL_BTCOEXIST))
3633                 return;
3634         if (dev->phy.type != B43_PHYTYPE_B && !dev->phy.gmode)
3635                 return;
3636
3637         hf = b43_hf_read(dev);
3638         if (sprom->boardflags_lo & B43_BFL_BTCMOD)
3639                 hf |= B43_HF_BTCOEXALT;
3640         else
3641                 hf |= B43_HF_BTCOEX;
3642         b43_hf_write(dev, hf);
3643         //TODO
3644 }
3645
3646 static void b43_bluetooth_coext_disable(struct b43_wldev *dev)
3647 {                               //TODO
3648 }
3649
3650 static void b43_imcfglo_timeouts_workaround(struct b43_wldev *dev)
3651 {
3652 #ifdef CONFIG_SSB_DRIVER_PCICORE
3653         struct ssb_bus *bus = dev->dev->bus;
3654         u32 tmp;
3655
3656         if (bus->pcicore.dev &&
3657             bus->pcicore.dev->id.coreid == SSB_DEV_PCI &&
3658             bus->pcicore.dev->id.revision <= 5) {
3659                 /* IMCFGLO timeouts workaround. */
3660                 tmp = ssb_read32(dev->dev, SSB_IMCFGLO);
3661                 tmp &= ~SSB_IMCFGLO_REQTO;
3662                 tmp &= ~SSB_IMCFGLO_SERTO;
3663                 switch (bus->bustype) {
3664                 case SSB_BUSTYPE_PCI:
3665                 case SSB_BUSTYPE_PCMCIA:
3666                         tmp |= 0x32;
3667                         break;
3668                 case SSB_BUSTYPE_SSB:
3669                         tmp |= 0x53;
3670                         break;
3671                 }
3672                 ssb_write32(dev->dev, SSB_IMCFGLO, tmp);
3673         }
3674 #endif /* CONFIG_SSB_DRIVER_PCICORE */
3675 }
3676
3677 /* Write the short and long frame retry limit values. */
3678 static void b43_set_retry_limits(struct b43_wldev *dev,
3679                                  unsigned int short_retry,
3680                                  unsigned int long_retry)
3681 {
3682         /* The retry limit is a 4-bit counter. Enforce this to avoid overflowing
3683          * the chip-internal counter. */
3684         short_retry = min(short_retry, (unsigned int)0xF);
3685         long_retry = min(long_retry, (unsigned int)0xF);
3686
3687         b43_shm_write16(dev, B43_SHM_SCRATCH, B43_SHM_SC_SRLIMIT,
3688                         short_retry);
3689         b43_shm_write16(dev, B43_SHM_SCRATCH, B43_SHM_SC_LRLIMIT,
3690                         long_retry);
3691 }
3692
3693 /* Shutdown a wireless core */
3694 /* Locking: wl->mutex */
3695 static void b43_wireless_core_exit(struct b43_wldev *dev)
3696 {
3697         struct b43_phy *phy = &dev->phy;
3698         u32 macctl;
3699
3700         B43_WARN_ON(b43_status(dev) > B43_STAT_INITIALIZED);
3701         if (b43_status(dev) != B43_STAT_INITIALIZED)
3702                 return;
3703         b43_set_status(dev, B43_STAT_UNINIT);
3704
3705         /* Stop the microcode PSM. */
3706         macctl = b43_read32(dev, B43_MMIO_MACCTL);
3707         macctl &= ~B43_MACCTL_PSM_RUN;
3708         macctl |= B43_MACCTL_PSM_JMP0;
3709         b43_write32(dev, B43_MMIO_MACCTL, macctl);
3710
3711         if (!dev->suspend_in_progress) {
3712                 b43_leds_exit(dev);
3713                 b43_rng_exit(dev->wl, false);
3714         }
3715         b43_dma_free(dev);
3716         b43_pio_free(dev);
3717         b43_chip_exit(dev);
3718         b43_radio_turn_off(dev, 1);
3719         b43_switch_analog(dev, 0);
3720         if (phy->dyn_tssi_tbl)
3721                 kfree(phy->tssi2dbm);
3722         kfree(phy->lo_control);
3723         phy->lo_control = NULL;
3724         if (dev->wl->current_beacon) {
3725                 dev_kfree_skb_any(dev->wl->current_beacon);
3726                 dev->wl->current_beacon = NULL;
3727         }
3728
3729         ssb_device_disable(dev->dev, 0);
3730         ssb_bus_may_powerdown(dev->dev->bus);
3731 }
3732
3733 /* Initialize a wireless core */
3734 static int b43_wireless_core_init(struct b43_wldev *dev)
3735 {
3736         struct b43_wl *wl = dev->wl;
3737         struct ssb_bus *bus = dev->dev->bus;
3738         struct ssb_sprom *sprom = &bus->sprom;
3739         struct b43_phy *phy = &dev->phy;
3740         int err;
3741         u32 hf, tmp;
3742
3743         B43_WARN_ON(b43_status(dev) != B43_STAT_UNINIT);
3744
3745         err = ssb_bus_powerup(bus, 0);
3746         if (err)
3747                 goto out;
3748         if (!ssb_device_is_enabled(dev->dev)) {
3749                 tmp = phy->gmode ? B43_TMSLOW_GMODE : 0;
3750                 b43_wireless_core_reset(dev, tmp);
3751         }
3752
3753         if ((phy->type == B43_PHYTYPE_B) || (phy->type == B43_PHYTYPE_G)) {
3754                 phy->lo_control =
3755                     kzalloc(sizeof(*(phy->lo_control)), GFP_KERNEL);
3756                 if (!phy->lo_control) {
3757                         err = -ENOMEM;
3758                         goto err_busdown;
3759                 }
3760         }
3761         setup_struct_wldev_for_init(dev);
3762
3763         err = b43_phy_init_tssi2dbm_table(dev);
3764         if (err)
3765                 goto err_kfree_lo_control;
3766
3767         /* Enable IRQ routing to this device. */
3768         ssb_pcicore_dev_irqvecs_enable(&bus->pcicore, dev->dev);
3769
3770         b43_imcfglo_timeouts_workaround(dev);
3771         b43_bluetooth_coext_disable(dev);
3772         b43_phy_early_init(dev);
3773         err = b43_chip_init(dev);
3774         if (err)
3775                 goto err_kfree_tssitbl;
3776         b43_shm_write16(dev, B43_SHM_SHARED,
3777                         B43_SHM_SH_WLCOREREV, dev->dev->id.revision);
3778         hf = b43_hf_read(dev);
3779         if (phy->type == B43_PHYTYPE_G) {
3780                 hf |= B43_HF_SYMW;
3781                 if (phy->rev == 1)
3782                         hf |= B43_HF_GDCW;
3783                 if (sprom->boardflags_lo & B43_BFL_PACTRL)
3784                         hf |= B43_HF_OFDMPABOOST;
3785         } else if (phy->type == B43_PHYTYPE_B) {
3786                 hf |= B43_HF_SYMW;
3787                 if (phy->rev >= 2 && phy->radio_ver == 0x2050)
3788                         hf &= ~B43_HF_GDCW;
3789         }
3790         b43_hf_write(dev, hf);
3791
3792         b43_set_retry_limits(dev, B43_DEFAULT_SHORT_RETRY_LIMIT,
3793                              B43_DEFAULT_LONG_RETRY_LIMIT);
3794         b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_SFFBLIM, 3);
3795         b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_LFFBLIM, 2);
3796
3797         /* Disable sending probe responses from firmware.
3798          * Setting the MaxTime to one usec will always trigger
3799          * a timeout, so we never send any probe resp.
3800          * A timeout of zero is infinite. */
3801         b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_PRMAXTIME, 1);
3802
3803         b43_rate_memory_init(dev);
3804
3805         /* Minimum Contention Window */
3806         if (phy->type == B43_PHYTYPE_B) {
3807                 b43_shm_write16(dev, B43_SHM_SCRATCH, B43_SHM_SC_MINCONT, 0x1F);
3808         } else {
3809                 b43_shm_write16(dev, B43_SHM_SCRATCH, B43_SHM_SC_MINCONT, 0xF);
3810         }
3811         /* Maximum Contention Window */
3812         b43_shm_write16(dev, B43_SHM_SCRATCH, B43_SHM_SC_MAXCONT, 0x3FF);
3813
3814         if ((dev->dev->bus->bustype == SSB_BUSTYPE_PCMCIA) || B43_FORCE_PIO) {
3815                 dev->__using_pio_transfers = 1;
3816                 err = b43_pio_init(dev);
3817         } else {
3818                 dev->__using_pio_transfers = 0;
3819                 err = b43_dma_init(dev);
3820         }
3821         if (err)
3822                 goto err_chip_exit;
3823         b43_qos_init(dev);
3824
3825 //FIXME
3826 #if 1
3827         b43_write16(dev, 0x0612, 0x0050);
3828         b43_shm_write16(dev, B43_SHM_SHARED, 0x0416, 0x0050);
3829         b43_shm_write16(dev, B43_SHM_SHARED, 0x0414, 0x01F4);
3830 #endif
3831
3832         b43_bluetooth_coext_enable(dev);
3833
3834         ssb_bus_powerup(bus, 1);        /* Enable dynamic PCTL */
3835         b43_upload_card_macaddress(dev);
3836         b43_security_init(dev);
3837         if (!dev->suspend_in_progress)
3838                 b43_rng_init(wl);
3839
3840         b43_set_status(dev, B43_STAT_INITIALIZED);
3841
3842         if (!dev->suspend_in_progress)
3843                 b43_leds_init(dev);
3844 out:
3845         return err;
3846
3847       err_chip_exit:
3848         b43_chip_exit(dev);
3849       err_kfree_tssitbl:
3850         if (phy->dyn_tssi_tbl)
3851                 kfree(phy->tssi2dbm);
3852       err_kfree_lo_control:
3853         kfree(phy->lo_control);
3854         phy->lo_control = NULL;
3855       err_busdown:
3856         ssb_bus_may_powerdown(bus);
3857         B43_WARN_ON(b43_status(dev) != B43_STAT_UNINIT);
3858         return err;
3859 }
3860
3861 static int b43_op_add_interface(struct ieee80211_hw *hw,
3862                                 struct ieee80211_if_init_conf *conf)
3863 {
3864         struct b43_wl *wl = hw_to_b43_wl(hw);
3865         struct b43_wldev *dev;
3866         unsigned long flags;
3867         int err = -EOPNOTSUPP;
3868
3869         /* TODO: allow WDS/AP devices to coexist */
3870
3871         if (conf->type != IEEE80211_IF_TYPE_AP &&
3872             conf->type != IEEE80211_IF_TYPE_STA &&
3873             conf->type != IEEE80211_IF_TYPE_WDS &&
3874             conf->type != IEEE80211_IF_TYPE_IBSS)
3875                 return -EOPNOTSUPP;
3876
3877         mutex_lock(&wl->mutex);
3878         if (wl->operating)
3879                 goto out_mutex_unlock;
3880
3881         b43dbg(wl, "Adding Interface type %d\n", conf->type);
3882
3883         dev = wl->current_dev;
3884         wl->operating = 1;
3885         wl->vif = conf->vif;
3886         wl->if_type = conf->type;
3887         memcpy(wl->mac_addr, conf->mac_addr, ETH_ALEN);
3888
3889         spin_lock_irqsave(&wl->irq_lock, flags);
3890         b43_adjust_opmode(dev);
3891         b43_upload_card_macaddress(dev);
3892         spin_unlock_irqrestore(&wl->irq_lock, flags);
3893
3894         err = 0;
3895  out_mutex_unlock:
3896         mutex_unlock(&wl->mutex);
3897
3898         return err;
3899 }
3900
3901 static void b43_op_remove_interface(struct ieee80211_hw *hw,
3902                                     struct ieee80211_if_init_conf *conf)
3903 {
3904         struct b43_wl *wl = hw_to_b43_wl(hw);
3905         struct b43_wldev *dev = wl->current_dev;
3906         unsigned long flags;
3907
3908         b43dbg(wl, "Removing Interface type %d\n", conf->type);
3909
3910         mutex_lock(&wl->mutex);
3911
3912         B43_WARN_ON(!wl->operating);
3913         B43_WARN_ON(wl->vif != conf->vif);
3914         wl->vif = NULL;
3915
3916         wl->operating = 0;
3917
3918         spin_lock_irqsave(&wl->irq_lock, flags);
3919         b43_adjust_opmode(dev);
3920         memset(wl->mac_addr, 0, ETH_ALEN);
3921         b43_upload_card_macaddress(dev);
3922         spin_unlock_irqrestore(&wl->irq_lock, flags);
3923
3924         mutex_unlock(&wl->mutex);
3925 }
3926
3927 static int b43_op_start(struct ieee80211_hw *hw)
3928 {
3929         struct b43_wl *wl = hw_to_b43_wl(hw);
3930         struct b43_wldev *dev = wl->current_dev;
3931         int did_init = 0;
3932         int err = 0;
3933         bool do_rfkill_exit = 0;
3934
3935         /* Kill all old instance specific information to make sure
3936          * the card won't use it in the short timeframe between start
3937          * and mac80211 reconfiguring it. */
3938         memset(wl->bssid, 0, ETH_ALEN);
3939         memset(wl->mac_addr, 0, ETH_ALEN);
3940         wl->filter_flags = 0;
3941         wl->radiotap_enabled = 0;
3942         b43_qos_clear(wl);
3943
3944         /* First register RFkill.
3945          * LEDs that are registered later depend on it. */
3946         b43_rfkill_init(dev);
3947
3948         mutex_lock(&wl->mutex);
3949
3950         if (b43_status(dev) < B43_STAT_INITIALIZED) {
3951                 err = b43_wireless_core_init(dev);
3952                 if (err) {
3953                         do_rfkill_exit = 1;
3954                         goto out_mutex_unlock;
3955                 }
3956                 did_init = 1;
3957         }
3958
3959         if (b43_status(dev) < B43_STAT_STARTED) {
3960                 err = b43_wireless_core_start(dev);
3961                 if (err) {
3962                         if (did_init)
3963                                 b43_wireless_core_exit(dev);
3964                         do_rfkill_exit = 1;
3965                         goto out_mutex_unlock;
3966                 }
3967         }
3968
3969  out_mutex_unlock:
3970         mutex_unlock(&wl->mutex);
3971
3972         if (do_rfkill_exit)
3973                 b43_rfkill_exit(dev);
3974
3975         return err;
3976 }
3977
3978 static void b43_op_stop(struct ieee80211_hw *hw)
3979 {
3980         struct b43_wl *wl = hw_to_b43_wl(hw);
3981         struct b43_wldev *dev = wl->current_dev;
3982
3983         b43_rfkill_exit(dev);
3984         cancel_work_sync(&(wl->qos_update_work));
3985
3986         mutex_lock(&wl->mutex);
3987         if (b43_status(dev) >= B43_STAT_STARTED)
3988                 b43_wireless_core_stop(dev);
3989         b43_wireless_core_exit(dev);
3990         mutex_unlock(&wl->mutex);
3991 }
3992
3993 static int b43_op_set_retry_limit(struct ieee80211_hw *hw,
3994                                   u32 short_retry_limit, u32 long_retry_limit)
3995 {
3996         struct b43_wl *wl = hw_to_b43_wl(hw);
3997         struct b43_wldev *dev;
3998         int err = 0;
3999
4000         mutex_lock(&wl->mutex);
4001         dev = wl->current_dev;
4002         if (unlikely(!dev || (b43_status(dev) < B43_STAT_INITIALIZED))) {
4003                 err = -ENODEV;
4004                 goto out_unlock;
4005         }
4006         b43_set_retry_limits(dev, short_retry_limit, long_retry_limit);
4007 out_unlock:
4008         mutex_unlock(&wl->mutex);
4009
4010         return err;
4011 }
4012
4013 static int b43_op_beacon_set_tim(struct ieee80211_hw *hw, int aid, int set)
4014 {
4015         struct b43_wl *wl = hw_to_b43_wl(hw);
4016         struct sk_buff *beacon;
4017         unsigned long flags;
4018
4019         /* We could modify the existing beacon and set the aid bit in
4020          * the TIM field, but that would probably require resizing and
4021          * moving of data within the beacon template.
4022          * Simply request a new beacon and let mac80211 do the hard work. */
4023         beacon = ieee80211_beacon_get(hw, wl->vif, NULL);
4024         if (unlikely(!beacon))
4025                 return -ENOMEM;
4026         spin_lock_irqsave(&wl->irq_lock, flags);
4027         b43_update_templates(wl, beacon);
4028         spin_unlock_irqrestore(&wl->irq_lock, flags);
4029
4030         return 0;
4031 }
4032
4033 static int b43_op_ibss_beacon_update(struct ieee80211_hw *hw,
4034                                      struct sk_buff *beacon,
4035                                      struct ieee80211_tx_control *ctl)
4036 {
4037         struct b43_wl *wl = hw_to_b43_wl(hw);
4038         unsigned long flags;
4039
4040         spin_lock_irqsave(&wl->irq_lock, flags);
4041         b43_update_templates(wl, beacon);
4042         spin_unlock_irqrestore(&wl->irq_lock, flags);
4043
4044         return 0;
4045 }
4046
4047 static void b43_op_sta_notify(struct ieee80211_hw *hw,
4048                               struct ieee80211_vif *vif,
4049                               enum sta_notify_cmd notify_cmd,
4050                               const u8 *addr)
4051 {
4052         struct b43_wl *wl = hw_to_b43_wl(hw);
4053
4054         B43_WARN_ON(!vif || wl->vif != vif);
4055 }
4056
4057 static const struct ieee80211_ops b43_hw_ops = {
4058         .tx                     = b43_op_tx,
4059         .conf_tx                = b43_op_conf_tx,
4060         .add_interface          = b43_op_add_interface,
4061         .remove_interface       = b43_op_remove_interface,
4062         .config                 = b43_op_config,
4063         .config_interface       = b43_op_config_interface,
4064         .configure_filter       = b43_op_configure_filter,
4065         .set_key                = b43_op_set_key,
4066         .get_stats              = b43_op_get_stats,
4067         .get_tx_stats           = b43_op_get_tx_stats,
4068         .start                  = b43_op_start,
4069         .stop                   = b43_op_stop,
4070         .set_retry_limit        = b43_op_set_retry_limit,
4071         .set_tim                = b43_op_beacon_set_tim,
4072         .beacon_update          = b43_op_ibss_beacon_update,
4073         .sta_notify             = b43_op_sta_notify,
4074 };
4075
4076 /* Hard-reset the chip. Do not call this directly.
4077  * Use b43_controller_restart()
4078  */
4079 static void b43_chip_reset(struct work_struct *work)
4080 {
4081         struct b43_wldev *dev =
4082             container_of(work, struct b43_wldev, restart_work);
4083         struct b43_wl *wl = dev->wl;
4084         int err = 0;
4085         int prev_status;
4086
4087         mutex_lock(&wl->mutex);
4088
4089         prev_status = b43_status(dev);
4090         /* Bring the device down... */
4091         if (prev_status >= B43_STAT_STARTED)
4092                 b43_wireless_core_stop(dev);
4093         if (prev_status >= B43_STAT_INITIALIZED)
4094                 b43_wireless_core_exit(dev);
4095
4096         /* ...and up again. */
4097         if (prev_status >= B43_STAT_INITIALIZED) {
4098                 err = b43_wireless_core_init(dev);
4099                 if (err)
4100                         goto out;
4101         }
4102         if (prev_status >= B43_STAT_STARTED) {
4103                 err = b43_wireless_core_start(dev);
4104                 if (err) {
4105                         b43_wireless_core_exit(dev);
4106                         goto out;
4107                 }
4108         }
4109       out:
4110         mutex_unlock(&wl->mutex);
4111         if (err)
4112                 b43err(wl, "Controller restart FAILED\n");
4113         else
4114                 b43info(wl, "Controller restarted\n");
4115 }
4116
4117 static int b43_setup_bands(struct b43_wldev *dev,
4118                            bool have_2ghz_phy, bool have_5ghz_phy)
4119 {
4120         struct ieee80211_hw *hw = dev->wl->hw;
4121
4122         if (have_2ghz_phy)
4123                 hw->wiphy->bands[IEEE80211_BAND_2GHZ] = &b43_band_2GHz;
4124         if (dev->phy.type == B43_PHYTYPE_N) {
4125                 if (have_5ghz_phy)
4126                         hw->wiphy->bands[IEEE80211_BAND_5GHZ] = &b43_band_5GHz_nphy;
4127         } else {
4128                 if (have_5ghz_phy)
4129                         hw->wiphy->bands[IEEE80211_BAND_5GHZ] = &b43_band_5GHz_aphy;
4130         }
4131
4132         dev->phy.supports_2ghz = have_2ghz_phy;
4133         dev->phy.supports_5ghz = have_5ghz_phy;
4134
4135         return 0;
4136 }
4137
4138 static void b43_wireless_core_detach(struct b43_wldev *dev)
4139 {
4140         /* We release firmware that late to not be required to re-request
4141          * is all the time when we reinit the core. */
4142         b43_release_firmware(dev);
4143 }
4144
4145 static int b43_wireless_core_attach(struct b43_wldev *dev)
4146 {
4147         struct b43_wl *wl = dev->wl;
4148         struct ssb_bus *bus = dev->dev->bus;
4149         struct pci_dev *pdev = bus->host_pci;
4150         int err;
4151         bool have_2ghz_phy = 0, have_5ghz_phy = 0;
4152         u32 tmp;
4153
4154         /* Do NOT do any device initialization here.
4155          * Do it in wireless_core_init() instead.
4156          * This function is for gathering basic information about the HW, only.
4157          * Also some structs may be set up here. But most likely you want to have
4158          * that in core_init(), too.
4159          */
4160
4161         err = ssb_bus_powerup(bus, 0);
4162         if (err) {
4163                 b43err(wl, "Bus powerup failed\n");
4164                 goto out;
4165         }
4166         /* Get the PHY type. */
4167         if (dev->dev->id.revision >= 5) {
4168                 u32 tmshigh;
4169
4170                 tmshigh = ssb_read32(dev->dev, SSB_TMSHIGH);
4171                 have_2ghz_phy = !!(tmshigh & B43_TMSHIGH_HAVE_2GHZ_PHY);
4172                 have_5ghz_phy = !!(tmshigh & B43_TMSHIGH_HAVE_5GHZ_PHY);
4173         } else
4174                 B43_WARN_ON(1);
4175
4176         dev->phy.gmode = have_2ghz_phy;
4177         tmp = dev->phy.gmode ? B43_TMSLOW_GMODE : 0;
4178         b43_wireless_core_reset(dev, tmp);
4179
4180         err = b43_phy_versioning(dev);
4181         if (err)
4182                 goto err_powerdown;
4183         /* Check if this device supports multiband. */
4184         if (!pdev ||
4185             (pdev->device != 0x4312 &&
4186              pdev->device != 0x4319 && pdev->device != 0x4324)) {
4187                 /* No multiband support. */
4188                 have_2ghz_phy = 0;
4189                 have_5ghz_phy = 0;
4190                 switch (dev->phy.type) {
4191                 case B43_PHYTYPE_A:
4192                         have_5ghz_phy = 1;
4193                         break;
4194                 case B43_PHYTYPE_G:
4195                 case B43_PHYTYPE_N:
4196                         have_2ghz_phy = 1;
4197                         break;
4198                 default:
4199                         B43_WARN_ON(1);
4200                 }
4201         }
4202         if (dev->phy.type == B43_PHYTYPE_A) {
4203                 /* FIXME */
4204                 b43err(wl, "IEEE 802.11a devices are unsupported\n");
4205                 err = -EOPNOTSUPP;
4206                 goto err_powerdown;
4207         }
4208         dev->phy.gmode = have_2ghz_phy;
4209         tmp = dev->phy.gmode ? B43_TMSLOW_GMODE : 0;
4210         b43_wireless_core_reset(dev, tmp);
4211
4212         err = b43_validate_chipaccess(dev);
4213         if (err)
4214                 goto err_powerdown;
4215         err = b43_setup_bands(dev, have_2ghz_phy, have_5ghz_phy);
4216         if (err)
4217                 goto err_powerdown;
4218
4219         /* Now set some default "current_dev" */
4220         if (!wl->current_dev)
4221                 wl->current_dev = dev;
4222         INIT_WORK(&dev->restart_work, b43_chip_reset);
4223
4224         b43_radio_turn_off(dev, 1);
4225         b43_switch_analog(dev, 0);
4226         ssb_device_disable(dev->dev, 0);
4227         ssb_bus_may_powerdown(bus);
4228
4229 out:
4230         return err;
4231
4232 err_powerdown:
4233         ssb_bus_may_powerdown(bus);
4234         return err;
4235 }
4236
4237 static void b43_one_core_detach(struct ssb_device *dev)
4238 {
4239         struct b43_wldev *wldev;
4240         struct b43_wl *wl;
4241
4242         wldev = ssb_get_drvdata(dev);
4243         wl = wldev->wl;
4244         cancel_work_sync(&wldev->restart_work);
4245         b43_debugfs_remove_device(wldev);
4246         b43_wireless_core_detach(wldev);
4247         list_del(&wldev->list);
4248         wl->nr_devs--;
4249         ssb_set_drvdata(dev, NULL);
4250         kfree(wldev);
4251 }
4252
4253 static int b43_one_core_attach(struct ssb_device *dev, struct b43_wl *wl)
4254 {
4255         struct b43_wldev *wldev;
4256         struct pci_dev *pdev;
4257         int err = -ENOMEM;
4258
4259         if (!list_empty(&wl->devlist)) {
4260                 /* We are not the first core on this chip. */
4261                 pdev = dev->bus->host_pci;
4262                 /* Only special chips support more than one wireless
4263                  * core, although some of the other chips have more than
4264                  * one wireless core as well. Check for this and
4265                  * bail out early.
4266                  */
4267                 if (!pdev ||
4268                     ((pdev->device != 0x4321) &&
4269                      (pdev->device != 0x4313) && (pdev->device != 0x431A))) {
4270                         b43dbg(wl, "Ignoring unconnected 802.11 core\n");
4271                         return -ENODEV;
4272                 }
4273         }
4274
4275         wldev = kzalloc(sizeof(*wldev), GFP_KERNEL);
4276         if (!wldev)
4277                 goto out;
4278
4279         wldev->dev = dev;
4280         wldev->wl = wl;
4281         b43_set_status(wldev, B43_STAT_UNINIT);
4282         wldev->bad_frames_preempt = modparam_bad_frames_preempt;
4283         tasklet_init(&wldev->isr_tasklet,
4284                      (void (*)(unsigned long))b43_interrupt_tasklet,
4285                      (unsigned long)wldev);
4286         INIT_LIST_HEAD(&wldev->list);
4287
4288         err = b43_wireless_core_attach(wldev);
4289         if (err)
4290                 goto err_kfree_wldev;
4291
4292         list_add(&wldev->list, &wl->devlist);
4293         wl->nr_devs++;
4294         ssb_set_drvdata(dev, wldev);
4295         b43_debugfs_add_device(wldev);
4296
4297       out:
4298         return err;
4299
4300       err_kfree_wldev:
4301         kfree(wldev);
4302         return err;
4303 }
4304
4305 static void b43_sprom_fixup(struct ssb_bus *bus)
4306 {
4307         /* boardflags workarounds */
4308         if (bus->boardinfo.vendor == SSB_BOARDVENDOR_DELL &&
4309             bus->chip_id == 0x4301 && bus->boardinfo.rev == 0x74)
4310                 bus->sprom.boardflags_lo |= B43_BFL_BTCOEXIST;
4311         if (bus->boardinfo.vendor == PCI_VENDOR_ID_APPLE &&
4312             bus->boardinfo.type == 0x4E && bus->boardinfo.rev > 0x40)
4313                 bus->sprom.boardflags_lo |= B43_BFL_PACTRL;
4314 }
4315
4316 static void b43_wireless_exit(struct ssb_device *dev, struct b43_wl *wl)
4317 {
4318         struct ieee80211_hw *hw = wl->hw;
4319
4320         ssb_set_devtypedata(dev, NULL);
4321         ieee80211_free_hw(hw);
4322 }
4323
4324 static int b43_wireless_init(struct ssb_device *dev)
4325 {
4326         struct ssb_sprom *sprom = &dev->bus->sprom;
4327         struct ieee80211_hw *hw;
4328         struct b43_wl *wl;
4329         int err = -ENOMEM;
4330
4331         b43_sprom_fixup(dev->bus);
4332
4333         hw = ieee80211_alloc_hw(sizeof(*wl), &b43_hw_ops);
4334         if (!hw) {
4335                 b43err(NULL, "Could not allocate ieee80211 device\n");
4336                 goto out;
4337         }
4338
4339         /* fill hw info */
4340         hw->flags = IEEE80211_HW_HOST_GEN_BEACON_TEMPLATE |
4341                     IEEE80211_HW_RX_INCLUDES_FCS;
4342         hw->max_signal = 100;
4343         hw->max_rssi = -110;
4344         hw->max_noise = -110;
4345         hw->queues = b43_modparam_qos ? 4 : 1;
4346         SET_IEEE80211_DEV(hw, dev->dev);
4347         if (is_valid_ether_addr(sprom->et1mac))
4348                 SET_IEEE80211_PERM_ADDR(hw, sprom->et1mac);
4349         else
4350                 SET_IEEE80211_PERM_ADDR(hw, sprom->il0mac);
4351
4352         /* Get and initialize struct b43_wl */
4353         wl = hw_to_b43_wl(hw);
4354         memset(wl, 0, sizeof(*wl));
4355         wl->hw = hw;
4356         spin_lock_init(&wl->irq_lock);
4357         spin_lock_init(&wl->leds_lock);
4358         spin_lock_init(&wl->shm_lock);
4359         mutex_init(&wl->mutex);
4360         INIT_LIST_HEAD(&wl->devlist);
4361         INIT_WORK(&wl->qos_update_work, b43_qos_update_work);
4362
4363         ssb_set_devtypedata(dev, wl);
4364         b43info(wl, "Broadcom %04X WLAN found\n", dev->bus->chip_id);
4365         err = 0;
4366       out:
4367         return err;
4368 }
4369
4370 static int b43_probe(struct ssb_device *dev, const struct ssb_device_id *id)
4371 {
4372         struct b43_wl *wl;
4373         int err;
4374         int first = 0;
4375
4376         wl = ssb_get_devtypedata(dev);
4377         if (!wl) {
4378                 /* Probing the first core. Must setup common struct b43_wl */
4379                 first = 1;
4380                 err = b43_wireless_init(dev);
4381                 if (err)
4382                         goto out;
4383                 wl = ssb_get_devtypedata(dev);
4384                 B43_WARN_ON(!wl);
4385         }
4386         err = b43_one_core_attach(dev, wl);
4387         if (err)
4388                 goto err_wireless_exit;
4389
4390         if (first) {
4391                 err = ieee80211_register_hw(wl->hw);
4392                 if (err)
4393                         goto err_one_core_detach;
4394         }
4395
4396       out:
4397         return err;
4398
4399       err_one_core_detach:
4400         b43_one_core_detach(dev);
4401       err_wireless_exit:
4402         if (first)
4403                 b43_wireless_exit(dev, wl);
4404         return err;
4405 }
4406
4407 static void b43_remove(struct ssb_device *dev)
4408 {
4409         struct b43_wl *wl = ssb_get_devtypedata(dev);
4410         struct b43_wldev *wldev = ssb_get_drvdata(dev);
4411
4412         B43_WARN_ON(!wl);
4413         if (wl->current_dev == wldev)
4414                 ieee80211_unregister_hw(wl->hw);
4415
4416         b43_one_core_detach(dev);
4417
4418         if (list_empty(&wl->devlist)) {
4419                 /* Last core on the chip unregistered.
4420                  * We can destroy common struct b43_wl.
4421                  */
4422                 b43_wireless_exit(dev, wl);
4423         }
4424 }
4425
4426 /* Perform a hardware reset. This can be called from any context. */
4427 void b43_controller_restart(struct b43_wldev *dev, const char *reason)
4428 {
4429         /* Must avoid requeueing, if we are in shutdown. */
4430         if (b43_status(dev) < B43_STAT_INITIALIZED)
4431                 return;
4432         b43info(dev->wl, "Controller RESET (%s) ...\n", reason);
4433         queue_work(dev->wl->hw->workqueue, &dev->restart_work);
4434 }
4435
4436 #ifdef CONFIG_PM
4437
4438 static int b43_suspend(struct ssb_device *dev, pm_message_t state)
4439 {
4440         struct b43_wldev *wldev = ssb_get_drvdata(dev);
4441         struct b43_wl *wl = wldev->wl;
4442
4443         b43dbg(wl, "Suspending...\n");
4444
4445         mutex_lock(&wl->mutex);
4446         wldev->suspend_in_progress = true;
4447         wldev->suspend_init_status = b43_status(wldev);
4448         if (wldev->suspend_init_status >= B43_STAT_STARTED)
4449                 b43_wireless_core_stop(wldev);
4450         if (wldev->suspend_init_status >= B43_STAT_INITIALIZED)
4451                 b43_wireless_core_exit(wldev);
4452         mutex_unlock(&wl->mutex);
4453
4454         b43dbg(wl, "Device suspended.\n");
4455
4456         return 0;
4457 }
4458
4459 static int b43_resume(struct ssb_device *dev)
4460 {
4461         struct b43_wldev *wldev = ssb_get_drvdata(dev);
4462         struct b43_wl *wl = wldev->wl;
4463         int err = 0;
4464
4465         b43dbg(wl, "Resuming...\n");
4466
4467         mutex_lock(&wl->mutex);
4468         if (wldev->suspend_init_status >= B43_STAT_INITIALIZED) {
4469                 err = b43_wireless_core_init(wldev);
4470                 if (err) {
4471                         b43err(wl, "Resume failed at core init\n");
4472                         goto out;
4473                 }
4474         }
4475         if (wldev->suspend_init_status >= B43_STAT_STARTED) {
4476                 err = b43_wireless_core_start(wldev);
4477                 if (err) {
4478                         b43_leds_exit(wldev);
4479                         b43_rng_exit(wldev->wl, true);
4480                         b43_wireless_core_exit(wldev);
4481                         b43err(wl, "Resume failed at core start\n");
4482                         goto out;
4483                 }
4484         }
4485         b43dbg(wl, "Device resumed.\n");
4486  out:
4487         wldev->suspend_in_progress = false;
4488         mutex_unlock(&wl->mutex);
4489         return err;
4490 }
4491
4492 #else /* CONFIG_PM */
4493 # define b43_suspend    NULL
4494 # define b43_resume     NULL
4495 #endif /* CONFIG_PM */
4496
4497 static struct ssb_driver b43_ssb_driver = {
4498         .name           = KBUILD_MODNAME,
4499         .id_table       = b43_ssb_tbl,
4500         .probe          = b43_probe,
4501         .remove         = b43_remove,
4502         .suspend        = b43_suspend,
4503         .resume         = b43_resume,
4504 };
4505
4506 static void b43_print_driverinfo(void)
4507 {
4508         const char *feat_pci = "", *feat_pcmcia = "", *feat_nphy = "",
4509                    *feat_leds = "", *feat_rfkill = "";
4510
4511 #ifdef CONFIG_B43_PCI_AUTOSELECT
4512         feat_pci = "P";
4513 #endif
4514 #ifdef CONFIG_B43_PCMCIA
4515         feat_pcmcia = "M";
4516 #endif
4517 #ifdef CONFIG_B43_NPHY
4518         feat_nphy = "N";
4519 #endif
4520 #ifdef CONFIG_B43_LEDS
4521         feat_leds = "L";
4522 #endif
4523 #ifdef CONFIG_B43_RFKILL
4524         feat_rfkill = "R";
4525 #endif
4526         printk(KERN_INFO "Broadcom 43xx driver loaded "
4527                "[ Features: %s%s%s%s%s, Firmware-ID: "
4528                B43_SUPPORTED_FIRMWARE_ID " ]\n",
4529                feat_pci, feat_pcmcia, feat_nphy,
4530                feat_leds, feat_rfkill);
4531 }
4532
4533 static int __init b43_init(void)
4534 {
4535         int err;
4536
4537         b43_debugfs_init();
4538         err = b43_pcmcia_init();
4539         if (err)
4540                 goto err_dfs_exit;
4541         err = ssb_driver_register(&b43_ssb_driver);
4542         if (err)
4543                 goto err_pcmcia_exit;
4544         b43_print_driverinfo();
4545
4546         return err;
4547
4548 err_pcmcia_exit:
4549         b43_pcmcia_exit();
4550 err_dfs_exit:
4551         b43_debugfs_exit();
4552         return err;
4553 }
4554
4555 static void __exit b43_exit(void)
4556 {
4557         ssb_driver_unregister(&b43_ssb_driver);
4558         b43_pcmcia_exit();
4559         b43_debugfs_exit();
4560 }
4561
4562 module_init(b43_init)
4563 module_exit(b43_exit)