drivers/net/wireless/b43/main.c

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