From fb2490f693ee0151c0d847d1e4c575313a46b13b Mon Sep 17 00:00:00 2001 From: =?utf8?q?J=C3=A9r=C3=B4me=20Pouiller?= Date: Thu, 19 Sep 2019 14:25:47 +0000 Subject: [PATCH] staging: wfx: implement 802.11 key handling MIME-Version: 1.0 Content-Type: text/plain; charset=utf8 Content-Transfer-Encoding: 8bit wfx_set_key() mostly copy bytes on correct offsets. A big piece of code for a simple work. Unfortunately, I did not found any way to factorize it. Signed-off-by: Jérôme Pouiller Link: https://lore.kernel.org/r/20190919142527.31797-20-Jerome.Pouiller@silabs.com Signed-off-by: Greg Kroah-Hartman --- drivers/staging/wfx/Makefile | 1 + drivers/staging/wfx/key.c | 258 +++++++++++++++++++++++++++++++++++++++++++ drivers/staging/wfx/key.h | 22 ++++ drivers/staging/wfx/main.c | 2 + drivers/staging/wfx/sta.c | 4 + drivers/staging/wfx/wfx.h | 19 ++++ 6 files changed, 306 insertions(+) create mode 100644 drivers/staging/wfx/key.c create mode 100644 drivers/staging/wfx/key.h diff --git a/drivers/staging/wfx/Makefile b/drivers/staging/wfx/Makefile index 2b8a5fa86fac..0d9c1ed092f6 100644 --- a/drivers/staging/wfx/Makefile +++ b/drivers/staging/wfx/Makefile @@ -14,6 +14,7 @@ wfx-y := \ data_rx.o \ scan.o \ sta.o \ + key.o \ main.o \ sta.o \ debug.o diff --git a/drivers/staging/wfx/key.c b/drivers/staging/wfx/key.c new file mode 100644 index 000000000000..4e7d2b510a9c --- /dev/null +++ b/drivers/staging/wfx/key.c @@ -0,0 +1,258 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * Key management related functions. + * + * Copyright (c) 2017-2019, Silicon Laboratories, Inc. + * Copyright (c) 2010, ST-Ericsson + */ +#include + +#include "key.h" +#include "wfx.h" +#include "hif_tx_mib.h" + +static int wfx_alloc_key(struct wfx_dev *wdev) +{ + int idx; + + idx = ffs(~wdev->key_map) - 1; + if (idx < 0 || idx >= MAX_KEY_ENTRIES) + return -1; + + wdev->key_map |= BIT(idx); + wdev->keys[idx].entry_index = idx; + return idx; +} + +static void wfx_free_key(struct wfx_dev *wdev, int idx) +{ + BUG_ON(!(wdev->key_map & BIT(idx))); + memset(&wdev->keys[idx], 0, sizeof(wdev->keys[idx])); + wdev->key_map &= ~BIT(idx); +} + +static uint8_t fill_wep_pair(struct hif_wep_pairwise_key *msg, + struct ieee80211_key_conf *key, u8 *peer_addr) +{ + WARN_ON(key->keylen > sizeof(msg->key_data)); + msg->key_length = key->keylen; + memcpy(msg->key_data, key->key, key->keylen); + ether_addr_copy(msg->peer_address, peer_addr); + return HIF_KEY_TYPE_WEP_PAIRWISE; +} + +static uint8_t fill_wep_group(struct hif_wep_group_key *msg, + struct ieee80211_key_conf *key) +{ + WARN_ON(key->keylen > sizeof(msg->key_data)); + msg->key_id = key->keyidx; + msg->key_length = key->keylen; + memcpy(msg->key_data, key->key, key->keylen); + return HIF_KEY_TYPE_WEP_DEFAULT; +} + +static uint8_t fill_tkip_pair(struct hif_tkip_pairwise_key *msg, + struct ieee80211_key_conf *key, u8 *peer_addr) +{ + uint8_t *keybuf = key->key; + + WARN_ON(key->keylen != sizeof(msg->tkip_key_data) + + sizeof(msg->tx_mic_key) + + sizeof(msg->rx_mic_key)); + memcpy(msg->tkip_key_data, keybuf, sizeof(msg->tkip_key_data)); + keybuf += sizeof(msg->tkip_key_data); + memcpy(msg->tx_mic_key, keybuf, sizeof(msg->tx_mic_key)); + keybuf += sizeof(msg->tx_mic_key); + memcpy(msg->rx_mic_key, keybuf, sizeof(msg->rx_mic_key)); + ether_addr_copy(msg->peer_address, peer_addr); + return HIF_KEY_TYPE_TKIP_PAIRWISE; +} + +static uint8_t fill_tkip_group(struct hif_tkip_group_key *msg, + struct ieee80211_key_conf *key, + struct ieee80211_key_seq *seq, + enum nl80211_iftype iftype) +{ + uint8_t *keybuf = key->key; + + WARN_ON(key->keylen != sizeof(msg->tkip_key_data) + + 2 * sizeof(msg->rx_mic_key)); + msg->key_id = key->keyidx; + memcpy(msg->rx_sequence_counter, &seq->tkip.iv16, sizeof(seq->tkip.iv16)); + memcpy(msg->rx_sequence_counter + sizeof(uint16_t), &seq->tkip.iv32, sizeof(seq->tkip.iv32)); + memcpy(msg->tkip_key_data, keybuf, sizeof(msg->tkip_key_data)); + keybuf += sizeof(msg->tkip_key_data); + if (iftype == NL80211_IFTYPE_AP) + // Use Tx MIC Key + memcpy(msg->rx_mic_key, keybuf + 0, sizeof(msg->rx_mic_key)); + else + // Use Rx MIC Key + memcpy(msg->rx_mic_key, keybuf + 8, sizeof(msg->rx_mic_key)); + return HIF_KEY_TYPE_TKIP_GROUP; +} + +static uint8_t fill_ccmp_pair(struct hif_aes_pairwise_key *msg, + struct ieee80211_key_conf *key, u8 *peer_addr) +{ + WARN_ON(key->keylen != sizeof(msg->aes_key_data)); + ether_addr_copy(msg->peer_address, peer_addr); + memcpy(msg->aes_key_data, key->key, key->keylen); + return HIF_KEY_TYPE_AES_PAIRWISE; +} + +static uint8_t fill_ccmp_group(struct hif_aes_group_key *msg, + struct ieee80211_key_conf *key, + struct ieee80211_key_seq *seq) +{ + WARN_ON(key->keylen != sizeof(msg->aes_key_data)); + memcpy(msg->aes_key_data, key->key, key->keylen); + memcpy(msg->rx_sequence_counter, seq->ccmp.pn, sizeof(seq->ccmp.pn)); + memreverse(msg->rx_sequence_counter, sizeof(seq->ccmp.pn)); + msg->key_id = key->keyidx; + return HIF_KEY_TYPE_AES_GROUP; +} + +static uint8_t fill_sms4_pair(struct hif_wapi_pairwise_key *msg, + struct ieee80211_key_conf *key, u8 *peer_addr) +{ + uint8_t *keybuf = key->key; + + WARN_ON(key->keylen != sizeof(msg->wapi_key_data) + + sizeof(msg->mic_key_data)); + ether_addr_copy(msg->peer_address, peer_addr); + memcpy(msg->wapi_key_data, keybuf, sizeof(msg->wapi_key_data)); + keybuf += sizeof(msg->wapi_key_data); + memcpy(msg->mic_key_data, keybuf, sizeof(msg->mic_key_data)); + msg->key_id = key->keyidx; + return HIF_KEY_TYPE_WAPI_PAIRWISE; +} + +static uint8_t fill_sms4_group(struct hif_wapi_group_key *msg, + struct ieee80211_key_conf *key) +{ + uint8_t *keybuf = key->key; + + WARN_ON(key->keylen != sizeof(msg->wapi_key_data) + + sizeof(msg->mic_key_data)); + memcpy(msg->wapi_key_data, keybuf, sizeof(msg->wapi_key_data)); + keybuf += sizeof(msg->wapi_key_data); + memcpy(msg->mic_key_data, keybuf, sizeof(msg->mic_key_data)); + msg->key_id = key->keyidx; + return HIF_KEY_TYPE_WAPI_GROUP; +} + +static uint8_t fill_aes_cmac_group(struct hif_igtk_group_key *msg, + struct ieee80211_key_conf *key, + struct ieee80211_key_seq *seq) +{ + WARN_ON(key->keylen != sizeof(msg->igtk_key_data)); + memcpy(msg->igtk_key_data, key->key, key->keylen); + memcpy(msg->ipn, seq->aes_cmac.pn, sizeof(seq->aes_cmac.pn)); + memreverse(msg->ipn, sizeof(seq->aes_cmac.pn)); + msg->key_id = key->keyidx; + return HIF_KEY_TYPE_IGTK_GROUP; +} + +static int wfx_add_key(struct wfx_vif *wvif, struct ieee80211_sta *sta, + struct ieee80211_key_conf *key) +{ + int ret; + struct hif_req_add_key *k; + struct ieee80211_key_seq seq; + struct wfx_dev *wdev = wvif->wdev; + int idx = wfx_alloc_key(wvif->wdev); + bool pairwise = key->flags & IEEE80211_KEY_FLAG_PAIRWISE; + + WARN_ON(key->flags & IEEE80211_KEY_FLAG_PAIRWISE && !sta); + ieee80211_get_key_rx_seq(key, 0, &seq); + if (idx < 0) + return -EINVAL; + k = &wdev->keys[idx]; + k->int_id = wvif->id; + if (key->cipher == WLAN_CIPHER_SUITE_WEP40 || key->cipher == WLAN_CIPHER_SUITE_WEP104) { + if (pairwise) + k->type = fill_wep_pair(&k->key.wep_pairwise_key, key, sta->addr); + else + k->type = fill_wep_group(&k->key.wep_group_key, key); + } else if (key->cipher == WLAN_CIPHER_SUITE_TKIP) { + if (pairwise) + k->type = fill_tkip_pair(&k->key.tkip_pairwise_key, key, sta->addr); + else + k->type = fill_tkip_group(&k->key.tkip_group_key, key, &seq, wvif->vif->type); + } else if (key->cipher == WLAN_CIPHER_SUITE_CCMP) { + if (pairwise) + k->type = fill_ccmp_pair(&k->key.aes_pairwise_key, key, sta->addr); + else + k->type = fill_ccmp_group(&k->key.aes_group_key, key, &seq); + } else if (key->cipher == WLAN_CIPHER_SUITE_SMS4) { + if (pairwise) + k->type = fill_sms4_pair(&k->key.wapi_pairwise_key, key, sta->addr); + else + k->type = fill_sms4_group(&k->key.wapi_group_key, key); + } else if (key->cipher == WLAN_CIPHER_SUITE_AES_CMAC) { + k->type = fill_aes_cmac_group(&k->key.igtk_group_key, key, &seq); + } else { + dev_warn(wdev->dev, "unsupported key type %d\n", key->cipher); + wfx_free_key(wdev, idx); + return -EOPNOTSUPP; + } + ret = hif_add_key(wdev, k); + if (ret) { + wfx_free_key(wdev, idx); + return -EOPNOTSUPP; + } + key->flags |= IEEE80211_KEY_FLAG_PUT_IV_SPACE | + IEEE80211_KEY_FLAG_RESERVE_TAILROOM; + key->hw_key_idx = idx; + return 0; +} + +static int wfx_remove_key(struct wfx_vif *wvif, struct ieee80211_key_conf *key) +{ + WARN(key->hw_key_idx >= MAX_KEY_ENTRIES, "corrupted hw_key_idx"); + wfx_free_key(wvif->wdev, key->hw_key_idx); + return hif_remove_key(wvif->wdev, key->hw_key_idx); +} + +int wfx_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd, + struct ieee80211_vif *vif, struct ieee80211_sta *sta, + struct ieee80211_key_conf *key) +{ + int ret = -EOPNOTSUPP; + struct wfx_vif *wvif = (struct wfx_vif *) vif->drv_priv; + + mutex_lock(&wvif->wdev->conf_mutex); + if (cmd == SET_KEY) + ret = wfx_add_key(wvif, sta, key); + if (cmd == DISABLE_KEY) + ret = wfx_remove_key(wvif, key); + mutex_unlock(&wvif->wdev->conf_mutex); + return ret; +} + +int wfx_upload_keys(struct wfx_vif *wvif) +{ + int i; + struct hif_req_add_key *key; + struct wfx_dev *wdev = wvif->wdev; + + for (i = 0; i < ARRAY_SIZE(wdev->keys); i++) { + if (wdev->key_map & BIT(i)) { + key = &wdev->keys[i]; + if (key->int_id == wvif->id) + hif_add_key(wdev, key); + } + } + return 0; +} + +void wfx_wep_key_work(struct work_struct *work) +{ + struct wfx_vif *wvif = container_of(work, struct wfx_vif, wep_key_work); + + wfx_tx_flush(wvif->wdev); + hif_wep_default_key_id(wvif, wvif->wep_default_key_id); + wfx_pending_requeue(wvif->wdev, wvif->wep_pending_skb); + wvif->wep_pending_skb = NULL; + wfx_tx_unlock(wvif->wdev); +} diff --git a/drivers/staging/wfx/key.h b/drivers/staging/wfx/key.h new file mode 100644 index 000000000000..9436ccdf4d3b --- /dev/null +++ b/drivers/staging/wfx/key.h @@ -0,0 +1,22 @@ +/* SPDX-License-Identifier: GPL-2.0-only */ +/* + * Implementation of mac80211 API. + * + * Copyright (c) 2017-2019, Silicon Laboratories, Inc. + * Copyright (c) 2010, ST-Ericsson + */ +#ifndef WFX_KEY_H +#define WFX_KEY_H + +#include + +struct wfx_dev; +struct wfx_vif; + +int wfx_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd, + struct ieee80211_vif *vif, struct ieee80211_sta *sta, + struct ieee80211_key_conf *key); +int wfx_upload_keys(struct wfx_vif *wvif); +void wfx_wep_key_work(struct work_struct *work); + +#endif /* WFX_STA_H */ diff --git a/drivers/staging/wfx/main.c b/drivers/staging/wfx/main.c index 06220bac5b75..e7bba24aae0b 100644 --- a/drivers/staging/wfx/main.c +++ b/drivers/staging/wfx/main.c @@ -27,6 +27,7 @@ #include "bus.h" #include "bh.h" #include "sta.h" +#include "key.h" #include "debug.h" #include "data_tx.h" #include "secure_link.h" @@ -56,6 +57,7 @@ static const struct ieee80211_ops wfx_ops = { .remove_interface = wfx_remove_interface, .tx = wfx_tx, .hw_scan = wfx_hw_scan, + .set_key = wfx_set_key, }; bool wfx_api_older_than(struct wfx_dev *wdev, int major, int minor) diff --git a/drivers/staging/wfx/sta.c b/drivers/staging/wfx/sta.c index 5a8140100e97..2e709b8a3bf4 100644 --- a/drivers/staging/wfx/sta.c +++ b/drivers/staging/wfx/sta.c @@ -9,6 +9,7 @@ #include "sta.h" #include "wfx.h" +#include "key.h" #include "scan.h" #include "hif_tx_mib.h" @@ -162,6 +163,9 @@ int wfx_add_interface(struct ieee80211_hw *hw, struct ieee80211_vif *vif) INIT_WORK(&wvif->mcast_stop_work, wfx_mcast_stop_work); timer_setup(&wvif->mcast_timeout, wfx_mcast_timeout, 0); + wvif->wep_default_key_id = -1; + INIT_WORK(&wvif->wep_key_work, wfx_wep_key_work); + sema_init(&wvif->scan.lock, 1); INIT_WORK(&wvif->scan.work, wfx_scan_work); INIT_DELAYED_WORK(&wvif->scan.timeout, wfx_scan_timeout); diff --git a/drivers/staging/wfx/wfx.h b/drivers/staging/wfx/wfx.h index 50c0d9c0e528..a86ddfaed825 100644 --- a/drivers/staging/wfx/wfx.h +++ b/drivers/staging/wfx/wfx.h @@ -48,6 +48,9 @@ struct wfx_dev { int tx_burst_idx; atomic_t tx_lock; + u32 key_map; + struct hif_req_add_key keys[MAX_KEY_ENTRIES]; + struct hif_rx_stats rx_stats; struct mutex rx_stats_lock; @@ -73,6 +76,9 @@ struct wfx_vif { struct work_struct mcast_start_work; struct work_struct mcast_stop_work; + s8 wep_default_key_id; + struct sk_buff *wep_pending_skb; + struct work_struct wep_key_work; struct tx_policy_cache tx_policy_cache; struct work_struct tx_policy_upload_work; @@ -120,6 +126,19 @@ static inline struct wfx_vif *wvif_iterate(struct wfx_dev *wdev, struct wfx_vif return NULL; } +static inline void memreverse(uint8_t *src, uint8_t length) +{ + uint8_t *lo = src; + uint8_t *hi = src + length - 1; + uint8_t swap; + + while (lo < hi) { + swap = *lo; + *lo++ = *hi; + *hi-- = swap; + } +} + static inline int memzcmp(void *src, unsigned int size) { uint8_t *buf = src; -- 2.11.0