1 files changed, 190 insertions, 86 deletions

diff --git a/drivers/net/wireless/quantenna/qtnfmac/commands.c b/drivers/net/wireless/quantenna/qtnfmac/commands.c
index b81f81bd1411..8bc8dd637315 100644
--- a/drivers/net/wireless/quantenna/qtnfmac/commands.c
+++ b/drivers/net/wireless/quantenna/qtnfmac/commands.c
@@ -147,56 +147,66 @@ static struct sk_buff *qtnf_cmd_alloc_new_cmdskb(u8 macid, u8 vifid, u16 cmd_no,
         return cmd_skb;
 }
 
-int qtnf_cmd_send_start_ap(struct qtnf_vif *vif)
+static void qtnf_cmd_tlv_ie_set_add(struct sk_buff *cmd_skb, u8 frame_type,
+                                    const u8 *buf, size_t len)
 {
-        struct sk_buff *cmd_skb;
-        u16 res_code = QLINK_CMD_RESULT_OK;
-        int ret;
+        struct qlink_tlv_ie_set *tlv;
 
-        cmd_skb = qtnf_cmd_alloc_new_cmdskb(vif->mac->macid, vif->vifid,
-                                            QLINK_CMD_START_AP,
-                                            sizeof(struct qlink_cmd));
-        if (unlikely(!cmd_skb))
-                return -ENOMEM;
+        tlv = (struct qlink_tlv_ie_set *)skb_put(cmd_skb, sizeof(*tlv) + len);
+        tlv->hdr.type = cpu_to_le16(QTN_TLV_ID_IE_SET);
+        tlv->hdr.len = cpu_to_le16(len + sizeof(*tlv) - sizeof(tlv->hdr));
+        tlv->type = frame_type;
+        tlv->flags = 0;
 
-        qtnf_bus_lock(vif->mac->bus);
+        if (len && buf)
+                memcpy(tlv->ie_data, buf, len);
+}
 
-        ret = qtnf_cmd_send(vif->mac->bus, cmd_skb, &res_code);
+static bool qtnf_cmd_start_ap_can_fit(const struct qtnf_vif *vif,
+                                      const struct cfg80211_ap_settings *s)
+{
+        unsigned int len = sizeof(struct qlink_cmd_start_ap);
 
-        if (unlikely(ret))
-                goto out;
+        len += s->ssid_len;
+        len += s->beacon.head_len;
+        len += s->beacon.tail_len;
+        len += s->beacon.beacon_ies_len;
+        len += s->beacon.proberesp_ies_len;
+        len += s->beacon.assocresp_ies_len;
+        len += s->beacon.probe_resp_len;
 
-        if (unlikely(res_code != QLINK_CMD_RESULT_OK)) {
-                pr_err("VIF%u.%u: CMD failed: %u\n", vif->mac->macid,
-                       vif->vifid, res_code);
-                ret = -EFAULT;
-                goto out;
-        }
+        if (cfg80211_chandef_valid(&s->chandef))
+                len += sizeof(struct qlink_tlv_chandef);
 
-        netif_carrier_on(vif->netdev);
+        if (len > (sizeof(struct qlink_cmd) + QTNF_MAX_CMD_BUF_SIZE)) {
+                pr_err("VIF%u.%u: can not fit AP settings: %u\n",
+                       vif->mac->macid, vif->vifid, len);
+                return false;
+        }
 
-out:
-        qtnf_bus_unlock(vif->mac->bus);
-        return ret;
+        return true;
 }
 
-int qtnf_cmd_send_config_ap(struct qtnf_vif *vif,
-                            const struct cfg80211_ap_settings *s)
+int qtnf_cmd_send_start_ap(struct qtnf_vif *vif,
+                           const struct cfg80211_ap_settings *s)
 {
         struct sk_buff *cmd_skb;
-        struct qlink_cmd_config_ap *cmd;
+        struct qlink_cmd_start_ap *cmd;
         struct qlink_auth_encr *aen;
         u16 res_code = QLINK_CMD_RESULT_OK;
         int ret;
         int i;
 
+        if (!qtnf_cmd_start_ap_can_fit(vif, s))
+                return -E2BIG;
+
         cmd_skb = qtnf_cmd_alloc_new_cmdskb(vif->mac->macid, vif->vifid,
-                                            QLINK_CMD_CONFIG_AP,
+                                            QLINK_CMD_START_AP,
                                             sizeof(*cmd));
         if (unlikely(!cmd_skb))
                 return -ENOMEM;
 
-        cmd = (struct qlink_cmd_config_ap *)cmd_skb->data;
+        cmd = (struct qlink_cmd_start_ap *)cmd_skb->data;
         cmd->dtim_period = s->dtim_period;
         cmd->beacon_interval = cpu_to_le16(s->beacon_interval);
         cmd->hidden_ssid = qlink_hidden_ssid_nl2q(s->hidden_ssid);
@@ -211,7 +221,6 @@ int qtnf_cmd_send_config_ap(struct qtnf_vif *vif,
         aen = &cmd->aen;
         aen->auth_type = s->auth_type;
         aen->privacy = !!s->privacy;
-        aen->mfp = 0;
         aen->wpa_versions = cpu_to_le32(s->crypto.wpa_versions);
         aen->cipher_group = cpu_to_le32(s->crypto.cipher_group);
         aen->n_ciphers_pairwise = cpu_to_le32(s->crypto.n_ciphers_pairwise);
@@ -241,6 +250,39 @@ int qtnf_cmd_send_config_ap(struct qtnf_vif *vif,
                 qlink_chandef_cfg2q(&s->chandef, &chtlv->chan);
         }
 
+        qtnf_cmd_tlv_ie_set_add(cmd_skb, QLINK_IE_SET_BEACON_HEAD,
+                                s->beacon.head, s->beacon.head_len);
+        qtnf_cmd_tlv_ie_set_add(cmd_skb, QLINK_IE_SET_BEACON_TAIL,
+                                s->beacon.tail, s->beacon.tail_len);
+        qtnf_cmd_tlv_ie_set_add(cmd_skb, QLINK_IE_SET_BEACON_IES,
+                                s->beacon.beacon_ies, s->beacon.beacon_ies_len);
+        qtnf_cmd_tlv_ie_set_add(cmd_skb, QLINK_IE_SET_PROBE_RESP,
+                                s->beacon.probe_resp, s->beacon.probe_resp_len);
+        qtnf_cmd_tlv_ie_set_add(cmd_skb, QLINK_IE_SET_PROBE_RESP_IES,
+                                s->beacon.proberesp_ies,
+                                s->beacon.proberesp_ies_len);
+        qtnf_cmd_tlv_ie_set_add(cmd_skb, QLINK_IE_SET_ASSOC_RESP,
+                                s->beacon.assocresp_ies,
+                                s->beacon.assocresp_ies_len);
+
+        if (s->ht_cap) {
+                struct qlink_tlv_hdr *tlv = (struct qlink_tlv_hdr *)
+                        skb_put(cmd_skb, sizeof(*tlv) + sizeof(*s->ht_cap));
+
+                tlv->type = cpu_to_le16(WLAN_EID_HT_CAPABILITY);
+                tlv->len = cpu_to_le16(sizeof(*s->ht_cap));
+                memcpy(tlv->val, s->ht_cap, sizeof(*s->ht_cap));
+        }
+
+        if (s->vht_cap) {
+                struct qlink_tlv_hdr *tlv = (struct qlink_tlv_hdr *)
+                        skb_put(cmd_skb, sizeof(*tlv) + sizeof(*s->vht_cap));
+
+                tlv->type = cpu_to_le16(WLAN_EID_VHT_CAPABILITY);
+                tlv->len = cpu_to_le16(sizeof(*s->vht_cap));
+                memcpy(tlv->val, s->vht_cap, sizeof(*s->vht_cap));
+        }
+
         qtnf_bus_lock(vif->mac->bus);
 
         ret = qtnf_cmd_send(vif->mac->bus, cmd_skb, &res_code);
@@ -255,6 +297,8 @@ int qtnf_cmd_send_config_ap(struct qtnf_vif *vif,
                 goto out;
         }
 
+        netif_carrier_on(vif->netdev);
+
 out:
         qtnf_bus_unlock(vif->mac->bus);
         return ret;
@@ -380,11 +424,10 @@ int qtnf_cmd_send_mgmt_set_appie(struct qtnf_vif *vif, u8 frame_type,
                                  const u8 *buf, size_t len)
 {
         struct sk_buff *cmd_skb;
-        struct qlink_cmd_mgmt_append_ie *cmd;
         u16 res_code = QLINK_CMD_RESULT_OK;
         int ret;
 
-        if (sizeof(*cmd) + len > QTNF_MAX_CMD_BUF_SIZE) {
+        if (len > QTNF_MAX_CMD_BUF_SIZE) {
                 pr_warn("VIF%u.%u: %u frame is too big: %zu\n", vif->mac->macid,
                         vif->vifid, frame_type, len);
                 return -E2BIG;
@@ -392,21 +435,13 @@ int qtnf_cmd_send_mgmt_set_appie(struct qtnf_vif *vif, u8 frame_type,
 
         cmd_skb = qtnf_cmd_alloc_new_cmdskb(vif->mac->macid, vif->vifid,
                                             QLINK_CMD_MGMT_SET_APPIE,
-                                            sizeof(*cmd));
+                                            sizeof(struct qlink_cmd));
         if (unlikely(!cmd_skb))
                 return -ENOMEM;
 
-        qtnf_bus_lock(vif->mac->bus);
-
-        cmd = (struct qlink_cmd_mgmt_append_ie *)cmd_skb->data;
-        cmd->type = frame_type;
-        cmd->flags = 0;
+        qtnf_cmd_tlv_ie_set_add(cmd_skb, frame_type, buf, len);
 
-        /* If len == 0 then IE buf for specified frame type
-         * should be cleared on EP.
-         */
-        if (len && buf)
-                qtnf_cmd_skb_put_buffer(cmd_skb, buf, len);
+        qtnf_bus_lock(vif->mac->bus);
 
         ret = qtnf_cmd_send(vif->mac->bus, cmd_skb, &res_code);
 
@@ -1090,7 +1125,6 @@ qtnf_cmd_resp_proc_mac_info(struct qtnf_wmac *mac,
         mac_info = &mac->macinfo;
 
         mac_info->bands_cap = resp_info->bands_cap;
-        mac_info->phymode_cap = resp_info->phymode_cap;
         memcpy(&mac_info->dev_mac, &resp_info->dev_mac,
                sizeof(mac_info->dev_mac));
 
@@ -1110,24 +1144,56 @@ qtnf_cmd_resp_proc_mac_info(struct qtnf_wmac *mac,
                         qlink_chan_width_mask_to_nl(le16_to_cpu(
                                         resp_info->radar_detect_widths));
 
-        memcpy(&mac_info->ht_cap, &resp_info->ht_cap, sizeof(mac_info->ht_cap));
-        memcpy(&mac_info->vht_cap, &resp_info->vht_cap,
-               sizeof(mac_info->vht_cap));
+        memcpy(&mac_info->ht_cap_mod_mask, &resp_info->ht_cap_mod_mask,
+               sizeof(mac_info->ht_cap_mod_mask));
+        memcpy(&mac_info->vht_cap_mod_mask, &resp_info->vht_cap_mod_mask,
+               sizeof(mac_info->vht_cap_mod_mask));
+}
+
+static void qtnf_cmd_resp_band_fill_htcap(const u8 *info,
+                                          struct ieee80211_sta_ht_cap *bcap)
+{
+        const struct ieee80211_ht_cap *ht_cap =
+                (const struct ieee80211_ht_cap *)info;
+
+        bcap->ht_supported = true;
+        bcap->cap = le16_to_cpu(ht_cap->cap_info);
+        bcap->ampdu_factor =
+                ht_cap->ampdu_params_info & IEEE80211_HT_AMPDU_PARM_FACTOR;
+        bcap->ampdu_density =
+                (ht_cap->ampdu_params_info & IEEE80211_HT_AMPDU_PARM_DENSITY) >>
+                IEEE80211_HT_AMPDU_PARM_DENSITY_SHIFT;
+        memcpy(&bcap->mcs, &ht_cap->mcs, sizeof(bcap->mcs));
+}
+
+static void qtnf_cmd_resp_band_fill_vhtcap(const u8 *info,
+                                           struct ieee80211_sta_vht_cap *bcap)
+{
+        const struct ieee80211_vht_cap *vht_cap =
+                (const struct ieee80211_vht_cap *)info;
+
+        bcap->vht_supported = true;
+        bcap->cap = le32_to_cpu(vht_cap->vht_cap_info);
+        memcpy(&bcap->vht_mcs, &vht_cap->supp_mcs, sizeof(bcap->vht_mcs));
 }
 
 static int
-qtnf_cmd_resp_fill_channels_info(struct ieee80211_supported_band *band,
-                                 struct qlink_resp_get_chan_info *resp,
-                                 size_t payload_len)
+qtnf_cmd_resp_fill_band_info(struct ieee80211_supported_band *band,
+                             struct qlink_resp_band_info_get *resp,
+                             size_t payload_len)
 {
         u16 tlv_type;
         size_t tlv_len;
+        size_t tlv_dlen;
         const struct qlink_tlv_hdr *tlv;
         const struct qlink_tlv_channel *qchan;
         struct ieee80211_channel *chan;
         unsigned int chidx = 0;
         u32 qflags;
 
+        memset(&band->ht_cap, 0, sizeof(band->ht_cap));
+        memset(&band->vht_cap, 0, sizeof(band->vht_cap));
+
         if (band->channels) {
                 if (band->n_channels == resp->num_chans) {
                         memset(band->channels, 0,
@@ -1155,7 +1221,8 @@ qtnf_cmd_resp_fill_channels_info(struct ieee80211_supported_band *band,
 
         while (payload_len >= sizeof(*tlv)) {
                 tlv_type = le16_to_cpu(tlv->type);
-                tlv_len = le16_to_cpu(tlv->len) + sizeof(*tlv);
+                tlv_dlen = le16_to_cpu(tlv->len);
+                tlv_len = tlv_dlen + sizeof(*tlv);
 
                 if (tlv_len > payload_len) {
                         pr_warn("malformed TLV 0x%.2X; LEN: %zu\n",
@@ -1241,13 +1308,32 @@ qtnf_cmd_resp_fill_channels_info(struct ieee80211_supported_band *band,
                                  chan->hw_value, chan->flags, chan->max_power,
                                  chan->max_reg_power);
                         break;
+                case WLAN_EID_HT_CAPABILITY:
+                        if (unlikely(tlv_dlen !=
+                                     sizeof(struct ieee80211_ht_cap))) {
+                                pr_err("bad HTCAP TLV len %zu\n", tlv_dlen);
+                                goto error_ret;
+                        }
+
+                        qtnf_cmd_resp_band_fill_htcap(tlv->val, &band->ht_cap);
+                        break;
+                case WLAN_EID_VHT_CAPABILITY:
+                        if (unlikely(tlv_dlen !=
+                                     sizeof(struct ieee80211_vht_cap))) {
+                                pr_err("bad VHTCAP TLV len %zu\n", tlv_dlen);
+                                goto error_ret;
+                        }
+
+                        qtnf_cmd_resp_band_fill_vhtcap(tlv->val,
+                                                       &band->vht_cap);
+                        break;
                 default:
                         pr_warn("unknown TLV type: %#x\n", tlv_type);
                         break;
                 }
 
                 payload_len -= tlv_len;
-                tlv = (struct qlink_tlv_hdr *)((u8 *)tlv + tlv_len);
+                tlv = (struct qlink_tlv_hdr *)(tlv->val + tlv_dlen);
         }
 
         if (payload_len) {
@@ -1469,13 +1555,13 @@ out:
         return ret;
 }
 
-int qtnf_cmd_get_mac_chan_info(struct qtnf_wmac *mac,
-                               struct ieee80211_supported_band *band)
+int qtnf_cmd_band_info_get(struct qtnf_wmac *mac,
+                           struct ieee80211_supported_band *band)
 {
         struct sk_buff *cmd_skb, *resp_skb = NULL;
         size_t info_len;
-        struct qlink_cmd_chans_info_get *cmd;
-        struct qlink_resp_get_chan_info *resp;
+        struct qlink_cmd_band_info_get *cmd;
+        struct qlink_resp_band_info_get *resp;
         u16 res_code = QLINK_CMD_RESULT_OK;
         int ret = 0;
         u8 qband;
@@ -1495,12 +1581,12 @@ int qtnf_cmd_get_mac_chan_info(struct qtnf_wmac *mac,
         }
 
         cmd_skb = qtnf_cmd_alloc_new_cmdskb(mac->macid, 0,
-                                            QLINK_CMD_CHANS_INFO_GET,
+                                            QLINK_CMD_BAND_INFO_GET,
                                             sizeof(*cmd));
         if (!cmd_skb)
                 return -ENOMEM;
 
-        cmd = (struct qlink_cmd_chans_info_get *)cmd_skb->data;
+        cmd = (struct qlink_cmd_band_info_get *)cmd_skb->data;
         cmd->band = qband;
 
         qtnf_bus_lock(mac->bus);
@@ -1517,7 +1603,7 @@ int qtnf_cmd_get_mac_chan_info(struct qtnf_wmac *mac,
                 goto out;
         }
 
-        resp = (struct qlink_resp_get_chan_info *)resp_skb->data;
+        resp = (struct qlink_resp_band_info_get *)resp_skb->data;
         if (resp->band != qband) {
                 pr_err("MAC%u: reply band %u != cmd band %u\n", mac->macid,
                        resp->band, qband);
@@ -1525,7 +1611,7 @@ int qtnf_cmd_get_mac_chan_info(struct qtnf_wmac *mac,
                 goto out;
         }
 
-        ret = qtnf_cmd_resp_fill_channels_info(band, resp, info_len);
+        ret = qtnf_cmd_resp_fill_band_info(band, resp, info_len);
 
 out:
         qtnf_bus_unlock(mac->bus);
@@ -1942,17 +2028,36 @@ out:
         return ret;
 }
 
+static void qtnf_cmd_channel_tlv_add(struct sk_buff *cmd_skb,
+                                     const struct ieee80211_channel *sc)
+{
+        struct qlink_tlv_channel *qchan;
+        u32 flags = 0;
+
+        qchan = skb_put_zero(cmd_skb, sizeof(*qchan));
+        qchan->hdr.type = cpu_to_le16(QTN_TLV_ID_CHANNEL);
+        qchan->hdr.len = cpu_to_le16(sizeof(*qchan) - sizeof(qchan->hdr));
+        qchan->center_freq = cpu_to_le16(sc->center_freq);
+        qchan->hw_value = cpu_to_le16(sc->hw_value);
+
+        if (sc->flags & IEEE80211_CHAN_NO_IR)
+                flags |= QLINK_CHAN_NO_IR;
+
+        if (sc->flags & IEEE80211_CHAN_RADAR)
+                flags |= QLINK_CHAN_RADAR;
+
+        qchan->flags = cpu_to_le32(flags);
+}
+
 int qtnf_cmd_send_scan(struct qtnf_wmac *mac)
 {
         struct sk_buff *cmd_skb;
         u16 res_code = QLINK_CMD_RESULT_OK;
         struct ieee80211_channel *sc;
         struct cfg80211_scan_request *scan_req = mac->scan_req;
-        struct qlink_tlv_channel *qchan;
         int n_channels;
         int count = 0;
         int ret;
-        u32 flags;
 
         if (scan_req->n_ssids > QTNF_MAX_SSID_LIST_LENGTH) {
                 pr_err("MAC%u: too many SSIDs in scan request\n", mac->macid);
@@ -1977,9 +2082,8 @@ int qtnf_cmd_send_scan(struct qtnf_wmac *mac)
         }
 
         if (scan_req->ie_len != 0)
-                qtnf_cmd_skb_put_tlv_arr(cmd_skb, QTN_TLV_ID_IE_SET,
-                                         scan_req->ie,
-                                         scan_req->ie_len);
+                qtnf_cmd_tlv_ie_set_add(cmd_skb, QLINK_IE_SET_PROBE_REQ,
+                                        scan_req->ie, scan_req->ie_len);
 
         if (scan_req->n_channels) {
                 n_channels = scan_req->n_channels;
@@ -1995,22 +2099,8 @@ int qtnf_cmd_send_scan(struct qtnf_wmac *mac)
                         pr_debug("MAC%u: scan chan=%d, freq=%d, flags=%#x\n",
                                  mac->macid, sc->hw_value, sc->center_freq,
                                  sc->flags);
-                        qchan = skb_put_zero(cmd_skb, sizeof(*qchan));
-                        flags = 0;
-
-                        qchan->hdr.type = cpu_to_le16(QTN_TLV_ID_CHANNEL);
-                        qchan->hdr.len = cpu_to_le16(sizeof(*qchan) -
-                                        sizeof(struct qlink_tlv_hdr));
-                        qchan->center_freq = cpu_to_le16(sc->center_freq);
-                        qchan->hw_value = cpu_to_le16(sc->hw_value);
-
-                        if (sc->flags & IEEE80211_CHAN_NO_IR)
-                                flags |= QLINK_CHAN_NO_IR;
-
-                        if (sc->flags & IEEE80211_CHAN_RADAR)
-                                flags |= QLINK_CHAN_RADAR;
 
-                        qchan->flags = cpu_to_le32(flags);
+                        qtnf_cmd_channel_tlv_add(cmd_skb, sc);
                         n_channels--;
                         count++;
                 }
@@ -2054,10 +2144,15 @@ int qtnf_cmd_send_connect(struct qtnf_vif *vif,
 
         ether_addr_copy(cmd->bssid, vif->bssid);
 
-        if (sme->channel)
-                cmd->channel = cpu_to_le16(sme->channel->hw_value);
+        if (sme->bssid_hint)
+                ether_addr_copy(cmd->bssid_hint, sme->bssid_hint);
+        else
+                eth_zero_addr(cmd->bssid_hint);
+
+        if (sme->prev_bssid)
+                ether_addr_copy(cmd->prev_bssid, sme->prev_bssid);
         else
-                cmd->channel = 0;
+                eth_zero_addr(cmd->prev_bssid);
 
         if ((sme->bg_scan_period > 0) &&
             (sme->bg_scan_period <= QTNF_MAX_BG_SCAN_PERIOD))
@@ -2075,11 +2170,18 @@ int qtnf_cmd_send_connect(struct qtnf_vif *vif,
                 connect_flags |= QLINK_STA_CONNECT_USE_RRM;
 
         cmd->flags = cpu_to_le32(connect_flags);
+        memcpy(&cmd->ht_capa, &sme->ht_capa, sizeof(cmd->ht_capa));
+        memcpy(&cmd->ht_capa_mask, &sme->ht_capa_mask,
+               sizeof(cmd->ht_capa_mask));
+        memcpy(&cmd->vht_capa, &sme->vht_capa, sizeof(cmd->vht_capa));
+        memcpy(&cmd->vht_capa_mask, &sme->vht_capa_mask,
+               sizeof(cmd->vht_capa_mask));
+        cmd->pbss = sme->pbss;
 
         aen = &cmd->aen;
         aen->auth_type = sme->auth_type;
         aen->privacy = !!sme->privacy;
-        aen->mfp = sme->mfp;
+        cmd->mfp = sme->mfp;
         aen->wpa_versions = cpu_to_le32(sme->crypto.wpa_versions);
         aen->cipher_group = cpu_to_le32(sme->crypto.cipher_group);
         aen->n_ciphers_pairwise = cpu_to_le32(sme->crypto.n_ciphers_pairwise);
@@ -2103,9 +2205,11 @@ int qtnf_cmd_send_connect(struct qtnf_vif *vif,
                                  sme->ssid_len);
 
         if (sme->ie_len != 0)
-                qtnf_cmd_skb_put_tlv_arr(cmd_skb, QTN_TLV_ID_IE_SET,
-                                         sme->ie,
-                                         sme->ie_len);
+                qtnf_cmd_tlv_ie_set_add(cmd_skb, QLINK_IE_SET_ASSOC_REQ,
+                                        sme->ie, sme->ie_len);
+
+        if (sme->channel)
+                qtnf_cmd_channel_tlv_add(cmd_skb, sme->channel);
 
         qtnf_bus_lock(vif->mac->bus);