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