drivers/net/wireless/b43/main.c

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