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