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         /* FIXME this is the wrong offset : it goes in tkip rx phase1 shm */
1528 #if 0
1529         b43_write_probe_resp_plcp(dev, 0x31A, size, &b43_b_ratetable[0]);
1530         b43_write_probe_resp_plcp(dev, 0x32C, size, &b43_b_ratetable[1]);
1531         b43_write_probe_resp_plcp(dev, 0x33E, size, &b43_b_ratetable[2]);
1532         b43_write_probe_resp_plcp(dev, 0x350, size, &b43_b_ratetable[3]);
1533 #endif
1534
1535         size = min((size_t) size, 0x200 - sizeof(struct b43_plcp_hdr6));
1536         b43_write_template_common(dev, probe_resp_data,
1537                                   size, ram_offset, shm_size_offset,
1538                                   rate->hw_value);
1539         kfree(probe_resp_data);
1540 }
1541
1542 static void b43_upload_beacon0(struct b43_wldev *dev)
1543 {
1544         struct b43_wl *wl = dev->wl;
1545
1546         if (wl->beacon0_uploaded)
1547                 return;
1548         b43_write_beacon_template(dev, 0x68, 0x18);
1549         /* FIXME: Probe resp upload doesn't really belong here,
1550          *        but we don't use that feature anyway. */
1551         b43_write_probe_resp_template(dev, 0x268, 0x4A,
1552                                       &__b43_ratetable[3]);
1553         wl->beacon0_uploaded = 1;
1554 }
1555
1556 static void b43_upload_beacon1(struct b43_wldev *dev)
1557 {
1558         struct b43_wl *wl = dev->wl;
1559
1560         if (wl->beacon1_uploaded)
1561                 return;
1562         b43_write_beacon_template(dev, 0x468, 0x1A);
1563         wl->beacon1_uploaded = 1;
1564 }
1565
1566 static void handle_irq_beacon(struct b43_wldev *dev)
1567 {
1568         struct b43_wl *wl = dev->wl;
1569         u32 cmd, beacon0_valid, beacon1_valid;
1570
1571         if (!b43_is_mode(wl, NL80211_IFTYPE_AP) &&
1572             !b43_is_mode(wl, NL80211_IFTYPE_MESH_POINT))
1573                 return;
1574
1575         /* This is the bottom half of the asynchronous beacon update. */
1576
1577         /* Ignore interrupt in the future. */
1578         dev->irq_mask &= ~B43_IRQ_BEACON;
1579
1580         cmd = b43_read32(dev, B43_MMIO_MACCMD);
1581         beacon0_valid = (cmd & B43_MACCMD_BEACON0_VALID);
1582         beacon1_valid = (cmd & B43_MACCMD_BEACON1_VALID);
1583
1584         /* Schedule interrupt manually, if busy. */
1585         if (beacon0_valid && beacon1_valid) {
1586                 b43_write32(dev, B43_MMIO_GEN_IRQ_REASON, B43_IRQ_BEACON);
1587                 dev->irq_mask |= B43_IRQ_BEACON;
1588                 return;
1589         }
1590
1591         if (unlikely(wl->beacon_templates_virgin)) {
1592                 /* We never uploaded a beacon before.
1593                  * Upload both templates now, but only mark one valid. */
1594                 wl->beacon_templates_virgin = 0;
1595                 b43_upload_beacon0(dev);
1596                 b43_upload_beacon1(dev);
1597                 cmd = b43_read32(dev, B43_MMIO_MACCMD);
1598                 cmd |= B43_MACCMD_BEACON0_VALID;
1599                 b43_write32(dev, B43_MMIO_MACCMD, cmd);
1600         } else {
1601                 if (!beacon0_valid) {
1602                         b43_upload_beacon0(dev);
1603                         cmd = b43_read32(dev, B43_MMIO_MACCMD);
1604                         cmd |= B43_MACCMD_BEACON0_VALID;
1605                         b43_write32(dev, B43_MMIO_MACCMD, cmd);
1606                 } else if (!beacon1_valid) {
1607                         b43_upload_beacon1(dev);
1608                         cmd = b43_read32(dev, B43_MMIO_MACCMD);
1609                         cmd |= B43_MACCMD_BEACON1_VALID;
1610                         b43_write32(dev, B43_MMIO_MACCMD, cmd);
1611                 }
1612         }
1613 }
1614
1615 static void b43_beacon_update_trigger_work(struct work_struct *work)
1616 {
1617         struct b43_wl *wl = container_of(work, struct b43_wl,
1618                                          beacon_update_trigger);
1619         struct b43_wldev *dev;
1620
1621         mutex_lock(&wl->mutex);
1622         dev = wl->current_dev;
1623         if (likely(dev && (b43_status(dev) >= B43_STAT_INITIALIZED))) {
1624                 spin_lock_irq(&wl->irq_lock);
1625                 /* update beacon right away or defer to irq */
1626                 handle_irq_beacon(dev);
1627                 /* The handler might have updated the IRQ mask. */
1628                 b43_write32(dev, B43_MMIO_GEN_IRQ_MASK, dev->irq_mask);
1629                 mmiowb();
1630                 spin_unlock_irq(&wl->irq_lock);
1631         }
1632         mutex_unlock(&wl->mutex);
1633 }
1634
1635 /* Asynchronously update the packet templates in template RAM.
1636  * Locking: Requires wl->irq_lock to be locked. */
1637 static void b43_update_templates(struct b43_wl *wl)
1638 {
1639         struct sk_buff *beacon;
1640
1641         /* This is the top half of the ansynchronous beacon update.
1642          * The bottom half is the beacon IRQ.
1643          * Beacon update must be asynchronous to avoid sending an
1644          * invalid beacon. This can happen for example, if the firmware
1645          * transmits a beacon while we are updating it. */
1646
1647         /* We could modify the existing beacon and set the aid bit in
1648          * the TIM field, but that would probably require resizing and
1649          * moving of data within the beacon template.
1650          * Simply request a new beacon and let mac80211 do the hard work. */
1651         beacon = ieee80211_beacon_get(wl->hw, wl->vif);
1652         if (unlikely(!beacon))
1653                 return;
1654
1655         if (wl->current_beacon)
1656                 dev_kfree_skb_any(wl->current_beacon);
1657         wl->current_beacon = beacon;
1658         wl->beacon0_uploaded = 0;
1659         wl->beacon1_uploaded = 0;
1660         ieee80211_queue_work(wl->hw, &wl->beacon_update_trigger);
1661 }
1662
1663 static void b43_set_beacon_int(struct b43_wldev *dev, u16 beacon_int)
1664 {
1665         b43_time_lock(dev);
1666         if (dev->dev->id.revision >= 3) {
1667                 b43_write32(dev, B43_MMIO_TSF_CFP_REP, (beacon_int << 16));
1668                 b43_write32(dev, B43_MMIO_TSF_CFP_START, (beacon_int << 10));
1669         } else {
1670                 b43_write16(dev, 0x606, (beacon_int >> 6));
1671                 b43_write16(dev, 0x610, beacon_int);
1672         }
1673         b43_time_unlock(dev);
1674         b43dbg(dev->wl, "Set beacon interval to %u\n", beacon_int);
1675 }
1676
1677 static void b43_handle_firmware_panic(struct b43_wldev *dev)
1678 {
1679         u16 reason;
1680
1681         /* Read the register that contains the reason code for the panic. */
1682         reason = b43_shm_read16(dev, B43_SHM_SCRATCH, B43_FWPANIC_REASON_REG);
1683         b43err(dev->wl, "Whoopsy, firmware panic! Reason: %u\n", reason);
1684
1685         switch (reason) {
1686         default:
1687                 b43dbg(dev->wl, "The panic reason is unknown.\n");
1688                 /* fallthrough */
1689         case B43_FWPANIC_DIE:
1690                 /* Do not restart the controller or firmware.
1691                  * The device is nonfunctional from now on.
1692                  * Restarting would result in this panic to trigger again,
1693                  * so we avoid that recursion. */
1694                 break;
1695         case B43_FWPANIC_RESTART:
1696                 b43_controller_restart(dev, "Microcode panic");
1697                 break;
1698         }
1699 }
1700
1701 static void handle_irq_ucode_debug(struct b43_wldev *dev)
1702 {
1703         unsigned int i, cnt;
1704         u16 reason, marker_id, marker_line;
1705         __le16 *buf;
1706
1707         /* The proprietary firmware doesn't have this IRQ. */
1708         if (!dev->fw.opensource)
1709                 return;
1710
1711         /* Read the register that contains the reason code for this IRQ. */
1712         reason = b43_shm_read16(dev, B43_SHM_SCRATCH, B43_DEBUGIRQ_REASON_REG);
1713
1714         switch (reason) {
1715         case B43_DEBUGIRQ_PANIC:
1716                 b43_handle_firmware_panic(dev);
1717                 break;
1718         case B43_DEBUGIRQ_DUMP_SHM:
1719                 if (!B43_DEBUG)
1720                         break; /* Only with driver debugging enabled. */
1721                 buf = kmalloc(4096, GFP_ATOMIC);
1722                 if (!buf) {
1723                         b43dbg(dev->wl, "SHM-dump: Failed to allocate memory\n");
1724                         goto out;
1725                 }
1726                 for (i = 0; i < 4096; i += 2) {
1727                         u16 tmp = b43_shm_read16(dev, B43_SHM_SHARED, i);
1728                         buf[i / 2] = cpu_to_le16(tmp);
1729                 }
1730                 b43info(dev->wl, "Shared memory dump:\n");
1731                 print_hex_dump(KERN_INFO, "", DUMP_PREFIX_OFFSET,
1732                                16, 2, buf, 4096, 1);
1733                 kfree(buf);
1734                 break;
1735         case B43_DEBUGIRQ_DUMP_REGS:
1736                 if (!B43_DEBUG)
1737                         break; /* Only with driver debugging enabled. */
1738                 b43info(dev->wl, "Microcode register dump:\n");
1739                 for (i = 0, cnt = 0; i < 64; i++) {
1740                         u16 tmp = b43_shm_read16(dev, B43_SHM_SCRATCH, i);
1741                         if (cnt == 0)
1742                                 printk(KERN_INFO);
1743                         printk("r%02u: 0x%04X  ", i, tmp);
1744                         cnt++;
1745                         if (cnt == 6) {
1746                                 printk("\n");
1747                                 cnt = 0;
1748                         }
1749                 }
1750                 printk("\n");
1751                 break;
1752         case B43_DEBUGIRQ_MARKER:
1753                 if (!B43_DEBUG)
1754                         break; /* Only with driver debugging enabled. */
1755                 marker_id = b43_shm_read16(dev, B43_SHM_SCRATCH,
1756                                            B43_MARKER_ID_REG);
1757                 marker_line = b43_shm_read16(dev, B43_SHM_SCRATCH,
1758                                              B43_MARKER_LINE_REG);
1759                 b43info(dev->wl, "The firmware just executed the MARKER(%u) "
1760                         "at line number %u\n",
1761                         marker_id, marker_line);
1762                 break;
1763         default:
1764                 b43dbg(dev->wl, "Debug-IRQ triggered for unknown reason: %u\n",
1765                        reason);
1766         }
1767 out:
1768         /* Acknowledge the debug-IRQ, so the firmware can continue. */
1769         b43_shm_write16(dev, B43_SHM_SCRATCH,
1770                         B43_DEBUGIRQ_REASON_REG, B43_DEBUGIRQ_ACK);
1771 }
1772
1773 /* Interrupt handler bottom-half */
1774 static void b43_interrupt_tasklet(struct b43_wldev *dev)
1775 {
1776         u32 reason;
1777         u32 dma_reason[ARRAY_SIZE(dev->dma_reason)];
1778         u32 merged_dma_reason = 0;
1779         int i;
1780         unsigned long flags;
1781
1782         spin_lock_irqsave(&dev->wl->irq_lock, flags);
1783
1784         B43_WARN_ON(b43_status(dev) != B43_STAT_STARTED);
1785
1786         reason = dev->irq_reason;
1787         for (i = 0; i < ARRAY_SIZE(dma_reason); i++) {
1788                 dma_reason[i] = dev->dma_reason[i];
1789                 merged_dma_reason |= dma_reason[i];
1790         }
1791
1792         if (unlikely(reason & B43_IRQ_MAC_TXERR))
1793                 b43err(dev->wl, "MAC transmission error\n");
1794
1795         if (unlikely(reason & B43_IRQ_PHY_TXERR)) {
1796                 b43err(dev->wl, "PHY transmission error\n");
1797                 rmb();
1798                 if (unlikely(atomic_dec_and_test(&dev->phy.txerr_cnt))) {
1799                         atomic_set(&dev->phy.txerr_cnt,
1800                                    B43_PHY_TX_BADNESS_LIMIT);
1801                         b43err(dev->wl, "Too many PHY TX errors, "
1802                                         "restarting the controller\n");
1803                         b43_controller_restart(dev, "PHY TX errors");
1804                 }
1805         }
1806
1807         if (unlikely(merged_dma_reason & (B43_DMAIRQ_FATALMASK |
1808                                           B43_DMAIRQ_NONFATALMASK))) {
1809                 if (merged_dma_reason & B43_DMAIRQ_FATALMASK) {
1810                         b43err(dev->wl, "Fatal DMA error: "
1811                                "0x%08X, 0x%08X, 0x%08X, "
1812                                "0x%08X, 0x%08X, 0x%08X\n",
1813                                dma_reason[0], dma_reason[1],
1814                                dma_reason[2], dma_reason[3],
1815                                dma_reason[4], dma_reason[5]);
1816                         b43_controller_restart(dev, "DMA error");
1817                         mmiowb();
1818                         spin_unlock_irqrestore(&dev->wl->irq_lock, flags);
1819                         return;
1820                 }
1821                 if (merged_dma_reason & B43_DMAIRQ_NONFATALMASK) {
1822                         b43err(dev->wl, "DMA error: "
1823                                "0x%08X, 0x%08X, 0x%08X, "
1824                                "0x%08X, 0x%08X, 0x%08X\n",
1825                                dma_reason[0], dma_reason[1],
1826                                dma_reason[2], dma_reason[3],
1827                                dma_reason[4], dma_reason[5]);
1828                 }
1829         }
1830
1831         if (unlikely(reason & B43_IRQ_UCODE_DEBUG))
1832                 handle_irq_ucode_debug(dev);
1833         if (reason & B43_IRQ_TBTT_INDI)
1834                 handle_irq_tbtt_indication(dev);
1835         if (reason & B43_IRQ_ATIM_END)
1836                 handle_irq_atim_end(dev);
1837         if (reason & B43_IRQ_BEACON)
1838                 handle_irq_beacon(dev);
1839         if (reason & B43_IRQ_PMQ)
1840                 handle_irq_pmq(dev);
1841         if (reason & B43_IRQ_TXFIFO_FLUSH_OK)
1842                 ;/* TODO */
1843         if (reason & B43_IRQ_NOISESAMPLE_OK)
1844                 handle_irq_noise(dev);
1845
1846         /* Check the DMA reason registers for received data. */
1847         if (dma_reason[0] & B43_DMAIRQ_RX_DONE) {
1848                 if (b43_using_pio_transfers(dev))
1849                         b43_pio_rx(dev->pio.rx_queue);
1850                 else
1851                         b43_dma_rx(dev->dma.rx_ring);
1852         }
1853         B43_WARN_ON(dma_reason[1] & B43_DMAIRQ_RX_DONE);
1854         B43_WARN_ON(dma_reason[2] & B43_DMAIRQ_RX_DONE);
1855         B43_WARN_ON(dma_reason[3] & B43_DMAIRQ_RX_DONE);
1856         B43_WARN_ON(dma_reason[4] & B43_DMAIRQ_RX_DONE);
1857         B43_WARN_ON(dma_reason[5] & B43_DMAIRQ_RX_DONE);
1858
1859         if (reason & B43_IRQ_TX_OK)
1860                 handle_irq_transmit_status(dev);
1861
1862         b43_write32(dev, B43_MMIO_GEN_IRQ_MASK, dev->irq_mask);
1863         mmiowb();
1864         spin_unlock_irqrestore(&dev->wl->irq_lock, flags);
1865 }
1866
1867 static void b43_interrupt_ack(struct b43_wldev *dev, u32 reason)
1868 {
1869         b43_write32(dev, B43_MMIO_GEN_IRQ_REASON, reason);
1870
1871         b43_write32(dev, B43_MMIO_DMA0_REASON, dev->dma_reason[0]);
1872         b43_write32(dev, B43_MMIO_DMA1_REASON, dev->dma_reason[1]);
1873         b43_write32(dev, B43_MMIO_DMA2_REASON, dev->dma_reason[2]);
1874         b43_write32(dev, B43_MMIO_DMA3_REASON, dev->dma_reason[3]);
1875         b43_write32(dev, B43_MMIO_DMA4_REASON, dev->dma_reason[4]);
1876 /* Unused ring
1877         b43_write32(dev, B43_MMIO_DMA5_REASON, dev->dma_reason[5]);
1878 */
1879 }
1880
1881 /* Interrupt handler top-half */
1882 static irqreturn_t b43_interrupt_handler(int irq, void *dev_id)
1883 {
1884         irqreturn_t ret = IRQ_NONE;
1885         struct b43_wldev *dev = dev_id;
1886         u32 reason;
1887
1888         B43_WARN_ON(!dev);
1889
1890         spin_lock(&dev->wl->irq_lock);
1891
1892         if (unlikely(b43_status(dev) < B43_STAT_STARTED)) {
1893                 /* This can only happen on shared IRQ lines. */
1894                 goto out;
1895         }
1896         reason = b43_read32(dev, B43_MMIO_GEN_IRQ_REASON);
1897         if (reason == 0xffffffff)       /* shared IRQ */
1898                 goto out;
1899         ret = IRQ_HANDLED;
1900         reason &= dev->irq_mask;
1901         if (!reason)
1902                 goto out;
1903
1904         dev->dma_reason[0] = b43_read32(dev, B43_MMIO_DMA0_REASON)
1905             & 0x0001DC00;
1906         dev->dma_reason[1] = b43_read32(dev, B43_MMIO_DMA1_REASON)
1907             & 0x0000DC00;
1908         dev->dma_reason[2] = b43_read32(dev, B43_MMIO_DMA2_REASON)
1909             & 0x0000DC00;
1910         dev->dma_reason[3] = b43_read32(dev, B43_MMIO_DMA3_REASON)
1911             & 0x0001DC00;
1912         dev->dma_reason[4] = b43_read32(dev, B43_MMIO_DMA4_REASON)
1913             & 0x0000DC00;
1914 /* Unused ring
1915         dev->dma_reason[5] = b43_read32(dev, B43_MMIO_DMA5_REASON)
1916             & 0x0000DC00;
1917 */
1918
1919         b43_interrupt_ack(dev, reason);
1920         /* disable all IRQs. They are enabled again in the bottom half. */
1921         b43_write32(dev, B43_MMIO_GEN_IRQ_MASK, 0);
1922         /* save the reason code and call our bottom half. */
1923         dev->irq_reason = reason;
1924         tasklet_schedule(&dev->isr_tasklet);
1925 out:
1926         mmiowb();
1927         spin_unlock(&dev->wl->irq_lock);
1928
1929         return ret;
1930 }
1931
1932 void b43_do_release_fw(struct b43_firmware_file *fw)
1933 {
1934         release_firmware(fw->data);
1935         fw->data = NULL;
1936         fw->filename = NULL;
1937 }
1938
1939 static void b43_release_firmware(struct b43_wldev *dev)
1940 {
1941         b43_do_release_fw(&dev->fw.ucode);
1942         b43_do_release_fw(&dev->fw.pcm);
1943         b43_do_release_fw(&dev->fw.initvals);
1944         b43_do_release_fw(&dev->fw.initvals_band);
1945 }
1946
1947 static void b43_print_fw_helptext(struct b43_wl *wl, bool error)
1948 {
1949         const char text[] =
1950                 "You must go to " \
1951                 "http://wireless.kernel.org/en/users/Drivers/b43#devicefirmware " \
1952                 "and download the correct firmware for this driver version. " \
1953                 "Please carefully read all instructions on this website.\n";
1954
1955         if (error)
1956                 b43err(wl, text);
1957         else
1958                 b43warn(wl, text);
1959 }
1960
1961 int b43_do_request_fw(struct b43_request_fw_context *ctx,
1962                       const char *name,
1963                       struct b43_firmware_file *fw)
1964 {
1965         const struct firmware *blob;
1966         struct b43_fw_header *hdr;
1967         u32 size;
1968         int err;
1969
1970         if (!name) {
1971                 /* Don't fetch anything. Free possibly cached firmware. */
1972                 /* FIXME: We should probably keep it anyway, to save some headache
1973                  * on suspend/resume with multiband devices. */
1974                 b43_do_release_fw(fw);
1975                 return 0;
1976         }
1977         if (fw->filename) {
1978                 if ((fw->type == ctx->req_type) &&
1979                     (strcmp(fw->filename, name) == 0))
1980                         return 0; /* Already have this fw. */
1981                 /* Free the cached firmware first. */
1982                 /* FIXME: We should probably do this later after we successfully
1983                  * got the new fw. This could reduce headache with multiband devices.
1984                  * We could also redesign this to cache the firmware for all possible
1985                  * bands all the time. */
1986                 b43_do_release_fw(fw);
1987         }
1988
1989         switch (ctx->req_type) {
1990         case B43_FWTYPE_PROPRIETARY:
1991                 snprintf(ctx->fwname, sizeof(ctx->fwname),
1992                          "b43%s/%s.fw",
1993                          modparam_fwpostfix, name);
1994                 break;
1995         case B43_FWTYPE_OPENSOURCE:
1996                 snprintf(ctx->fwname, sizeof(ctx->fwname),
1997                          "b43-open%s/%s.fw",
1998                          modparam_fwpostfix, name);
1999                 break;
2000         default:
2001                 B43_WARN_ON(1);
2002                 return -ENOSYS;
2003         }
2004         err = request_firmware(&blob, ctx->fwname, ctx->dev->dev->dev);
2005         if (err == -ENOENT) {
2006                 snprintf(ctx->errors[ctx->req_type],
2007                          sizeof(ctx->errors[ctx->req_type]),
2008                          "Firmware file \"%s\" not found\n", ctx->fwname);
2009                 return err;
2010         } else if (err) {
2011                 snprintf(ctx->errors[ctx->req_type],
2012                          sizeof(ctx->errors[ctx->req_type]),
2013                          "Firmware file \"%s\" request failed (err=%d)\n",
2014                          ctx->fwname, err);
2015                 return err;
2016         }
2017         if (blob->size < sizeof(struct b43_fw_header))
2018                 goto err_format;
2019         hdr = (struct b43_fw_header *)(blob->data);
2020         switch (hdr->type) {
2021         case B43_FW_TYPE_UCODE:
2022         case B43_FW_TYPE_PCM:
2023                 size = be32_to_cpu(hdr->size);
2024                 if (size != blob->size - sizeof(struct b43_fw_header))
2025                         goto err_format;
2026                 /* fallthrough */
2027         case B43_FW_TYPE_IV:
2028                 if (hdr->ver != 1)
2029                         goto err_format;
2030                 break;
2031         default:
2032                 goto err_format;
2033         }
2034
2035         fw->data = blob;
2036         fw->filename = name;
2037         fw->type = ctx->req_type;
2038
2039         return 0;
2040
2041 err_format:
2042         snprintf(ctx->errors[ctx->req_type],
2043                  sizeof(ctx->errors[ctx->req_type]),
2044                  "Firmware file \"%s\" format error.\n", ctx->fwname);
2045         release_firmware(blob);
2046
2047         return -EPROTO;
2048 }
2049
2050 static int b43_try_request_fw(struct b43_request_fw_context *ctx)
2051 {
2052         struct b43_wldev *dev = ctx->dev;
2053         struct b43_firmware *fw = &ctx->dev->fw;
2054         const u8 rev = ctx->dev->dev->id.revision;
2055         const char *filename;
2056         u32 tmshigh;
2057         int err;
2058
2059         /* Get microcode */
2060         tmshigh = ssb_read32(dev->dev, SSB_TMSHIGH);
2061         if ((rev >= 5) && (rev <= 10))
2062                 filename = "ucode5";
2063         else if ((rev >= 11) && (rev <= 12))
2064                 filename = "ucode11";
2065         else if (rev >= 13)
2066                 filename = "ucode13";
2067         else
2068                 goto err_no_ucode;
2069         err = b43_do_request_fw(ctx, filename, &fw->ucode);
2070         if (err)
2071                 goto err_load;
2072
2073         /* Get PCM code */
2074         if ((rev >= 5) && (rev <= 10))
2075                 filename = "pcm5";
2076         else if (rev >= 11)
2077                 filename = NULL;
2078         else
2079                 goto err_no_pcm;
2080         fw->pcm_request_failed = 0;
2081         err = b43_do_request_fw(ctx, filename, &fw->pcm);
2082         if (err == -ENOENT) {
2083                 /* We did not find a PCM file? Not fatal, but
2084                  * core rev <= 10 must do without hwcrypto then. */
2085                 fw->pcm_request_failed = 1;
2086         } else if (err)
2087                 goto err_load;
2088
2089         /* Get initvals */
2090         switch (dev->phy.type) {
2091         case B43_PHYTYPE_A:
2092                 if ((rev >= 5) && (rev <= 10)) {
2093                         if (tmshigh & B43_TMSHIGH_HAVE_2GHZ_PHY)
2094                                 filename = "a0g1initvals5";
2095                         else
2096                                 filename = "a0g0initvals5";
2097                 } else
2098                         goto err_no_initvals;
2099                 break;
2100         case B43_PHYTYPE_G:
2101                 if ((rev >= 5) && (rev <= 10))
2102                         filename = "b0g0initvals5";
2103                 else if (rev >= 13)
2104                         filename = "b0g0initvals13";
2105                 else
2106                         goto err_no_initvals;
2107                 break;
2108         case B43_PHYTYPE_N:
2109                 if ((rev >= 11) && (rev <= 12))
2110                         filename = "n0initvals11";
2111                 else
2112                         goto err_no_initvals;
2113                 break;
2114         default:
2115                 goto err_no_initvals;
2116         }
2117         err = b43_do_request_fw(ctx, filename, &fw->initvals);
2118         if (err)
2119                 goto err_load;
2120
2121         /* Get bandswitch initvals */
2122         switch (dev->phy.type) {
2123         case B43_PHYTYPE_A:
2124                 if ((rev >= 5) && (rev <= 10)) {
2125                         if (tmshigh & B43_TMSHIGH_HAVE_2GHZ_PHY)
2126                                 filename = "a0g1bsinitvals5";
2127                         else
2128                                 filename = "a0g0bsinitvals5";
2129                 } else if (rev >= 11)
2130                         filename = NULL;
2131                 else
2132                         goto err_no_initvals;
2133                 break;
2134         case B43_PHYTYPE_G:
2135                 if ((rev >= 5) && (rev <= 10))
2136                         filename = "b0g0bsinitvals5";
2137                 else if (rev >= 11)
2138                         filename = NULL;
2139                 else
2140                         goto err_no_initvals;
2141                 break;
2142         case B43_PHYTYPE_N:
2143                 if ((rev >= 11) && (rev <= 12))
2144                         filename = "n0bsinitvals11";
2145                 else
2146                         goto err_no_initvals;
2147                 break;
2148         default:
2149                 goto err_no_initvals;
2150         }
2151         err = b43_do_request_fw(ctx, filename, &fw->initvals_band);
2152         if (err)
2153                 goto err_load;
2154
2155         return 0;
2156
2157 err_no_ucode:
2158         err = ctx->fatal_failure = -EOPNOTSUPP;
2159         b43err(dev->wl, "The driver does not know which firmware (ucode) "
2160                "is required for your device (wl-core rev %u)\n", rev);
2161         goto error;
2162
2163 err_no_pcm:
2164         err = ctx->fatal_failure = -EOPNOTSUPP;
2165         b43err(dev->wl, "The driver does not know which firmware (PCM) "
2166                "is required for your device (wl-core rev %u)\n", rev);
2167         goto error;
2168
2169 err_no_initvals:
2170         err = ctx->fatal_failure = -EOPNOTSUPP;
2171         b43err(dev->wl, "The driver does not know which firmware (initvals) "
2172                "is required for your device (wl-core rev %u)\n", rev);
2173         goto error;
2174
2175 err_load:
2176         /* We failed to load this firmware image. The error message
2177          * already is in ctx->errors. Return and let our caller decide
2178          * what to do. */
2179         goto error;
2180
2181 error:
2182         b43_release_firmware(dev);
2183         return err;
2184 }
2185
2186 static int b43_request_firmware(struct b43_wldev *dev)
2187 {
2188         struct b43_request_fw_context *ctx;
2189         unsigned int i;
2190         int err;
2191         const char *errmsg;
2192
2193         ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
2194         if (!ctx)
2195                 return -ENOMEM;
2196         ctx->dev = dev;
2197
2198         ctx->req_type = B43_FWTYPE_PROPRIETARY;
2199         err = b43_try_request_fw(ctx);
2200         if (!err)
2201                 goto out; /* Successfully loaded it. */
2202         err = ctx->fatal_failure;
2203         if (err)
2204                 goto out;
2205
2206         ctx->req_type = B43_FWTYPE_OPENSOURCE;
2207         err = b43_try_request_fw(ctx);
2208         if (!err)
2209                 goto out; /* Successfully loaded it. */
2210         err = ctx->fatal_failure;
2211         if (err)
2212                 goto out;
2213
2214         /* Could not find a usable firmware. Print the errors. */
2215         for (i = 0; i < B43_NR_FWTYPES; i++) {
2216                 errmsg = ctx->errors[i];
2217                 if (strlen(errmsg))
2218                         b43err(dev->wl, errmsg);
2219         }
2220         b43_print_fw_helptext(dev->wl, 1);
2221         err = -ENOENT;
2222
2223 out:
2224         kfree(ctx);
2225         return err;
2226 }
2227
2228 static int b43_upload_microcode(struct b43_wldev *dev)
2229 {
2230         const size_t hdr_len = sizeof(struct b43_fw_header);
2231         const __be32 *data;
2232         unsigned int i, len;
2233         u16 fwrev, fwpatch, fwdate, fwtime;
2234         u32 tmp, macctl;
2235         int err = 0;
2236
2237         /* Jump the microcode PSM to offset 0 */
2238         macctl = b43_read32(dev, B43_MMIO_MACCTL);
2239         B43_WARN_ON(macctl & B43_MACCTL_PSM_RUN);
2240         macctl |= B43_MACCTL_PSM_JMP0;
2241         b43_write32(dev, B43_MMIO_MACCTL, macctl);
2242         /* Zero out all microcode PSM registers and shared memory. */
2243         for (i = 0; i < 64; i++)
2244                 b43_shm_write16(dev, B43_SHM_SCRATCH, i, 0);
2245         for (i = 0; i < 4096; i += 2)
2246                 b43_shm_write16(dev, B43_SHM_SHARED, i, 0);
2247
2248         /* Upload Microcode. */
2249         data = (__be32 *) (dev->fw.ucode.data->data + hdr_len);
2250         len = (dev->fw.ucode.data->size - hdr_len) / sizeof(__be32);
2251         b43_shm_control_word(dev, B43_SHM_UCODE | B43_SHM_AUTOINC_W, 0x0000);
2252         for (i = 0; i < len; i++) {
2253                 b43_write32(dev, B43_MMIO_SHM_DATA, be32_to_cpu(data[i]));
2254                 udelay(10);
2255         }
2256
2257         if (dev->fw.pcm.data) {
2258                 /* Upload PCM data. */
2259                 data = (__be32 *) (dev->fw.pcm.data->data + hdr_len);
2260                 len = (dev->fw.pcm.data->size - hdr_len) / sizeof(__be32);
2261                 b43_shm_control_word(dev, B43_SHM_HW, 0x01EA);
2262                 b43_write32(dev, B43_MMIO_SHM_DATA, 0x00004000);
2263                 /* No need for autoinc bit in SHM_HW */
2264                 b43_shm_control_word(dev, B43_SHM_HW, 0x01EB);
2265                 for (i = 0; i < len; i++) {
2266                         b43_write32(dev, B43_MMIO_SHM_DATA, be32_to_cpu(data[i]));
2267                         udelay(10);
2268                 }
2269         }
2270
2271         b43_write32(dev, B43_MMIO_GEN_IRQ_REASON, B43_IRQ_ALL);
2272
2273         /* Start the microcode PSM */
2274         macctl = b43_read32(dev, B43_MMIO_MACCTL);
2275         macctl &= ~B43_MACCTL_PSM_JMP0;
2276         macctl |= B43_MACCTL_PSM_RUN;
2277         b43_write32(dev, B43_MMIO_MACCTL, macctl);
2278
2279         /* Wait for the microcode to load and respond */
2280         i = 0;
2281         while (1) {
2282                 tmp = b43_read32(dev, B43_MMIO_GEN_IRQ_REASON);
2283                 if (tmp == B43_IRQ_MAC_SUSPENDED)
2284                         break;
2285                 i++;
2286                 if (i >= 20) {
2287                         b43err(dev->wl, "Microcode not responding\n");
2288                         b43_print_fw_helptext(dev->wl, 1);
2289                         err = -ENODEV;
2290                         goto error;
2291                 }
2292                 msleep_interruptible(50);
2293                 if (signal_pending(current)) {
2294                         err = -EINTR;
2295                         goto error;
2296                 }
2297         }
2298         b43_read32(dev, B43_MMIO_GEN_IRQ_REASON);       /* dummy read */
2299
2300         /* Get and check the revisions. */
2301         fwrev = b43_shm_read16(dev, B43_SHM_SHARED, B43_SHM_SH_UCODEREV);
2302         fwpatch = b43_shm_read16(dev, B43_SHM_SHARED, B43_SHM_SH_UCODEPATCH);
2303         fwdate = b43_shm_read16(dev, B43_SHM_SHARED, B43_SHM_SH_UCODEDATE);
2304         fwtime = b43_shm_read16(dev, B43_SHM_SHARED, B43_SHM_SH_UCODETIME);
2305
2306         if (fwrev <= 0x128) {
2307                 b43err(dev->wl, "YOUR FIRMWARE IS TOO OLD. Firmware from "
2308                        "binary drivers older than version 4.x is unsupported. "
2309                        "You must upgrade your firmware files.\n");
2310                 b43_print_fw_helptext(dev->wl, 1);
2311                 err = -EOPNOTSUPP;
2312                 goto error;
2313         }
2314         dev->fw.rev = fwrev;
2315         dev->fw.patch = fwpatch;
2316         dev->fw.opensource = (fwdate == 0xFFFF);
2317
2318         /* Default to use-all-queues. */
2319         dev->wl->hw->queues = dev->wl->mac80211_initially_registered_queues;
2320         dev->qos_enabled = !!modparam_qos;
2321         /* Default to firmware/hardware crypto acceleration. */
2322         dev->hwcrypto_enabled = 1;
2323
2324         if (dev->fw.opensource) {
2325                 u16 fwcapa;
2326
2327                 /* Patchlevel info is encoded in the "time" field. */
2328                 dev->fw.patch = fwtime;
2329                 b43info(dev->wl, "Loading OpenSource firmware version %u.%u\n",
2330                         dev->fw.rev, dev->fw.patch);
2331
2332                 fwcapa = b43_fwcapa_read(dev);
2333                 if (!(fwcapa & B43_FWCAPA_HWCRYPTO) || dev->fw.pcm_request_failed) {
2334                         b43info(dev->wl, "Hardware crypto acceleration not supported by firmware\n");
2335                         /* Disable hardware crypto and fall back to software crypto. */
2336                         dev->hwcrypto_enabled = 0;
2337                 }
2338                 if (!(fwcapa & B43_FWCAPA_QOS)) {
2339                         b43info(dev->wl, "QoS not supported by firmware\n");
2340                         /* Disable QoS. Tweak hw->queues to 1. It will be restored before
2341                          * ieee80211_unregister to make sure the networking core can
2342                          * properly free possible resources. */
2343                         dev->wl->hw->queues = 1;
2344                         dev->qos_enabled = 0;
2345                 }
2346         } else {
2347                 b43info(dev->wl, "Loading firmware version %u.%u "
2348                         "(20%.2i-%.2i-%.2i %.2i:%.2i:%.2i)\n",
2349                         fwrev, fwpatch,
2350                         (fwdate >> 12) & 0xF, (fwdate >> 8) & 0xF, fwdate & 0xFF,
2351                         (fwtime >> 11) & 0x1F, (fwtime >> 5) & 0x3F, fwtime & 0x1F);
2352                 if (dev->fw.pcm_request_failed) {
2353                         b43warn(dev->wl, "No \"pcm5.fw\" firmware file found. "
2354                                 "Hardware accelerated cryptography is disabled.\n");
2355                         b43_print_fw_helptext(dev->wl, 0);
2356                 }
2357         }
2358
2359         if (b43_is_old_txhdr_format(dev)) {
2360                 /* We're over the deadline, but we keep support for old fw
2361                  * until it turns out to be in major conflict with something new. */
2362                 b43warn(dev->wl, "You are using an old firmware image. "
2363                         "Support for old firmware will be removed soon "
2364                         "(official deadline was July 2008).\n");
2365                 b43_print_fw_helptext(dev->wl, 0);
2366         }
2367
2368         return 0;
2369
2370 error:
2371         macctl = b43_read32(dev, B43_MMIO_MACCTL);
2372         macctl &= ~B43_MACCTL_PSM_RUN;
2373         macctl |= B43_MACCTL_PSM_JMP0;
2374         b43_write32(dev, B43_MMIO_MACCTL, macctl);
2375
2376         return err;
2377 }
2378
2379 static int b43_write_initvals(struct b43_wldev *dev,
2380                               const struct b43_iv *ivals,
2381                               size_t count,
2382                               size_t array_size)
2383 {
2384         const struct b43_iv *iv;
2385         u16 offset;
2386         size_t i;
2387         bool bit32;
2388
2389         BUILD_BUG_ON(sizeof(struct b43_iv) != 6);
2390         iv = ivals;
2391         for (i = 0; i < count; i++) {
2392                 if (array_size < sizeof(iv->offset_size))
2393                         goto err_format;
2394                 array_size -= sizeof(iv->offset_size);
2395                 offset = be16_to_cpu(iv->offset_size);
2396                 bit32 = !!(offset & B43_IV_32BIT);
2397                 offset &= B43_IV_OFFSET_MASK;
2398                 if (offset >= 0x1000)
2399                         goto err_format;
2400                 if (bit32) {
2401                         u32 value;
2402
2403                         if (array_size < sizeof(iv->data.d32))
2404                                 goto err_format;
2405                         array_size -= sizeof(iv->data.d32);
2406
2407                         value = get_unaligned_be32(&iv->data.d32);
2408                         b43_write32(dev, offset, value);
2409
2410                         iv = (const struct b43_iv *)((const uint8_t *)iv +
2411                                                         sizeof(__be16) +
2412                                                         sizeof(__be32));
2413                 } else {
2414                         u16 value;
2415
2416                         if (array_size < sizeof(iv->data.d16))
2417                                 goto err_format;
2418                         array_size -= sizeof(iv->data.d16);
2419
2420                         value = be16_to_cpu(iv->data.d16);
2421                         b43_write16(dev, offset, value);
2422
2423                         iv = (const struct b43_iv *)((const uint8_t *)iv +
2424                                                         sizeof(__be16) +
2425                                                         sizeof(__be16));
2426                 }
2427         }
2428         if (array_size)
2429                 goto err_format;
2430
2431         return 0;
2432
2433 err_format:
2434         b43err(dev->wl, "Initial Values Firmware file-format error.\n");
2435         b43_print_fw_helptext(dev->wl, 1);
2436
2437         return -EPROTO;
2438 }
2439
2440 static int b43_upload_initvals(struct b43_wldev *dev)
2441 {
2442         const size_t hdr_len = sizeof(struct b43_fw_header);
2443         const struct b43_fw_header *hdr;
2444         struct b43_firmware *fw = &dev->fw;
2445         const struct b43_iv *ivals;
2446         size_t count;
2447         int err;
2448
2449         hdr = (const struct b43_fw_header *)(fw->initvals.data->data);
2450         ivals = (const struct b43_iv *)(fw->initvals.data->data + hdr_len);
2451         count = be32_to_cpu(hdr->size);
2452         err = b43_write_initvals(dev, ivals, count,
2453                                  fw->initvals.data->size - hdr_len);
2454         if (err)
2455                 goto out;
2456         if (fw->initvals_band.data) {
2457                 hdr = (const struct b43_fw_header *)(fw->initvals_band.data->data);
2458                 ivals = (const struct b43_iv *)(fw->initvals_band.data->data + hdr_len);
2459                 count = be32_to_cpu(hdr->size);
2460                 err = b43_write_initvals(dev, ivals, count,
2461                                          fw->initvals_band.data->size - hdr_len);
2462                 if (err)
2463                         goto out;
2464         }
2465 out:
2466
2467         return err;
2468 }
2469
2470 /* Initialize the GPIOs
2471  * http://bcm-specs.sipsolutions.net/GPIO
2472  */
2473 static int b43_gpio_init(struct b43_wldev *dev)
2474 {
2475         struct ssb_bus *bus = dev->dev->bus;
2476         struct ssb_device *gpiodev, *pcidev = NULL;
2477         u32 mask, set;
2478
2479         b43_write32(dev, B43_MMIO_MACCTL, b43_read32(dev, B43_MMIO_MACCTL)
2480                     & ~B43_MACCTL_GPOUTSMSK);
2481
2482         b43_write16(dev, B43_MMIO_GPIO_MASK, b43_read16(dev, B43_MMIO_GPIO_MASK)
2483                     | 0x000F);
2484
2485         mask = 0x0000001F;
2486         set = 0x0000000F;
2487         if (dev->dev->bus->chip_id == 0x4301) {
2488                 mask |= 0x0060;
2489                 set |= 0x0060;
2490         }
2491         if (0 /* FIXME: conditional unknown */ ) {
2492                 b43_write16(dev, B43_MMIO_GPIO_MASK,
2493                             b43_read16(dev, B43_MMIO_GPIO_MASK)
2494                             | 0x0100);
2495                 mask |= 0x0180;
2496                 set |= 0x0180;
2497         }
2498         if (dev->dev->bus->sprom.boardflags_lo & B43_BFL_PACTRL) {
2499                 b43_write16(dev, B43_MMIO_GPIO_MASK,
2500                             b43_read16(dev, B43_MMIO_GPIO_MASK)
2501                             | 0x0200);
2502                 mask |= 0x0200;
2503                 set |= 0x0200;
2504         }
2505         if (dev->dev->id.revision >= 2)
2506                 mask |= 0x0010; /* FIXME: This is redundant. */
2507
2508 #ifdef CONFIG_SSB_DRIVER_PCICORE
2509         pcidev = bus->pcicore.dev;
2510 #endif
2511         gpiodev = bus->chipco.dev ? : pcidev;
2512         if (!gpiodev)
2513                 return 0;
2514         ssb_write32(gpiodev, B43_GPIO_CONTROL,
2515                     (ssb_read32(gpiodev, B43_GPIO_CONTROL)
2516                      & mask) | set);
2517
2518         return 0;
2519 }
2520
2521 /* Turn off all GPIO stuff. Call this on module unload, for example. */
2522 static void b43_gpio_cleanup(struct b43_wldev *dev)
2523 {
2524         struct ssb_bus *bus = dev->dev->bus;
2525         struct ssb_device *gpiodev, *pcidev = NULL;
2526
2527 #ifdef CONFIG_SSB_DRIVER_PCICORE
2528         pcidev = bus->pcicore.dev;
2529 #endif
2530         gpiodev = bus->chipco.dev ? : pcidev;
2531         if (!gpiodev)
2532                 return;
2533         ssb_write32(gpiodev, B43_GPIO_CONTROL, 0);
2534 }
2535
2536 /* http://bcm-specs.sipsolutions.net/EnableMac */
2537 void b43_mac_enable(struct b43_wldev *dev)
2538 {
2539         if (b43_debug(dev, B43_DBG_FIRMWARE)) {
2540                 u16 fwstate;
2541
2542                 fwstate = b43_shm_read16(dev, B43_SHM_SHARED,
2543                                          B43_SHM_SH_UCODESTAT);
2544                 if ((fwstate != B43_SHM_SH_UCODESTAT_SUSP) &&
2545                     (fwstate != B43_SHM_SH_UCODESTAT_SLEEP)) {
2546                         b43err(dev->wl, "b43_mac_enable(): The firmware "
2547                                "should be suspended, but current state is %u\n",
2548                                fwstate);
2549                 }
2550         }
2551
2552         dev->mac_suspended--;
2553         B43_WARN_ON(dev->mac_suspended < 0);
2554         if (dev->mac_suspended == 0) {
2555                 b43_write32(dev, B43_MMIO_MACCTL,
2556                             b43_read32(dev, B43_MMIO_MACCTL)
2557                             | B43_MACCTL_ENABLED);
2558                 b43_write32(dev, B43_MMIO_GEN_IRQ_REASON,
2559                             B43_IRQ_MAC_SUSPENDED);
2560                 /* Commit writes */
2561                 b43_read32(dev, B43_MMIO_MACCTL);
2562                 b43_read32(dev, B43_MMIO_GEN_IRQ_REASON);
2563                 b43_power_saving_ctl_bits(dev, 0);
2564         }
2565 }
2566
2567 /* http://bcm-specs.sipsolutions.net/SuspendMAC */
2568 void b43_mac_suspend(struct b43_wldev *dev)
2569 {
2570         int i;
2571         u32 tmp;
2572
2573         might_sleep();
2574         B43_WARN_ON(dev->mac_suspended < 0);
2575
2576         if (dev->mac_suspended == 0) {
2577                 b43_power_saving_ctl_bits(dev, B43_PS_AWAKE);
2578                 b43_write32(dev, B43_MMIO_MACCTL,
2579                             b43_read32(dev, B43_MMIO_MACCTL)
2580                             & ~B43_MACCTL_ENABLED);
2581                 /* force pci to flush the write */
2582                 b43_read32(dev, B43_MMIO_MACCTL);
2583                 for (i = 35; i; i--) {
2584                         tmp = b43_read32(dev, B43_MMIO_GEN_IRQ_REASON);
2585                         if (tmp & B43_IRQ_MAC_SUSPENDED)
2586                                 goto out;
2587                         udelay(10);
2588                 }
2589                 /* Hm, it seems this will take some time. Use msleep(). */
2590                 for (i = 40; i; i--) {
2591                         tmp = b43_read32(dev, B43_MMIO_GEN_IRQ_REASON);
2592                         if (tmp & B43_IRQ_MAC_SUSPENDED)
2593                                 goto out;
2594                         msleep(1);
2595                 }
2596                 b43err(dev->wl, "MAC suspend failed\n");
2597         }
2598 out:
2599         dev->mac_suspended++;
2600 }
2601
2602 static void b43_adjust_opmode(struct b43_wldev *dev)
2603 {
2604         struct b43_wl *wl = dev->wl;
2605         u32 ctl;
2606         u16 cfp_pretbtt;
2607
2608         ctl = b43_read32(dev, B43_MMIO_MACCTL);
2609         /* Reset status to STA infrastructure mode. */
2610         ctl &= ~B43_MACCTL_AP;
2611         ctl &= ~B43_MACCTL_KEEP_CTL;
2612         ctl &= ~B43_MACCTL_KEEP_BADPLCP;
2613         ctl &= ~B43_MACCTL_KEEP_BAD;
2614         ctl &= ~B43_MACCTL_PROMISC;
2615         ctl &= ~B43_MACCTL_BEACPROMISC;
2616         ctl |= B43_MACCTL_INFRA;
2617
2618         if (b43_is_mode(wl, NL80211_IFTYPE_AP) ||
2619             b43_is_mode(wl, NL80211_IFTYPE_MESH_POINT))
2620                 ctl |= B43_MACCTL_AP;
2621         else if (b43_is_mode(wl, NL80211_IFTYPE_ADHOC))
2622                 ctl &= ~B43_MACCTL_INFRA;
2623
2624         if (wl->filter_flags & FIF_CONTROL)
2625                 ctl |= B43_MACCTL_KEEP_CTL;
2626         if (wl->filter_flags & FIF_FCSFAIL)
2627                 ctl |= B43_MACCTL_KEEP_BAD;
2628         if (wl->filter_flags & FIF_PLCPFAIL)
2629                 ctl |= B43_MACCTL_KEEP_BADPLCP;
2630         if (wl->filter_flags & FIF_PROMISC_IN_BSS)
2631                 ctl |= B43_MACCTL_PROMISC;
2632         if (wl->filter_flags & FIF_BCN_PRBRESP_PROMISC)
2633                 ctl |= B43_MACCTL_BEACPROMISC;
2634
2635         /* Workaround: On old hardware the HW-MAC-address-filter
2636          * doesn't work properly, so always run promisc in filter
2637          * it in software. */
2638         if (dev->dev->id.revision <= 4)
2639                 ctl |= B43_MACCTL_PROMISC;
2640
2641         b43_write32(dev, B43_MMIO_MACCTL, ctl);
2642
2643         cfp_pretbtt = 2;
2644         if ((ctl & B43_MACCTL_INFRA) && !(ctl & B43_MACCTL_AP)) {
2645                 if (dev->dev->bus->chip_id == 0x4306 &&
2646                     dev->dev->bus->chip_rev == 3)
2647                         cfp_pretbtt = 100;
2648                 else
2649                         cfp_pretbtt = 50;
2650         }
2651         b43_write16(dev, 0x612, cfp_pretbtt);
2652 }
2653
2654 static void b43_rate_memory_write(struct b43_wldev *dev, u16 rate, int is_ofdm)
2655 {
2656         u16 offset;
2657
2658         if (is_ofdm) {
2659                 offset = 0x480;
2660                 offset += (b43_plcp_get_ratecode_ofdm(rate) & 0x000F) * 2;
2661         } else {
2662                 offset = 0x4C0;
2663                 offset += (b43_plcp_get_ratecode_cck(rate) & 0x000F) * 2;
2664         }
2665         b43_shm_write16(dev, B43_SHM_SHARED, offset + 0x20,
2666                         b43_shm_read16(dev, B43_SHM_SHARED, offset));
2667 }
2668
2669 static void b43_rate_memory_init(struct b43_wldev *dev)
2670 {
2671         switch (dev->phy.type) {
2672         case B43_PHYTYPE_A:
2673         case B43_PHYTYPE_G:
2674         case B43_PHYTYPE_N:
2675                 b43_rate_memory_write(dev, B43_OFDM_RATE_6MB, 1);
2676                 b43_rate_memory_write(dev, B43_OFDM_RATE_12MB, 1);
2677                 b43_rate_memory_write(dev, B43_OFDM_RATE_18MB, 1);
2678                 b43_rate_memory_write(dev, B43_OFDM_RATE_24MB, 1);
2679                 b43_rate_memory_write(dev, B43_OFDM_RATE_36MB, 1);
2680                 b43_rate_memory_write(dev, B43_OFDM_RATE_48MB, 1);
2681                 b43_rate_memory_write(dev, B43_OFDM_RATE_54MB, 1);
2682                 if (dev->phy.type == B43_PHYTYPE_A)
2683                         break;
2684                 /* fallthrough */
2685         case B43_PHYTYPE_B:
2686                 b43_rate_memory_write(dev, B43_CCK_RATE_1MB, 0);
2687                 b43_rate_memory_write(dev, B43_CCK_RATE_2MB, 0);
2688                 b43_rate_memory_write(dev, B43_CCK_RATE_5MB, 0);
2689                 b43_rate_memory_write(dev, B43_CCK_RATE_11MB, 0);
2690                 break;
2691         default:
2692                 B43_WARN_ON(1);
2693         }
2694 }
2695
2696 /* Set the default values for the PHY TX Control Words. */
2697 static void b43_set_phytxctl_defaults(struct b43_wldev *dev)
2698 {
2699         u16 ctl = 0;
2700
2701         ctl |= B43_TXH_PHY_ENC_CCK;
2702         ctl |= B43_TXH_PHY_ANT01AUTO;
2703         ctl |= B43_TXH_PHY_TXPWR;
2704
2705         b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_BEACPHYCTL, ctl);
2706         b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_ACKCTSPHYCTL, ctl);
2707         b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_PRPHYCTL, ctl);
2708 }
2709
2710 /* Set the TX-Antenna for management frames sent by firmware. */
2711 static void b43_mgmtframe_txantenna(struct b43_wldev *dev, int antenna)
2712 {
2713         u16 ant;
2714         u16 tmp;
2715
2716         ant = b43_antenna_to_phyctl(antenna);
2717
2718         /* For ACK/CTS */
2719         tmp = b43_shm_read16(dev, B43_SHM_SHARED, B43_SHM_SH_ACKCTSPHYCTL);
2720         tmp = (tmp & ~B43_TXH_PHY_ANT) | ant;
2721         b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_ACKCTSPHYCTL, tmp);
2722         /* For Probe Resposes */
2723         tmp = b43_shm_read16(dev, B43_SHM_SHARED, B43_SHM_SH_PRPHYCTL);
2724         tmp = (tmp & ~B43_TXH_PHY_ANT) | ant;
2725         b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_PRPHYCTL, tmp);
2726 }
2727
2728 /* This is the opposite of b43_chip_init() */
2729 static void b43_chip_exit(struct b43_wldev *dev)
2730 {
2731         b43_phy_exit(dev);
2732         b43_gpio_cleanup(dev);
2733         /* firmware is released later */
2734 }
2735
2736 /* Initialize the chip
2737  * http://bcm-specs.sipsolutions.net/ChipInit
2738  */
2739 static int b43_chip_init(struct b43_wldev *dev)
2740 {
2741         struct b43_phy *phy = &dev->phy;
2742         int err;
2743         u32 value32, macctl;
2744         u16 value16;
2745
2746         /* Initialize the MAC control */
2747         macctl = B43_MACCTL_IHR_ENABLED | B43_MACCTL_SHM_ENABLED;
2748         if (dev->phy.gmode)
2749                 macctl |= B43_MACCTL_GMODE;
2750         macctl |= B43_MACCTL_INFRA;
2751         b43_write32(dev, B43_MMIO_MACCTL, macctl);
2752
2753         err = b43_request_firmware(dev);
2754         if (err)
2755                 goto out;
2756         err = b43_upload_microcode(dev);
2757         if (err)
2758                 goto out;       /* firmware is released later */
2759
2760         err = b43_gpio_init(dev);
2761         if (err)
2762                 goto out;       /* firmware is released later */
2763
2764         err = b43_upload_initvals(dev);
2765         if (err)
2766                 goto err_gpio_clean;
2767
2768         /* Turn the Analog on and initialize the PHY. */
2769         phy->ops->switch_analog(dev, 1);
2770         err = b43_phy_init(dev);
2771         if (err)
2772                 goto err_gpio_clean;
2773
2774         /* Disable Interference Mitigation. */
2775         if (phy->ops->interf_mitigation)
2776                 phy->ops->interf_mitigation(dev, B43_INTERFMODE_NONE);
2777
2778         /* Select the antennae */
2779         if (phy->ops->set_rx_antenna)
2780                 phy->ops->set_rx_antenna(dev, B43_ANTENNA_DEFAULT);
2781         b43_mgmtframe_txantenna(dev, B43_ANTENNA_DEFAULT);
2782
2783         if (phy->type == B43_PHYTYPE_B) {
2784                 value16 = b43_read16(dev, 0x005E);
2785                 value16 |= 0x0004;
2786                 b43_write16(dev, 0x005E, value16);
2787         }
2788         b43_write32(dev, 0x0100, 0x01000000);
2789         if (dev->dev->id.revision < 5)
2790                 b43_write32(dev, 0x010C, 0x01000000);
2791
2792         b43_write32(dev, B43_MMIO_MACCTL, b43_read32(dev, B43_MMIO_MACCTL)
2793                     & ~B43_MACCTL_INFRA);
2794         b43_write32(dev, B43_MMIO_MACCTL, b43_read32(dev, B43_MMIO_MACCTL)
2795                     | B43_MACCTL_INFRA);
2796
2797         /* Probe Response Timeout value */
2798         /* FIXME: Default to 0, has to be set by ioctl probably... :-/ */
2799         b43_shm_write16(dev, B43_SHM_SHARED, 0x0074, 0x0000);
2800
2801         /* Initially set the wireless operation mode. */
2802         b43_adjust_opmode(dev);
2803
2804         if (dev->dev->id.revision < 3) {
2805                 b43_write16(dev, 0x060E, 0x0000);
2806                 b43_write16(dev, 0x0610, 0x8000);
2807                 b43_write16(dev, 0x0604, 0x0000);
2808                 b43_write16(dev, 0x0606, 0x0200);
2809         } else {
2810                 b43_write32(dev, 0x0188, 0x80000000);
2811                 b43_write32(dev, 0x018C, 0x02000000);
2812         }
2813         b43_write32(dev, B43_MMIO_GEN_IRQ_REASON, 0x00004000);
2814         b43_write32(dev, B43_MMIO_DMA0_IRQ_MASK, 0x0001DC00);
2815         b43_write32(dev, B43_MMIO_DMA1_IRQ_MASK, 0x0000DC00);
2816         b43_write32(dev, B43_MMIO_DMA2_IRQ_MASK, 0x0000DC00);
2817         b43_write32(dev, B43_MMIO_DMA3_IRQ_MASK, 0x0001DC00);
2818         b43_write32(dev, B43_MMIO_DMA4_IRQ_MASK, 0x0000DC00);
2819         b43_write32(dev, B43_MMIO_DMA5_IRQ_MASK, 0x0000DC00);
2820
2821         value32 = ssb_read32(dev->dev, SSB_TMSLOW);
2822         value32 |= 0x00100000;
2823         ssb_write32(dev->dev, SSB_TMSLOW, value32);
2824
2825         b43_write16(dev, B43_MMIO_POWERUP_DELAY,
2826                     dev->dev->bus->chipco.fast_pwrup_delay);
2827
2828         err = 0;
2829         b43dbg(dev->wl, "Chip initialized\n");
2830 out:
2831         return err;
2832
2833 err_gpio_clean:
2834         b43_gpio_cleanup(dev);
2835         return err;
2836 }
2837
2838 static void b43_periodic_every60sec(struct b43_wldev *dev)
2839 {
2840         const struct b43_phy_operations *ops = dev->phy.ops;
2841
2842         if (ops->pwork_60sec)
2843                 ops->pwork_60sec(dev);
2844
2845         /* Force check the TX power emission now. */
2846         b43_phy_txpower_check(dev, B43_TXPWR_IGNORE_TIME);
2847 }
2848
2849 static void b43_periodic_every30sec(struct b43_wldev *dev)
2850 {
2851         /* Update device statistics. */
2852         b43_calculate_link_quality(dev);
2853 }
2854
2855 static void b43_periodic_every15sec(struct b43_wldev *dev)
2856 {
2857         struct b43_phy *phy = &dev->phy;
2858         u16 wdr;
2859
2860         if (dev->fw.opensource) {
2861                 /* Check if the firmware is still alive.
2862                  * It will reset the watchdog counter to 0 in its idle loop. */
2863                 wdr = b43_shm_read16(dev, B43_SHM_SCRATCH, B43_WATCHDOG_REG);
2864                 if (unlikely(wdr)) {
2865                         b43err(dev->wl, "Firmware watchdog: The firmware died!\n");
2866                         b43_controller_restart(dev, "Firmware watchdog");
2867                         return;
2868                 } else {
2869                         b43_shm_write16(dev, B43_SHM_SCRATCH,
2870                                         B43_WATCHDOG_REG, 1);
2871                 }
2872         }
2873
2874         if (phy->ops->pwork_15sec)
2875                 phy->ops->pwork_15sec(dev);
2876
2877         atomic_set(&phy->txerr_cnt, B43_PHY_TX_BADNESS_LIMIT);
2878         wmb();
2879 }
2880
2881 static void do_periodic_work(struct b43_wldev *dev)
2882 {
2883         unsigned int state;
2884
2885         state = dev->periodic_state;
2886         if (state % 4 == 0)
2887                 b43_periodic_every60sec(dev);
2888         if (state % 2 == 0)
2889                 b43_periodic_every30sec(dev);
2890         b43_periodic_every15sec(dev);
2891 }
2892
2893 /* Periodic work locking policy:
2894  *      The whole periodic work handler is protected by
2895  *      wl->mutex. If another lock is needed somewhere in the
2896  *      pwork callchain, it's aquired in-place, where it's needed.
2897  */
2898 static void b43_periodic_work_handler(struct work_struct *work)
2899 {
2900         struct b43_wldev *dev = container_of(work, struct b43_wldev,
2901                                              periodic_work.work);
2902         struct b43_wl *wl = dev->wl;
2903         unsigned long delay;
2904
2905         mutex_lock(&wl->mutex);
2906
2907         if (unlikely(b43_status(dev) != B43_STAT_STARTED))
2908                 goto out;
2909         if (b43_debug(dev, B43_DBG_PWORK_STOP))
2910                 goto out_requeue;
2911
2912         do_periodic_work(dev);
2913
2914         dev->periodic_state++;
2915 out_requeue:
2916         if (b43_debug(dev, B43_DBG_PWORK_FAST))
2917                 delay = msecs_to_jiffies(50);
2918         else
2919                 delay = round_jiffies_relative(HZ * 15);
2920         ieee80211_queue_delayed_work(wl->hw, &dev->periodic_work, delay);
2921 out:
2922         mutex_unlock(&wl->mutex);
2923 }
2924
2925 static void b43_periodic_tasks_setup(struct b43_wldev *dev)
2926 {
2927         struct delayed_work *work = &dev->periodic_work;
2928
2929         dev->periodic_state = 0;
2930         INIT_DELAYED_WORK(work, b43_periodic_work_handler);
2931         ieee80211_queue_delayed_work(dev->wl->hw, work, 0);
2932 }
2933
2934 /* Check if communication with the device works correctly. */
2935 static int b43_validate_chipaccess(struct b43_wldev *dev)
2936 {
2937         u32 v, backup0, backup4;
2938
2939         backup0 = b43_shm_read32(dev, B43_SHM_SHARED, 0);
2940         backup4 = b43_shm_read32(dev, B43_SHM_SHARED, 4);
2941
2942         /* Check for read/write and endianness problems. */
2943         b43_shm_write32(dev, B43_SHM_SHARED, 0, 0x55AAAA55);
2944         if (b43_shm_read32(dev, B43_SHM_SHARED, 0) != 0x55AAAA55)
2945                 goto error;
2946         b43_shm_write32(dev, B43_SHM_SHARED, 0, 0xAA5555AA);
2947         if (b43_shm_read32(dev, B43_SHM_SHARED, 0) != 0xAA5555AA)
2948                 goto error;
2949
2950         /* Check if unaligned 32bit SHM_SHARED access works properly.
2951          * However, don't bail out on failure, because it's noncritical. */
2952         b43_shm_write16(dev, B43_SHM_SHARED, 0, 0x1122);
2953         b43_shm_write16(dev, B43_SHM_SHARED, 2, 0x3344);
2954         b43_shm_write16(dev, B43_SHM_SHARED, 4, 0x5566);
2955         b43_shm_write16(dev, B43_SHM_SHARED, 6, 0x7788);
2956         if (b43_shm_read32(dev, B43_SHM_SHARED, 2) != 0x55663344)
2957                 b43warn(dev->wl, "Unaligned 32bit SHM read access is broken\n");
2958         b43_shm_write32(dev, B43_SHM_SHARED, 2, 0xAABBCCDD);
2959         if (b43_shm_read16(dev, B43_SHM_SHARED, 0) != 0x1122 ||
2960             b43_shm_read16(dev, B43_SHM_SHARED, 2) != 0xCCDD ||
2961             b43_shm_read16(dev, B43_SHM_SHARED, 4) != 0xAABB ||
2962             b43_shm_read16(dev, B43_SHM_SHARED, 6) != 0x7788)
2963                 b43warn(dev->wl, "Unaligned 32bit SHM write access is broken\n");
2964
2965         b43_shm_write32(dev, B43_SHM_SHARED, 0, backup0);
2966         b43_shm_write32(dev, B43_SHM_SHARED, 4, backup4);
2967
2968         if ((dev->dev->id.revision >= 3) && (dev->dev->id.revision <= 10)) {
2969                 /* The 32bit register shadows the two 16bit registers
2970                  * with update sideeffects. Validate this. */
2971                 b43_write16(dev, B43_MMIO_TSF_CFP_START, 0xAAAA);
2972                 b43_write32(dev, B43_MMIO_TSF_CFP_START, 0xCCCCBBBB);
2973                 if (b43_read16(dev, B43_MMIO_TSF_CFP_START_LOW) != 0xBBBB)
2974                         goto error;
2975                 if (b43_read16(dev, B43_MMIO_TSF_CFP_START_HIGH) != 0xCCCC)
2976                         goto error;
2977         }
2978         b43_write32(dev, B43_MMIO_TSF_CFP_START, 0);
2979
2980         v = b43_read32(dev, B43_MMIO_MACCTL);
2981         v |= B43_MACCTL_GMODE;
2982         if (v != (B43_MACCTL_GMODE | B43_MACCTL_IHR_ENABLED))
2983                 goto error;
2984
2985         return 0;
2986 error:
2987         b43err(dev->wl, "Failed to validate the chipaccess\n");
2988         return -ENODEV;
2989 }
2990
2991 static void b43_security_init(struct b43_wldev *dev)
2992 {
2993         dev->max_nr_keys = (dev->dev->id.revision >= 5) ? 58 : 20;
2994         B43_WARN_ON(dev->max_nr_keys > ARRAY_SIZE(dev->key));
2995         dev->ktp = b43_shm_read16(dev, B43_SHM_SHARED, B43_SHM_SH_KTP);
2996         /* KTP is a word address, but we address SHM bytewise.
2997          * So multiply by two.
2998          */
2999         dev->ktp *= 2;
3000         if (dev->dev->id.revision >= 5) {
3001                 /* Number of RCMTA address slots */
3002                 b43_write16(dev, B43_MMIO_RCMTA_COUNT, dev->max_nr_keys - 8);
3003         }
3004         b43_clear_keys(dev);
3005 }
3006
3007 #ifdef CONFIG_B43_HWRNG
3008 static int b43_rng_read(struct hwrng *rng, u32 *data)
3009 {
3010         struct b43_wl *wl = (struct b43_wl *)rng->priv;
3011         unsigned long flags;
3012
3013         /* Don't take wl->mutex here, as it could deadlock with
3014          * hwrng internal locking. It's not needed to take
3015          * wl->mutex here, anyway. */
3016
3017         spin_lock_irqsave(&wl->irq_lock, flags);
3018         *data = b43_read16(wl->current_dev, B43_MMIO_RNG);
3019         spin_unlock_irqrestore(&wl->irq_lock, flags);
3020
3021         return (sizeof(u16));
3022 }
3023 #endif /* CONFIG_B43_HWRNG */
3024
3025 static void b43_rng_exit(struct b43_wl *wl)
3026 {
3027 #ifdef CONFIG_B43_HWRNG
3028         if (wl->rng_initialized)
3029                 hwrng_unregister(&wl->rng);
3030 #endif /* CONFIG_B43_HWRNG */
3031 }
3032
3033 static int b43_rng_init(struct b43_wl *wl)
3034 {
3035         int err = 0;
3036
3037 #ifdef CONFIG_B43_HWRNG
3038         snprintf(wl->rng_name, ARRAY_SIZE(wl->rng_name),
3039                  "%s_%s", KBUILD_MODNAME, wiphy_name(wl->hw->wiphy));
3040         wl->rng.name = wl->rng_name;
3041         wl->rng.data_read = b43_rng_read;
3042         wl->rng.priv = (unsigned long)wl;
3043         wl->rng_initialized = 1;
3044         err = hwrng_register(&wl->rng);
3045         if (err) {
3046                 wl->rng_initialized = 0;
3047                 b43err(wl, "Failed to register the random "
3048                        "number generator (%d)\n", err);
3049         }
3050 #endif /* CONFIG_B43_HWRNG */
3051
3052         return err;
3053 }
3054
3055 static int b43_op_tx(struct ieee80211_hw *hw,
3056                      struct sk_buff *skb)
3057 {
3058         struct b43_wl *wl = hw_to_b43_wl(hw);
3059         struct b43_wldev *dev = wl->current_dev;
3060         unsigned long flags;
3061         int err;
3062
3063         if (unlikely(skb->len < 2 + 2 + 6)) {
3064                 /* Too short, this can't be a valid frame. */
3065                 goto drop_packet;
3066         }
3067         B43_WARN_ON(skb_shinfo(skb)->nr_frags);
3068         if (unlikely(!dev))
3069                 goto drop_packet;
3070
3071         /* Transmissions on seperate queues can run concurrently. */
3072         read_lock_irqsave(&wl->tx_lock, flags);
3073
3074         err = -ENODEV;
3075         if (likely(b43_status(dev) >= B43_STAT_STARTED)) {
3076                 if (b43_using_pio_transfers(dev))
3077                         err = b43_pio_tx(dev, skb);
3078                 else
3079                         err = b43_dma_tx(dev, skb);
3080         }
3081
3082         read_unlock_irqrestore(&wl->tx_lock, flags);
3083
3084         if (unlikely(err))
3085                 goto drop_packet;
3086         return NETDEV_TX_OK;
3087
3088 drop_packet:
3089         /* We can not transmit this packet. Drop it. */
3090         dev_kfree_skb_any(skb);
3091         return NETDEV_TX_OK;
3092 }
3093
3094 /* Locking: wl->irq_lock */
3095 static void b43_qos_params_upload(struct b43_wldev *dev,
3096                                   const struct ieee80211_tx_queue_params *p,
3097                                   u16 shm_offset)
3098 {
3099         u16 params[B43_NR_QOSPARAMS];
3100         int bslots, tmp;
3101         unsigned int i;
3102
3103         bslots = b43_read16(dev, B43_MMIO_RNG) & p->cw_min;
3104
3105         memset(&params, 0, sizeof(params));
3106
3107         params[B43_QOSPARAM_TXOP] = p->txop * 32;
3108         params[B43_QOSPARAM_CWMIN] = p->cw_min;
3109         params[B43_QOSPARAM_CWMAX] = p->cw_max;
3110         params[B43_QOSPARAM_CWCUR] = p->cw_min;
3111         params[B43_QOSPARAM_AIFS] = p->aifs;
3112         params[B43_QOSPARAM_BSLOTS] = bslots;
3113         params[B43_QOSPARAM_REGGAP] = bslots + p->aifs;
3114
3115         for (i = 0; i < ARRAY_SIZE(params); i++) {
3116                 if (i == B43_QOSPARAM_STATUS) {
3117                         tmp = b43_shm_read16(dev, B43_SHM_SHARED,
3118                                              shm_offset + (i * 2));
3119                         /* Mark the parameters as updated. */
3120                         tmp |= 0x100;
3121                         b43_shm_write16(dev, B43_SHM_SHARED,
3122                                         shm_offset + (i * 2),
3123                                         tmp);
3124                 } else {
3125                         b43_shm_write16(dev, B43_SHM_SHARED,
3126                                         shm_offset + (i * 2),
3127                                         params[i]);
3128                 }
3129         }
3130 }
3131
3132 /* Mapping of mac80211 queue numbers to b43 QoS SHM offsets. */
3133 static const u16 b43_qos_shm_offsets[] = {
3134         /* [mac80211-queue-nr] = SHM_OFFSET, */
3135         [0] = B43_QOS_VOICE,
3136         [1] = B43_QOS_VIDEO,
3137         [2] = B43_QOS_BESTEFFORT,
3138         [3] = B43_QOS_BACKGROUND,
3139 };
3140
3141 /* Update all QOS parameters in hardware. */
3142 static void b43_qos_upload_all(struct b43_wldev *dev)
3143 {
3144         struct b43_wl *wl = dev->wl;
3145         struct b43_qos_params *params;
3146         unsigned int i;
3147
3148         BUILD_BUG_ON(ARRAY_SIZE(b43_qos_shm_offsets) !=
3149                      ARRAY_SIZE(wl->qos_params));
3150
3151         b43_mac_suspend(dev);
3152         for (i = 0; i < ARRAY_SIZE(wl->qos_params); i++) {
3153                 params = &(wl->qos_params[i]);
3154                 b43_qos_params_upload(dev, &(params->p),
3155                                       b43_qos_shm_offsets[i]);
3156         }
3157         b43_mac_enable(dev);
3158 }
3159
3160 static void b43_qos_clear(struct b43_wl *wl)
3161 {
3162         struct b43_qos_params *params;
3163         unsigned int i;
3164
3165         /* Initialize QoS parameters to sane defaults. */
3166
3167         BUILD_BUG_ON(ARRAY_SIZE(b43_qos_shm_offsets) !=
3168                      ARRAY_SIZE(wl->qos_params));
3169
3170         for (i = 0; i < ARRAY_SIZE(wl->qos_params); i++) {
3171                 params = &(wl->qos_params[i]);
3172
3173                 switch (b43_qos_shm_offsets[i]) {
3174                 case B43_QOS_VOICE:
3175                         params->p.txop = 0;
3176                         params->p.aifs = 2;
3177                         params->p.cw_min = 0x0001;
3178                         params->p.cw_max = 0x0001;
3179                         break;
3180                 case B43_QOS_VIDEO:
3181                         params->p.txop = 0;
3182                         params->p.aifs = 2;
3183                         params->p.cw_min = 0x0001;
3184                         params->p.cw_max = 0x0001;
3185                         break;
3186                 case B43_QOS_BESTEFFORT:
3187                         params->p.txop = 0;
3188                         params->p.aifs = 3;
3189                         params->p.cw_min = 0x0001;
3190                         params->p.cw_max = 0x03FF;
3191                         break;
3192                 case B43_QOS_BACKGROUND:
3193                         params->p.txop = 0;
3194                         params->p.aifs = 7;
3195                         params->p.cw_min = 0x0001;
3196                         params->p.cw_max = 0x03FF;
3197                         break;
3198                 default:
3199                         B43_WARN_ON(1);
3200                 }
3201         }
3202 }
3203
3204 /* Initialize the core's QOS capabilities */
3205 static void b43_qos_init(struct b43_wldev *dev)
3206 {
3207         /* Upload the current QOS parameters. */
3208         b43_qos_upload_all(dev);
3209
3210         /* Enable QOS support. */
3211         b43_hf_write(dev, b43_hf_read(dev) | B43_HF_EDCF);
3212         b43_write16(dev, B43_MMIO_IFSCTL,
3213                     b43_read16(dev, B43_MMIO_IFSCTL)
3214                     | B43_MMIO_IFSCTL_USE_EDCF);
3215 }
3216
3217 static int b43_op_conf_tx(struct ieee80211_hw *hw, u16 _queue,
3218                           const struct ieee80211_tx_queue_params *params)
3219 {
3220         struct b43_wl *wl = hw_to_b43_wl(hw);
3221         struct b43_wldev *dev;
3222         unsigned int queue = (unsigned int)_queue;
3223         int err = -ENODEV;
3224
3225         if (queue >= ARRAY_SIZE(wl->qos_params)) {
3226                 /* Queue not available or don't support setting
3227                  * params on this queue. Return success to not
3228                  * confuse mac80211. */
3229                 return 0;
3230         }
3231         BUILD_BUG_ON(ARRAY_SIZE(b43_qos_shm_offsets) !=
3232                      ARRAY_SIZE(wl->qos_params));
3233
3234         mutex_lock(&wl->mutex);
3235         dev = wl->current_dev;
3236         if (unlikely(!dev || (b43_status(dev) < B43_STAT_INITIALIZED)))
3237                 goto out_unlock;
3238
3239         memcpy(&(wl->qos_params[queue].p), params, sizeof(*params));
3240         b43_mac_suspend(dev);
3241         b43_qos_params_upload(dev, &(wl->qos_params[queue].p),
3242                               b43_qos_shm_offsets[queue]);
3243         b43_mac_enable(dev);
3244         err = 0;
3245
3246 out_unlock:
3247         mutex_unlock(&wl->mutex);
3248
3249         return err;
3250 }
3251
3252 static int b43_op_get_tx_stats(struct ieee80211_hw *hw,
3253                                struct ieee80211_tx_queue_stats *stats)
3254 {
3255         struct b43_wl *wl = hw_to_b43_wl(hw);
3256         struct b43_wldev *dev = wl->current_dev;
3257         unsigned long flags;
3258         int err = -ENODEV;
3259
3260         if (!dev)
3261                 goto out;
3262         spin_lock_irqsave(&wl->irq_lock, flags);
3263         if (likely(b43_status(dev) >= B43_STAT_STARTED)) {
3264                 if (b43_using_pio_transfers(dev))
3265                         b43_pio_get_tx_stats(dev, stats);
3266                 else
3267                         b43_dma_get_tx_stats(dev, stats);
3268                 err = 0;
3269         }
3270         spin_unlock_irqrestore(&wl->irq_lock, flags);
3271 out:
3272         return err;
3273 }
3274
3275 static int b43_op_get_stats(struct ieee80211_hw *hw,
3276                             struct ieee80211_low_level_stats *stats)
3277 {
3278         struct b43_wl *wl = hw_to_b43_wl(hw);
3279         unsigned long flags;
3280
3281         spin_lock_irqsave(&wl->irq_lock, flags);
3282         memcpy(stats, &wl->ieee_stats, sizeof(*stats));
3283         spin_unlock_irqrestore(&wl->irq_lock, flags);
3284
3285         return 0;
3286 }
3287
3288 static u64 b43_op_get_tsf(struct ieee80211_hw *hw)
3289 {
3290         struct b43_wl *wl = hw_to_b43_wl(hw);
3291         struct b43_wldev *dev;
3292         u64 tsf;
3293
3294         mutex_lock(&wl->mutex);
3295         spin_lock_irq(&wl->irq_lock);
3296         dev = wl->current_dev;
3297
3298         if (dev && (b43_status(dev) >= B43_STAT_INITIALIZED))
3299                 b43_tsf_read(dev, &tsf);
3300         else
3301                 tsf = 0;
3302
3303         spin_unlock_irq(&wl->irq_lock);
3304         mutex_unlock(&wl->mutex);
3305
3306         return tsf;
3307 }
3308
3309 static void b43_op_set_tsf(struct ieee80211_hw *hw, u64 tsf)
3310 {
3311         struct b43_wl *wl = hw_to_b43_wl(hw);
3312         struct b43_wldev *dev;
3313
3314         mutex_lock(&wl->mutex);
3315         spin_lock_irq(&wl->irq_lock);
3316         dev = wl->current_dev;
3317
3318         if (dev && (b43_status(dev) >= B43_STAT_INITIALIZED))
3319                 b43_tsf_write(dev, tsf);
3320
3321         spin_unlock_irq(&wl->irq_lock);
3322         mutex_unlock(&wl->mutex);
3323 }
3324
3325 static void b43_put_phy_into_reset(struct b43_wldev *dev)
3326 {
3327         struct ssb_device *sdev = dev->dev;
3328         u32 tmslow;
3329
3330         tmslow = ssb_read32(sdev, SSB_TMSLOW);
3331         tmslow &= ~B43_TMSLOW_GMODE;
3332         tmslow |= B43_TMSLOW_PHYRESET;
3333         tmslow |= SSB_TMSLOW_FGC;
3334         ssb_write32(sdev, SSB_TMSLOW, tmslow);
3335         msleep(1);
3336
3337         tmslow = ssb_read32(sdev, SSB_TMSLOW);
3338         tmslow &= ~SSB_TMSLOW_FGC;
3339         tmslow |= B43_TMSLOW_PHYRESET;
3340         ssb_write32(sdev, SSB_TMSLOW, tmslow);
3341         msleep(1);
3342 }
3343
3344 static const char *band_to_string(enum ieee80211_band band)
3345 {
3346         switch (band) {
3347         case IEEE80211_BAND_5GHZ:
3348                 return "5";
3349         case IEEE80211_BAND_2GHZ:
3350                 return "2.4";
3351         default:
3352                 break;
3353         }
3354         B43_WARN_ON(1);
3355         return "";
3356 }
3357
3358 /* Expects wl->mutex locked */
3359 static int b43_switch_band(struct b43_wl *wl, struct ieee80211_channel *chan)
3360 {
3361         struct b43_wldev *up_dev = NULL;
3362         struct b43_wldev *down_dev;
3363         struct b43_wldev *d;
3364         int err;
3365         bool uninitialized_var(gmode);
3366         int prev_status;
3367
3368         /* Find a device and PHY which supports the band. */
3369         list_for_each_entry(d, &wl->devlist, list) {
3370                 switch (chan->band) {
3371                 case IEEE80211_BAND_5GHZ:
3372                         if (d->phy.supports_5ghz) {
3373                                 up_dev = d;
3374                                 gmode = 0;
3375                         }
3376                         break;
3377                 case IEEE80211_BAND_2GHZ:
3378                         if (d->phy.supports_2ghz) {
3379                                 up_dev = d;
3380                                 gmode = 1;
3381                         }
3382                         break;
3383                 default:
3384                         B43_WARN_ON(1);
3385                         return -EINVAL;
3386                 }
3387                 if (up_dev)
3388                         break;
3389         }
3390         if (!up_dev) {
3391                 b43err(wl, "Could not find a device for %s-GHz band operation\n",
3392                        band_to_string(chan->band));
3393                 return -ENODEV;
3394         }
3395         if ((up_dev == wl->current_dev) &&
3396             (!!wl->current_dev->phy.gmode == !!gmode)) {
3397                 /* This device is already running. */
3398                 return 0;
3399         }
3400         b43dbg(wl, "Switching to %s-GHz band\n",
3401                band_to_string(chan->band));
3402         down_dev = wl->current_dev;
3403
3404         prev_status = b43_status(down_dev);
3405         /* Shutdown the currently running core. */
3406         if (prev_status >= B43_STAT_STARTED)
3407                 b43_wireless_core_stop(down_dev);
3408         if (prev_status >= B43_STAT_INITIALIZED)
3409                 b43_wireless_core_exit(down_dev);
3410
3411         if (down_dev != up_dev) {
3412                 /* We switch to a different core, so we put PHY into
3413                  * RESET on the old core. */
3414                 b43_put_phy_into_reset(down_dev);
3415         }
3416
3417         /* Now start the new core. */
3418         up_dev->phy.gmode = gmode;
3419         if (prev_status >= B43_STAT_INITIALIZED) {
3420                 err = b43_wireless_core_init(up_dev);
3421                 if (err) {
3422                         b43err(wl, "Fatal: Could not initialize device for "
3423                                "selected %s-GHz band\n",
3424                                band_to_string(chan->band));
3425                         goto init_failure;
3426                 }
3427         }
3428         if (prev_status >= B43_STAT_STARTED) {
3429                 err = b43_wireless_core_start(up_dev);
3430                 if (err) {
3431                         b43err(wl, "Fatal: Coult not start device for "
3432                                "selected %s-GHz band\n",
3433                                band_to_string(chan->band));
3434                         b43_wireless_core_exit(up_dev);
3435                         goto init_failure;
3436                 }
3437         }
3438         B43_WARN_ON(b43_status(up_dev) != prev_status);
3439
3440         wl->current_dev = up_dev;
3441
3442         return 0;
3443 init_failure:
3444         /* Whoops, failed to init the new core. No core is operating now. */
3445         wl->current_dev = NULL;
3446         return err;
3447 }
3448
3449 /* Write the short and long frame retry limit values. */
3450 static void b43_set_retry_limits(struct b43_wldev *dev,
3451                                  unsigned int short_retry,
3452                                  unsigned int long_retry)
3453 {
3454         /* The retry limit is a 4-bit counter. Enforce this to avoid overflowing
3455          * the chip-internal counter. */
3456         short_retry = min(short_retry, (unsigned int)0xF);
3457         long_retry = min(long_retry, (unsigned int)0xF);
3458
3459         b43_shm_write16(dev, B43_SHM_SCRATCH, B43_SHM_SC_SRLIMIT,
3460                         short_retry);
3461         b43_shm_write16(dev, B43_SHM_SCRATCH, B43_SHM_SC_LRLIMIT,
3462                         long_retry);
3463 }
3464
3465 static int b43_op_config(struct ieee80211_hw *hw, u32 changed)
3466 {
3467         struct b43_wl *wl = hw_to_b43_wl(hw);
3468         struct b43_wldev *dev;
3469         struct b43_phy *phy;
3470         struct ieee80211_conf *conf = &hw->conf;
3471         unsigned long flags;
3472         int antenna;
3473         int err = 0;
3474
3475         mutex_lock(&wl->mutex);
3476
3477         /* Switch the band (if necessary). This might change the active core. */
3478         err = b43_switch_band(wl, conf->channel);
3479         if (err)
3480                 goto out_unlock_mutex;
3481         dev = wl->current_dev;
3482         phy = &dev->phy;
3483
3484         b43_mac_suspend(dev);
3485
3486         if (changed & IEEE80211_CONF_CHANGE_RETRY_LIMITS)
3487                 b43_set_retry_limits(dev, conf->short_frame_max_tx_count,
3488                                           conf->long_frame_max_tx_count);
3489         changed &= ~IEEE80211_CONF_CHANGE_RETRY_LIMITS;
3490         if (!changed)
3491                 goto out_mac_enable;
3492
3493         /* Switch to the requested channel.
3494          * The firmware takes care of races with the TX handler. */
3495         if (conf->channel->hw_value != phy->channel)
3496                 b43_switch_channel(dev, conf->channel->hw_value);
3497
3498         dev->wl->radiotap_enabled = !!(conf->flags & IEEE80211_CONF_RADIOTAP);
3499
3500         /* Adjust the desired TX power level. */
3501         if (conf->power_level != 0) {
3502                 spin_lock_irqsave(&wl->irq_lock, flags);
3503                 if (conf->power_level != phy->desired_txpower) {
3504                         phy->desired_txpower = conf->power_level;
3505                         b43_phy_txpower_check(dev, B43_TXPWR_IGNORE_TIME |
3506                                                    B43_TXPWR_IGNORE_TSSI);
3507                 }
3508                 spin_unlock_irqrestore(&wl->irq_lock, flags);
3509         }
3510
3511         /* Antennas for RX and management frame TX. */
3512         antenna = B43_ANTENNA_DEFAULT;
3513         b43_mgmtframe_txantenna(dev, antenna);
3514         antenna = B43_ANTENNA_DEFAULT;
3515         if (phy->ops->set_rx_antenna)
3516                 phy->ops->set_rx_antenna(dev, antenna);
3517
3518         if (wl->radio_enabled != phy->radio_on) {
3519                 if (wl->radio_enabled) {
3520                         b43_software_rfkill(dev, false);
3521                         b43info(dev->wl, "Radio turned on by software\n");
3522                         if (!dev->radio_hw_enable) {
3523                                 b43info(dev->wl, "The hardware RF-kill button "
3524                                         "still turns the radio physically off. "
3525                                         "Press the button to turn it on.\n");
3526                         }
3527                 } else {
3528                         b43_software_rfkill(dev, true);
3529                         b43info(dev->wl, "Radio turned off by software\n");
3530                 }
3531         }
3532
3533 out_mac_enable:
3534         b43_mac_enable(dev);
3535 out_unlock_mutex:
3536         mutex_unlock(&wl->mutex);
3537
3538         return err;
3539 }
3540
3541 static void b43_update_basic_rates(struct b43_wldev *dev, u32 brates)
3542 {
3543         struct ieee80211_supported_band *sband =
3544                 dev->wl->hw->wiphy->bands[b43_current_band(dev->wl)];
3545         struct ieee80211_rate *rate;
3546         int i;
3547         u16 basic, direct, offset, basic_offset, rateptr;
3548
3549         for (i = 0; i < sband->n_bitrates; i++) {
3550                 rate = &sband->bitrates[i];
3551
3552                 if (b43_is_cck_rate(rate->hw_value)) {
3553                         direct = B43_SHM_SH_CCKDIRECT;
3554                         basic = B43_SHM_SH_CCKBASIC;
3555                         offset = b43_plcp_get_ratecode_cck(rate->hw_value);
3556                         offset &= 0xF;
3557                 } else {
3558                         direct = B43_SHM_SH_OFDMDIRECT;
3559                         basic = B43_SHM_SH_OFDMBASIC;
3560                         offset = b43_plcp_get_ratecode_ofdm(rate->hw_value);
3561                         offset &= 0xF;
3562                 }
3563
3564                 rate = ieee80211_get_response_rate(sband, brates, rate->bitrate);
3565
3566                 if (b43_is_cck_rate(rate->hw_value)) {
3567                         basic_offset = b43_plcp_get_ratecode_cck(rate->hw_value);
3568                         basic_offset &= 0xF;
3569                 } else {
3570                         basic_offset = b43_plcp_get_ratecode_ofdm(rate->hw_value);
3571                         basic_offset &= 0xF;
3572                 }
3573
3574                 /*
3575                  * Get the pointer that we need to point to
3576                  * from the direct map
3577                  */
3578                 rateptr = b43_shm_read16(dev, B43_SHM_SHARED,
3579                                          direct + 2 * basic_offset);
3580                 /* and write it to the basic map */
3581                 b43_shm_write16(dev, B43_SHM_SHARED, basic + 2 * offset,
3582                                 rateptr);
3583         }
3584 }
3585
3586 static void b43_op_bss_info_changed(struct ieee80211_hw *hw,
3587                                     struct ieee80211_vif *vif,
3588                                     struct ieee80211_bss_conf *conf,
3589                                     u32 changed)
3590 {
3591         struct b43_wl *wl = hw_to_b43_wl(hw);
3592         struct b43_wldev *dev;
3593         unsigned long flags;
3594
3595         mutex_lock(&wl->mutex);
3596
3597         dev = wl->current_dev;
3598         if (!dev || b43_status(dev) < B43_STAT_STARTED)
3599                 goto out_unlock_mutex;
3600
3601         B43_WARN_ON(wl->vif != vif);
3602
3603         spin_lock_irqsave(&wl->irq_lock, flags);
3604         if (changed & BSS_CHANGED_BSSID) {
3605                 if (conf->bssid)
3606                         memcpy(wl->bssid, conf->bssid, ETH_ALEN);
3607                 else
3608                         memset(wl->bssid, 0, ETH_ALEN);
3609         }
3610
3611         if (b43_status(dev) >= B43_STAT_INITIALIZED) {
3612                 if (changed & BSS_CHANGED_BEACON &&
3613                     (b43_is_mode(wl, NL80211_IFTYPE_AP) ||
3614                      b43_is_mode(wl, NL80211_IFTYPE_MESH_POINT) ||
3615                      b43_is_mode(wl, NL80211_IFTYPE_ADHOC)))
3616                         b43_update_templates(wl);
3617
3618                 if (changed & BSS_CHANGED_BSSID)
3619                         b43_write_mac_bssid_templates(dev);
3620         }
3621         spin_unlock_irqrestore(&wl->irq_lock, flags);
3622
3623         b43_mac_suspend(dev);
3624
3625         /* Update templates for AP/mesh mode. */
3626         if (changed & BSS_CHANGED_BEACON_INT &&
3627             (b43_is_mode(wl, NL80211_IFTYPE_AP) ||
3628              b43_is_mode(wl, NL80211_IFTYPE_MESH_POINT) ||
3629              b43_is_mode(wl, NL80211_IFTYPE_ADHOC)))
3630                 b43_set_beacon_int(dev, conf->beacon_int);
3631
3632         if (changed & BSS_CHANGED_BASIC_RATES)
3633                 b43_update_basic_rates(dev, conf->basic_rates);
3634
3635         if (changed & BSS_CHANGED_ERP_SLOT) {
3636                 if (conf->use_short_slot)
3637                         b43_short_slot_timing_enable(dev);
3638                 else
3639                         b43_short_slot_timing_disable(dev);
3640         }
3641
3642         b43_mac_enable(dev);
3643 out_unlock_mutex:
3644         mutex_unlock(&wl->mutex);
3645 }
3646
3647 static int b43_op_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
3648                           struct ieee80211_vif *vif, struct ieee80211_sta *sta,
3649                           struct ieee80211_key_conf *key)
3650 {
3651         struct b43_wl *wl = hw_to_b43_wl(hw);
3652         struct b43_wldev *dev;
3653         u8 algorithm;
3654         u8 index;
3655         int err;
3656         static const u8 bcast_addr[ETH_ALEN] = { 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };
3657
3658         if (modparam_nohwcrypt)
3659                 return -ENOSPC; /* User disabled HW-crypto */
3660
3661         mutex_lock(&wl->mutex);
3662         spin_lock_irq(&wl->irq_lock);
3663         write_lock(&wl->tx_lock);
3664         /* Why do we need all this locking here?
3665          * mutex     -> Every config operation must take it.
3666          * irq_lock  -> We modify the dev->key array, which is accessed
3667          *              in the IRQ handlers.
3668          * tx_lock   -> We modify the dev->key array, which is accessed
3669          *              in the TX handler.
3670          */
3671
3672         dev = wl->current_dev;
3673         err = -ENODEV;
3674         if (!dev || b43_status(dev) < B43_STAT_INITIALIZED)
3675                 goto out_unlock;
3676
3677         if (dev->fw.pcm_request_failed || !dev->hwcrypto_enabled) {
3678                 /* We don't have firmware for the crypto engine.
3679                  * Must use software-crypto. */
3680                 err = -EOPNOTSUPP;
3681                 goto out_unlock;
3682         }
3683
3684         err = -EINVAL;
3685         switch (key->alg) {
3686         case ALG_WEP:
3687                 if (key->keylen == WLAN_KEY_LEN_WEP40)
3688                         algorithm = B43_SEC_ALGO_WEP40;
3689                 else
3690                         algorithm = B43_SEC_ALGO_WEP104;
3691                 break;
3692         case ALG_TKIP:
3693                 algorithm = B43_SEC_ALGO_TKIP;
3694                 break;
3695         case ALG_CCMP:
3696                 algorithm = B43_SEC_ALGO_AES;
3697                 break;
3698         default:
3699                 B43_WARN_ON(1);
3700                 goto out_unlock;
3701         }
3702         index = (u8) (key->keyidx);
3703         if (index > 3)
3704                 goto out_unlock;
3705
3706         switch (cmd) {
3707         case SET_KEY:
3708                 if (algorithm == B43_SEC_ALGO_TKIP) {
3709                         /* FIXME: No TKIP hardware encryption for now. */
3710                         err = -EOPNOTSUPP;
3711                         goto out_unlock;
3712                 }
3713
3714                 if (key->flags & IEEE80211_KEY_FLAG_PAIRWISE) {
3715                         if (WARN_ON(!sta)) {
3716                                 err = -EOPNOTSUPP;
3717                                 goto out_unlock;
3718                         }
3719                         /* Pairwise key with an assigned MAC address. */
3720                         err = b43_key_write(dev, -1, algorithm,
3721                                             key->key, key->keylen,
3722                                             sta->addr, key);
3723                 } else {
3724                         /* Group key */
3725                         err = b43_key_write(dev, index, algorithm,
3726                                             key->key, key->keylen, NULL, key);
3727                 }
3728                 if (err)
3729                         goto out_unlock;
3730
3731                 if (algorithm == B43_SEC_ALGO_WEP40 ||
3732                     algorithm == B43_SEC_ALGO_WEP104) {
3733                         b43_hf_write(dev, b43_hf_read(dev) | B43_HF_USEDEFKEYS);
3734                 } else {
3735                         b43_hf_write(dev,
3736                                      b43_hf_read(dev) & ~B43_HF_USEDEFKEYS);
3737                 }
3738                 key->flags |= IEEE80211_KEY_FLAG_GENERATE_IV;
3739                 break;
3740         case DISABLE_KEY: {
3741                 err = b43_key_clear(dev, key->hw_key_idx);
3742                 if (err)
3743                         goto out_unlock;
3744                 break;
3745         }
3746         default:
3747                 B43_WARN_ON(1);
3748         }
3749
3750 out_unlock:
3751         if (!err) {
3752                 b43dbg(wl, "%s hardware based encryption for keyidx: %d, "
3753                        "mac: %pM\n",
3754                        cmd == SET_KEY ? "Using" : "Disabling", key->keyidx,
3755                        sta ? sta->addr : bcast_addr);
3756                 b43_dump_keymemory(dev);
3757         }
3758         write_unlock(&wl->tx_lock);
3759         spin_unlock_irq(&wl->irq_lock);
3760         mutex_unlock(&wl->mutex);
3761
3762         return err;
3763 }
3764
3765 static void b43_op_configure_filter(struct ieee80211_hw *hw,
3766                                     unsigned int changed, unsigned int *fflags,
3767                                     int mc_count, struct dev_addr_list *mc_list)
3768 {
3769         struct b43_wl *wl = hw_to_b43_wl(hw);
3770         struct b43_wldev *dev = wl->current_dev;
3771         unsigned long flags;
3772
3773         if (!dev) {
3774                 *fflags = 0;
3775                 return;
3776         }
3777
3778         spin_lock_irqsave(&wl->irq_lock, flags);
3779         *fflags &= FIF_PROMISC_IN_BSS |
3780                   FIF_ALLMULTI |
3781                   FIF_FCSFAIL |
3782                   FIF_PLCPFAIL |
3783                   FIF_CONTROL |
3784                   FIF_OTHER_BSS |
3785                   FIF_BCN_PRBRESP_PROMISC;
3786
3787         changed &= FIF_PROMISC_IN_BSS |
3788                    FIF_ALLMULTI |
3789                    FIF_FCSFAIL |
3790                    FIF_PLCPFAIL |
3791                    FIF_CONTROL |
3792                    FIF_OTHER_BSS |
3793                    FIF_BCN_PRBRESP_PROMISC;
3794
3795         wl->filter_flags = *fflags;
3796
3797         if (changed && b43_status(dev) >= B43_STAT_INITIALIZED)
3798                 b43_adjust_opmode(dev);
3799         spin_unlock_irqrestore(&wl->irq_lock, flags);
3800 }
3801
3802 /* Locking: wl->mutex */
3803 static void b43_wireless_core_stop(struct b43_wldev *dev)
3804 {
3805         struct b43_wl *wl = dev->wl;
3806         unsigned long flags;
3807
3808         if (b43_status(dev) < B43_STAT_STARTED)
3809                 return;
3810
3811         /* Disable and sync interrupts. We must do this before than
3812          * setting the status to INITIALIZED, as the interrupt handler
3813          * won't care about IRQs then. */
3814         spin_lock_irqsave(&wl->irq_lock, flags);
3815         b43_write32(dev, B43_MMIO_GEN_IRQ_MASK, 0);
3816         b43_read32(dev, B43_MMIO_GEN_IRQ_MASK); /* flush */
3817         spin_unlock_irqrestore(&wl->irq_lock, flags);
3818         b43_synchronize_irq(dev);
3819
3820         write_lock_irqsave(&wl->tx_lock, flags);
3821         b43_set_status(dev, B43_STAT_INITIALIZED);
3822         write_unlock_irqrestore(&wl->tx_lock, flags);
3823
3824         b43_pio_stop(dev);
3825         mutex_unlock(&wl->mutex);
3826         /* Must unlock as it would otherwise deadlock. No races here.
3827          * Cancel the possibly running self-rearming periodic work. */
3828         cancel_delayed_work_sync(&dev->periodic_work);
3829         mutex_lock(&wl->mutex);
3830
3831         b43_mac_suspend(dev);
3832         free_irq(dev->dev->irq, dev);
3833         b43dbg(wl, "Wireless interface stopped\n");
3834 }
3835
3836 /* Locking: wl->mutex */
3837 static int b43_wireless_core_start(struct b43_wldev *dev)
3838 {
3839         int err;
3840
3841         B43_WARN_ON(b43_status(dev) != B43_STAT_INITIALIZED);
3842
3843         drain_txstatus_queue(dev);
3844         err = request_irq(dev->dev->irq, b43_interrupt_handler,
3845                           IRQF_SHARED, KBUILD_MODNAME, dev);
3846         if (err) {
3847                 b43err(dev->wl, "Cannot request IRQ-%d\n", dev->dev->irq);
3848                 goto out;
3849         }
3850
3851         /* We are ready to run. */
3852         b43_set_status(dev, B43_STAT_STARTED);
3853
3854         /* Start data flow (TX/RX). */
3855         b43_mac_enable(dev);
3856         b43_write32(dev, B43_MMIO_GEN_IRQ_MASK, dev->irq_mask);
3857
3858         /* Start maintainance work */
3859         b43_periodic_tasks_setup(dev);
3860
3861         b43dbg(dev->wl, "Wireless interface started\n");
3862       out:
3863         return err;
3864 }
3865
3866 /* Get PHY and RADIO versioning numbers */
3867 static int b43_phy_versioning(struct b43_wldev *dev)
3868 {
3869         struct b43_phy *phy = &dev->phy;
3870         u32 tmp;
3871         u8 analog_type;
3872         u8 phy_type;
3873         u8 phy_rev;
3874         u16 radio_manuf;
3875         u16 radio_ver;
3876         u16 radio_rev;
3877         int unsupported = 0;
3878
3879         /* Get PHY versioning */
3880         tmp = b43_read16(dev, B43_MMIO_PHY_VER);
3881         analog_type = (tmp & B43_PHYVER_ANALOG) >> B43_PHYVER_ANALOG_SHIFT;
3882         phy_type = (tmp & B43_PHYVER_TYPE) >> B43_PHYVER_TYPE_SHIFT;
3883         phy_rev = (tmp & B43_PHYVER_VERSION);
3884         switch (phy_type) {
3885         case B43_PHYTYPE_A:
3886                 if (phy_rev >= 4)
3887                         unsupported = 1;
3888                 break;
3889         case B43_PHYTYPE_B:
3890                 if (phy_rev != 2 && phy_rev != 4 && phy_rev != 6
3891                     && phy_rev != 7)
3892                         unsupported = 1;
3893                 break;
3894         case B43_PHYTYPE_G:
3895                 if (phy_rev > 9)
3896                         unsupported = 1;
3897                 break;
3898 #ifdef CONFIG_B43_NPHY
3899         case B43_PHYTYPE_N:
3900                 if (phy_rev > 4)
3901                         unsupported = 1;
3902                 break;
3903 #endif
3904 #ifdef CONFIG_B43_PHY_LP
3905         case B43_PHYTYPE_LP:
3906                 if (phy_rev > 1)
3907                         unsupported = 1;
3908                 break;
3909 #endif
3910         default:
3911                 unsupported = 1;
3912         };
3913         if (unsupported) {
3914                 b43err(dev->wl, "FOUND UNSUPPORTED PHY "
3915                        "(Analog %u, Type %u, Revision %u)\n",
3916                        analog_type, phy_type, phy_rev);
3917                 return -EOPNOTSUPP;
3918         }
3919         b43dbg(dev->wl, "Found PHY: Analog %u, Type %u, Revision %u\n",
3920                analog_type, phy_type, phy_rev);
3921
3922         /* Get RADIO versioning */
3923         if (dev->dev->bus->chip_id == 0x4317) {
3924                 if (dev->dev->bus->chip_rev == 0)
3925                         tmp = 0x3205017F;
3926                 else if (dev->dev->bus->chip_rev == 1)
3927                         tmp = 0x4205017F;
3928                 else
3929                         tmp = 0x5205017F;
3930         } else {
3931                 b43_write16(dev, B43_MMIO_RADIO_CONTROL, B43_RADIOCTL_ID);
3932                 tmp = b43_read16(dev, B43_MMIO_RADIO_DATA_LOW);
3933                 b43_write16(dev, B43_MMIO_RADIO_CONTROL, B43_RADIOCTL_ID);
3934                 tmp |= (u32)b43_read16(dev, B43_MMIO_RADIO_DATA_HIGH) << 16;
3935         }
3936         radio_manuf = (tmp & 0x00000FFF);
3937         radio_ver = (tmp & 0x0FFFF000) >> 12;
3938         radio_rev = (tmp & 0xF0000000) >> 28;
3939         if (radio_manuf != 0x17F /* Broadcom */)
3940                 unsupported = 1;
3941         switch (phy_type) {
3942         case B43_PHYTYPE_A:
3943                 if (radio_ver != 0x2060)
3944                         unsupported = 1;
3945                 if (radio_rev != 1)
3946                         unsupported = 1;
3947                 if (radio_manuf != 0x17F)
3948                         unsupported = 1;
3949                 break;
3950         case B43_PHYTYPE_B:
3951                 if ((radio_ver & 0xFFF0) != 0x2050)
3952                         unsupported = 1;
3953                 break;
3954         case B43_PHYTYPE_G:
3955                 if (radio_ver != 0x2050)
3956                         unsupported = 1;
3957                 break;
3958         case B43_PHYTYPE_N:
3959                 if (radio_ver != 0x2055 && radio_ver != 0x2056)
3960                         unsupported = 1;
3961                 break;
3962         case B43_PHYTYPE_LP:
3963                 if (radio_ver != 0x2062)
3964                         unsupported = 1;
3965                 break;
3966         default:
3967                 B43_WARN_ON(1);
3968         }
3969         if (unsupported) {
3970                 b43err(dev->wl, "FOUND UNSUPPORTED RADIO "
3971                        "(Manuf 0x%X, Version 0x%X, Revision %u)\n",
3972                        radio_manuf, radio_ver, radio_rev);
3973                 return -EOPNOTSUPP;
3974         }
3975         b43dbg(dev->wl, "Found Radio: Manuf 0x%X, Version 0x%X, Revision %u\n",
3976                radio_manuf, radio_ver, radio_rev);
3977
3978         phy->radio_manuf = radio_manuf;
3979         phy->radio_ver = radio_ver;
3980         phy->radio_rev = radio_rev;
3981
3982         phy->analog = analog_type;
3983         phy->type = phy_type;
3984         phy->rev = phy_rev;
3985
3986         return 0;
3987 }
3988
3989 static void setup_struct_phy_for_init(struct b43_wldev *dev,
3990                                       struct b43_phy *phy)
3991 {
3992         phy->hardware_power_control = !!modparam_hwpctl;
3993         phy->next_txpwr_check_time = jiffies;
3994         /* PHY TX errors counter. */
3995         atomic_set(&phy->txerr_cnt, B43_PHY_TX_BADNESS_LIMIT);
3996
3997 #if B43_DEBUG
3998         phy->phy_locked = 0;
3999         phy->radio_locked = 0;
4000 #endif
4001 }
4002
4003 static void setup_struct_wldev_for_init(struct b43_wldev *dev)
4004 {
4005         dev->dfq_valid = 0;
4006
4007         /* Assume the radio is enabled. If it's not enabled, the state will
4008          * immediately get fixed on the first periodic work run. */
4009         dev->radio_hw_enable = 1;
4010
4011         /* Stats */
4012         memset(&dev->stats, 0, sizeof(dev->stats));
4013
4014         setup_struct_phy_for_init(dev, &dev->phy);
4015
4016         /* IRQ related flags */
4017         dev->irq_reason = 0;
4018         memset(dev->dma_reason, 0, sizeof(dev->dma_reason));
4019         dev->irq_mask = B43_IRQ_MASKTEMPLATE;
4020         if (b43_modparam_verbose < B43_VERBOSITY_DEBUG)
4021                 dev->irq_mask &= ~B43_IRQ_PHY_TXERR;
4022
4023         dev->mac_suspended = 1;
4024
4025         /* Noise calculation context */
4026         memset(&dev->noisecalc, 0, sizeof(dev->noisecalc));
4027 }
4028
4029 static void b43_bluetooth_coext_enable(struct b43_wldev *dev)
4030 {
4031         struct ssb_sprom *sprom = &dev->dev->bus->sprom;
4032         u64 hf;
4033
4034         if (!modparam_btcoex)
4035                 return;
4036         if (!(sprom->boardflags_lo & B43_BFL_BTCOEXIST))
4037                 return;
4038         if (dev->phy.type != B43_PHYTYPE_B && !dev->phy.gmode)
4039                 return;
4040
4041         hf = b43_hf_read(dev);
4042         if (sprom->boardflags_lo & B43_BFL_BTCMOD)
4043                 hf |= B43_HF_BTCOEXALT;
4044         else
4045                 hf |= B43_HF_BTCOEX;
4046         b43_hf_write(dev, hf);
4047 }
4048
4049 static void b43_bluetooth_coext_disable(struct b43_wldev *dev)
4050 {
4051         if (!modparam_btcoex)
4052                 return;
4053         //TODO
4054 }
4055
4056 static void b43_imcfglo_timeouts_workaround(struct b43_wldev *dev)
4057 {
4058 #ifdef CONFIG_SSB_DRIVER_PCICORE
4059         struct ssb_bus *bus = dev->dev->bus;
4060         u32 tmp;
4061
4062         if (bus->pcicore.dev &&
4063             bus->pcicore.dev->id.coreid == SSB_DEV_PCI &&
4064             bus->pcicore.dev->id.revision <= 5) {
4065                 /* IMCFGLO timeouts workaround. */
4066                 tmp = ssb_read32(dev->dev, SSB_IMCFGLO);
4067                 tmp &= ~SSB_IMCFGLO_REQTO;
4068                 tmp &= ~SSB_IMCFGLO_SERTO;
4069                 switch (bus->bustype) {
4070                 case SSB_BUSTYPE_PCI:
4071                 case SSB_BUSTYPE_PCMCIA:
4072                         tmp |= 0x32;
4073                         break;
4074                 case SSB_BUSTYPE_SSB:
4075                         tmp |= 0x53;
4076                         break;
4077                 }
4078                 ssb_write32(dev->dev, SSB_IMCFGLO, tmp);
4079         }
4080 #endif /* CONFIG_SSB_DRIVER_PCICORE */
4081 }
4082
4083 static void b43_set_synth_pu_delay(struct b43_wldev *dev, bool idle)
4084 {
4085         u16 pu_delay;
4086
4087         /* The time value is in microseconds. */
4088         if (dev->phy.type == B43_PHYTYPE_A)
4089                 pu_delay = 3700;
4090         else
4091                 pu_delay = 1050;
4092         if (b43_is_mode(dev->wl, NL80211_IFTYPE_ADHOC) || idle)
4093                 pu_delay = 500;
4094         if ((dev->phy.radio_ver == 0x2050) && (dev->phy.radio_rev == 8))
4095                 pu_delay = max(pu_delay, (u16)2400);
4096
4097         b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_SPUWKUP, pu_delay);
4098 }
4099
4100 /* Set the TSF CFP pre-TargetBeaconTransmissionTime. */
4101 static void b43_set_pretbtt(struct b43_wldev *dev)
4102 {
4103         u16 pretbtt;
4104
4105         /* The time value is in microseconds. */
4106         if (b43_is_mode(dev->wl, NL80211_IFTYPE_ADHOC)) {
4107                 pretbtt = 2;
4108         } else {
4109                 if (dev->phy.type == B43_PHYTYPE_A)
4110                         pretbtt = 120;
4111                 else
4112                         pretbtt = 250;
4113         }
4114         b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_PRETBTT, pretbtt);
4115         b43_write16(dev, B43_MMIO_TSF_CFP_PRETBTT, pretbtt);
4116 }
4117
4118 /* Shutdown a wireless core */
4119 /* Locking: wl->mutex */
4120 static void b43_wireless_core_exit(struct b43_wldev *dev)
4121 {
4122         u32 macctl;
4123
4124         B43_WARN_ON(b43_status(dev) > B43_STAT_INITIALIZED);
4125         if (b43_status(dev) != B43_STAT_INITIALIZED)
4126                 return;
4127         b43_set_status(dev, B43_STAT_UNINIT);
4128
4129         /* Stop the microcode PSM. */
4130         macctl = b43_read32(dev, B43_MMIO_MACCTL);
4131         macctl &= ~B43_MACCTL_PSM_RUN;
4132         macctl |= B43_MACCTL_PSM_JMP0;
4133         b43_write32(dev, B43_MMIO_MACCTL, macctl);
4134
4135         if (!dev->suspend_in_progress) {
4136                 b43_leds_exit(dev);
4137                 b43_rng_exit(dev->wl);
4138         }
4139         b43_dma_free(dev);
4140         b43_pio_free(dev);
4141         b43_chip_exit(dev);
4142         dev->phy.ops->switch_analog(dev, 0);
4143         if (dev->wl->current_beacon) {
4144                 dev_kfree_skb_any(dev->wl->current_beacon);
4145                 dev->wl->current_beacon = NULL;
4146         }
4147
4148         ssb_device_disable(dev->dev, 0);
4149         ssb_bus_may_powerdown(dev->dev->bus);
4150 }
4151
4152 /* Initialize a wireless core */
4153 static int b43_wireless_core_init(struct b43_wldev *dev)
4154 {
4155         struct b43_wl *wl = dev->wl;
4156         struct ssb_bus *bus = dev->dev->bus;
4157         struct ssb_sprom *sprom = &bus->sprom;
4158         struct b43_phy *phy = &dev->phy;
4159         int err;
4160         u64 hf;
4161         u32 tmp;
4162
4163         B43_WARN_ON(b43_status(dev) != B43_STAT_UNINIT);
4164
4165         err = ssb_bus_powerup(bus, 0);
4166         if (err)
4167                 goto out;
4168         if (!ssb_device_is_enabled(dev->dev)) {
4169                 tmp = phy->gmode ? B43_TMSLOW_GMODE : 0;
4170                 b43_wireless_core_reset(dev, tmp);
4171         }
4172
4173         /* Reset all data structures. */
4174         setup_struct_wldev_for_init(dev);
4175         phy->ops->prepare_structs(dev);
4176
4177         /* Enable IRQ routing to this device. */
4178         ssb_pcicore_dev_irqvecs_enable(&bus->pcicore, dev->dev);
4179
4180         b43_imcfglo_timeouts_workaround(dev);
4181         b43_bluetooth_coext_disable(dev);
4182         if (phy->ops->prepare_hardware) {
4183                 err = phy->ops->prepare_hardware(dev);
4184                 if (err)
4185                         goto err_busdown;
4186         }
4187         err = b43_chip_init(dev);
4188         if (err)
4189                 goto err_busdown;
4190         b43_shm_write16(dev, B43_SHM_SHARED,
4191                         B43_SHM_SH_WLCOREREV, dev->dev->id.revision);
4192         hf = b43_hf_read(dev);
4193         if (phy->type == B43_PHYTYPE_G) {
4194                 hf |= B43_HF_SYMW;
4195                 if (phy->rev == 1)
4196                         hf |= B43_HF_GDCW;
4197                 if (sprom->boardflags_lo & B43_BFL_PACTRL)
4198                         hf |= B43_HF_OFDMPABOOST;
4199         }
4200         if (phy->radio_ver == 0x2050) {
4201                 if (phy->radio_rev == 6)
4202                         hf |= B43_HF_4318TSSI;
4203                 if (phy->radio_rev < 6)
4204                         hf |= B43_HF_VCORECALC;
4205         }
4206         if (sprom->boardflags_lo & B43_BFL_XTAL_NOSLOW)
4207                 hf |= B43_HF_DSCRQ; /* Disable slowclock requests from ucode. */
4208 #ifdef CONFIG_SSB_DRIVER_PCICORE
4209         if ((bus->bustype == SSB_BUSTYPE_PCI) &&
4210             (bus->pcicore.dev->id.revision <= 10))
4211                 hf |= B43_HF_PCISCW; /* PCI slow clock workaround. */
4212 #endif
4213         hf &= ~B43_HF_SKCFPUP;
4214         b43_hf_write(dev, hf);
4215
4216         b43_set_retry_limits(dev, B43_DEFAULT_SHORT_RETRY_LIMIT,
4217                              B43_DEFAULT_LONG_RETRY_LIMIT);
4218         b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_SFFBLIM, 3);
4219         b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_LFFBLIM, 2);
4220
4221         /* Disable sending probe responses from firmware.
4222          * Setting the MaxTime to one usec will always trigger
4223          * a timeout, so we never send any probe resp.
4224          * A timeout of zero is infinite. */
4225         b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_PRMAXTIME, 1);
4226
4227         b43_rate_memory_init(dev);
4228         b43_set_phytxctl_defaults(dev);
4229
4230         /* Minimum Contention Window */
4231         if (phy->type == B43_PHYTYPE_B) {
4232                 b43_shm_write16(dev, B43_SHM_SCRATCH, B43_SHM_SC_MINCONT, 0x1F);
4233         } else {
4234                 b43_shm_write16(dev, B43_SHM_SCRATCH, B43_SHM_SC_MINCONT, 0xF);
4235         }
4236         /* Maximum Contention Window */
4237         b43_shm_write16(dev, B43_SHM_SCRATCH, B43_SHM_SC_MAXCONT, 0x3FF);
4238
4239         if ((dev->dev->bus->bustype == SSB_BUSTYPE_PCMCIA) || B43_FORCE_PIO) {
4240                 dev->__using_pio_transfers = 1;
4241                 err = b43_pio_init(dev);
4242         } else {
4243                 dev->__using_pio_transfers = 0;
4244                 err = b43_dma_init(dev);
4245         }
4246         if (err)
4247                 goto err_chip_exit;
4248         b43_qos_init(dev);
4249         b43_set_synth_pu_delay(dev, 1);
4250         b43_bluetooth_coext_enable(dev);
4251
4252         ssb_bus_powerup(bus, !(sprom->boardflags_lo & B43_BFL_XTAL_NOSLOW));
4253         b43_upload_card_macaddress(dev);
4254         b43_security_init(dev);
4255         if (!dev->suspend_in_progress)
4256                 b43_rng_init(wl);
4257
4258         b43_set_status(dev, B43_STAT_INITIALIZED);
4259
4260         if (!dev->suspend_in_progress)
4261                 b43_leds_init(dev);
4262 out:
4263         return err;
4264
4265 err_chip_exit:
4266         b43_chip_exit(dev);
4267 err_busdown:
4268         ssb_bus_may_powerdown(bus);
4269         B43_WARN_ON(b43_status(dev) != B43_STAT_UNINIT);
4270         return err;
4271 }
4272
4273 static int b43_op_add_interface(struct ieee80211_hw *hw,
4274                                 struct ieee80211_if_init_conf *conf)
4275 {
4276         struct b43_wl *wl = hw_to_b43_wl(hw);
4277         struct b43_wldev *dev;
4278         unsigned long flags;
4279         int err = -EOPNOTSUPP;
4280
4281         /* TODO: allow WDS/AP devices to coexist */
4282
4283         if (conf->type != NL80211_IFTYPE_AP &&
4284             conf->type != NL80211_IFTYPE_MESH_POINT &&
4285             conf->type != NL80211_IFTYPE_STATION &&
4286             conf->type != NL80211_IFTYPE_WDS &&
4287             conf->type != NL80211_IFTYPE_ADHOC)
4288                 return -EOPNOTSUPP;
4289
4290         mutex_lock(&wl->mutex);
4291         if (wl->operating)
4292                 goto out_mutex_unlock;
4293
4294         b43dbg(wl, "Adding Interface type %d\n", conf->type);
4295
4296         dev = wl->current_dev;
4297         wl->operating = 1;
4298         wl->vif = conf->vif;
4299         wl->if_type = conf->type;
4300         memcpy(wl->mac_addr, conf->mac_addr, ETH_ALEN);
4301
4302         spin_lock_irqsave(&wl->irq_lock, flags);
4303         b43_adjust_opmode(dev);
4304         b43_set_pretbtt(dev);
4305         b43_set_synth_pu_delay(dev, 0);
4306         b43_upload_card_macaddress(dev);
4307         spin_unlock_irqrestore(&wl->irq_lock, flags);
4308
4309         err = 0;
4310  out_mutex_unlock:
4311         mutex_unlock(&wl->mutex);
4312
4313         return err;
4314 }
4315
4316 static void b43_op_remove_interface(struct ieee80211_hw *hw,
4317                                     struct ieee80211_if_init_conf *conf)
4318 {
4319         struct b43_wl *wl = hw_to_b43_wl(hw);
4320         struct b43_wldev *dev = wl->current_dev;
4321         unsigned long flags;
4322
4323         b43dbg(wl, "Removing Interface type %d\n", conf->type);
4324
4325         mutex_lock(&wl->mutex);
4326
4327         B43_WARN_ON(!wl->operating);
4328         B43_WARN_ON(wl->vif != conf->vif);
4329         wl->vif = NULL;
4330
4331         wl->operating = 0;
4332
4333         spin_lock_irqsave(&wl->irq_lock, flags);
4334         b43_adjust_opmode(dev);
4335         memset(wl->mac_addr, 0, ETH_ALEN);
4336         b43_upload_card_macaddress(dev);
4337         spin_unlock_irqrestore(&wl->irq_lock, flags);
4338
4339         mutex_unlock(&wl->mutex);
4340 }
4341
4342 static int b43_op_start(struct ieee80211_hw *hw)
4343 {
4344         struct b43_wl *wl = hw_to_b43_wl(hw);
4345         struct b43_wldev *dev = wl->current_dev;
4346         int did_init = 0;
4347         int err = 0;
4348
4349         /* Kill all old instance specific information to make sure
4350          * the card won't use it in the short timeframe between start
4351          * and mac80211 reconfiguring it. */
4352         memset(wl->bssid, 0, ETH_ALEN);
4353         memset(wl->mac_addr, 0, ETH_ALEN);
4354         wl->filter_flags = 0;
4355         wl->radiotap_enabled = 0;
4356         b43_qos_clear(wl);
4357         wl->beacon0_uploaded = 0;
4358         wl->beacon1_uploaded = 0;
4359         wl->beacon_templates_virgin = 1;
4360         wl->radio_enabled = 1;
4361
4362         mutex_lock(&wl->mutex);
4363
4364         if (b43_status(dev) < B43_STAT_INITIALIZED) {
4365                 err = b43_wireless_core_init(dev);
4366                 if (err)
4367                         goto out_mutex_unlock;
4368                 did_init = 1;
4369         }
4370
4371         if (b43_status(dev) < B43_STAT_STARTED) {
4372                 err = b43_wireless_core_start(dev);
4373                 if (err) {
4374                         if (did_init)
4375                                 b43_wireless_core_exit(dev);
4376                         goto out_mutex_unlock;
4377                 }
4378         }
4379
4380         /* XXX: only do if device doesn't support rfkill irq */
4381         wiphy_rfkill_start_polling(hw->wiphy);
4382
4383  out_mutex_unlock:
4384         mutex_unlock(&wl->mutex);
4385
4386         return err;
4387 }
4388
4389 static void b43_op_stop(struct ieee80211_hw *hw)
4390 {
4391         struct b43_wl *wl = hw_to_b43_wl(hw);
4392         struct b43_wldev *dev = wl->current_dev;
4393
4394         cancel_work_sync(&(wl->beacon_update_trigger));
4395
4396         mutex_lock(&wl->mutex);
4397         if (b43_status(dev) >= B43_STAT_STARTED)
4398                 b43_wireless_core_stop(dev);
4399         b43_wireless_core_exit(dev);
4400         wl->radio_enabled = 0;
4401         mutex_unlock(&wl->mutex);
4402
4403         cancel_work_sync(&(wl->txpower_adjust_work));
4404 }
4405
4406 static int b43_op_beacon_set_tim(struct ieee80211_hw *hw,
4407                                  struct ieee80211_sta *sta, bool set)
4408 {
4409         struct b43_wl *wl = hw_to_b43_wl(hw);
4410         unsigned long flags;
4411
4412         spin_lock_irqsave(&wl->irq_lock, flags);
4413         b43_update_templates(wl);
4414         spin_unlock_irqrestore(&wl->irq_lock, flags);
4415
4416         return 0;
4417 }
4418
4419 static void b43_op_sta_notify(struct ieee80211_hw *hw,
4420                               struct ieee80211_vif *vif,
4421                               enum sta_notify_cmd notify_cmd,
4422                               struct ieee80211_sta *sta)
4423 {
4424         struct b43_wl *wl = hw_to_b43_wl(hw);
4425
4426         B43_WARN_ON(!vif || wl->vif != vif);
4427 }
4428
4429 static void b43_op_sw_scan_start_notifier(struct ieee80211_hw *hw)
4430 {
4431         struct b43_wl *wl = hw_to_b43_wl(hw);
4432         struct b43_wldev *dev;
4433
4434         mutex_lock(&wl->mutex);
4435         dev = wl->current_dev;
4436         if (dev && (b43_status(dev) >= B43_STAT_INITIALIZED)) {
4437                 /* Disable CFP update during scan on other channels. */
4438                 b43_hf_write(dev, b43_hf_read(dev) | B43_HF_SKCFPUP);
4439         }
4440         mutex_unlock(&wl->mutex);
4441 }
4442
4443 static void b43_op_sw_scan_complete_notifier(struct ieee80211_hw *hw)
4444 {
4445         struct b43_wl *wl = hw_to_b43_wl(hw);
4446         struct b43_wldev *dev;
4447
4448         mutex_lock(&wl->mutex);
4449         dev = wl->current_dev;
4450         if (dev && (b43_status(dev) >= B43_STAT_INITIALIZED)) {
4451                 /* Re-enable CFP update. */
4452                 b43_hf_write(dev, b43_hf_read(dev) & ~B43_HF_SKCFPUP);
4453         }
4454         mutex_unlock(&wl->mutex);
4455 }
4456
4457 static const struct ieee80211_ops b43_hw_ops = {
4458         .tx                     = b43_op_tx,
4459         .conf_tx                = b43_op_conf_tx,
4460         .add_interface          = b43_op_add_interface,
4461         .remove_interface       = b43_op_remove_interface,
4462         .config                 = b43_op_config,
4463         .bss_info_changed       = b43_op_bss_info_changed,
4464         .configure_filter       = b43_op_configure_filter,
4465         .set_key                = b43_op_set_key,
4466         .get_stats              = b43_op_get_stats,
4467         .get_tx_stats           = b43_op_get_tx_stats,
4468         .get_tsf                = b43_op_get_tsf,
4469         .set_tsf                = b43_op_set_tsf,
4470         .start                  = b43_op_start,
4471         .stop                   = b43_op_stop,
4472         .set_tim                = b43_op_beacon_set_tim,
4473         .sta_notify             = b43_op_sta_notify,
4474         .sw_scan_start          = b43_op_sw_scan_start_notifier,
4475         .sw_scan_complete       = b43_op_sw_scan_complete_notifier,
4476         .rfkill_poll            = b43_rfkill_poll,
4477 };
4478
4479 /* Hard-reset the chip. Do not call this directly.
4480  * Use b43_controller_restart()
4481  */
4482 static void b43_chip_reset(struct work_struct *work)
4483 {
4484         struct b43_wldev *dev =
4485             container_of(work, struct b43_wldev, restart_work);
4486         struct b43_wl *wl = dev->wl;
4487         int err = 0;
4488         int prev_status;
4489
4490         mutex_lock(&wl->mutex);
4491
4492         prev_status = b43_status(dev);
4493         /* Bring the device down... */
4494         if (prev_status >= B43_STAT_STARTED)
4495                 b43_wireless_core_stop(dev);
4496         if (prev_status >= B43_STAT_INITIALIZED)
4497                 b43_wireless_core_exit(dev);
4498
4499         /* ...and up again. */
4500         if (prev_status >= B43_STAT_INITIALIZED) {
4501                 err = b43_wireless_core_init(dev);
4502                 if (err)
4503                         goto out;
4504         }
4505         if (prev_status >= B43_STAT_STARTED) {
4506                 err = b43_wireless_core_start(dev);
4507                 if (err) {
4508                         b43_wireless_core_exit(dev);
4509                         goto out;
4510                 }
4511         }
4512 out:
4513         if (err)
4514                 wl->current_dev = NULL; /* Failed to init the dev. */
4515         mutex_unlock(&wl->mutex);
4516         if (err)
4517                 b43err(wl, "Controller restart FAILED\n");
4518         else
4519                 b43info(wl, "Controller restarted\n");
4520 }
4521
4522 static int b43_setup_bands(struct b43_wldev *dev,
4523                            bool have_2ghz_phy, bool have_5ghz_phy)
4524 {
4525         struct ieee80211_hw *hw = dev->wl->hw;
4526
4527         if (have_2ghz_phy)
4528                 hw->wiphy->bands[IEEE80211_BAND_2GHZ] = &b43_band_2GHz;
4529         if (dev->phy.type == B43_PHYTYPE_N) {
4530                 if (have_5ghz_phy)
4531                         hw->wiphy->bands[IEEE80211_BAND_5GHZ] = &b43_band_5GHz_nphy;
4532         } else {
4533                 if (have_5ghz_phy)
4534                         hw->wiphy->bands[IEEE80211_BAND_5GHZ] = &b43_band_5GHz_aphy;
4535         }
4536
4537         dev->phy.supports_2ghz = have_2ghz_phy;
4538         dev->phy.supports_5ghz = have_5ghz_phy;
4539
4540         return 0;
4541 }
4542
4543 static void b43_wireless_core_detach(struct b43_wldev *dev)
4544 {
4545         /* We release firmware that late to not be required to re-request
4546          * is all the time when we reinit the core. */
4547         b43_release_firmware(dev);
4548         b43_phy_free(dev);
4549 }
4550
4551 static int b43_wireless_core_attach(struct b43_wldev *dev)
4552 {
4553         struct b43_wl *wl = dev->wl;
4554         struct ssb_bus *bus = dev->dev->bus;
4555         struct pci_dev *pdev = bus->host_pci;
4556         int err;
4557         bool have_2ghz_phy = 0, have_5ghz_phy = 0;
4558         u32 tmp;
4559
4560         /* Do NOT do any device initialization here.
4561          * Do it in wireless_core_init() instead.
4562          * This function is for gathering basic information about the HW, only.
4563          * Also some structs may be set up here. But most likely you want to have
4564          * that in core_init(), too.
4565          */
4566
4567         err = ssb_bus_powerup(bus, 0);
4568         if (err) {
4569                 b43err(wl, "Bus powerup failed\n");
4570                 goto out;
4571         }
4572         /* Get the PHY type. */
4573         if (dev->dev->id.revision >= 5) {
4574                 u32 tmshigh;
4575
4576                 tmshigh = ssb_read32(dev->dev, SSB_TMSHIGH);
4577                 have_2ghz_phy = !!(tmshigh & B43_TMSHIGH_HAVE_2GHZ_PHY);
4578                 have_5ghz_phy = !!(tmshigh & B43_TMSHIGH_HAVE_5GHZ_PHY);
4579         } else
4580                 B43_WARN_ON(1);
4581
4582         dev->phy.gmode = have_2ghz_phy;
4583         dev->phy.radio_on = 1;
4584         tmp = dev->phy.gmode ? B43_TMSLOW_GMODE : 0;
4585         b43_wireless_core_reset(dev, tmp);
4586
4587         err = b43_phy_versioning(dev);
4588         if (err)
4589                 goto err_powerdown;
4590         /* Check if this device supports multiband. */
4591         if (!pdev ||
4592             (pdev->device != 0x4312 &&
4593              pdev->device != 0x4319 && pdev->device != 0x4324)) {
4594                 /* No multiband support. */
4595                 have_2ghz_phy = 0;
4596                 have_5ghz_phy = 0;
4597                 switch (dev->phy.type) {
4598                 case B43_PHYTYPE_A:
4599                         have_5ghz_phy = 1;
4600                         break;
4601                 case B43_PHYTYPE_G:
4602                 case B43_PHYTYPE_N:
4603                 case B43_PHYTYPE_LP:
4604                         have_2ghz_phy = 1;
4605                         break;
4606                 default:
4607                         B43_WARN_ON(1);
4608                 }
4609         }
4610         if (dev->phy.type == B43_PHYTYPE_A) {
4611                 /* FIXME */
4612                 b43err(wl, "IEEE 802.11a devices are unsupported\n");
4613                 err = -EOPNOTSUPP;
4614                 goto err_powerdown;
4615         }
4616         if (1 /* disable A-PHY */) {
4617                 /* FIXME: For now we disable the A-PHY on multi-PHY devices. */
4618                 if (dev->phy.type != B43_PHYTYPE_N) {
4619                         have_2ghz_phy = 1;
4620                         have_5ghz_phy = 0;
4621                 }
4622         }
4623
4624         err = b43_phy_allocate(dev);
4625         if (err)
4626                 goto err_powerdown;
4627
4628         dev->phy.gmode = have_2ghz_phy;
4629         tmp = dev->phy.gmode ? B43_TMSLOW_GMODE : 0;
4630         b43_wireless_core_reset(dev, tmp);
4631
4632         err = b43_validate_chipaccess(dev);
4633         if (err)
4634                 goto err_phy_free;
4635         err = b43_setup_bands(dev, have_2ghz_phy, have_5ghz_phy);
4636         if (err)
4637                 goto err_phy_free;
4638
4639         /* Now set some default "current_dev" */
4640         if (!wl->current_dev)
4641                 wl->current_dev = dev;
4642         INIT_WORK(&dev->restart_work, b43_chip_reset);
4643
4644         dev->phy.ops->switch_analog(dev, 0);
4645         ssb_device_disable(dev->dev, 0);
4646         ssb_bus_may_powerdown(bus);
4647
4648 out:
4649         return err;
4650
4651 err_phy_free:
4652         b43_phy_free(dev);
4653 err_powerdown:
4654         ssb_bus_may_powerdown(bus);
4655         return err;
4656 }
4657
4658 static void b43_one_core_detach(struct ssb_device *dev)
4659 {
4660         struct b43_wldev *wldev;
4661         struct b43_wl *wl;
4662
4663         /* Do not cancel ieee80211-workqueue based work here.
4664          * See comment in b43_remove(). */
4665
4666         wldev = ssb_get_drvdata(dev);
4667         wl = wldev->wl;
4668         b43_debugfs_remove_device(wldev);
4669         b43_wireless_core_detach(wldev);
4670         list_del(&wldev->list);
4671         wl->nr_devs--;
4672         ssb_set_drvdata(dev, NULL);
4673         kfree(wldev);
4674 }
4675
4676 static int b43_one_core_attach(struct ssb_device *dev, struct b43_wl *wl)
4677 {
4678         struct b43_wldev *wldev;
4679         struct pci_dev *pdev;
4680         int err = -ENOMEM;
4681
4682         if (!list_empty(&wl->devlist)) {
4683                 /* We are not the first core on this chip. */
4684                 pdev = dev->bus->host_pci;
4685                 /* Only special chips support more than one wireless
4686                  * core, although some of the other chips have more than
4687                  * one wireless core as well. Check for this and
4688                  * bail out early.
4689                  */
4690                 if (!pdev ||
4691                     ((pdev->device != 0x4321) &&
4692                      (pdev->device != 0x4313) && (pdev->device != 0x431A))) {
4693                         b43dbg(wl, "Ignoring unconnected 802.11 core\n");
4694                         return -ENODEV;
4695                 }
4696         }
4697
4698         wldev = kzalloc(sizeof(*wldev), GFP_KERNEL);
4699         if (!wldev)
4700                 goto out;
4701
4702         wldev->dev = dev;
4703         wldev->wl = wl;
4704         b43_set_status(wldev, B43_STAT_UNINIT);
4705         wldev->bad_frames_preempt = modparam_bad_frames_preempt;
4706         tasklet_init(&wldev->isr_tasklet,
4707                      (void (*)(unsigned long))b43_interrupt_tasklet,
4708                      (unsigned long)wldev);
4709         INIT_LIST_HEAD(&wldev->list);
4710
4711         err = b43_wireless_core_attach(wldev);
4712         if (err)
4713                 goto err_kfree_wldev;
4714
4715         list_add(&wldev->list, &wl->devlist);
4716         wl->nr_devs++;
4717         ssb_set_drvdata(dev, wldev);
4718         b43_debugfs_add_device(wldev);
4719
4720       out:
4721         return err;
4722
4723       err_kfree_wldev:
4724         kfree(wldev);
4725         return err;
4726 }
4727
4728 #define IS_PDEV(pdev, _vendor, _device, _subvendor, _subdevice)         ( \
4729         (pdev->vendor == PCI_VENDOR_ID_##_vendor) &&                    \
4730         (pdev->device == _device) &&                                    \
4731         (pdev->subsystem_vendor == PCI_VENDOR_ID_##_subvendor) &&       \
4732         (pdev->subsystem_device == _subdevice)                          )
4733
4734 static void b43_sprom_fixup(struct ssb_bus *bus)
4735 {
4736         struct pci_dev *pdev;
4737
4738         /* boardflags workarounds */
4739         if (bus->boardinfo.vendor == SSB_BOARDVENDOR_DELL &&
4740             bus->chip_id == 0x4301 && bus->boardinfo.rev == 0x74)
4741                 bus->sprom.boardflags_lo |= B43_BFL_BTCOEXIST;
4742         if (bus->boardinfo.vendor == PCI_VENDOR_ID_APPLE &&
4743             bus->boardinfo.type == 0x4E && bus->boardinfo.rev > 0x40)
4744                 bus->sprom.boardflags_lo |= B43_BFL_PACTRL;
4745         if (bus->bustype == SSB_BUSTYPE_PCI) {
4746                 pdev = bus->host_pci;
4747                 if (IS_PDEV(pdev, BROADCOM, 0x4318, ASUSTEK, 0x100F) ||
4748                     IS_PDEV(pdev, BROADCOM, 0x4320,    DELL, 0x0003) ||
4749                     IS_PDEV(pdev, BROADCOM, 0x4320,      HP, 0x12f8) ||
4750                     IS_PDEV(pdev, BROADCOM, 0x4320, LINKSYS, 0x0015) ||
4751                     IS_PDEV(pdev, BROADCOM, 0x4320, LINKSYS, 0x0014) ||
4752                     IS_PDEV(pdev, BROADCOM, 0x4320, LINKSYS, 0x0013) ||
4753                     IS_PDEV(pdev, BROADCOM, 0x4320, MOTOROLA, 0x7010))
4754                         bus->sprom.boardflags_lo &= ~B43_BFL_BTCOEXIST;
4755         }
4756 }
4757
4758 static void b43_wireless_exit(struct ssb_device *dev, struct b43_wl *wl)
4759 {
4760         struct ieee80211_hw *hw = wl->hw;
4761
4762         ssb_set_devtypedata(dev, NULL);
4763         ieee80211_free_hw(hw);
4764 }
4765
4766 static int b43_wireless_init(struct ssb_device *dev)
4767 {
4768         struct ssb_sprom *sprom = &dev->bus->sprom;
4769         struct ieee80211_hw *hw;
4770         struct b43_wl *wl;
4771         int err = -ENOMEM;
4772
4773         b43_sprom_fixup(dev->bus);
4774
4775         hw = ieee80211_alloc_hw(sizeof(*wl), &b43_hw_ops);
4776         if (!hw) {
4777                 b43err(NULL, "Could not allocate ieee80211 device\n");
4778                 goto out;
4779         }
4780         wl = hw_to_b43_wl(hw);
4781
4782         /* fill hw info */
4783         hw->flags = IEEE80211_HW_RX_INCLUDES_FCS |
4784                     IEEE80211_HW_SIGNAL_DBM |
4785                     IEEE80211_HW_NOISE_DBM;
4786
4787         hw->wiphy->interface_modes =
4788                 BIT(NL80211_IFTYPE_AP) |
4789                 BIT(NL80211_IFTYPE_MESH_POINT) |
4790                 BIT(NL80211_IFTYPE_STATION) |
4791                 BIT(NL80211_IFTYPE_WDS) |
4792                 BIT(NL80211_IFTYPE_ADHOC);
4793
4794         hw->queues = modparam_qos ? 4 : 1;
4795         wl->mac80211_initially_registered_queues = hw->queues;
4796         hw->max_rates = 2;
4797         SET_IEEE80211_DEV(hw, dev->dev);
4798         if (is_valid_ether_addr(sprom->et1mac))
4799                 SET_IEEE80211_PERM_ADDR(hw, sprom->et1mac);
4800         else
4801                 SET_IEEE80211_PERM_ADDR(hw, sprom->il0mac);
4802
4803         /* Initialize struct b43_wl */
4804         wl->hw = hw;
4805         spin_lock_init(&wl->irq_lock);
4806         rwlock_init(&wl->tx_lock);
4807         spin_lock_init(&wl->leds_lock);
4808         spin_lock_init(&wl->shm_lock);
4809         mutex_init(&wl->mutex);
4810         INIT_LIST_HEAD(&wl->devlist);
4811         INIT_WORK(&wl->beacon_update_trigger, b43_beacon_update_trigger_work);
4812         INIT_WORK(&wl->txpower_adjust_work, b43_phy_txpower_adjust_work);
4813
4814         ssb_set_devtypedata(dev, wl);
4815         b43info(wl, "Broadcom %04X WLAN found (core revision %u)\n",
4816                 dev->bus->chip_id, dev->id.revision);
4817         err = 0;
4818 out:
4819         return err;
4820 }
4821
4822 static int b43_probe(struct ssb_device *dev, const struct ssb_device_id *id)
4823 {
4824         struct b43_wl *wl;
4825         int err;
4826         int first = 0;
4827
4828         wl = ssb_get_devtypedata(dev);
4829         if (!wl) {
4830                 /* Probing the first core. Must setup common struct b43_wl */
4831                 first = 1;
4832                 err = b43_wireless_init(dev);
4833                 if (err)
4834                         goto out;
4835                 wl = ssb_get_devtypedata(dev);
4836                 B43_WARN_ON(!wl);
4837         }
4838         err = b43_one_core_attach(dev, wl);
4839         if (err)
4840                 goto err_wireless_exit;
4841
4842         if (first) {
4843                 err = ieee80211_register_hw(wl->hw);
4844                 if (err)
4845                         goto err_one_core_detach;
4846         }
4847
4848       out:
4849         return err;
4850
4851       err_one_core_detach:
4852         b43_one_core_detach(dev);
4853       err_wireless_exit:
4854         if (first)
4855                 b43_wireless_exit(dev, wl);
4856         return err;
4857 }
4858
4859 static void b43_remove(struct ssb_device *dev)
4860 {
4861         struct b43_wl *wl = ssb_get_devtypedata(dev);
4862         struct b43_wldev *wldev = ssb_get_drvdata(dev);
4863
4864         /* We must cancel any work here before unregistering from ieee80211,
4865          * as the ieee80211 unreg will destroy the workqueue. */
4866         cancel_work_sync(&wldev->restart_work);
4867
4868         B43_WARN_ON(!wl);
4869         if (wl->current_dev == wldev) {
4870                 /* Restore the queues count before unregistering, because firmware detect
4871                  * might have modified it. Restoring is important, so the networking
4872                  * stack can properly free resources. */
4873                 wl->hw->queues = wl->mac80211_initially_registered_queues;
4874                 ieee80211_unregister_hw(wl->hw);
4875         }
4876
4877         b43_one_core_detach(dev);
4878
4879         if (list_empty(&wl->devlist)) {
4880                 /* Last core on the chip unregistered.
4881                  * We can destroy common struct b43_wl.
4882                  */
4883                 b43_wireless_exit(dev, wl);
4884         }
4885 }
4886
4887 /* Perform a hardware reset. This can be called from any context. */
4888 void b43_controller_restart(struct b43_wldev *dev, const char *reason)
4889 {
4890         /* Must avoid requeueing, if we are in shutdown. */
4891         if (b43_status(dev) < B43_STAT_INITIALIZED)
4892                 return;
4893         b43info(dev->wl, "Controller RESET (%s) ...\n", reason);
4894         ieee80211_queue_work(dev->wl->hw, &dev->restart_work);
4895 }
4896
4897 #ifdef CONFIG_PM
4898
4899 static int b43_suspend(struct ssb_device *dev, pm_message_t state)
4900 {
4901         struct b43_wldev *wldev = ssb_get_drvdata(dev);
4902         struct b43_wl *wl = wldev->wl;
4903
4904         b43dbg(wl, "Suspending...\n");
4905
4906         mutex_lock(&wl->mutex);
4907         wldev->suspend_in_progress = true;
4908         wldev->suspend_init_status = b43_status(wldev);
4909         if (wldev->suspend_init_status >= B43_STAT_STARTED)
4910                 b43_wireless_core_stop(wldev);
4911         if (wldev->suspend_init_status >= B43_STAT_INITIALIZED)
4912                 b43_wireless_core_exit(wldev);
4913         mutex_unlock(&wl->mutex);
4914
4915         b43dbg(wl, "Device suspended.\n");
4916
4917         return 0;
4918 }
4919
4920 static int b43_resume(struct ssb_device *dev)
4921 {
4922         struct b43_wldev *wldev = ssb_get_drvdata(dev);
4923         struct b43_wl *wl = wldev->wl;
4924         int err = 0;
4925
4926         b43dbg(wl, "Resuming...\n");
4927
4928         mutex_lock(&wl->mutex);
4929         if (wldev->suspend_init_status >= B43_STAT_INITIALIZED) {
4930                 err = b43_wireless_core_init(wldev);
4931                 if (err) {
4932                         b43err(wl, "Resume failed at core init\n");
4933                         goto out;
4934                 }
4935         }
4936         if (wldev->suspend_init_status >= B43_STAT_STARTED) {
4937                 err = b43_wireless_core_start(wldev);
4938                 if (err) {
4939                         b43_leds_exit(wldev);
4940                         b43_rng_exit(wldev->wl);
4941                         b43_wireless_core_exit(wldev);
4942                         b43err(wl, "Resume failed at core start\n");
4943                         goto out;
4944                 }
4945         }
4946         b43dbg(wl, "Device resumed.\n");
4947  out:
4948         wldev->suspend_in_progress = false;
4949         mutex_unlock(&wl->mutex);
4950         return err;
4951 }
4952
4953 #else /* CONFIG_PM */
4954 # define b43_suspend    NULL
4955 # define b43_resume     NULL
4956 #endif /* CONFIG_PM */
4957
4958 static struct ssb_driver b43_ssb_driver = {
4959         .name           = KBUILD_MODNAME,
4960         .id_table       = b43_ssb_tbl,
4961         .probe          = b43_probe,
4962         .remove         = b43_remove,
4963         .suspend        = b43_suspend,
4964         .resume         = b43_resume,
4965 };
4966
4967 static void b43_print_driverinfo(void)
4968 {
4969         const char *feat_pci = "", *feat_pcmcia = "", *feat_nphy = "",
4970                    *feat_leds = "";
4971
4972 #ifdef CONFIG_B43_PCI_AUTOSELECT
4973         feat_pci = "P";
4974 #endif
4975 #ifdef CONFIG_B43_PCMCIA
4976         feat_pcmcia = "M";
4977 #endif
4978 #ifdef CONFIG_B43_NPHY
4979         feat_nphy = "N";
4980 #endif
4981 #ifdef CONFIG_B43_LEDS
4982         feat_leds = "L";
4983 #endif
4984         printk(KERN_INFO "Broadcom 43xx driver loaded "
4985                "[ Features: %s%s%s%s, Firmware-ID: "
4986                B43_SUPPORTED_FIRMWARE_ID " ]\n",
4987                feat_pci, feat_pcmcia, feat_nphy,
4988                feat_leds);
4989 }
4990
4991 static int __init b43_init(void)
4992 {
4993         int err;
4994
4995         b43_debugfs_init();
4996         err = b43_pcmcia_init();
4997         if (err)
4998                 goto err_dfs_exit;
4999         err = ssb_driver_register(&b43_ssb_driver);
5000         if (err)
5001                 goto err_pcmcia_exit;
5002         b43_print_driverinfo();
5003
5004         return err;
5005
5006 err_pcmcia_exit:
5007         b43_pcmcia_exit();
5008 err_dfs_exit:
5009         b43_debugfs_exit();
5010         return err;
5011 }
5012
5013 static void __exit b43_exit(void)
5014 {
5015         ssb_driver_unregister(&b43_ssb_driver);
5016         b43_pcmcia_exit();
5017         b43_debugfs_exit();
5018 }
5019
5020 module_init(b43_init)
5021 module_exit(b43_exit)