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