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