rt2500usb.c

/*
	Copyright (C) 2004 - 2009 Ivo van Doorn <IvDoorn@gmail.com>
	<http://rt2x00.serialmonkey.com>

	This program is free software; you can redistribute it and/or modify
	it under the terms of the GNU General Public License as published by
	the Free Software Foundation; either version 2 of the License, or
	(at your option) any later version.

	This program is distributed in the hope that it will be useful,
	but WITHOUT ANY WARRANTY; without even the implied warranty of
	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
	GNU General Public License for more details.

	You should have received a copy of the GNU General Public License
	along with this program; if not, write to the
	Free Software Foundation, Inc.,
	59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
 */

/*
	Module: rt2500usb
	Abstract: rt2500usb device specific routines.
	Supported chipsets: RT2570.
 */

#include <linux/delay.h>
#include <linux/etherdevice.h>
#include <linux/init.h>
#include <linux/kernel.h>
#include <linux/module.h>
#include <linux/slab.h>
#include <linux/usb.h>

#include "rt2x00.h"
#include "rt2x00usb.h"
#include "rt2500usb.h"

/*
 * Allow hardware encryption to be disabled.
 */
static int modparam_nohwcrypt;
module_param_named(nohwcrypt, modparam_nohwcrypt, bool, S_IRUGO);
MODULE_PARM_DESC(nohwcrypt, "Disable hardware encryption.");

/*
 * Register access.
 * All access to the CSR registers will go through the methods
 * rt2500usb_register_read and rt2500usb_register_write.
 * BBP and RF register require indirect register access,
 * and use the CSR registers BBPCSR and RFCSR to achieve this.
 * These indirect registers work with busy bits,
 * and we will try maximal REGISTER_BUSY_COUNT times to access
 * the register while taking a REGISTER_BUSY_DELAY us delay
 * between each attampt. When the busy bit is still set at that time,
 * the access attempt is considered to have failed,
 * and we will print an error.
 * If the csr_mutex is already held then the _lock variants must
 * be used instead.
 */
static inline void rt2500usb_register_read(struct rt2x00_dev *rt2x00dev,
					   const unsigned int offset,
					   u16 *value)
{
	__le16 reg;
	rt2x00usb_vendor_request_buff(rt2x00dev, USB_MULTI_READ,
				      USB_VENDOR_REQUEST_IN, offset,
				      &reg, sizeof(reg), REGISTER_TIMEOUT);
	*value = le16_to_cpu(reg);
}

static inline void rt2500usb_register_read_lock(struct rt2x00_dev *rt2x00dev,
						const unsigned int offset,
						u16 *value)
{
	__le16 reg;
	rt2x00usb_vendor_req_buff_lock(rt2x00dev, USB_MULTI_READ,
				       USB_VENDOR_REQUEST_IN, offset,
				       &reg, sizeof(reg), REGISTER_TIMEOUT);
	*value = le16_to_cpu(reg);
}

static inline void rt2500usb_register_multiread(struct rt2x00_dev *rt2x00dev,
						const unsigned int offset,
						void *value, const u16 length)
{
	rt2x00usb_vendor_request_buff(rt2x00dev, USB_MULTI_READ,
				      USB_VENDOR_REQUEST_IN, offset,
				      value, length,
				      REGISTER_TIMEOUT16(length));
}

static inline void rt2500usb_register_write(struct rt2x00_dev *rt2x00dev,
					    const unsigned int offset,
					    u16 value)
{
	__le16 reg = cpu_to_le16(value);
	rt2x00usb_vendor_request_buff(rt2x00dev, USB_MULTI_WRITE,
				      USB_VENDOR_REQUEST_OUT, offset,
				      &reg, sizeof(reg), REGISTER_TIMEOUT);
}

static inline void rt2500usb_register_write_lock(struct rt2x00_dev *rt2x00dev,
						 const unsigned int offset,
						 u16 value)
{
	__le16 reg = cpu_to_le16(value);
	rt2x00usb_vendor_req_buff_lock(rt2x00dev, USB_MULTI_WRITE,
				       USB_VENDOR_REQUEST_OUT, offset,
				       &reg, sizeof(reg), REGISTER_TIMEOUT);
}

static inline void rt2500usb_register_multiwrite(struct rt2x00_dev *rt2x00dev,
						 const unsigned int offset,
						 void *value, const u16 length)
{
	rt2x00usb_vendor_request_buff(rt2x00dev, USB_MULTI_WRITE,
				      USB_VENDOR_REQUEST_OUT, offset,
				      value, length,
				      REGISTER_TIMEOUT16(length));
}

static int rt2500usb_regbusy_read(struct rt2x00_dev *rt2x00dev,
				  const unsigned int offset,
				  struct rt2x00_field16 field,
				  u16 *reg)
{
	unsigned int i;

	for (i = 0; i < REGISTER_BUSY_COUNT; i++) {
		rt2500usb_register_read_lock(rt2x00dev, offset, reg);
		if (!rt2x00_get_field16(*reg, field))
			return 1;
		udelay(REGISTER_BUSY_DELAY);
	}

	ERROR(rt2x00dev, "Indirect register access failed: "
	      "offset=0x%.08x, value=0x%.08x\n", offset, *reg);
	*reg = ~0;

	return 0;
}

#define WAIT_FOR_BBP(__dev, __reg) \
	rt2500usb_regbusy_read((__dev), PHY_CSR8, PHY_CSR8_BUSY, (__reg))
#define WAIT_FOR_RF(__dev, __reg) \
	rt2500usb_regbusy_read((__dev), PHY_CSR10, PHY_CSR10_RF_BUSY, (__reg))

static void rt2500usb_bbp_write(struct rt2x00_dev *rt2x00dev,
				const unsigned int word, const u8 value)
{
	u16 reg;

	mutex_lock(&rt2x00dev->csr_mutex);

	/*
	 * Wait until the BBP becomes available, afterwards we
	 * can safely write the new data into the register.
	 */
	if (WAIT_FOR_BBP(rt2x00dev, &reg)) {
		reg = 0;
		rt2x00_set_field16(&reg, PHY_CSR7_DATA, value);
		rt2x00_set_field16(&reg, PHY_CSR7_REG_ID, word);
		rt2x00_set_field16(&reg, PHY_CSR7_READ_CONTROL, 0);

		rt2500usb_register_write_lock(rt2x00dev, PHY_CSR7, reg);
	}

	mutex_unlock(&rt2x00dev->csr_mutex);
}

static void rt2500usb_bbp_read(struct rt2x00_dev *rt2x00dev,
			       const unsigned int word, u8 *value)
{
	u16 reg;

	mutex_lock(&rt2x00dev->csr_mutex);

	/*
	 * Wait until the BBP becomes available, afterwards we
	 * can safely write the read request into the register.
	 * After the data has been written, we wait until hardware
	 * returns the correct value, if at any time the register
	 * doesn't become available in time, reg will be 0xffffffff
	 * which means we return 0xff to the caller.
	 */
	if (WAIT_FOR_BBP(rt2x00dev, &reg)) {
		reg = 0;
		rt2x00_set_field16(&reg, PHY_CSR7_REG_ID, word);
		rt2x00_set_field16(&reg, PHY_CSR7_READ_CONTROL, 1);

		rt2500usb_register_write_lock(rt2x00dev, PHY_CSR7, reg);

		if (WAIT_FOR_BBP(rt2x00dev, &reg))
			rt2500usb_register_read_lock(rt2x00dev, PHY_CSR7, &reg);
	}

	*value = rt2x00_get_field16(reg, PHY_CSR7_DATA);

	mutex_unlock(&rt2x00dev->csr_mutex);
}

static void rt2500usb_rf_write(struct rt2x00_dev *rt2x00dev,
			       const unsigned int word, const u32 value)
{
	u16 reg;

	mutex_lock(&rt2x00dev->csr_mutex);

	/*
	 * Wait until the RF becomes available, afterwards we
	 * can safely write the new data into the register.
	 */
	if (WAIT_FOR_RF(rt2x00dev, &reg)) {
		reg = 0;
		rt2x00_set_field16(&reg, PHY_CSR9_RF_VALUE, value);
		rt2500usb_register_write_lock(rt2x00dev, PHY_CSR9, reg);

		reg = 0;
		rt2x00_set_field16(&reg, PHY_CSR10_RF_VALUE, value >> 16);
		rt2x00_set_field16(&reg, PHY_CSR10_RF_NUMBER_OF_BITS, 20);
		rt2x00_set_field16(&reg, PHY_CSR10_RF_IF_SELECT, 0);
		rt2x00_set_field16(&reg, PHY_CSR10_RF_BUSY, 1);

		rt2500usb_register_write_lock(rt2x00dev, PHY_CSR10, reg);
		rt2x00_rf_write(rt2x00dev, word, value);
	}

	mutex_unlock(&rt2x00dev->csr_mutex);
}

#ifdef CONFIG_RT2X00_LIB_DEBUGFS
static void _rt2500usb_register_read(struct rt2x00_dev *rt2x00dev,
				     const unsigned int offset,
				     u32 *value)
{
	rt2500usb_register_read(rt2x00dev, offset, (u16 *)value);
}

static void _rt2500usb_register_write(struct rt2x00_dev *rt2x00dev,
				      const unsigned int offset,
				      u32 value)
{
	rt2500usb_register_write(rt2x00dev, offset, value);
}

static const struct rt2x00debug rt2500usb_rt2x00debug = {
	.owner	= THIS_MODULE,
	.csr	= {
		.read		= _rt2500usb_register_read,
		.write		= _rt2500usb_register_write,
		.flags		= RT2X00DEBUGFS_OFFSET,
		.word_base	= CSR_REG_BASE,
		.word_size	= sizeof(u16),
		.word_count	= CSR_REG_SIZE / sizeof(u16),
	},
	.eeprom	= {
		.read		= rt2x00_eeprom_read,
		.write		= rt2x00_eeprom_write,
		.word_base	= EEPROM_BASE,
		.word_size	= sizeof(u16),
		.word_count	= EEPROM_SIZE / sizeof(u16),
	},
	.bbp	= {
		.read		= rt2500usb_bbp_read,
		.write		= rt2500usb_bbp_write,
		.word_base	= BBP_BASE,
		.word_size	= sizeof(u8),
		.word_count	= BBP_SIZE / sizeof(u8),
	},
	.rf	= {
		.read		= rt2x00_rf_read,
		.write		= rt2500usb_rf_write,
		.word_base	= RF_BASE,
		.word_size	= sizeof(u32),
		.word_count	= RF_SIZE / sizeof(u32),
	},
};
#endif /* CONFIG_RT2X00_LIB_DEBUGFS */

static int rt2500usb_rfkill_poll(struct rt2x00_dev *rt2x00dev)
{
	u16 reg;

	rt2500usb_register_read(rt2x00dev, MAC_CSR19, &reg);
	return rt2x00_get_field32(reg, MAC_CSR19_BIT7);
}

#ifdef CONFIG_RT2X00_LIB_LEDS
static void rt2500usb_brightness_set(struct led_classdev *led_cdev,
				     enum led_brightness brightness)
{
	struct rt2x00_led *led =
	    container_of(led_cdev, struct rt2x00_led, led_dev);
	unsigned int enabled = brightness != LED_OFF;
	u16 reg;

	rt2500usb_register_read(led->rt2x00dev, MAC_CSR20, &reg);

	if (led->type == LED_TYPE_RADIO || led->type == LED_TYPE_ASSOC)
		rt2x00_set_field16(&reg, MAC_CSR20_LINK, enabled);
	else if (led->type == LED_TYPE_ACTIVITY)
		rt2x00_set_field16(&reg, MAC_CSR20_ACTIVITY, enabled);

	rt2500usb_register_write(led->rt2x00dev, MAC_CSR20, reg);
}

static int rt2500usb_blink_set(struct led_classdev *led_cdev,
			       unsigned long *delay_on,
			       unsigned long *delay_off)
{
	struct rt2x00_led *led =
	    container_of(led_cdev, struct rt2x00_led, led_dev);
	u16 reg;

	rt2500usb_register_read(led->rt2x00dev, MAC_CSR21, &reg);
	rt2x00_set_field16(&reg, MAC_CSR21_ON_PERIOD, *delay_on);
	rt2x00_set_field16(&reg, MAC_CSR21_OFF_PERIOD, *delay_off);
	rt2500usb_register_write(led->rt2x00dev, MAC_CSR21, reg);

	return 0;
}

static void rt2500usb_init_led(struct rt2x00_dev *rt2x00dev,
			       struct rt2x00_led *led,
			       enum led_type type)
{
	led->rt2x00dev = rt2x00dev;
	led->type = type;
	led->led_dev.brightness_set = rt2500usb_brightness_set;
	led->led_dev.blink_set = rt2500usb_blink_set;
	led->flags = LED_INITIALIZED;
}
#endif /* CONFIG_RT2X00_LIB_LEDS */

/*
 * Configuration handlers.
 */

/*
 * rt2500usb does not differentiate between shared and pairwise
 * keys, so we should use the same function for both key types.
 */
static int rt2500usb_config_key(struct rt2x00_dev *rt2x00dev,
				struct rt2x00lib_crypto *crypto,
				struct ieee80211_key_conf *key)
{
	u32 mask;
	u16 reg;
	enum cipher curr_cipher;

	if (crypto->cmd == SET_KEY) {
		/*
		 * Disallow to set WEP key other than with index 0,
		 * it is known that not work at least on some hardware.
		 * SW crypto will be used in that case.
		 */
		if ((key->cipher == WLAN_CIPHER_SUITE_WEP40 ||
		     key->cipher == WLAN_CIPHER_SUITE_WEP104) &&
		    key->keyidx != 0)
			return -EOPNOTSUPP;

		/*
		 * Pairwise key will always be entry 0, but this
		 * could collide with a shared key on the same
		 * position...
		 */
		mask = TXRX_CSR0_KEY_ID.bit_mask;

		rt2500usb_register_read(rt2x00dev, TXRX_CSR0, &reg);
		curr_cipher = rt2x00_get_field16(reg, TXRX_CSR0_ALGORITHM);
		reg &= mask;

		if (reg && reg == mask)
			return -ENOSPC;

		reg = rt2x00_get_field16(reg, TXRX_CSR0_KEY_ID);

		key->hw_key_idx += reg ? ffz(reg) : 0;
		/*
		 * Hardware requires that all keys use the same cipher
		 * (e.g. TKIP-only, AES-only, but not TKIP+AES).
		 * If this is not the first key, compare the cipher with the
		 * first one and fall back to SW crypto if not the same.
		 */
		if (key->hw_key_idx > 0 && crypto->cipher != curr_cipher)
			return -EOPNOTSUPP;

		rt2500usb_register_multiwrite(rt2x00dev, KEY_ENTRY(key->hw_key_idx),
					      crypto->key, sizeof(crypto->key));

		/*
		 * The driver does not support the IV/EIV generation
		 * in hardware. However it demands the data to be provided
		 * both separately as well as inside the frame.
		 * We already provided the CONFIG_CRYPTO_COPY_IV to rt2x00lib
		 * to ensure rt2x00lib will not strip the data from the
		 * frame after the copy, now we must tell mac80211
		 * to generate the IV/EIV data.
		 */
		key->flags |= IEEE80211_KEY_FLAG_GENERATE_IV;
		key->flags |= IEEE80211_KEY_FLAG_GENERATE_MMIC;
	}

	/*
	 * TXRX_CSR0_KEY_ID contains only single-bit fields to indicate
	 * a particular key is valid.
	 */
	rt2500usb_register_read(rt2x00dev, TXRX_CSR0, &reg);
	rt2x00_set_field16(&reg, TXRX_CSR0_ALGORITHM, crypto->cipher);
	rt2x00_set_field16(&reg, TXRX_CSR0_IV_OFFSET, IEEE80211_HEADER);

	mask = rt2x00_get_field16(reg, TXRX_CSR0_KEY_ID);
	if (crypto->cmd == SET_KEY)
		mask |= 1 << key->hw_key_idx;
	else if (crypto->cmd == DISABLE_KEY)
		mask &= ~(1 << key->hw_key_idx);
	rt2x00_set_field16(&reg, TXRX_CSR0_KEY_ID, mask);
	rt2500usb_register_write(rt2x00dev, TXRX_CSR0, reg);

	return 0;
}

static void rt2500usb_config_filter(struct rt2x00_dev *rt2x00dev,
				    const unsigned int filter_flags)
{
	u16 reg;

	/*
	 * Start configuration steps.
	 * Note that the version error will always be dropped
	 * and broadcast frames will always be accepted since
	 * there is no filter for it at this time.
	 */
	rt2500usb_register_read(rt2x00dev, TXRX_CSR2, &reg);
	rt2x00_set_field16(&reg, TXRX_CSR2_DROP_CRC,
			   !(filter_flags & FIF_FCSFAIL));
	rt2x00_set_field16(&reg, TXRX_CSR2_DROP_PHYSICAL,
			   !(filter_flags & FIF_PLCPFAIL));
	rt2x00_set_field16(&reg, TXRX_CSR2_DROP_CONTROL,
			   !(filter_flags & FIF_CONTROL));
	rt2x00_set_field16(&reg, TXRX_CSR2_DROP_NOT_TO_ME,
			   !(filter_flags & FIF_PROMISC_IN_BSS));
	rt2x00_set_field16(&reg, TXRX_CSR2_DROP_TODS,
			   !(filter_flags & FIF_PROMISC_IN_BSS) &&
			   !rt2x00dev->intf_ap_count);
	rt2x00_set_field16(&reg, TXRX_CSR2_DROP_VERSION_ERROR, 1);
	rt2x00_set_field16(&reg, TXRX_CSR2_DROP_MULTICAST,
			   !(filter_flags & FIF_ALLMULTI));
	rt2x00_set_field16(&reg, TXRX_CSR2_DROP_BROADCAST, 0);
	rt2500usb_register_write(rt2x00dev, TXRX_CSR2, reg);
}

static void rt2500usb_config_intf(struct rt2x00_dev *rt2x00dev,
				  struct rt2x00_intf *intf,
				  struct rt2x00intf_conf *conf,
				  const unsigned int flags)
{
	unsigned int bcn_preload;
	u16 reg;

	if (flags & CONFIG_UPDATE_TYPE) {
		/*
		 * Enable beacon config
		 */
		bcn_preload = PREAMBLE + GET_DURATION(IEEE80211_HEADER, 20);
		rt2500usb_register_read(rt2x00dev, TXRX_CSR20, &reg);
		rt2x00_set_field16(&reg, TXRX_CSR20_OFFSET, bcn_preload >> 6);
		rt2x00_set_field16(&reg, TXRX_CSR20_BCN_EXPECT_WINDOW,
				   2 * (conf->type != NL80211_IFTYPE_STATION));
		rt2500usb_register_write(rt2x00dev, TXRX_CSR20, reg);

		/*
		 * Enable synchronisation.
		 */
		rt2500usb_register_read(rt2x00dev, TXRX_CSR18, &reg);
		rt2x00_set_field16(&reg, TXRX_CSR18_OFFSET, 0);
		rt2500usb_register_write(rt2x00dev, TXRX_CSR18, reg);

		rt2500usb_register_read(rt2x00dev, TXRX_CSR19, &reg);
		rt2x00_set_field16(&reg, TXRX_CSR19_TSF_SYNC, conf->sync);
		rt2500usb_register_write(rt2x00dev, TXRX_CSR19, reg);
	}

	if (flags & CONFIG_UPDATE_MAC)
		rt2500usb_register_multiwrite(rt2x00dev, MAC_CSR2, conf->mac,
					      (3 * sizeof(__le16)));

	if (flags & CONFIG_UPDATE_BSSID)
		rt2500usb_register_multiwrite(rt2x00dev, MAC_CSR5, conf->bssid,
					      (3 * sizeof(__le16)));
}

static void rt2500usb_config_erp(struct rt2x00_dev *rt2x00dev,
				 struct rt2x00lib_erp *erp,
				 u32 changed)
{
	u16 reg;

	if (changed & BSS_CHANGED_ERP_PREAMBLE) {
		rt2500usb_register_read(rt2x00dev, TXRX_CSR10, &reg);
		rt2x00_set_field16(&reg, TXRX_CSR10_AUTORESPOND_PREAMBLE,
				   !!erp->short_preamble);
		rt2500usb_register_write(rt2x00dev, TXRX_CSR10, reg);
	}

	if (changed & BSS_CHANGED_BASIC_RATES)
		rt2500usb_register_write(rt2x00dev, TXRX_CSR11,
					 erp->basic_rates);

	if (changed & BSS_CHANGED_BEACON_INT) {
		rt2500usb_register_read(rt2x00dev, TXRX_CSR18, &reg);
		rt2x00_set_field16(&reg, TXRX_CSR18_INTERVAL,
				   erp->beacon_int * 4);
		rt2500usb_register_write(rt2x00dev, TXRX_CSR18, reg);
	}

	if (changed & BSS_CHANGED_ERP_SLOT) {
		rt2500usb_register_write(rt2x00dev, MAC_CSR10, erp->slot_time);
		rt2500usb_register_write(rt2x00dev, MAC_CSR11, erp->sifs);
		rt2500usb_register_write(rt2x00dev, MAC_CSR12, erp->eifs);
	}
}

static void rt2500usb_config_ant(struct rt2x00_dev *rt2x00dev,
				 struct antenna_setup *ant)
{
	u8 r2;
	u8 r14;
	u16 csr5;
	u16 csr6;

	/*
	 * We should never come here because rt2x00lib is supposed
	 * to catch this and send us the correct antenna explicitely.
	 */
	BUG_ON(ant->rx == ANTENNA_SW_DIVERSITY ||
	       ant->tx == ANTENNA_SW_DIVERSITY);

	rt2500usb_bbp_read(rt2x00dev, 2, &r2);
	rt2500usb_bbp_read(rt2x00dev, 14, &r14);
	rt2500usb_register_read(rt2x00dev, PHY_CSR5, &csr5);
	rt2500usb_register_read(rt2x00dev, PHY_CSR6, &csr6);

	/*
	 * Configure the TX antenna.
	 */
	switch (ant->tx) {
	case ANTENNA_HW_DIVERSITY:
		rt2x00_set_field8(&r2, BBP_R2_TX_ANTENNA, 1);
		rt2x00_set_field16(&csr5, PHY_CSR5_CCK, 1);
		rt2x00_set_field16(&csr6, PHY_CSR6_OFDM, 1);
		break;
	case ANTENNA_A:
		rt2x00_set_field8(&r2, BBP_R2_TX_ANTENNA, 0);
		rt2x00_set_field16(&csr5, PHY_CSR5_CCK, 0);
		rt2x00_set_field16(&csr6, PHY_CSR6_OFDM, 0);
		break;
	case ANTENNA_B:
	default:
		rt2x00_set_field8(&r2, BBP_R2_TX_ANTENNA, 2);
		rt2x00_set_field16(&csr5, PHY_CSR5_CCK, 2);
		rt2x00_set_field16(&csr6, PHY_CSR6_OFDM, 2);
		break;
	}

	/*
	 * Configure the RX antenna.
	 */
	switch (ant->rx) {
	case ANTENNA_HW_DIVERSITY:
		rt2x00_set_field8(&r14, BBP_R14_RX_ANTENNA, 1);
		break;
	case ANTENNA_A:
		rt2x00_set_field8(&r14, BBP_R14_RX_ANTENNA, 0);
		break;
	case ANTENNA_B:
	default:
		rt2x00_set_field8(&r14, BBP_R14_RX_ANTENNA, 2);
		break;
	}

	/*
	 * RT2525E and RT5222 need to flip TX I/Q
	 */
	if (rt2x00_rf(rt2x00dev, RF2525E) || rt2x00_rf(rt2x00dev, RF5222)) {
		rt2x00_set_field8(&r2, BBP_R2_TX_IQ_FLIP, 1);
		rt2x00_set_field16(&csr5, PHY_CSR5_CCK_FLIP, 1);
		rt2x00_set_field16(&csr6, PHY_CSR6_OFDM_FLIP, 1);

		/*
		 * RT2525E does not need RX I/Q Flip.
		 */
		if (rt2x00_rf(rt2x00dev, RF2525E))
			rt2x00_set_field8(&r14, BBP_R14_RX_IQ_FLIP, 0);
	} else {
		rt2x00_set_field16(&csr5, PHY_CSR5_CCK_FLIP, 0);
		rt2x00_set_field16(&csr6, PHY_CSR6_OFDM_FLIP, 0);
	}

	rt2500usb_bbp_write(rt2x00dev, 2, r2);
	rt2500usb_bbp_write(rt2x00dev, 14, r14);
	rt2500usb_register_write(rt2x00dev, PHY_CSR5, csr5);
	rt2500usb_register_write(rt2x00dev, PHY_CSR6, csr6);
}

static void rt2500usb_config_channel(struct rt2x00_dev *rt2x00dev,
				     struct rf_channel *rf, const int txpower)
{
	/*
	 * Set TXpower.
	 */
	rt2x00_set_field32(&rf->rf3, RF3_TXPOWER, TXPOWER_TO_DEV(txpower));

	/*
	 * For RT2525E we should first set the channel to half band higher.
	 */
	if (rt2x00_rf(rt2x00dev, RF2525E)) {
		static const u32 vals[] = {
			0x000008aa, 0x000008ae, 0x000008ae, 0x000008b2,
			0x000008b2, 0x000008b6, 0x000008b6, 0x000008ba,
			0x000008ba, 0x000008be, 0x000008b7, 0x00000902,
			0x00000902, 0x00000906
		};

		rt2500usb_rf_write(rt2x00dev, 2, vals[rf->channel - 1]);
		if (rf->rf4)
			rt2500usb_rf_write(rt2x00dev, 4, rf->rf4);
	}

	rt2500usb_rf_write(rt2x00dev, 1, rf->rf1);
	rt2500usb_rf_write(rt2x00dev, 2, rf->rf2);
	rt2500usb_rf_write(rt2x00dev, 3, rf->rf3);
	if (rf->rf4)
		rt2500usb_rf_write(rt2x00dev, 4, rf->rf4);
}

static void rt2500usb_config_txpower(struct rt2x00_dev *rt2x00dev,
				     const int txpower)
{
	u32 rf3;

	rt2x00_rf_read(rt2x00dev, 3, &rf3);
	rt2x00_set_field32(&rf3, RF3_TXPOWER, TXPOWER_TO_DEV(txpower));
	rt2500usb_rf_write(rt2x00dev, 3, rf3);
}

static void rt2500usb_config_ps(struct rt2x00_dev *rt2x00dev,
				struct rt2x00lib_conf *libconf)
{
	enum dev_state state =
	    (libconf->conf->flags & IEEE80211_CONF_PS) ?
		STATE_SLEEP : STATE_AWAKE;
	u16 reg;

	if (state == STATE_SLEEP) {
		rt2500usb_register_read(rt2x00dev, MAC_CSR18, &reg);
		rt2x00_set_field16(&reg, MAC_CSR18_DELAY_AFTER_BEACON,
				   rt2x00dev->beacon_int - 20);
		rt2x00_set_field16(&reg, MAC_CSR18_BEACONS_BEFORE_WAKEUP,
				   libconf->conf->listen_interval - 1);

		/* We must first disable autowake before it can be enabled */
		rt2x00_set_field16(&reg, MAC_CSR18_AUTO_WAKE, 0);
		rt2500usb_register_write(rt2x00dev, MAC_CSR18, reg);

		rt2x00_set_field16(&reg, MAC_CSR18_AUTO_WAKE, 1);
		rt2500usb_register_write(rt2x00dev, MAC_CSR18, reg);
	} else {
		rt2500usb_register_read(rt2x00dev, MAC_CSR18, &reg);
		rt2x00_set_field16(&reg, MAC_CSR18_AUTO_WAKE, 0);
		rt2500usb_register_write(rt2x00dev, MAC_CSR18, reg);
	}

	rt2x00dev->ops->lib->set_device_state(rt2x00dev, state);
}

static void rt2500usb_config(struct rt2x00_dev *rt2x00dev,
			     struct rt2x00lib_conf *libconf,
			     const unsigned int flags)
{
	if (flags & IEEE80211_CONF_CHANGE_CHANNEL)
		rt2500usb_config_channel(rt2x00dev, &libconf->rf,
					 libconf->conf->power_level);
	if ((flags & IEEE80211_CONF_CHANGE_POWER) &&
	    !(flags & IEEE80211_CONF_CHANGE_CHANNEL))
		rt2500usb_config_txpower(rt2x00dev,
					 libconf->conf->power_level);
	if (flags & IEEE80211_CONF_CHANGE_PS)
		rt2500usb_config_ps(rt2x00dev, libconf);
}

/*
 * Link tuning
 */
static void rt2500usb_link_stats(struct rt2x00_dev *rt2x00dev,
				 struct link_qual *qual)
{
	u16 reg;

	/*
	 * Update FCS error count from register.
	 */
	rt2500usb_register_read(rt2x00dev, STA_CSR0, &reg);
	qual->rx_failed = rt2x00_get_field16(reg, STA_CSR0_FCS_ERROR);

	/*
	 * Update False CCA count from register.
	 */
	rt2500usb_register_read(rt2x00dev, STA_CSR3, &reg);
	qual->false_cca = rt2x00_get_field16(reg, STA_CSR3_FALSE_CCA_ERROR);
}

static void rt2500usb_reset_tuner(struct rt2x00_dev *rt2x00dev,
				  struct link_qual *qual)
{
	u16 eeprom;
	u16 value;

	rt2x00_eeprom_read(rt2x00dev, EEPROM_BBPTUNE_R24, &eeprom);
	value = rt2x00_get_field16(eeprom, EEPROM_BBPTUNE_R24_LOW);
	rt2500usb_bbp_write(rt2x00dev, 24, value);

	rt2x00_eeprom_read(rt2x00dev, EEPROM_BBPTUNE_R25, &eeprom);
	value = rt2x00_get_field16(eeprom, EEPROM_BBPTUNE_R25_LOW);
	rt2500usb_bbp_write(rt2x00dev, 25, value);

	rt2x00_eeprom_read(rt2x00dev, EEPROM_BBPTUNE_R61, &eeprom);
	value = rt2x00_get_field16(eeprom, EEPROM_BBPTUNE_R61_LOW);
	rt2500usb_bbp_write(rt2x00dev, 61, value);

	rt2x00_eeprom_read(rt2x00dev, EEPROM_BBPTUNE_VGC, &eeprom);
	value = rt2x00_get_field16(eeprom, EEPROM_BBPTUNE_VGCUPPER);
	rt2500usb_bbp_write(rt2x00dev, 17, value);

	qual->vgc_level = value;
}

/*
 * Queue handlers.
 */
static void rt2500usb_start_queue(struct data_queue *queue)
{
	struct rt2x00_dev *rt2x00dev = queue->rt2x00dev;
	u16 reg;

	switch (queue->qid) {
	case QID_RX:
		rt2500usb_register_read(rt2x00dev, TXRX_CSR2, &reg);
		rt2x00_set_field16(&reg, TXRX_CSR2_DISABLE_RX, 0);
		rt2500usb_register_write(rt2x00dev, TXRX_CSR2, reg);
		break;
	case QID_BEACON:
		rt2500usb_register_read(rt2x00dev, TXRX_CSR19, &reg);
		rt2x00_set_field16(&reg, TXRX_CSR19_TSF_COUNT, 1);
		rt2x00_set_field16(&reg, TXRX_CSR19_TBCN, 1);
		rt2x00_set_field16(&reg, TXRX_CSR19_BEACON_GEN, 1);
		rt2500usb_register_write(rt2x00dev, TXRX_CSR19, reg);
		break;
	default:
		break;
	}
}

static void rt2500usb_stop_queue(struct data_queue *queue)
{
	struct rt2x00_dev *rt2x00dev = queue->rt2x00dev;
	u16 reg;

	switch (queue->qid) {
	case QID_RX:
		rt2500usb_register_read(rt2x00dev, TXRX_CSR2, &reg);
		rt2x00_set_field16(&reg, TXRX_CSR2_DISABLE_RX, 1);
		rt2500usb_register_write(rt2x00dev, TXRX_CSR2, reg);
		break;
	case QID_BEACON:
		rt2500usb_register_read(rt2x00dev, TXRX_CSR19, &reg);
		rt2x00_set_field16(&reg, TXRX_CSR19_TSF_COUNT, 0);
		rt2x00_set_field16(&reg, TXRX_CSR19_TBCN, 0);
		rt2x00_set_field16(&reg, TXRX_CSR19_BEACON_GEN, 0);
		rt2500usb_register_write(rt2x00dev, TXRX_CSR19, reg);
		break;
	default:
		break;
	}
}

/*
 * Initialization functions.
 */
static int rt2500usb_init_registers(struct rt2x00_dev *rt2x00dev)
{
	u16 reg;

	rt2x00usb_vendor_request_sw(rt2x00dev, USB_DEVICE_MODE, 0x0001,
				    USB_MODE_TEST, REGISTER_TIMEOUT);
	rt2x00usb_vendor_request_sw(rt2x00dev, USB_SINGLE_WRITE, 0x0308,
				    0x00f0, REGISTER_TIMEOUT);

	rt2500usb_register_read(rt2x00dev, TXRX_CSR2, &reg);
	rt2x00_set_field16(&reg, TXRX_CSR2_DISABLE_RX, 1);
	rt2500usb_register_write(rt2x00dev, TXRX_CSR2, reg);

	rt2500usb_register_write(rt2x00dev, MAC_CSR13, 0x1111);
	rt2500usb_register_write(rt2x00dev, MAC_CSR14, 0x1e11);

	rt2500usb_register_read(rt2x00dev, MAC_CSR1, &reg);
	rt2x00_set_field16(&reg, MAC_CSR1_SOFT_RESET, 1);
	rt2x00_set_field16(&reg, MAC_CSR1_BBP_RESET, 1);
	rt2x00_set_field16(&reg, MAC_CSR1_HOST_READY, 0);
	rt2500usb_register_write(rt2x00dev, MAC_CSR1, reg);

	rt2500usb_register_read(rt2x00dev, MAC_CSR1, &reg);
	rt2x00_set_field16(&reg, MAC_CSR1_SOFT_RESET, 0);
	rt2x00_set_field16(&reg, MAC_CSR1_BBP_RESET, 0);
	rt2x00_set_field16(&reg, MAC_CSR1_HOST_READY, 0);
	rt2500usb_register_write(rt2x00dev, MAC_CSR1, reg);

	rt2500usb_register_read(rt2x00dev, TXRX_CSR5, &reg);
	rt2x00_set_field16(&reg, TXRX_CSR5_BBP_ID0, 13);
	rt2x00_set_field16(&reg, TXRX_CSR5_BBP_ID0_VALID, 1);
	rt2x00_set_field16(&reg, TXRX_CSR5_BBP_ID1, 12);
	rt2x00_set_field16(&reg, TXRX_CSR5_BBP_ID1_VALID, 1);
	rt2500usb_register_write(rt2x00dev, TXRX_CSR5, reg);

	rt2500usb_register_read(rt2x00dev, TXRX_CSR6, &reg);
	rt2x00_set_field16(&reg, TXRX_CSR6_BBP_ID0, 10);
	rt2x00_set_field16(&reg, TXRX_CSR6_BBP_ID0_VALID, 1);
	rt2x00_set_field16(&reg, TXRX_CSR6_BBP_ID1, 11);
	rt2x00_set_field16(&reg, TXRX_CSR6_BBP_ID1_VALID, 1);
	rt2500usb_register_write(rt2x00dev, TXRX_CSR6, reg);

	rt2500usb_register_read(rt2x00dev, TXRX_CSR7, &reg);
	rt2x00_set_field16(&reg, TXRX_CSR7_BBP_ID0, 7);
	rt2x00_set_field16(&reg, TXRX_CSR7_BBP_ID0_VALID, 1);
	rt2x00_set_field16(&reg, TXRX_CSR7_BBP_ID1, 6);
	rt2x00_set_field16(&reg, TXRX_CSR7_BBP_ID1_VALID, 1);
	rt2500usb_register_write(rt2x00dev, TXRX_CSR7, reg);

	rt2500usb_register_read(rt2x00dev, TXRX_CSR8, &reg);
	rt2x00_set_field16(&reg, TXRX_CSR8_BBP_ID0, 5);
	rt2x00_set_field16(&reg, TXRX_CSR8_BBP_ID0_VALID, 1);
	rt2x00_set_field16(&reg, TXRX_CSR8_BBP_ID1, 0);
	rt2x00_set_field16(&reg, TXRX_CSR8_BBP_ID1_VALID, 0);
	rt2500usb_register_write(rt2x00dev, TXRX_CSR8, reg);

	rt2500usb_register_read(rt2x00dev, TXRX_CSR19, &reg);
	rt2x00_set_field16(&reg, TXRX_CSR19_TSF_COUNT, 0);
	rt2x00_set_field16(&reg, TXRX_CSR19_TSF_SYNC, 0);
	rt2x00_set_field16(&reg, TXRX_CSR19_TBCN, 0);
	rt2x00_set_field16(&reg, TXRX_CSR19_BEACON_GEN, 0);
	rt2500usb_register_write(rt2x00dev, TXRX_CSR19, reg);

	rt2500usb_register_write(rt2x00dev, TXRX_CSR21, 0xe78f);
	rt2500usb_register_write(rt2x00dev, MAC_CSR9, 0xff1d);

	if (rt2x00dev->ops->lib->set_device_state(rt2x00dev, STATE_AWAKE))
		return -EBUSY;

	rt2500usb_register_read(rt2x00dev, MAC_CSR1, &reg);
	rt2x00_set_field16(&reg, MAC_CSR1_SOFT_RESET, 0);
	rt2x00_set_field16(&reg, MAC_CSR1_BBP_RESET, 0);
	rt2x00_set_field16(&reg, MAC_CSR1_HOST_READY, 1);
	rt2500usb_register_write(rt2x00dev, MAC_CSR1, reg);

	if (rt2x00_rev(rt2x00dev) >= RT2570_VERSION_C) {
		rt2500usb_register_read(rt2x00dev, PHY_CSR2, &reg);
		rt2x00_set_field16(&reg, PHY_CSR2_LNA, 0);
	} else {
		reg = 0;
		rt2x00_set_field16(&reg, PHY_CSR2_LNA, 1);
		rt2x00_set_field16(&reg, PHY_CSR2_LNA_MODE, 3);
	}
	rt2500usb_register_write(rt2x00dev, PHY_CSR2, reg);

	rt2500usb_register_write(rt2x00dev, MAC_CSR11, 0x0002);
	rt2500usb_register_write(rt2x00dev, MAC_CSR22, 0x0053);
	rt2500usb_register_write(rt2x00dev, MAC_CSR15, 0x01ee);
	rt2500usb_register_write(rt2x00dev, MAC_CSR16, 0x0000);

	rt2500usb_register_read(rt2x00dev, MAC_CSR8, &reg);
	rt2x00_set_field16(&reg, MAC_CSR8_MAX_FRAME_UNIT,
			   rt2x00dev->rx->data_size);
	rt2500usb_register_write(rt2x00dev, MAC_CSR8, reg);

	rt2500usb_register_read(rt2x00dev, TXRX_CSR0, &reg);
	rt2x00_set_field16(&reg, TXRX_CSR0_ALGORITHM, CIPHER_NONE);
	rt2x00_set_field16(&reg, TXRX_CSR0_IV_OFFSET, IEEE80211_HEADER);
	rt2x00_set_field16(&reg, TXRX_CSR0_KEY_ID, 0);
	rt2500usb_register_write(rt2x00dev, TXRX_CSR0, reg);

	rt2500usb_register_read(rt2x00dev, MAC_CSR18, &reg);
	rt2x00_set_field16(&reg, MAC_CSR18_DELAY_AFTER_BEACON, 90);
	rt2500usb_register_write(rt2x00dev, MAC_CSR18, reg);

	rt2500usb_register_read(rt2x00dev, PHY_CSR4, &reg);
	rt2x00_set_field16(&reg, PHY_CSR4_LOW_RF_LE, 1);
	rt2500usb_register_write(rt2x00dev, PHY_CSR4, reg);

	rt2500usb_register_read(rt2x00dev, TXRX_CSR1, &reg);
	rt2x00_set_field16(&reg, TXRX_CSR1_AUTO_SEQUENCE, 1);
	rt2500usb_register_write(rt2x00dev, TXRX_CSR1, reg);

	return 0;
}

static int rt2500usb_wait_bbp_ready(struct rt2x00_dev *rt2x00dev)
{
	unsigned int i;
	u8 value;

	for (i = 0; i < REGISTER_BUSY_COUNT; i++) {
		rt2500usb_bbp_read(rt2x00dev, 0, &value);
		if ((value != 0xff) && (value != 0x00))
			return 0;
		udelay(REGISTER_BUSY_DELAY);
	}

	ERROR(rt2x00dev, "BBP register access failed, aborting.\n");
	return -EACCES;
}

static int rt2500usb_init_bbp(struct rt2x00_dev *rt2x00dev)
{
	unsigned int i;
	u16 eeprom;
	u8 value;
	u8 reg_id;

	if (unlikely(rt2500usb_wait_bbp_ready(rt2x00dev)))
		return -EACCES;

	rt2500usb_bbp_write(rt2x00dev, 3, 0x02);
	rt2500usb_bbp_write(rt2x00dev, 4, 0x19);
	rt2500usb_bbp_write(rt2x00dev, 14, 0x1c);
	rt2500usb_bbp_write(rt2x00dev, 15, 0x30);
	rt2500usb_bbp_write(rt2x00dev, 16, 0xac);
	rt2500usb_bbp_write(rt2x00dev, 18, 0x18);
	rt2500usb_bbp_write(rt2x00dev, 19, 0xff);
	rt2500usb_bbp_write(rt2x00dev, 20, 0x1e);
	rt2500usb_bbp_write(rt2x00dev, 21, 0x08);
	rt2500usb_bbp_write(rt2x00dev, 22, 0x08);
	rt2500usb_bbp_write(rt2x00dev, 23, 0x08);
	rt2500usb_bbp_write(rt2x00dev, 24, 0x80);
	rt2500usb_bbp_write(rt2x00dev, 25, 0x50);
	rt2500usb_bbp_write(rt2x00dev, 26, 0x08);
	rt2500usb_bbp_write(rt2x00dev, 27, 0x23);
	rt2500usb_bbp_write(rt2x00dev, 30, 0x10);
	rt2500usb_bbp_write(rt2x00dev, 31, 0x2b);
	rt2500usb_bbp_write(rt2x00dev, 32, 0xb9);
	rt2500usb_bbp_write(rt2x00dev, 34, 0x12);
	rt2500usb_bbp_write(rt2x00dev, 35, 0x50);
	rt2500usb_bbp_write(rt2x00dev, 39, 0xc4);
	rt2500usb_bbp_write(rt2x00dev, 40, 0x02);
	rt2500usb_bbp_write(rt2x00dev, 41, 0x60);
	rt2500usb_bbp_write(rt2x00dev, 53, 0x10);
	rt2500usb_bbp_write(rt2x00dev, 54, 0x18);
	rt2500usb_bbp_write(rt2x00dev, 56, 0x08);
	rt2500usb_bbp_write(rt2x00dev, 57, 0x10);
	rt2500usb_bbp_write(rt2x00dev, 58, 0x08);
	rt2500usb_bbp_write(rt2x00dev, 61, 0x60);
	rt2500usb_bbp_write(rt2x00dev, 62, 0x10);
	rt2500usb_bbp_write(rt2x00dev, 75, 0xff);

	for (i = 0; i < EEPROM_BBP_SIZE; i++) {
		rt2x00_eeprom_read(rt2x00dev, EEPROM_BBP_START + i, &eeprom);

		if (eeprom != 0xffff && eeprom != 0x0000) {
			reg_id = rt2x00_get_field16(eeprom, EEPROM_BBP_REG_ID);
			value = rt2x00_get_field16(eeprom, EEPROM_BBP_VALUE);
			rt2500usb_bbp_write(rt2x00dev, reg_id, value);
		}
	}

	return 0;
}

/*
 * Device state switch handlers.
 */
static int rt2500usb_enable_radio(struct rt2x00_dev *rt2x00dev)
{
	/*
	 * Initialize all registers.
	 */
	if (unlikely(rt2500usb_init_registers(rt2x00dev) ||
		     rt2500usb_init_bbp(rt2x00dev)))
		return -EIO;

	return 0;
}

static void rt2500usb_disable_radio(struct rt2x00_dev *rt2x00dev)
{
	rt2500usb_register_write(rt2x00dev, MAC_CSR13, 0x2121);
	rt2500usb_register_write(rt2x00dev, MAC_CSR14, 0x2121);

	/*
	 * Disable synchronisation.
	 */