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