rndis_wlan.c

	ret = rndis_set_oid(usbdev, OID_802_11_SSID, ssid, sizeof(*ssid));
	if (ret < 0) {
		netdev_warn(usbdev->net, "setting SSID failed (%08X)\n", ret);
		return ret;
	}
	if (ret == 0) {
		memcpy(&priv->essid, ssid, sizeof(priv->essid));
		priv->radio_on = true;
		netdev_dbg(usbdev->net, "%s(): radio_on = true\n", __func__);
	}

	return ret;
}

static int set_bssid(struct usbnet *usbdev, u8 bssid[ETH_ALEN])
{
	int ret;

	ret = rndis_set_oid(usbdev, OID_802_11_BSSID, bssid, ETH_ALEN);
	if (ret < 0) {
		netdev_warn(usbdev->net, "setting BSSID[%pM] failed (%08X)\n",
			    bssid, ret);
		return ret;
	}

	return ret;
}

static int clear_bssid(struct usbnet *usbdev)
{
	u8 broadcast_mac[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};

	return set_bssid(usbdev, broadcast_mac);
}

static int get_bssid(struct usbnet *usbdev, u8 bssid[ETH_ALEN])
{
	int ret, len;

	len = ETH_ALEN;
	ret = rndis_query_oid(usbdev, OID_802_11_BSSID, bssid, &len);

	if (ret != 0)
		memset(bssid, 0, ETH_ALEN);

	return ret;
}

static int get_association_info(struct usbnet *usbdev,
			struct ndis_80211_assoc_info *info, int len)
{
	return rndis_query_oid(usbdev, OID_802_11_ASSOCIATION_INFORMATION,
				info, &len);
}

static bool is_associated(struct usbnet *usbdev)
{
	struct rndis_wlan_private *priv = get_rndis_wlan_priv(usbdev);
	u8 bssid[ETH_ALEN];
	int ret;

	if (!priv->radio_on)
		return false;

	ret = get_bssid(usbdev, bssid);

	return (ret == 0 && !is_zero_ether_addr(bssid));
}

static int disassociate(struct usbnet *usbdev, bool reset_ssid)
{
	struct rndis_wlan_private *priv = get_rndis_wlan_priv(usbdev);
	struct ndis_80211_ssid ssid;
	int i, ret = 0;

	if (priv->radio_on) {
		ret = rndis_set_oid(usbdev, OID_802_11_DISASSOCIATE, NULL, 0);
		if (ret == 0) {
			priv->radio_on = false;
			netdev_dbg(usbdev->net, "%s(): radio_on = false\n",
				   __func__);

			if (reset_ssid)
				msleep(100);
		}
	}

	/* disassociate causes radio to be turned off; if reset_ssid
	 * is given, set random ssid to enable radio */
	if (reset_ssid) {
		/* Set device to infrastructure mode so we don't get ad-hoc
		 * 'media connect' indications with the random ssid.
		 */
		set_infra_mode(usbdev, NDIS_80211_INFRA_INFRA);

		ssid.length = cpu_to_le32(sizeof(ssid.essid));
		get_random_bytes(&ssid.essid[2], sizeof(ssid.essid)-2);
		ssid.essid[0] = 0x1;
		ssid.essid[1] = 0xff;
		for (i = 2; i < sizeof(ssid.essid); i++)
			ssid.essid[i] = 0x1 + (ssid.essid[i] * 0xfe / 0xff);
		ret = set_essid(usbdev, &ssid);
	}
	return ret;
}

static int set_auth_mode(struct usbnet *usbdev, u32 wpa_version,
				enum nl80211_auth_type auth_type, int keymgmt)
{
	struct rndis_wlan_private *priv = get_rndis_wlan_priv(usbdev);
	__le32 tmp;
	int auth_mode, ret;

	netdev_dbg(usbdev->net, "%s(): wpa_version=0x%x authalg=0x%x keymgmt=0x%x\n",
		   __func__, wpa_version, auth_type, keymgmt);

	if (wpa_version & NL80211_WPA_VERSION_2) {
		if (keymgmt & RNDIS_WLAN_KEY_MGMT_802_1X)
			auth_mode = NDIS_80211_AUTH_WPA2;
		else
			auth_mode = NDIS_80211_AUTH_WPA2_PSK;
	} else if (wpa_version & NL80211_WPA_VERSION_1) {
		if (keymgmt & RNDIS_WLAN_KEY_MGMT_802_1X)
			auth_mode = NDIS_80211_AUTH_WPA;
		else if (keymgmt & RNDIS_WLAN_KEY_MGMT_PSK)
			auth_mode = NDIS_80211_AUTH_WPA_PSK;
		else
			auth_mode = NDIS_80211_AUTH_WPA_NONE;
	} else if (auth_type == NL80211_AUTHTYPE_SHARED_KEY)
		auth_mode = NDIS_80211_AUTH_SHARED;
	else if (auth_type == NL80211_AUTHTYPE_OPEN_SYSTEM)
		auth_mode = NDIS_80211_AUTH_OPEN;
	else if (auth_type == NL80211_AUTHTYPE_AUTOMATIC)
		auth_mode = NDIS_80211_AUTH_AUTO_SWITCH;
	else
		return -ENOTSUPP;

	tmp = cpu_to_le32(auth_mode);
	ret = rndis_set_oid(usbdev, OID_802_11_AUTHENTICATION_MODE, &tmp,
								sizeof(tmp));
	if (ret != 0) {
		netdev_warn(usbdev->net, "setting auth mode failed (%08X)\n",
			    ret);
		return ret;
	}

	priv->wpa_version = wpa_version;

	return 0;
}

static int set_priv_filter(struct usbnet *usbdev)
{
	struct rndis_wlan_private *priv = get_rndis_wlan_priv(usbdev);
	__le32 tmp;

	netdev_dbg(usbdev->net, "%s(): wpa_version=0x%x\n",
		   __func__, priv->wpa_version);

	if (priv->wpa_version & NL80211_WPA_VERSION_2 ||
	    priv->wpa_version & NL80211_WPA_VERSION_1)
		tmp = cpu_to_le32(NDIS_80211_PRIV_8021X_WEP);
	else
		tmp = cpu_to_le32(NDIS_80211_PRIV_ACCEPT_ALL);

	return rndis_set_oid(usbdev, OID_802_11_PRIVACY_FILTER, &tmp,
								sizeof(tmp));
}

static int set_encr_mode(struct usbnet *usbdev, int pairwise, int groupwise)
{
	__le32 tmp;
	int encr_mode, ret;

	netdev_dbg(usbdev->net, "%s(): cipher_pair=0x%x cipher_group=0x%x\n",
		   __func__, pairwise, groupwise);

	if (pairwise & RNDIS_WLAN_ALG_CCMP)
		encr_mode = NDIS_80211_ENCR_CCMP_ENABLED;
	else if (pairwise & RNDIS_WLAN_ALG_TKIP)
		encr_mode = NDIS_80211_ENCR_TKIP_ENABLED;
	else if (pairwise & RNDIS_WLAN_ALG_WEP)
		encr_mode = NDIS_80211_ENCR_WEP_ENABLED;
	else if (groupwise & RNDIS_WLAN_ALG_CCMP)
		encr_mode = NDIS_80211_ENCR_CCMP_ENABLED;
	else if (groupwise & RNDIS_WLAN_ALG_TKIP)
		encr_mode = NDIS_80211_ENCR_TKIP_ENABLED;
	else
		encr_mode = NDIS_80211_ENCR_DISABLED;

	tmp = cpu_to_le32(encr_mode);
	ret = rndis_set_oid(usbdev, OID_802_11_ENCRYPTION_STATUS, &tmp,
								sizeof(tmp));
	if (ret != 0) {
		netdev_warn(usbdev->net, "setting encr mode failed (%08X)\n",
			    ret);
		return ret;
	}

	return 0;
}

static int set_infra_mode(struct usbnet *usbdev, int mode)
{
	struct rndis_wlan_private *priv = get_rndis_wlan_priv(usbdev);
	__le32 tmp;
	int ret;

	netdev_dbg(usbdev->net, "%s(): infra_mode=0x%x\n",
		   __func__, priv->infra_mode);

	tmp = cpu_to_le32(mode);
	ret = rndis_set_oid(usbdev, OID_802_11_INFRASTRUCTURE_MODE, &tmp,
								sizeof(tmp));
	if (ret != 0) {
		netdev_warn(usbdev->net, "setting infra mode failed (%08X)\n",
			    ret);
		return ret;
	}

	/* NDIS drivers clear keys when infrastructure mode is
	 * changed. But Linux tools assume otherwise. So set the
	 * keys */
	restore_keys(usbdev);

	priv->infra_mode = mode;
	return 0;
}

static int set_rts_threshold(struct usbnet *usbdev, u32 rts_threshold)
{
	__le32 tmp;

	netdev_dbg(usbdev->net, "%s(): %i\n", __func__, rts_threshold);

	if (rts_threshold < 0 || rts_threshold > 2347)
		rts_threshold = 2347;

	tmp = cpu_to_le32(rts_threshold);
	return rndis_set_oid(usbdev, OID_802_11_RTS_THRESHOLD, &tmp,
								sizeof(tmp));
}

static int set_frag_threshold(struct usbnet *usbdev, u32 frag_threshold)
{
	__le32 tmp;

	netdev_dbg(usbdev->net, "%s(): %i\n", __func__, frag_threshold);

	if (frag_threshold < 256 || frag_threshold > 2346)
		frag_threshold = 2346;

	tmp = cpu_to_le32(frag_threshold);
	return rndis_set_oid(usbdev, OID_802_11_FRAGMENTATION_THRESHOLD, &tmp,
								sizeof(tmp));
}

static void set_default_iw_params(struct usbnet *usbdev)
{
	set_infra_mode(usbdev, NDIS_80211_INFRA_INFRA);
	set_auth_mode(usbdev, 0, NL80211_AUTHTYPE_OPEN_SYSTEM,
						RNDIS_WLAN_KEY_MGMT_NONE);
	set_priv_filter(usbdev);
	set_encr_mode(usbdev, RNDIS_WLAN_ALG_NONE, RNDIS_WLAN_ALG_NONE);
}

static int deauthenticate(struct usbnet *usbdev)
{
	int ret;

	ret = disassociate(usbdev, true);
	set_default_iw_params(usbdev);
	return ret;
}

static int set_channel(struct usbnet *usbdev, int channel)
{
	struct ndis_80211_conf config;
	unsigned int dsconfig;
	int len, ret;

	netdev_dbg(usbdev->net, "%s(%d)\n", __func__, channel);

	/* this OID is valid only when not associated */
	if (is_associated(usbdev))
		return 0;

	dsconfig = ieee80211_dsss_chan_to_freq(channel) * 1000;

	len = sizeof(config);
	ret = rndis_query_oid(usbdev, OID_802_11_CONFIGURATION, &config, &len);
	if (ret < 0) {
		netdev_dbg(usbdev->net, "%s(): querying configuration failed\n",
			   __func__);
		return ret;
	}

	config.ds_config = cpu_to_le32(dsconfig);
	ret = rndis_set_oid(usbdev, OID_802_11_CONFIGURATION, &config,
								sizeof(config));

	netdev_dbg(usbdev->net, "%s(): %d -> %d\n", __func__, channel, ret);

	return ret;
}

/* index must be 0 - N, as per NDIS  */
static int add_wep_key(struct usbnet *usbdev, const u8 *key, int key_len,
								int index)
{
	struct rndis_wlan_private *priv = get_rndis_wlan_priv(usbdev);
	struct ndis_80211_wep_key ndis_key;
	u32 cipher;
	int ret;

	netdev_dbg(usbdev->net, "%s(idx: %d, len: %d)\n",
		   __func__, index, key_len);

	if ((key_len != 5 && key_len != 13) || index < 0 || index > 3)
		return -EINVAL;

	if (key_len == 5)
		cipher = WLAN_CIPHER_SUITE_WEP40;
	else
		cipher = WLAN_CIPHER_SUITE_WEP104;

	memset(&ndis_key, 0, sizeof(ndis_key));

	ndis_key.size = cpu_to_le32(sizeof(ndis_key));
	ndis_key.length = cpu_to_le32(key_len);
	ndis_key.index = cpu_to_le32(index);
	memcpy(&ndis_key.material, key, key_len);

	if (index == priv->encr_tx_key_index) {
		ndis_key.index |= NDIS_80211_ADDWEP_TRANSMIT_KEY;
		ret = set_encr_mode(usbdev, RNDIS_WLAN_ALG_WEP,
							RNDIS_WLAN_ALG_NONE);
		if (ret)
			netdev_warn(usbdev->net, "encryption couldn't be enabled (%08X)\n",
				    ret);
	}

	ret = rndis_set_oid(usbdev, OID_802_11_ADD_WEP, &ndis_key,
							sizeof(ndis_key));
	if (ret != 0) {
		netdev_warn(usbdev->net, "adding encryption key %d failed (%08X)\n",
			    index + 1, ret);
		return ret;
	}

	priv->encr_keys[index].len = key_len;
	priv->encr_keys[index].cipher = cipher;
	memcpy(&priv->encr_keys[index].material, key, key_len);
	memset(&priv->encr_keys[index].bssid, 0xff, ETH_ALEN);

	return 0;
}

static int add_wpa_key(struct usbnet *usbdev, const u8 *key, int key_len,
			int index, const u8 *addr, const u8 *rx_seq,
			int seq_len, u32 cipher, __le32 flags)
{
	struct rndis_wlan_private *priv = get_rndis_wlan_priv(usbdev);
	struct ndis_80211_key ndis_key;
	bool is_addr_ok;
	int ret;

	if (index < 0 || index >= 4) {
		netdev_dbg(usbdev->net, "%s(): index out of range (%i)\n",
			   __func__, index);
		return -EINVAL;
	}
	if (key_len > sizeof(ndis_key.material) || key_len < 0) {
		netdev_dbg(usbdev->net, "%s(): key length out of range (%i)\n",
			   __func__, key_len);
		return -EINVAL;
	}
	if (flags & NDIS_80211_ADDKEY_SET_INIT_RECV_SEQ) {
		if (!rx_seq || seq_len <= 0) {
			netdev_dbg(usbdev->net, "%s(): recv seq flag without buffer\n",
				   __func__);
			return -EINVAL;
		}
		if (rx_seq && seq_len > sizeof(ndis_key.rsc)) {
			netdev_dbg(usbdev->net, "%s(): too big recv seq buffer\n", __func__);
			return -EINVAL;
		}
	}

	is_addr_ok = addr && !is_zero_ether_addr(addr) &&
					!is_broadcast_ether_addr(addr);
	if ((flags & NDIS_80211_ADDKEY_PAIRWISE_KEY) && !is_addr_ok) {
		netdev_dbg(usbdev->net, "%s(): pairwise but bssid invalid (%pM)\n",
			   __func__, addr);
		return -EINVAL;
	}

	netdev_dbg(usbdev->net, "%s(%i): flags:%i%i%i\n",
		   __func__, index,
		   !!(flags & NDIS_80211_ADDKEY_TRANSMIT_KEY),
		   !!(flags & NDIS_80211_ADDKEY_PAIRWISE_KEY),
		   !!(flags & NDIS_80211_ADDKEY_SET_INIT_RECV_SEQ));

	memset(&ndis_key, 0, sizeof(ndis_key));

	ndis_key.size = cpu_to_le32(sizeof(ndis_key) -
				sizeof(ndis_key.material) + key_len);
	ndis_key.length = cpu_to_le32(key_len);
	ndis_key.index = cpu_to_le32(index) | flags;

	if (cipher == WLAN_CIPHER_SUITE_TKIP && key_len == 32) {
		/* wpa_supplicant gives us the Michael MIC RX/TX keys in
		 * different order than NDIS spec, so swap the order here. */
		memcpy(ndis_key.material, key, 16);
		memcpy(ndis_key.material + 16, key + 24, 8);
		memcpy(ndis_key.material + 24, key + 16, 8);
	} else
		memcpy(ndis_key.material, key, key_len);

	if (flags & NDIS_80211_ADDKEY_SET_INIT_RECV_SEQ)
		memcpy(ndis_key.rsc, rx_seq, seq_len);

	if (flags & NDIS_80211_ADDKEY_PAIRWISE_KEY) {
		/* pairwise key */
		memcpy(ndis_key.bssid, addr, ETH_ALEN);
	} else {
		/* group key */
		if (priv->infra_mode == NDIS_80211_INFRA_ADHOC)
			memset(ndis_key.bssid, 0xff, ETH_ALEN);
		else
			get_bssid(usbdev, ndis_key.bssid);
	}

	ret = rndis_set_oid(usbdev, OID_802_11_ADD_KEY, &ndis_key,
					le32_to_cpu(ndis_key.size));
	netdev_dbg(usbdev->net, "%s(): OID_802_11_ADD_KEY -> %08X\n",
		   __func__, ret);
	if (ret != 0)
		return ret;

	memset(&priv->encr_keys[index], 0, sizeof(priv->encr_keys[index]));
	priv->encr_keys[index].len = key_len;
	priv->encr_keys[index].cipher = cipher;
	memcpy(&priv->encr_keys[index].material, key, key_len);
	if (flags & NDIS_80211_ADDKEY_PAIRWISE_KEY)
		memcpy(&priv->encr_keys[index].bssid, ndis_key.bssid, ETH_ALEN);
	else
		memset(&priv->encr_keys[index].bssid, 0xff, ETH_ALEN);

	if (flags & NDIS_80211_ADDKEY_TRANSMIT_KEY)
		priv->encr_tx_key_index = index;

	return 0;
}

static int restore_key(struct usbnet *usbdev, int key_idx)
{
	struct rndis_wlan_private *priv = get_rndis_wlan_priv(usbdev);
	struct rndis_wlan_encr_key key;

	if (is_wpa_key(priv, key_idx))
		return 0;

	key = priv->encr_keys[key_idx];

	netdev_dbg(usbdev->net, "%s(): %i:%i\n", __func__, key_idx, key.len);

	if (key.len == 0)
		return 0;

	return add_wep_key(usbdev, key.material, key.len, key_idx);
}

static void restore_keys(struct usbnet *usbdev)
{
	int i;

	for (i = 0; i < 4; i++)
		restore_key(usbdev, i);
}

static void clear_key(struct rndis_wlan_private *priv, int idx)
{
	memset(&priv->encr_keys[idx], 0, sizeof(priv->encr_keys[idx]));
}

/* remove_key is for both wep and wpa */
static int remove_key(struct usbnet *usbdev, int index, const u8 *bssid)
{
	struct rndis_wlan_private *priv = get_rndis_wlan_priv(usbdev);
	struct ndis_80211_remove_key remove_key;
	__le32 keyindex;
	bool is_wpa;
	int ret;

	if (priv->encr_keys[index].len == 0)
		return 0;

	is_wpa = is_wpa_key(priv, index);

	netdev_dbg(usbdev->net, "%s(): %i:%s:%i\n",
		   __func__, index, is_wpa ? "wpa" : "wep",
		   priv->encr_keys[index].len);

	clear_key(priv, index);

	if (is_wpa) {
		remove_key.size = cpu_to_le32(sizeof(remove_key));
		remove_key.index = cpu_to_le32(index);
		if (bssid) {
			/* pairwise key */
			if (!is_broadcast_ether_addr(bssid))
				remove_key.index |=
					NDIS_80211_ADDKEY_PAIRWISE_KEY;
			memcpy(remove_key.bssid, bssid,
					sizeof(remove_key.bssid));
		} else
			memset(remove_key.bssid, 0xff,
						sizeof(remove_key.bssid));

		ret = rndis_set_oid(usbdev, OID_802_11_REMOVE_KEY, &remove_key,
							sizeof(remove_key));
		if (ret != 0)
			return ret;
	} else {
		keyindex = cpu_to_le32(index);
		ret = rndis_set_oid(usbdev, OID_802_11_REMOVE_WEP, &keyindex,
							sizeof(keyindex));
		if (ret != 0) {
			netdev_warn(usbdev->net,
				    "removing encryption key %d failed (%08X)\n",
				    index, ret);
			return ret;
		}
	}

	/* if it is transmit key, disable encryption */
	if (index == priv->encr_tx_key_index)
		set_encr_mode(usbdev, RNDIS_WLAN_ALG_NONE, RNDIS_WLAN_ALG_NONE);

	return 0;
}

static void set_multicast_list(struct usbnet *usbdev)
{
	struct rndis_wlan_private *priv = get_rndis_wlan_priv(usbdev);
	struct dev_mc_list *mclist;
	__le32 filter, basefilter;
	int ret;
	char *mc_addrs = NULL;
	int mc_count;

	basefilter = filter = RNDIS_PACKET_TYPE_DIRECTED |
			      RNDIS_PACKET_TYPE_BROADCAST;

	if (usbdev->net->flags & IFF_PROMISC) {
		filter |= RNDIS_PACKET_TYPE_PROMISCUOUS |
			RNDIS_PACKET_TYPE_ALL_LOCAL;
	} else if (usbdev->net->flags & IFF_ALLMULTI) {
		filter |= RNDIS_PACKET_TYPE_ALL_MULTICAST;
	}

	if (filter != basefilter)
		goto set_filter;

	/*
	 * mc_list should be accessed holding the lock, so copy addresses to
	 * local buffer first.
	 */
	netif_addr_lock_bh(usbdev->net);
	mc_count = netdev_mc_count(usbdev->net);
	if (mc_count > priv->multicast_size) {
		filter |= RNDIS_PACKET_TYPE_ALL_MULTICAST;
	} else if (mc_count) {
		int i = 0;

		mc_addrs = kmalloc(mc_count * ETH_ALEN, GFP_ATOMIC);
		if (!mc_addrs) {
			netdev_warn(usbdev->net,
				    "couldn't alloc %d bytes of memory\n",
				    mc_count * ETH_ALEN);
			netif_addr_unlock_bh(usbdev->net);
			return;
		}

		netdev_for_each_mc_addr(mclist, usbdev->net)
			memcpy(mc_addrs + i++ * ETH_ALEN,
			       mclist->dmi_addr, ETH_ALEN);
	}
	netif_addr_unlock_bh(usbdev->net);

	if (filter != basefilter)
		goto set_filter;

	if (mc_count) {
		ret = rndis_set_oid(usbdev, OID_802_3_MULTICAST_LIST, mc_addrs,
				    mc_count * ETH_ALEN);
		kfree(mc_addrs);
		if (ret == 0)
			filter |= RNDIS_PACKET_TYPE_MULTICAST;
		else
			filter |= RNDIS_PACKET_TYPE_ALL_MULTICAST;

		netdev_dbg(usbdev->net, "OID_802_3_MULTICAST_LIST(%d, max: %d) -> %d\n",
			   mc_count, priv->multicast_size, ret);
	}

set_filter:
	ret = rndis_set_oid(usbdev, OID_GEN_CURRENT_PACKET_FILTER, &filter,
							sizeof(filter));
	if (ret < 0) {
		netdev_warn(usbdev->net, "couldn't set packet filter: %08x\n",
			    le32_to_cpu(filter));
	}

	netdev_dbg(usbdev->net, "OID_GEN_CURRENT_PACKET_FILTER(%08x) -> %d\n",
		   le32_to_cpu(filter), ret);
}

#ifdef DEBUG
static void debug_print_pmkids(struct usbnet *usbdev,
				struct ndis_80211_pmkid *pmkids,
				const char *func_str)
{
	struct rndis_wlan_private *priv = get_rndis_wlan_priv(usbdev);
	int i, len, count, max_pmkids, entry_len;

	max_pmkids = priv->wdev.wiphy->max_num_pmkids;
	len = le32_to_cpu(pmkids->length);
	count = le32_to_cpu(pmkids->bssid_info_count);

	entry_len = (count > 0) ? (len - sizeof(*pmkids)) / count : -1;

	netdev_dbg(usbdev->net, "%s(): %d PMKIDs (data len: %d, entry len: "
				"%d)\n", func_str, count, len, entry_len);

	if (count > max_pmkids)
		count = max_pmkids;

	for (i = 0; i < count; i++) {
		u32 *tmp = (u32 *)pmkids->bssid_info[i].pmkid;

		netdev_dbg(usbdev->net, "%s():  bssid: %pM, "
				"pmkid: %08X:%08X:%08X:%08X\n",
				func_str, pmkids->bssid_info[i].bssid,
				cpu_to_be32(tmp[0]), cpu_to_be32(tmp[1]),
				cpu_to_be32(tmp[2]), cpu_to_be32(tmp[3]));
	}
}
#else
static void debug_print_pmkids(struct usbnet *usbdev,
				struct ndis_80211_pmkid *pmkids,
				const char *func_str)
{
	return;
}
#endif

static struct ndis_80211_pmkid *get_device_pmkids(struct usbnet *usbdev)
{
	struct rndis_wlan_private *priv = get_rndis_wlan_priv(usbdev);
	struct ndis_80211_pmkid *pmkids;
	int len, ret, max_pmkids;

	max_pmkids = priv->wdev.wiphy->max_num_pmkids;
	len = sizeof(*pmkids) + max_pmkids * sizeof(pmkids->bssid_info[0]);

	pmkids = kzalloc(len, GFP_KERNEL);
	if (!pmkids)
		return ERR_PTR(-ENOMEM);

	pmkids->length = cpu_to_le32(len);
	pmkids->bssid_info_count = cpu_to_le32(max_pmkids);

	ret = rndis_query_oid(usbdev, OID_802_11_PMKID, pmkids, &len);
	if (ret < 0) {
		netdev_dbg(usbdev->net, "%s(): OID_802_11_PMKID(%d, %d)"
				" -> %d\n", __func__, len, max_pmkids, ret);

		kfree(pmkids);
		return ERR_PTR(ret);
	}

	if (le32_to_cpu(pmkids->bssid_info_count) > max_pmkids)
		pmkids->bssid_info_count = cpu_to_le32(max_pmkids);

	debug_print_pmkids(usbdev, pmkids, __func__);

	return pmkids;
}

static int set_device_pmkids(struct usbnet *usbdev,
				struct ndis_80211_pmkid *pmkids)
{
	int ret, len, num_pmkids;

	num_pmkids = le32_to_cpu(pmkids->bssid_info_count);
	len = sizeof(*pmkids) + num_pmkids * sizeof(pmkids->bssid_info[0]);
	pmkids->length = cpu_to_le32(len);

	debug_print_pmkids(usbdev, pmkids, __func__);

	ret = rndis_set_oid(usbdev, OID_802_11_PMKID, pmkids,
						le32_to_cpu(pmkids->length));
	if (ret < 0) {
		netdev_dbg(usbdev->net, "%s(): OID_802_11_PMKID(%d, %d) -> %d"
				"\n", __func__, len, num_pmkids, ret);
	}

	kfree(pmkids);
	return ret;
}

static struct ndis_80211_pmkid *remove_pmkid(struct usbnet *usbdev,
						struct ndis_80211_pmkid *pmkids,
						struct cfg80211_pmksa *pmksa,
						int max_pmkids)
{
	int i, len, count, newlen, err;

	len = le32_to_cpu(pmkids->length);
	count = le32_to_cpu(pmkids->bssid_info_count);

	if (count > max_pmkids)
		count = max_pmkids;

	for (i = 0; i < count; i++)
		if (!compare_ether_addr(pmkids->bssid_info[i].bssid,
							pmksa->bssid))
			break;

	/* pmkid not found */
	if (i == count) {
		netdev_dbg(usbdev->net, "%s(): bssid not found (%pM)\n",
					__func__, pmksa->bssid);
		err = -ENOENT;
		goto error;
	}

	for (; i + 1 < count; i++)
		pmkids->bssid_info[i] = pmkids->bssid_info[i + 1];

	count--;
	newlen = sizeof(*pmkids) + count * sizeof(pmkids->bssid_info[0]);

	pmkids->length = cpu_to_le32(newlen);
	pmkids->bssid_info_count = cpu_to_le32(count);

	return pmkids;
error:
	kfree(pmkids);
	return ERR_PTR(err);
}

static struct ndis_80211_pmkid *update_pmkid(struct usbnet *usbdev,
						struct ndis_80211_pmkid *pmkids,
						struct cfg80211_pmksa *pmksa,
						int max_pmkids)
{
	int i, err, len, count, newlen;

	len = le32_to_cpu(pmkids->length);
	count = le32_to_cpu(pmkids->bssid_info_count);

	if (count > max_pmkids)
		count = max_pmkids;

	/* update with new pmkid */
	for (i = 0; i < count; i++) {
		if (compare_ether_addr(pmkids->bssid_info[i].bssid,
							pmksa->bssid))
			continue;

		memcpy(pmkids->bssid_info[i].pmkid, pmksa->pmkid,
								WLAN_PMKID_LEN);

		return pmkids;
	}

	/* out of space, return error */
	if (i == max_pmkids) {
		netdev_dbg(usbdev->net, "%s(): out of space\n", __func__);
		err = -ENOSPC;
		goto error;
	}

	/* add new pmkid */
	newlen = sizeof(*pmkids) + (count + 1) * sizeof(pmkids->bssid_info[0]);

	pmkids = krealloc(pmkids, newlen, GFP_KERNEL);
	if (!pmkids) {
		err = -ENOMEM;
		goto error;
	}

	pmkids->length = cpu_to_le32(newlen);
	pmkids->bssid_info_count = cpu_to_le32(count + 1);

	memcpy(pmkids->bssid_info[count].bssid, pmksa->bssid, ETH_ALEN);
	memcpy(pmkids->bssid_info[count].pmkid, pmksa->pmkid, WLAN_PMKID_LEN);

	return pmkids;
error:
	kfree(pmkids);
	return ERR_PTR(err);
}

/*
 * cfg80211 ops
 */
static int rndis_change_virtual_intf(struct wiphy *wiphy,
					struct net_device *dev,
					enum nl80211_iftype type, u32 *flags,
					struct vif_params *params)
{
	struct rndis_wlan_private *priv = wiphy_priv(wiphy);
	struct usbnet *usbdev = priv->usbdev;
	int mode;

	switch (type) {
	case NL80211_IFTYPE_ADHOC:
		mode = NDIS_80211_INFRA_ADHOC;
		break;
	case NL80211_IFTYPE_STATION:
		mode = NDIS_80211_INFRA_INFRA;
		break;
	default:
		return -EINVAL;
	}

	priv->wdev.iftype = type;

	return set_infra_mode(usbdev, mode);
}

static int rndis_set_wiphy_params(struct wiphy *wiphy, u32 changed)
{
	struct rndis_wlan_private *priv = wiphy_priv(wiphy);
	struct usbnet *usbdev = priv->usbdev;
	int err;

	if (changed & WIPHY_PARAM_FRAG_THRESHOLD) {
		err = set_frag_threshold(usbdev, wiphy->frag_threshold);
		if (err < 0)
			return err;
	}

	if (changed & WIPHY_PARAM_RTS_THRESHOLD) {
		err = set_rts_threshold(usbdev, wiphy->rts_threshold);
		if (err < 0)
			return err;
	}

	return 0;
}

static int rndis_set_tx_power(struct wiphy *wiphy, enum tx_power_setting type,
				int dbm)
{
	struct rndis_wlan_private *priv = wiphy_priv(wiphy);
	struct usbnet *usbdev = priv->usbdev;

	netdev_dbg(usbdev->net, "%s(): type:0x%x dbm:%i\n",
		   __func__, type, dbm);

	/* Device doesn't support changing txpower after initialization, only
	 * turn off/on radio. Support 'auto' mode and setting same dBm that is
	 * currently used.
	 */
	if (type == TX_POWER_AUTOMATIC || dbm == get_bcm4320_power_dbm(priv)) {
		if (!priv->radio_on)
			disassociate(usbdev, true); /* turn on radio */

		return 0;
	}

	return -ENOTSUPP;
}

static int rndis_get_tx_power(struct wiphy *wiphy, int *dbm)
{
	struct rndis_wlan_private *priv = wiphy_priv(wiphy);
	struct usbnet *usbdev = priv->usbdev;

	*dbm = get_bcm4320_power_dbm(priv);

	netdev_dbg(usbdev->net, "%s(): dbm:%i\n", __func__, *dbm);

	return 0;
}

#define SCAN_DELAY_JIFFIES (6 * HZ)
static int rndis_scan(struct wiphy *wiphy, struct net_device *dev,
			struct cfg80211_scan_request *request)
{
	struct usbnet *usbdev = netdev_priv(dev);
	struct rndis_wlan_private *priv = get_rndis_wlan_priv(usbdev);
	int ret;
	__le32 tmp;

	netdev_dbg(usbdev->net, "cfg80211.scan\n");

	/* Get current bssid list from device before new scan, as new scan
	 * clears internal bssid list.
	 */
	rndis_check_bssid_list(usbdev);

	if (!request)
		return -EINVAL;

	if (priv->scan_request && priv->scan_request != request)
		return -EBUSY;

	priv->scan_request = request;

	tmp = cpu_to_le32(1);
	ret = rndis_set_oid(usbdev, OID_802_11_BSSID_LIST_SCAN, &tmp,
							sizeof(tmp));
	if (ret == 0) {
		/* Wait before retrieving scan results from device */
		queue_delayed_work(priv->workqueue, &priv->scan_work,
			SCAN_DELAY_JIFFIES);
	}

	return ret;
}

static struct cfg80211_bss *rndis_bss_info_update(struct usbnet *usbdev,
					struct ndis_80211_bssid_ex *bssid)
{
	struct rndis_wlan_private *priv = get_rndis_wlan_priv(usbdev);
	struct ieee80211_channel *channel;
	s32 signal;
	u64 timestamp;
	u16 capability;
	u16 beacon_interval;
	struct ndis_80211_fixed_ies *fixed;
	int ie_len, bssid_len;
	u8 *ie;

	netdev_dbg(usbdev->net, " found bssid: '%.32s' [%pM]\n",
		   bssid->ssid.essid, bssid->mac);

	/* parse bssid structure */
	bssid_len = le32_to_cpu(bssid->length);

	if (bssid_len < sizeof(struct ndis_80211_bssid_ex) +
			sizeof(struct ndis_80211_fixed_ies))
		return NULL;

	fixed = (struct ndis_80211_fixed_ies *)bssid->ies;

	ie = (void *)(bssid->ies + sizeof(struct ndis_80211_fixed_ies));
	ie_len = min(bssid_len - (int)sizeof(*bssid),
					(int)le32_to_cpu(bssid->ie_length));
	ie_len -= sizeof(struct ndis_80211_fixed_ies);
	if (ie_len < 0)
		return NULL;

	/* extract data for cfg80211_inform_bss */
	channel = ieee80211_get_channel(priv->wdev.wiphy,
			KHZ_TO_MHZ(le32_to_cpu(bssid->config.ds_config)));
	if (!channel)
		return NULL;

	signal = level_to_qual(le32_to_cpu(bssid->rssi));
	timestamp = le64_to_cpu(*(__le64 *)fixed->timestamp);
	capability = le16_to_cpu(fixed->capabilities);
	beacon_interval = le16_to_cpu(fixed->beacon_interval);

	return cfg80211_inform_bss(priv->wdev.wiphy, channel, bssid->mac,
		timestamp, capability, beacon_interval, ie, ie_len, signal,
		GFP_KERNEL);
}

static int rndis_check_bssid_list(struct usbnet *usbdev)
{
	void *buf = NULL;
	struct ndis_80211_bssid_list_ex *bssid_list;
	struct ndis_80211_bssid_ex *bssid;
	int ret = -EINVAL, len, count, bssid_len;
	bool resized = false;

	netdev_dbg(usbdev->net, "check_bssid_list\n");

	len = CONTROL_BUFFER_SIZE;
resize_buf:
	buf = kmalloc(len, GFP_KERNEL);
	if (!buf) {
		ret = -ENOMEM;
		goto out;
	}

	ret = rndis_query_oid(usbdev, OID_802_11_BSSID_LIST, buf, &len);
	if (ret != 0)
		goto out;

	if (!resized && len > CONTROL_BUFFER_SIZE) {
		resized = true;