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