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