+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]));
+}