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

   1 /*
   2
   3   Broadcom B43 wireless driver
   4
   5   Copyright (c) 2005 Martin Langer <martin-langer@gmx.de>
   6   Copyright (c) 2005 Stefano Brivio <stefano.brivio@polimi.it>
   7   Copyright (c) 2005-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         if (changed & BSS_CHANGED_BSSID) {
3557                 spin_lock_irqsave(&wl->irq_lock, flags);
3558                 if (conf->bssid)
3559                         memcpy(wl->bssid, conf->bssid, ETH_ALEN);
3560                 else
3561                         memset(wl->bssid, 0, ETH_ALEN);
3562
3563                 if (b43_status(dev) >= B43_STAT_INITIALIZED) {
3564                         if (b43_is_mode(wl, NL80211_IFTYPE_AP) ||
3565                             b43_is_mode(wl, NL80211_IFTYPE_MESH_POINT)) {
3566                                 B43_WARN_ON(vif->type != wl->if_type);
3567                                 if (changed & BSS_CHANGED_BEACON)
3568                                         b43_update_templates(wl);
3569                         } else if (b43_is_mode(wl, NL80211_IFTYPE_ADHOC)) {
3570                                 if (changed & BSS_CHANGED_BEACON)
3571                                         b43_update_templates(wl);
3572                         }
3573                         b43_write_mac_bssid_templates(dev);
3574                 }
3575                 spin_unlock_irqrestore(&wl->irq_lock, flags);
3576         }
3577
3578         b43_mac_suspend(dev);
3579
3580         /* Update templates for AP/mesh mode. */
3581         if (changed & BSS_CHANGED_BEACON_INT &&
3582             (b43_is_mode(wl, NL80211_IFTYPE_AP) ||
3583              b43_is_mode(wl, NL80211_IFTYPE_MESH_POINT) ||
3584              b43_is_mode(wl, NL80211_IFTYPE_ADHOC)))
3585                 b43_set_beacon_int(dev, conf->beacon_int);
3586
3587         if (changed & BSS_CHANGED_BASIC_RATES)
3588                 b43_update_basic_rates(dev, conf->basic_rates);
3589
3590         if (changed & BSS_CHANGED_ERP_SLOT) {
3591                 if (conf->use_short_slot)
3592                         b43_short_slot_timing_enable(dev);
3593                 else
3594                         b43_short_slot_timing_disable(dev);
3595         }
3596
3597         b43_mac_enable(dev);
3598 out_unlock_mutex:
3599         mutex_unlock(&wl->mutex);
3600 }
3601
3602 static int b43_op_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
3603                           struct ieee80211_vif *vif, struct ieee80211_sta *sta,
3604                           struct ieee80211_key_conf *key)
3605 {
3606         struct b43_wl *wl = hw_to_b43_wl(hw);
3607         struct b43_wldev *dev;
3608         u8 algorithm;
3609         u8 index;
3610         int err;
3611         static const u8 bcast_addr[ETH_ALEN] = { 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };
3612
3613         if (modparam_nohwcrypt)
3614                 return -ENOSPC; /* User disabled HW-crypto */
3615
3616         mutex_lock(&wl->mutex);
3617         spin_lock_irq(&wl->irq_lock);
3618         write_lock(&wl->tx_lock);
3619         /* Why do we need all this locking here?
3620          * mutex     -> Every config operation must take it.
3621          * irq_lock  -> We modify the dev->key array, which is accessed
3622          *              in the IRQ handlers.
3623          * tx_lock   -> We modify the dev->key array, which is accessed
3624          *              in the TX handler.
3625          */
3626
3627         dev = wl->current_dev;
3628         err = -ENODEV;
3629         if (!dev || b43_status(dev) < B43_STAT_INITIALIZED)
3630                 goto out_unlock;
3631
3632         if (dev->fw.pcm_request_failed) {
3633                 /* We don't have firmware for the crypto engine.
3634                  * Must use software-crypto. */
3635                 err = -EOPNOTSUPP;
3636                 goto out_unlock;
3637         }
3638
3639         err = -EINVAL;
3640         switch (key->alg) {
3641         case ALG_WEP:
3642                 if (key->keylen == LEN_WEP40)
3643                         algorithm = B43_SEC_ALGO_WEP40;
3644                 else
3645                         algorithm = B43_SEC_ALGO_WEP104;
3646                 break;
3647         case ALG_TKIP:
3648                 algorithm = B43_SEC_ALGO_TKIP;
3649                 break;
3650         case ALG_CCMP:
3651                 algorithm = B43_SEC_ALGO_AES;
3652                 break;
3653         default:
3654                 B43_WARN_ON(1);
3655                 goto out_unlock;
3656         }
3657         index = (u8) (key->keyidx);
3658         if (index > 3)
3659                 goto out_unlock;
3660
3661         switch (cmd) {
3662         case SET_KEY:
3663                 if (algorithm == B43_SEC_ALGO_TKIP) {
3664                         /* FIXME: No TKIP hardware encryption for now. */
3665                         err = -EOPNOTSUPP;
3666                         goto out_unlock;
3667                 }
3668
3669                 if (key->flags & IEEE80211_KEY_FLAG_PAIRWISE) {
3670                         if (WARN_ON(!sta)) {
3671                                 err = -EOPNOTSUPP;
3672                                 goto out_unlock;
3673                         }
3674                         /* Pairwise key with an assigned MAC address. */
3675                         err = b43_key_write(dev, -1, algorithm,
3676                                             key->key, key->keylen,
3677                                             sta->addr, key);
3678                 } else {
3679                         /* Group key */
3680                         err = b43_key_write(dev, index, algorithm,
3681                                             key->key, key->keylen, NULL, key);
3682                 }
3683                 if (err)
3684                         goto out_unlock;
3685
3686                 if (algorithm == B43_SEC_ALGO_WEP40 ||
3687                     algorithm == B43_SEC_ALGO_WEP104) {
3688                         b43_hf_write(dev, b43_hf_read(dev) | B43_HF_USEDEFKEYS);
3689                 } else {
3690                         b43_hf_write(dev,
3691                                      b43_hf_read(dev) & ~B43_HF_USEDEFKEYS);
3692                 }
3693                 key->flags |= IEEE80211_KEY_FLAG_GENERATE_IV;
3694                 break;
3695         case DISABLE_KEY: {
3696                 err = b43_key_clear(dev, key->hw_key_idx);
3697                 if (err)
3698                         goto out_unlock;
3699                 break;
3700         }
3701         default:
3702                 B43_WARN_ON(1);
3703         }
3704
3705 out_unlock:
3706         if (!err) {
3707                 b43dbg(wl, "%s hardware based encryption for keyidx: %d, "
3708                        "mac: %pM\n",
3709                        cmd == SET_KEY ? "Using" : "Disabling", key->keyidx,
3710                        sta ? sta->addr : bcast_addr);
3711                 b43_dump_keymemory(dev);
3712         }
3713         write_unlock(&wl->tx_lock);
3714         spin_unlock_irq(&wl->irq_lock);
3715         mutex_unlock(&wl->mutex);
3716
3717         return err;
3718 }
3719
3720 static void b43_op_configure_filter(struct ieee80211_hw *hw,
3721                                     unsigned int changed, unsigned int *fflags,
3722                                     int mc_count, struct dev_addr_list *mc_list)
3723 {
3724         struct b43_wl *wl = hw_to_b43_wl(hw);
3725         struct b43_wldev *dev = wl->current_dev;
3726         unsigned long flags;
3727
3728         if (!dev) {
3729                 *fflags = 0;
3730                 return;
3731         }
3732
3733         spin_lock_irqsave(&wl->irq_lock, flags);
3734         *fflags &= FIF_PROMISC_IN_BSS |
3735                   FIF_ALLMULTI |
3736                   FIF_FCSFAIL |
3737                   FIF_PLCPFAIL |
3738                   FIF_CONTROL |
3739                   FIF_OTHER_BSS |
3740                   FIF_BCN_PRBRESP_PROMISC;
3741
3742         changed &= FIF_PROMISC_IN_BSS |
3743                    FIF_ALLMULTI |
3744                    FIF_FCSFAIL |
3745                    FIF_PLCPFAIL |
3746                    FIF_CONTROL |
3747                    FIF_OTHER_BSS |
3748                    FIF_BCN_PRBRESP_PROMISC;
3749
3750         wl->filter_flags = *fflags;
3751
3752         if (changed && b43_status(dev) >= B43_STAT_INITIALIZED)
3753                 b43_adjust_opmode(dev);
3754         spin_unlock_irqrestore(&wl->irq_lock, flags);
3755 }
3756
3757 /* Locking: wl->mutex */
3758 static void b43_wireless_core_stop(struct b43_wldev *dev)
3759 {
3760         struct b43_wl *wl = dev->wl;
3761         unsigned long flags;
3762
3763         if (b43_status(dev) < B43_STAT_STARTED)
3764                 return;
3765
3766         /* Disable and sync interrupts. We must do this before than
3767          * setting the status to INITIALIZED, as the interrupt handler
3768          * won't care about IRQs then. */
3769         spin_lock_irqsave(&wl->irq_lock, flags);
3770         b43_write32(dev, B43_MMIO_GEN_IRQ_MASK, 0);
3771         b43_read32(dev, B43_MMIO_GEN_IRQ_MASK); /* flush */
3772         spin_unlock_irqrestore(&wl->irq_lock, flags);
3773         b43_synchronize_irq(dev);
3774
3775         write_lock_irqsave(&wl->tx_lock, flags);
3776         b43_set_status(dev, B43_STAT_INITIALIZED);
3777         write_unlock_irqrestore(&wl->tx_lock, flags);
3778
3779         b43_pio_stop(dev);
3780         mutex_unlock(&wl->mutex);
3781         /* Must unlock as it would otherwise deadlock. No races here.
3782          * Cancel the possibly running self-rearming periodic work. */
3783         cancel_delayed_work_sync(&dev->periodic_work);
3784         mutex_lock(&wl->mutex);
3785
3786         b43_mac_suspend(dev);
3787         free_irq(dev->dev->irq, dev);
3788         b43dbg(wl, "Wireless interface stopped\n");
3789 }
3790
3791 /* Locking: wl->mutex */
3792 static int b43_wireless_core_start(struct b43_wldev *dev)
3793 {
3794         int err;
3795
3796         B43_WARN_ON(b43_status(dev) != B43_STAT_INITIALIZED);
3797
3798         drain_txstatus_queue(dev);
3799         err = request_irq(dev->dev->irq, b43_interrupt_handler,
3800                           IRQF_SHARED, KBUILD_MODNAME, dev);
3801         if (err) {
3802                 b43err(dev->wl, "Cannot request IRQ-%d\n", dev->dev->irq);
3803                 goto out;
3804         }
3805
3806         /* We are ready to run. */
3807         b43_set_status(dev, B43_STAT_STARTED);
3808
3809         /* Start data flow (TX/RX). */
3810         b43_mac_enable(dev);
3811         b43_write32(dev, B43_MMIO_GEN_IRQ_MASK, dev->irq_mask);
3812
3813         /* Start maintainance work */
3814         b43_periodic_tasks_setup(dev);
3815
3816         b43dbg(dev->wl, "Wireless interface started\n");
3817       out:
3818         return err;
3819 }
3820
3821 /* Get PHY and RADIO versioning numbers */
3822 static int b43_phy_versioning(struct b43_wldev *dev)
3823 {
3824         struct b43_phy *phy = &dev->phy;
3825         u32 tmp;
3826         u8 analog_type;
3827         u8 phy_type;
3828         u8 phy_rev;
3829         u16 radio_manuf;
3830         u16 radio_ver;
3831         u16 radio_rev;
3832         int unsupported = 0;
3833
3834         /* Get PHY versioning */
3835         tmp = b43_read16(dev, B43_MMIO_PHY_VER);
3836         analog_type = (tmp & B43_PHYVER_ANALOG) >> B43_PHYVER_ANALOG_SHIFT;
3837         phy_type = (tmp & B43_PHYVER_TYPE) >> B43_PHYVER_TYPE_SHIFT;
3838         phy_rev = (tmp & B43_PHYVER_VERSION);
3839         switch (phy_type) {
3840         case B43_PHYTYPE_A:
3841                 if (phy_rev >= 4)
3842                         unsupported = 1;
3843                 break;
3844         case B43_PHYTYPE_B:
3845                 if (phy_rev != 2 && phy_rev != 4 && phy_rev != 6
3846                     && phy_rev != 7)
3847                         unsupported = 1;
3848                 break;
3849         case B43_PHYTYPE_G:
3850                 if (phy_rev > 9)
3851                         unsupported = 1;
3852                 break;
3853 #ifdef CONFIG_B43_NPHY
3854         case B43_PHYTYPE_N:
3855                 if (phy_rev > 4)
3856                         unsupported = 1;
3857                 break;
3858 #endif
3859 #ifdef CONFIG_B43_PHY_LP
3860         case B43_PHYTYPE_LP:
3861                 if (phy_rev > 1)
3862                         unsupported = 1;
3863                 break;
3864 #endif
3865         default:
3866                 unsupported = 1;
3867         };
3868         if (unsupported) {
3869                 b43err(dev->wl, "FOUND UNSUPPORTED PHY "
3870                        "(Analog %u, Type %u, Revision %u)\n",
3871                        analog_type, phy_type, phy_rev);
3872                 return -EOPNOTSUPP;
3873         }
3874         b43dbg(dev->wl, "Found PHY: Analog %u, Type %u, Revision %u\n",
3875                analog_type, phy_type, phy_rev);
3876
3877         /* Get RADIO versioning */
3878         if (dev->dev->bus->chip_id == 0x4317) {
3879                 if (dev->dev->bus->chip_rev == 0)
3880                         tmp = 0x3205017F;
3881                 else if (dev->dev->bus->chip_rev == 1)
3882                         tmp = 0x4205017F;
3883                 else
3884                         tmp = 0x5205017F;
3885         } else {
3886                 b43_write16(dev, B43_MMIO_RADIO_CONTROL, B43_RADIOCTL_ID);
3887                 tmp = b43_read16(dev, B43_MMIO_RADIO_DATA_LOW);
3888                 b43_write16(dev, B43_MMIO_RADIO_CONTROL, B43_RADIOCTL_ID);
3889                 tmp |= (u32)b43_read16(dev, B43_MMIO_RADIO_DATA_HIGH) << 16;
3890         }
3891         radio_manuf = (tmp & 0x00000FFF);
3892         radio_ver = (tmp & 0x0FFFF000) >> 12;
3893         radio_rev = (tmp & 0xF0000000) >> 28;
3894         if (radio_manuf != 0x17F /* Broadcom */)
3895                 unsupported = 1;
3896         switch (phy_type) {
3897         case B43_PHYTYPE_A:
3898                 if (radio_ver != 0x2060)
3899                         unsupported = 1;
3900                 if (radio_rev != 1)
3901                         unsupported = 1;
3902                 if (radio_manuf != 0x17F)
3903                         unsupported = 1;
3904                 break;
3905         case B43_PHYTYPE_B:
3906                 if ((radio_ver & 0xFFF0) != 0x2050)
3907                         unsupported = 1;
3908                 break;
3909         case B43_PHYTYPE_G:
3910                 if (radio_ver != 0x2050)
3911                         unsupported = 1;
3912                 break;
3913         case B43_PHYTYPE_N:
3914                 if (radio_ver != 0x2055 && radio_ver != 0x2056)
3915                         unsupported = 1;
3916                 break;
3917         case B43_PHYTYPE_LP:
3918                 if (radio_ver != 0x2062)
3919                         unsupported = 1;
3920                 break;
3921         default:
3922                 B43_WARN_ON(1);
3923         }
3924         if (unsupported) {
3925                 b43err(dev->wl, "FOUND UNSUPPORTED RADIO "
3926                        "(Manuf 0x%X, Version 0x%X, Revision %u)\n",
3927                        radio_manuf, radio_ver, radio_rev);
3928                 return -EOPNOTSUPP;
3929         }
3930         b43dbg(dev->wl, "Found Radio: Manuf 0x%X, Version 0x%X, Revision %u\n",
3931                radio_manuf, radio_ver, radio_rev);
3932
3933         phy->radio_manuf = radio_manuf;
3934         phy->radio_ver = radio_ver;
3935         phy->radio_rev = radio_rev;
3936
3937         phy->analog = analog_type;
3938         phy->type = phy_type;
3939         phy->rev = phy_rev;
3940
3941         return 0;
3942 }
3943
3944 static void setup_struct_phy_for_init(struct b43_wldev *dev,
3945                                       struct b43_phy *phy)
3946 {
3947         phy->hardware_power_control = !!modparam_hwpctl;
3948         phy->next_txpwr_check_time = jiffies;
3949         /* PHY TX errors counter. */
3950         atomic_set(&phy->txerr_cnt, B43_PHY_TX_BADNESS_LIMIT);
3951
3952 #if B43_DEBUG
3953         phy->phy_locked = 0;
3954         phy->radio_locked = 0;
3955 #endif
3956 }
3957
3958 static void setup_struct_wldev_for_init(struct b43_wldev *dev)
3959 {
3960         dev->dfq_valid = 0;
3961
3962         /* Assume the radio is enabled. If it's not enabled, the state will
3963          * immediately get fixed on the first periodic work run. */
3964         dev->radio_hw_enable = 1;
3965
3966         /* Stats */
3967         memset(&dev->stats, 0, sizeof(dev->stats));
3968
3969         setup_struct_phy_for_init(dev, &dev->phy);
3970
3971         /* IRQ related flags */
3972         dev->irq_reason = 0;
3973         memset(dev->dma_reason, 0, sizeof(dev->dma_reason));
3974         dev->irq_mask = B43_IRQ_MASKTEMPLATE;
3975         if (b43_modparam_verbose < B43_VERBOSITY_DEBUG)
3976                 dev->irq_mask &= ~B43_IRQ_PHY_TXERR;
3977
3978         dev->mac_suspended = 1;
3979
3980         /* Noise calculation context */
3981         memset(&dev->noisecalc, 0, sizeof(dev->noisecalc));
3982 }
3983
3984 static void b43_bluetooth_coext_enable(struct b43_wldev *dev)
3985 {
3986         struct ssb_sprom *sprom = &dev->dev->bus->sprom;
3987         u64 hf;
3988
3989         if (!modparam_btcoex)
3990                 return;
3991         if (!(sprom->boardflags_lo & B43_BFL_BTCOEXIST))
3992                 return;
3993         if (dev->phy.type != B43_PHYTYPE_B && !dev->phy.gmode)
3994                 return;
3995
3996         hf = b43_hf_read(dev);
3997         if (sprom->boardflags_lo & B43_BFL_BTCMOD)
3998                 hf |= B43_HF_BTCOEXALT;
3999         else
4000                 hf |= B43_HF_BTCOEX;
4001         b43_hf_write(dev, hf);
4002 }
4003
4004 static void b43_bluetooth_coext_disable(struct b43_wldev *dev)
4005 {
4006         if (!modparam_btcoex)
4007                 return;
4008         //TODO
4009 }
4010
4011 static void b43_imcfglo_timeouts_workaround(struct b43_wldev *dev)
4012 {
4013 #ifdef CONFIG_SSB_DRIVER_PCICORE
4014         struct ssb_bus *bus = dev->dev->bus;
4015         u32 tmp;
4016
4017         if (bus->pcicore.dev &&
4018             bus->pcicore.dev->id.coreid == SSB_DEV_PCI &&
4019             bus->pcicore.dev->id.revision <= 5) {
4020                 /* IMCFGLO timeouts workaround. */
4021                 tmp = ssb_read32(dev->dev, SSB_IMCFGLO);
4022                 tmp &= ~SSB_IMCFGLO_REQTO;
4023                 tmp &= ~SSB_IMCFGLO_SERTO;
4024                 switch (bus->bustype) {
4025                 case SSB_BUSTYPE_PCI:
4026                 case SSB_BUSTYPE_PCMCIA:
4027                         tmp |= 0x32;
4028                         break;
4029                 case SSB_BUSTYPE_SSB:
4030                         tmp |= 0x53;
4031                         break;
4032                 }
4033                 ssb_write32(dev->dev, SSB_IMCFGLO, tmp);
4034         }
4035 #endif /* CONFIG_SSB_DRIVER_PCICORE */
4036 }
4037
4038 static void b43_set_synth_pu_delay(struct b43_wldev *dev, bool idle)
4039 {
4040         u16 pu_delay;
4041
4042         /* The time value is in microseconds. */
4043         if (dev->phy.type == B43_PHYTYPE_A)
4044                 pu_delay = 3700;
4045         else
4046                 pu_delay = 1050;
4047         if (b43_is_mode(dev->wl, NL80211_IFTYPE_ADHOC) || idle)
4048                 pu_delay = 500;
4049         if ((dev->phy.radio_ver == 0x2050) && (dev->phy.radio_rev == 8))
4050                 pu_delay = max(pu_delay, (u16)2400);
4051
4052         b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_SPUWKUP, pu_delay);
4053 }
4054
4055 /* Set the TSF CFP pre-TargetBeaconTransmissionTime. */
4056 static void b43_set_pretbtt(struct b43_wldev *dev)
4057 {
4058         u16 pretbtt;
4059
4060         /* The time value is in microseconds. */
4061         if (b43_is_mode(dev->wl, NL80211_IFTYPE_ADHOC)) {
4062                 pretbtt = 2;
4063         } else {
4064                 if (dev->phy.type == B43_PHYTYPE_A)
4065                         pretbtt = 120;
4066                 else
4067                         pretbtt = 250;
4068         }
4069         b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_PRETBTT, pretbtt);
4070         b43_write16(dev, B43_MMIO_TSF_CFP_PRETBTT, pretbtt);
4071 }
4072
4073 /* Shutdown a wireless core */
4074 /* Locking: wl->mutex */
4075 static void b43_wireless_core_exit(struct b43_wldev *dev)
4076 {
4077         u32 macctl;
4078
4079         B43_WARN_ON(b43_status(dev) > B43_STAT_INITIALIZED);
4080         if (b43_status(dev) != B43_STAT_INITIALIZED)
4081                 return;
4082         b43_set_status(dev, B43_STAT_UNINIT);
4083
4084         /* Stop the microcode PSM. */
4085         macctl = b43_read32(dev, B43_MMIO_MACCTL);
4086         macctl &= ~B43_MACCTL_PSM_RUN;
4087         macctl |= B43_MACCTL_PSM_JMP0;
4088         b43_write32(dev, B43_MMIO_MACCTL, macctl);
4089
4090         if (!dev->suspend_in_progress) {
4091                 b43_leds_exit(dev);
4092                 b43_rng_exit(dev->wl);
4093         }
4094         b43_dma_free(dev);
4095         b43_pio_free(dev);
4096         b43_chip_exit(dev);
4097         dev->phy.ops->switch_analog(dev, 0);
4098         if (dev->wl->current_beacon) {
4099                 dev_kfree_skb_any(dev->wl->current_beacon);
4100                 dev->wl->current_beacon = NULL;
4101         }
4102
4103         ssb_device_disable(dev->dev, 0);
4104         ssb_bus_may_powerdown(dev->dev->bus);
4105 }
4106
4107 /* Initialize a wireless core */
4108 static int b43_wireless_core_init(struct b43_wldev *dev)
4109 {
4110         struct b43_wl *wl = dev->wl;
4111         struct ssb_bus *bus = dev->dev->bus;
4112         struct ssb_sprom *sprom = &bus->sprom;
4113         struct b43_phy *phy = &dev->phy;
4114         int err;
4115         u64 hf;
4116         u32 tmp;
4117
4118         B43_WARN_ON(b43_status(dev) != B43_STAT_UNINIT);
4119
4120         err = ssb_bus_powerup(bus, 0);
4121         if (err)
4122                 goto out;
4123         if (!ssb_device_is_enabled(dev->dev)) {
4124                 tmp = phy->gmode ? B43_TMSLOW_GMODE : 0;
4125                 b43_wireless_core_reset(dev, tmp);
4126         }
4127
4128         /* Reset all data structures. */
4129         setup_struct_wldev_for_init(dev);
4130         phy->ops->prepare_structs(dev);
4131
4132         /* Enable IRQ routing to this device. */
4133         ssb_pcicore_dev_irqvecs_enable(&bus->pcicore, dev->dev);
4134
4135         b43_imcfglo_timeouts_workaround(dev);
4136         b43_bluetooth_coext_disable(dev);
4137         if (phy->ops->prepare_hardware) {
4138                 err = phy->ops->prepare_hardware(dev);
4139                 if (err)
4140                         goto err_busdown;
4141         }
4142         err = b43_chip_init(dev);
4143         if (err)
4144                 goto err_busdown;
4145         b43_shm_write16(dev, B43_SHM_SHARED,
4146                         B43_SHM_SH_WLCOREREV, dev->dev->id.revision);
4147         hf = b43_hf_read(dev);
4148         if (phy->type == B43_PHYTYPE_G) {
4149                 hf |= B43_HF_SYMW;
4150                 if (phy->rev == 1)
4151                         hf |= B43_HF_GDCW;
4152                 if (sprom->boardflags_lo & B43_BFL_PACTRL)
4153                         hf |= B43_HF_OFDMPABOOST;
4154         }
4155         if (phy->radio_ver == 0x2050) {
4156                 if (phy->radio_rev == 6)
4157                         hf |= B43_HF_4318TSSI;
4158                 if (phy->radio_rev < 6)
4159                         hf |= B43_HF_VCORECALC;
4160         }
4161         if (sprom->boardflags_lo & B43_BFL_XTAL_NOSLOW)
4162                 hf |= B43_HF_DSCRQ; /* Disable slowclock requests from ucode. */
4163 #ifdef CONFIG_SSB_DRIVER_PCICORE
4164         if ((bus->bustype == SSB_BUSTYPE_PCI) &&
4165             (bus->pcicore.dev->id.revision <= 10))
4166                 hf |= B43_HF_PCISCW; /* PCI slow clock workaround. */
4167 #endif
4168         hf &= ~B43_HF_SKCFPUP;
4169         b43_hf_write(dev, hf);
4170
4171         b43_set_retry_limits(dev, B43_DEFAULT_SHORT_RETRY_LIMIT,
4172                              B43_DEFAULT_LONG_RETRY_LIMIT);
4173         b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_SFFBLIM, 3);
4174         b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_LFFBLIM, 2);
4175
4176         /* Disable sending probe responses from firmware.
4177          * Setting the MaxTime to one usec will always trigger
4178          * a timeout, so we never send any probe resp.
4179          * A timeout of zero is infinite. */
4180         b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_PRMAXTIME, 1);
4181
4182         b43_rate_memory_init(dev);
4183         b43_set_phytxctl_defaults(dev);
4184
4185         /* Minimum Contention Window */
4186         if (phy->type == B43_PHYTYPE_B) {
4187                 b43_shm_write16(dev, B43_SHM_SCRATCH, B43_SHM_SC_MINCONT, 0x1F);
4188         } else {
4189                 b43_shm_write16(dev, B43_SHM_SCRATCH, B43_SHM_SC_MINCONT, 0xF);
4190         }
4191         /* Maximum Contention Window */
4192         b43_shm_write16(dev, B43_SHM_SCRATCH, B43_SHM_SC_MAXCONT, 0x3FF);
4193
4194         if ((dev->dev->bus->bustype == SSB_BUSTYPE_PCMCIA) || B43_FORCE_PIO) {
4195                 dev->__using_pio_transfers = 1;
4196                 err = b43_pio_init(dev);
4197         } else {
4198                 dev->__using_pio_transfers = 0;
4199                 err = b43_dma_init(dev);
4200         }
4201         if (err)
4202                 goto err_chip_exit;
4203         b43_qos_init(dev);
4204         b43_set_synth_pu_delay(dev, 1);
4205         b43_bluetooth_coext_enable(dev);
4206
4207         ssb_bus_powerup(bus, !(sprom->boardflags_lo & B43_BFL_XTAL_NOSLOW));
4208         b43_upload_card_macaddress(dev);
4209         b43_security_init(dev);
4210         if (!dev->suspend_in_progress)
4211                 b43_rng_init(wl);
4212
4213         b43_set_status(dev, B43_STAT_INITIALIZED);
4214
4215         if (!dev->suspend_in_progress)
4216                 b43_leds_init(dev);
4217 out:
4218         return err;
4219
4220 err_chip_exit:
4221         b43_chip_exit(dev);
4222 err_busdown:
4223         ssb_bus_may_powerdown(bus);
4224         B43_WARN_ON(b43_status(dev) != B43_STAT_UNINIT);
4225         return err;
4226 }
4227
4228 static int b43_op_add_interface(struct ieee80211_hw *hw,
4229                                 struct ieee80211_if_init_conf *conf)
4230 {
4231         struct b43_wl *wl = hw_to_b43_wl(hw);
4232         struct b43_wldev *dev;
4233         unsigned long flags;
4234         int err = -EOPNOTSUPP;
4235
4236         /* TODO: allow WDS/AP devices to coexist */
4237
4238         if (conf->type != NL80211_IFTYPE_AP &&
4239             conf->type != NL80211_IFTYPE_MESH_POINT &&
4240             conf->type != NL80211_IFTYPE_STATION &&
4241             conf->type != NL80211_IFTYPE_WDS &&
4242             conf->type != NL80211_IFTYPE_ADHOC)
4243                 return -EOPNOTSUPP;
4244
4245         mutex_lock(&wl->mutex);
4246         if (wl->operating)
4247                 goto out_mutex_unlock;
4248
4249         b43dbg(wl, "Adding Interface type %d\n", conf->type);
4250
4251         dev = wl->current_dev;
4252         wl->operating = 1;
4253         wl->vif = conf->vif;
4254         wl->if_type = conf->type;
4255         memcpy(wl->mac_addr, conf->mac_addr, ETH_ALEN);
4256
4257         spin_lock_irqsave(&wl->irq_lock, flags);
4258         b43_adjust_opmode(dev);
4259         b43_set_pretbtt(dev);
4260         b43_set_synth_pu_delay(dev, 0);
4261         b43_upload_card_macaddress(dev);
4262         spin_unlock_irqrestore(&wl->irq_lock, flags);
4263
4264         err = 0;
4265  out_mutex_unlock:
4266         mutex_unlock(&wl->mutex);
4267
4268         return err;
4269 }
4270
4271 static void b43_op_remove_interface(struct ieee80211_hw *hw,
4272                                     struct ieee80211_if_init_conf *conf)
4273 {
4274         struct b43_wl *wl = hw_to_b43_wl(hw);
4275         struct b43_wldev *dev = wl->current_dev;
4276         unsigned long flags;
4277
4278         b43dbg(wl, "Removing Interface type %d\n", conf->type);
4279
4280         mutex_lock(&wl->mutex);
4281
4282         B43_WARN_ON(!wl->operating);
4283         B43_WARN_ON(wl->vif != conf->vif);
4284         wl->vif = NULL;
4285
4286         wl->operating = 0;
4287
4288         spin_lock_irqsave(&wl->irq_lock, flags);
4289         b43_adjust_opmode(dev);
4290         memset(wl->mac_addr, 0, ETH_ALEN);
4291         b43_upload_card_macaddress(dev);
4292         spin_unlock_irqrestore(&wl->irq_lock, flags);
4293
4294         mutex_unlock(&wl->mutex);
4295 }
4296
4297 static int b43_op_start(struct ieee80211_hw *hw)
4298 {
4299         struct b43_wl *wl = hw_to_b43_wl(hw);
4300         struct b43_wldev *dev = wl->current_dev;
4301         int did_init = 0;
4302         int err = 0;
4303         bool do_rfkill_exit = 0;
4304
4305         /* Kill all old instance specific information to make sure
4306          * the card won't use it in the short timeframe between start
4307          * and mac80211 reconfiguring it. */
4308         memset(wl->bssid, 0, ETH_ALEN);
4309         memset(wl->mac_addr, 0, ETH_ALEN);
4310         wl->filter_flags = 0;
4311         wl->radiotap_enabled = 0;
4312         b43_qos_clear(wl);
4313         wl->beacon0_uploaded = 0;
4314         wl->beacon1_uploaded = 0;
4315         wl->beacon_templates_virgin = 1;
4316
4317         /* First register RFkill.
4318          * LEDs that are registered later depend on it. */
4319         b43_rfkill_init(dev);
4320
4321         mutex_lock(&wl->mutex);
4322
4323         if (b43_status(dev) < B43_STAT_INITIALIZED) {
4324                 err = b43_wireless_core_init(dev);
4325                 if (err) {
4326                         do_rfkill_exit = 1;
4327                         goto out_mutex_unlock;
4328                 }
4329                 did_init = 1;
4330         }
4331
4332         if (b43_status(dev) < B43_STAT_STARTED) {
4333                 err = b43_wireless_core_start(dev);
4334                 if (err) {
4335                         if (did_init)
4336                                 b43_wireless_core_exit(dev);
4337                         do_rfkill_exit = 1;
4338                         goto out_mutex_unlock;
4339                 }
4340         }
4341
4342  out_mutex_unlock:
4343         mutex_unlock(&wl->mutex);
4344
4345         if (do_rfkill_exit)
4346                 b43_rfkill_exit(dev);
4347
4348         return err;
4349 }
4350
4351 static void b43_op_stop(struct ieee80211_hw *hw)
4352 {
4353         struct b43_wl *wl = hw_to_b43_wl(hw);
4354         struct b43_wldev *dev = wl->current_dev;
4355
4356         b43_rfkill_exit(dev);
4357         cancel_work_sync(&(wl->beacon_update_trigger));
4358
4359         mutex_lock(&wl->mutex);
4360         if (b43_status(dev) >= B43_STAT_STARTED)
4361                 b43_wireless_core_stop(dev);
4362         b43_wireless_core_exit(dev);
4363         mutex_unlock(&wl->mutex);
4364
4365         cancel_work_sync(&(wl->txpower_adjust_work));
4366 }
4367
4368 static int b43_op_beacon_set_tim(struct ieee80211_hw *hw,
4369                                  struct ieee80211_sta *sta, bool set)
4370 {
4371         struct b43_wl *wl = hw_to_b43_wl(hw);
4372         unsigned long flags;
4373
4374         spin_lock_irqsave(&wl->irq_lock, flags);
4375         b43_update_templates(wl);
4376         spin_unlock_irqrestore(&wl->irq_lock, flags);
4377
4378         return 0;
4379 }
4380
4381 static void b43_op_sta_notify(struct ieee80211_hw *hw,
4382                               struct ieee80211_vif *vif,
4383                               enum sta_notify_cmd notify_cmd,
4384                               struct ieee80211_sta *sta)
4385 {
4386         struct b43_wl *wl = hw_to_b43_wl(hw);
4387
4388         B43_WARN_ON(!vif || wl->vif != vif);
4389 }
4390
4391 static void b43_op_sw_scan_start_notifier(struct ieee80211_hw *hw)
4392 {
4393         struct b43_wl *wl = hw_to_b43_wl(hw);
4394         struct b43_wldev *dev;
4395
4396         mutex_lock(&wl->mutex);
4397         dev = wl->current_dev;
4398         if (dev && (b43_status(dev) >= B43_STAT_INITIALIZED)) {
4399                 /* Disable CFP update during scan on other channels. */
4400                 b43_hf_write(dev, b43_hf_read(dev) | B43_HF_SKCFPUP);
4401         }
4402         mutex_unlock(&wl->mutex);
4403 }
4404
4405 static void b43_op_sw_scan_complete_notifier(struct ieee80211_hw *hw)
4406 {
4407         struct b43_wl *wl = hw_to_b43_wl(hw);
4408         struct b43_wldev *dev;
4409
4410         mutex_lock(&wl->mutex);
4411         dev = wl->current_dev;
4412         if (dev && (b43_status(dev) >= B43_STAT_INITIALIZED)) {
4413                 /* Re-enable CFP update. */
4414                 b43_hf_write(dev, b43_hf_read(dev) & ~B43_HF_SKCFPUP);
4415         }
4416         mutex_unlock(&wl->mutex);
4417 }
4418
4419 static const struct ieee80211_ops b43_hw_ops = {
4420         .tx                     = b43_op_tx,
4421         .conf_tx                = b43_op_conf_tx,
4422         .add_interface          = b43_op_add_interface,
4423         .remove_interface       = b43_op_remove_interface,
4424         .config                 = b43_op_config,
4425         .bss_info_changed       = b43_op_bss_info_changed,
4426         .configure_filter       = b43_op_configure_filter,
4427         .set_key                = b43_op_set_key,
4428         .get_stats              = b43_op_get_stats,
4429         .get_tx_stats           = b43_op_get_tx_stats,
4430         .get_tsf                = b43_op_get_tsf,
4431         .set_tsf                = b43_op_set_tsf,
4432         .start                  = b43_op_start,
4433         .stop                   = b43_op_stop,
4434         .set_tim                = b43_op_beacon_set_tim,
4435         .sta_notify             = b43_op_sta_notify,
4436         .sw_scan_start          = b43_op_sw_scan_start_notifier,
4437         .sw_scan_complete       = b43_op_sw_scan_complete_notifier,
4438 };
4439
4440 /* Hard-reset the chip. Do not call this directly.
4441  * Use b43_controller_restart()
4442  */
4443 static void b43_chip_reset(struct work_struct *work)
4444 {
4445         struct b43_wldev *dev =
4446             container_of(work, struct b43_wldev, restart_work);
4447         struct b43_wl *wl = dev->wl;
4448         int err = 0;
4449         int prev_status;
4450
4451         mutex_lock(&wl->mutex);
4452
4453         prev_status = b43_status(dev);
4454         /* Bring the device down... */
4455         if (prev_status >= B43_STAT_STARTED)
4456                 b43_wireless_core_stop(dev);
4457         if (prev_status >= B43_STAT_INITIALIZED)
4458                 b43_wireless_core_exit(dev);
4459
4460         /* ...and up again. */
4461         if (prev_status >= B43_STAT_INITIALIZED) {
4462                 err = b43_wireless_core_init(dev);
4463                 if (err)
4464                         goto out;
4465         }
4466         if (prev_status >= B43_STAT_STARTED) {
4467                 err = b43_wireless_core_start(dev);
4468                 if (err) {
4469                         b43_wireless_core_exit(dev);
4470                         goto out;
4471                 }
4472         }
4473 out:
4474         if (err)
4475                 wl->current_dev = NULL; /* Failed to init the dev. */
4476         mutex_unlock(&wl->mutex);
4477         if (err)
4478                 b43err(wl, "Controller restart FAILED\n");
4479         else
4480                 b43info(wl, "Controller restarted\n");
4481 }
4482
4483 static int b43_setup_bands(struct b43_wldev *dev,
4484                            bool have_2ghz_phy, bool have_5ghz_phy)
4485 {
4486         struct ieee80211_hw *hw = dev->wl->hw;
4487
4488         if (have_2ghz_phy)
4489                 hw->wiphy->bands[IEEE80211_BAND_2GHZ] = &b43_band_2GHz;
4490         if (dev->phy.type == B43_PHYTYPE_N) {
4491                 if (have_5ghz_phy)
4492                         hw->wiphy->bands[IEEE80211_BAND_5GHZ] = &b43_band_5GHz_nphy;
4493         } else {
4494                 if (have_5ghz_phy)
4495                         hw->wiphy->bands[IEEE80211_BAND_5GHZ] = &b43_band_5GHz_aphy;
4496         }
4497
4498         dev->phy.supports_2ghz = have_2ghz_phy;
4499         dev->phy.supports_5ghz = have_5ghz_phy;
4500
4501         return 0;
4502 }
4503
4504 static void b43_wireless_core_detach(struct b43_wldev *dev)
4505 {
4506         /* We release firmware that late to not be required to re-request
4507          * is all the time when we reinit the core. */
4508         b43_release_firmware(dev);
4509         b43_phy_free(dev);
4510 }
4511
4512 static int b43_wireless_core_attach(struct b43_wldev *dev)
4513 {
4514         struct b43_wl *wl = dev->wl;
4515         struct ssb_bus *bus = dev->dev->bus;
4516         struct pci_dev *pdev = bus->host_pci;
4517         int err;
4518         bool have_2ghz_phy = 0, have_5ghz_phy = 0;
4519         u32 tmp;
4520
4521         /* Do NOT do any device initialization here.
4522          * Do it in wireless_core_init() instead.
4523          * This function is for gathering basic information about the HW, only.
4524          * Also some structs may be set up here. But most likely you want to have
4525          * that in core_init(), too.
4526          */
4527
4528         err = ssb_bus_powerup(bus, 0);
4529         if (err) {
4530                 b43err(wl, "Bus powerup failed\n");
4531                 goto out;
4532         }
4533         /* Get the PHY type. */
4534         if (dev->dev->id.revision >= 5) {
4535                 u32 tmshigh;
4536
4537                 tmshigh = ssb_read32(dev->dev, SSB_TMSHIGH);
4538                 have_2ghz_phy = !!(tmshigh & B43_TMSHIGH_HAVE_2GHZ_PHY);
4539                 have_5ghz_phy = !!(tmshigh & B43_TMSHIGH_HAVE_5GHZ_PHY);
4540         } else
4541                 B43_WARN_ON(1);
4542
4543         dev->phy.gmode = have_2ghz_phy;
4544         tmp = dev->phy.gmode ? B43_TMSLOW_GMODE : 0;
4545         b43_wireless_core_reset(dev, tmp);
4546
4547         err = b43_phy_versioning(dev);
4548         if (err)
4549                 goto err_powerdown;
4550         /* Check if this device supports multiband. */
4551         if (!pdev ||
4552             (pdev->device != 0x4312 &&
4553              pdev->device != 0x4319 && pdev->device != 0x4324)) {
4554                 /* No multiband support. */
4555                 have_2ghz_phy = 0;
4556                 have_5ghz_phy = 0;
4557                 switch (dev->phy.type) {
4558                 case B43_PHYTYPE_A:
4559                         have_5ghz_phy = 1;
4560                         break;
4561                 case B43_PHYTYPE_G:
4562                 case B43_PHYTYPE_N:
4563                 case B43_PHYTYPE_LP:
4564                         have_2ghz_phy = 1;
4565                         break;
4566                 default:
4567                         B43_WARN_ON(1);
4568                 }
4569         }
4570         if (dev->phy.type == B43_PHYTYPE_A) {
4571                 /* FIXME */
4572                 b43err(wl, "IEEE 802.11a devices are unsupported\n");
4573                 err = -EOPNOTSUPP;
4574                 goto err_powerdown;
4575         }
4576         if (1 /* disable A-PHY */) {
4577                 /* FIXME: For now we disable the A-PHY on multi-PHY devices. */
4578                 if (dev->phy.type != B43_PHYTYPE_N) {
4579                         have_2ghz_phy = 1;
4580                         have_5ghz_phy = 0;
4581                 }
4582         }
4583
4584         err = b43_phy_allocate(dev);
4585         if (err)
4586                 goto err_powerdown;
4587
4588         dev->phy.gmode = have_2ghz_phy;
4589         tmp = dev->phy.gmode ? B43_TMSLOW_GMODE : 0;
4590         b43_wireless_core_reset(dev, tmp);
4591
4592         err = b43_validate_chipaccess(dev);
4593         if (err)
4594                 goto err_phy_free;
4595         err = b43_setup_bands(dev, have_2ghz_phy, have_5ghz_phy);
4596         if (err)
4597                 goto err_phy_free;
4598
4599         /* Now set some default "current_dev" */
4600         if (!wl->current_dev)
4601                 wl->current_dev = dev;
4602         INIT_WORK(&dev->restart_work, b43_chip_reset);
4603
4604         dev->phy.ops->switch_analog(dev, 0);
4605         ssb_device_disable(dev->dev, 0);
4606         ssb_bus_may_powerdown(bus);
4607
4608 out:
4609         return err;
4610
4611 err_phy_free:
4612         b43_phy_free(dev);
4613 err_powerdown:
4614         ssb_bus_may_powerdown(bus);
4615         return err;
4616 }
4617
4618 static void b43_one_core_detach(struct ssb_device *dev)
4619 {
4620         struct b43_wldev *wldev;
4621         struct b43_wl *wl;
4622
4623         /* Do not cancel ieee80211-workqueue based work here.
4624          * See comment in b43_remove(). */
4625
4626         wldev = ssb_get_drvdata(dev);
4627         wl = wldev->wl;
4628         b43_debugfs_remove_device(wldev);
4629         b43_wireless_core_detach(wldev);
4630         list_del(&wldev->list);
4631         wl->nr_devs--;
4632         ssb_set_drvdata(dev, NULL);
4633         kfree(wldev);
4634 }
4635
4636 static int b43_one_core_attach(struct ssb_device *dev, struct b43_wl *wl)
4637 {
4638         struct b43_wldev *wldev;
4639         struct pci_dev *pdev;
4640         int err = -ENOMEM;
4641
4642         if (!list_empty(&wl->devlist)) {
4643                 /* We are not the first core on this chip. */
4644                 pdev = dev->bus->host_pci;
4645                 /* Only special chips support more than one wireless
4646                  * core, although some of the other chips have more than
4647                  * one wireless core as well. Check for this and
4648                  * bail out early.
4649                  */
4650                 if (!pdev ||
4651                     ((pdev->device != 0x4321) &&
4652                      (pdev->device != 0x4313) && (pdev->device != 0x431A))) {
4653                         b43dbg(wl, "Ignoring unconnected 802.11 core\n");
4654                         return -ENODEV;
4655                 }
4656         }
4657
4658         wldev = kzalloc(sizeof(*wldev), GFP_KERNEL);
4659         if (!wldev)
4660                 goto out;
4661
4662         wldev->dev = dev;
4663         wldev->wl = wl;
4664         b43_set_status(wldev, B43_STAT_UNINIT);
4665         wldev->bad_frames_preempt = modparam_bad_frames_preempt;
4666         tasklet_init(&wldev->isr_tasklet,
4667                      (void (*)(unsigned long))b43_interrupt_tasklet,
4668                      (unsigned long)wldev);
4669         INIT_LIST_HEAD(&wldev->list);
4670
4671         err = b43_wireless_core_attach(wldev);
4672         if (err)
4673                 goto err_kfree_wldev;
4674
4675         list_add(&wldev->list, &wl->devlist);
4676         wl->nr_devs++;
4677         ssb_set_drvdata(dev, wldev);
4678         b43_debugfs_add_device(wldev);
4679
4680       out:
4681         return err;
4682
4683       err_kfree_wldev:
4684         kfree(wldev);
4685         return err;
4686 }
4687
4688 #define IS_PDEV(pdev, _vendor, _device, _subvendor, _subdevice)         ( \
4689         (pdev->vendor == PCI_VENDOR_ID_##_vendor) &&                    \
4690         (pdev->device == _device) &&                                    \
4691         (pdev->subsystem_vendor == PCI_VENDOR_ID_##_subvendor) &&       \
4692         (pdev->subsystem_device == _subdevice)                          )
4693
4694 static void b43_sprom_fixup(struct ssb_bus *bus)
4695 {
4696         struct pci_dev *pdev;
4697
4698         /* boardflags workarounds */
4699         if (bus->boardinfo.vendor == SSB_BOARDVENDOR_DELL &&
4700             bus->chip_id == 0x4301 && bus->boardinfo.rev == 0x74)
4701                 bus->sprom.boardflags_lo |= B43_BFL_BTCOEXIST;
4702         if (bus->boardinfo.vendor == PCI_VENDOR_ID_APPLE &&
4703             bus->boardinfo.type == 0x4E && bus->boardinfo.rev > 0x40)
4704                 bus->sprom.boardflags_lo |= B43_BFL_PACTRL;
4705         if (bus->bustype == SSB_BUSTYPE_PCI) {
4706                 pdev = bus->host_pci;
4707                 if (IS_PDEV(pdev, BROADCOM, 0x4318, ASUSTEK, 0x100F) ||
4708                     IS_PDEV(pdev, BROADCOM, 0x4320,    DELL, 0x0003) ||
4709                     IS_PDEV(pdev, BROADCOM, 0x4320,      HP, 0x12f8) ||
4710                     IS_PDEV(pdev, BROADCOM, 0x4320, LINKSYS, 0x0015) ||
4711                     IS_PDEV(pdev, BROADCOM, 0x4320, LINKSYS, 0x0014) ||
4712                     IS_PDEV(pdev, BROADCOM, 0x4320, LINKSYS, 0x0013) ||
4713                     IS_PDEV(pdev, BROADCOM, 0x4320, MOTOROLA, 0x7010))
4714                         bus->sprom.boardflags_lo &= ~B43_BFL_BTCOEXIST;
4715         }
4716 }
4717
4718 static void b43_wireless_exit(struct ssb_device *dev, struct b43_wl *wl)
4719 {
4720         struct ieee80211_hw *hw = wl->hw;
4721
4722         ssb_set_devtypedata(dev, NULL);
4723         ieee80211_free_hw(hw);
4724 }
4725
4726 static int b43_wireless_init(struct ssb_device *dev)
4727 {
4728         struct ssb_sprom *sprom = &dev->bus->sprom;
4729         struct ieee80211_hw *hw;
4730         struct b43_wl *wl;
4731         int err = -ENOMEM;
4732
4733         b43_sprom_fixup(dev->bus);
4734
4735         hw = ieee80211_alloc_hw(sizeof(*wl), &b43_hw_ops);
4736         if (!hw) {
4737                 b43err(NULL, "Could not allocate ieee80211 device\n");
4738                 goto out;
4739         }
4740
4741         /* fill hw info */
4742         hw->flags = IEEE80211_HW_RX_INCLUDES_FCS |
4743                     IEEE80211_HW_SIGNAL_DBM |
4744                     IEEE80211_HW_NOISE_DBM;
4745
4746         hw->wiphy->interface_modes =
4747                 BIT(NL80211_IFTYPE_AP) |
4748                 BIT(NL80211_IFTYPE_MESH_POINT) |
4749                 BIT(NL80211_IFTYPE_STATION) |
4750                 BIT(NL80211_IFTYPE_WDS) |
4751                 BIT(NL80211_IFTYPE_ADHOC);
4752
4753         hw->queues = b43_modparam_qos ? 4 : 1;
4754         hw->max_rates = 2;
4755         SET_IEEE80211_DEV(hw, dev->dev);
4756         if (is_valid_ether_addr(sprom->et1mac))
4757                 SET_IEEE80211_PERM_ADDR(hw, sprom->et1mac);
4758         else
4759                 SET_IEEE80211_PERM_ADDR(hw, sprom->il0mac);
4760
4761         /* Get and initialize struct b43_wl */
4762         wl = hw_to_b43_wl(hw);
4763         memset(wl, 0, sizeof(*wl));
4764         wl->hw = hw;
4765         spin_lock_init(&wl->irq_lock);
4766         rwlock_init(&wl->tx_lock);
4767         spin_lock_init(&wl->leds_lock);
4768         spin_lock_init(&wl->shm_lock);
4769         mutex_init(&wl->mutex);
4770         INIT_LIST_HEAD(&wl->devlist);
4771         INIT_WORK(&wl->beacon_update_trigger, b43_beacon_update_trigger_work);
4772         INIT_WORK(&wl->txpower_adjust_work, b43_phy_txpower_adjust_work);
4773
4774         ssb_set_devtypedata(dev, wl);
4775         b43info(wl, "Broadcom %04X WLAN found (core revision %u)\n",
4776                 dev->bus->chip_id, dev->id.revision);
4777         err = 0;
4778 out:
4779         return err;
4780 }
4781
4782 static int b43_probe(struct ssb_device *dev, const struct ssb_device_id *id)
4783 {
4784         struct b43_wl *wl;
4785         int err;
4786         int first = 0;
4787
4788         wl = ssb_get_devtypedata(dev);
4789         if (!wl) {
4790                 /* Probing the first core. Must setup common struct b43_wl */
4791                 first = 1;
4792                 err = b43_wireless_init(dev);
4793                 if (err)
4794                         goto out;
4795                 wl = ssb_get_devtypedata(dev);
4796                 B43_WARN_ON(!wl);
4797         }
4798         err = b43_one_core_attach(dev, wl);
4799         if (err)
4800                 goto err_wireless_exit;
4801
4802         if (first) {
4803                 err = ieee80211_register_hw(wl->hw);
4804                 if (err)
4805                         goto err_one_core_detach;
4806         }
4807
4808       out:
4809         return err;
4810
4811       err_one_core_detach:
4812         b43_one_core_detach(dev);
4813       err_wireless_exit:
4814         if (first)
4815                 b43_wireless_exit(dev, wl);
4816         return err;
4817 }
4818
4819 static void b43_remove(struct ssb_device *dev)
4820 {
4821         struct b43_wl *wl = ssb_get_devtypedata(dev);
4822         struct b43_wldev *wldev = ssb_get_drvdata(dev);
4823
4824         /* We must cancel any work here before unregistering from ieee80211,
4825          * as the ieee80211 unreg will destroy the workqueue. */
4826         cancel_work_sync(&wldev->restart_work);
4827
4828         B43_WARN_ON(!wl);
4829         if (wl->current_dev == wldev)
4830                 ieee80211_unregister_hw(wl->hw);
4831
4832         b43_one_core_detach(dev);
4833
4834         if (list_empty(&wl->devlist)) {
4835                 /* Last core on the chip unregistered.
4836                  * We can destroy common struct b43_wl.
4837                  */
4838                 b43_wireless_exit(dev, wl);
4839         }
4840 }
4841
4842 /* Perform a hardware reset. This can be called from any context. */
4843 void b43_controller_restart(struct b43_wldev *dev, const char *reason)
4844 {
4845         /* Must avoid requeueing, if we are in shutdown. */
4846         if (b43_status(dev) < B43_STAT_INITIALIZED)
4847                 return;
4848         b43info(dev->wl, "Controller RESET (%s) ...\n", reason);
4849         queue_work(dev->wl->hw->workqueue, &dev->restart_work);
4850 }
4851
4852 #ifdef CONFIG_PM
4853
4854 static int b43_suspend(struct ssb_device *dev, pm_message_t state)
4855 {
4856         struct b43_wldev *wldev = ssb_get_drvdata(dev);
4857         struct b43_wl *wl = wldev->wl;
4858
4859         b43dbg(wl, "Suspending...\n");
4860
4861         mutex_lock(&wl->mutex);
4862         wldev->suspend_in_progress = true;
4863         wldev->suspend_init_status = b43_status(wldev);
4864         if (wldev->suspend_init_status >= B43_STAT_STARTED)
4865                 b43_wireless_core_stop(wldev);
4866         if (wldev->suspend_init_status >= B43_STAT_INITIALIZED)
4867                 b43_wireless_core_exit(wldev);
4868         mutex_unlock(&wl->mutex);
4869
4870         b43dbg(wl, "Device suspended.\n");
4871
4872         return 0;
4873 }
4874
4875 static int b43_resume(struct ssb_device *dev)
4876 {
4877         struct b43_wldev *wldev = ssb_get_drvdata(dev);
4878         struct b43_wl *wl = wldev->wl;
4879         int err = 0;
4880
4881         b43dbg(wl, "Resuming...\n");
4882
4883         mutex_lock(&wl->mutex);
4884         if (wldev->suspend_init_status >= B43_STAT_INITIALIZED) {
4885                 err = b43_wireless_core_init(wldev);
4886                 if (err) {
4887                         b43err(wl, "Resume failed at core init\n");
4888                         goto out;
4889                 }
4890         }
4891         if (wldev->suspend_init_status >= B43_STAT_STARTED) {
4892                 err = b43_wireless_core_start(wldev);
4893                 if (err) {
4894                         b43_leds_exit(wldev);
4895                         b43_rng_exit(wldev->wl);
4896                         b43_wireless_core_exit(wldev);
4897                         b43err(wl, "Resume failed at core start\n");
4898                         goto out;
4899                 }
4900         }
4901         b43dbg(wl, "Device resumed.\n");
4902  out:
4903         wldev->suspend_in_progress = false;
4904         mutex_unlock(&wl->mutex);
4905         return err;
4906 }
4907
4908 #else /* CONFIG_PM */
4909 # define b43_suspend    NULL
4910 # define b43_resume     NULL
4911 #endif /* CONFIG_PM */
4912
4913 static struct ssb_driver b43_ssb_driver = {
4914         .name           = KBUILD_MODNAME,
4915         .id_table       = b43_ssb_tbl,
4916         .probe          = b43_probe,
4917         .remove         = b43_remove,
4918         .suspend        = b43_suspend,
4919         .resume         = b43_resume,
4920 };
4921
4922 static void b43_print_driverinfo(void)
4923 {
4924         const char *feat_pci = "", *feat_pcmcia = "", *feat_nphy = "",
4925                    *feat_leds = "", *feat_rfkill = "";
4926
4927 #ifdef CONFIG_B43_PCI_AUTOSELECT
4928         feat_pci = "P";
4929 #endif
4930 #ifdef CONFIG_B43_PCMCIA
4931         feat_pcmcia = "M";
4932 #endif
4933 #ifdef CONFIG_B43_NPHY
4934         feat_nphy = "N";
4935 #endif
4936 #ifdef CONFIG_B43_LEDS
4937         feat_leds = "L";
4938 #endif
4939 #ifdef CONFIG_B43_RFKILL
4940         feat_rfkill = "R";
4941 #endif
4942         printk(KERN_INFO "Broadcom 43xx driver loaded "
4943                "[ Features: %s%s%s%s%s, Firmware-ID: "
4944                B43_SUPPORTED_FIRMWARE_ID " ]\n",
4945                feat_pci, feat_pcmcia, feat_nphy,
4946                feat_leds, feat_rfkill);
4947 }
4948
4949 static int __init b43_init(void)
4950 {
4951         int err;
4952
4953         b43_debugfs_init();
4954         err = b43_pcmcia_init();
4955         if (err)
4956                 goto err_dfs_exit;
4957         err = ssb_driver_register(&b43_ssb_driver);
4958         if (err)
4959                 goto err_pcmcia_exit;
4960         b43_print_driverinfo();
4961
4962         return err;
4963
4964 err_pcmcia_exit:
4965         b43_pcmcia_exit();
4966 err_dfs_exit:
4967         b43_debugfs_exit();
4968         return err;
4969 }
4970
4971 static void __exit b43_exit(void)
4972 {
4973         ssb_driver_unregister(&b43_ssb_driver);
4974         b43_pcmcia_exit();
4975         b43_debugfs_exit();
4976 }
4977
4978 module_init(b43_init)
4979 module_exit(b43_exit)