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