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