drivers/net/wireless/quantenna/qtnfmac/commands.c

   1 /*
   2  * Copyright (c) 2015-2016 Quantenna Communications, Inc.
   3  *
   4  * This program is free software; you can redistribute it and/or
   5  * modify it under the terms of the GNU General Public License
   6  * as published by the Free Software Foundation; either version 2
   7  * of the License, or (at your option) any later version.
   8  *
   9  * This program is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  */
  15
  16 #include <linux/types.h>
  17 #include <linux/skbuff.h>
  18
  19 #include "cfg80211.h"
  20 #include "core.h"
  21 #include "qlink.h"
  22 #include "qlink_util.h"
  23 #include "bus.h"
  24 #include "commands.h"
  25
  26 static int qtnf_cmd_check_reply_header(const struct qlink_resp *resp,
  27                                        u16 cmd_id, u8 mac_id, u8 vif_id,
  28                                        size_t resp_size)
  29 {
  30         if (unlikely(le16_to_cpu(resp->cmd_id) != cmd_id)) {
  31                 pr_warn("VIF%u.%u CMD%x: bad cmd_id in response: 0x%.4X\n",
  32                         mac_id, vif_id, cmd_id, le16_to_cpu(resp->cmd_id));
  33                 return -EINVAL;
  34         }
  35
  36         if (unlikely(resp->macid != mac_id)) {
  37                 pr_warn("VIF%u.%u CMD%x: bad MAC in response: %u\n",
  38                         mac_id, vif_id, cmd_id, resp->macid);
  39                 return -EINVAL;
  40         }
  41
  42         if (unlikely(resp->vifid != vif_id)) {
  43                 pr_warn("VIF%u.%u CMD%x: bad VIF in response: %u\n",
  44                         mac_id, vif_id, cmd_id, resp->vifid);
  45                 return -EINVAL;
  46         }
  47
  48         if (unlikely(le16_to_cpu(resp->mhdr.len) < resp_size)) {
  49                 pr_warn("VIF%u.%u CMD%x: bad response size %u < %zu\n",
  50                         mac_id, vif_id, cmd_id,
  51                         le16_to_cpu(resp->mhdr.len), resp_size);
  52                 return -ENOSPC;
  53         }
  54
  55         return 0;
  56 }
  57
  58 static int qtnf_cmd_resp_result_decode(enum qlink_cmd_result qcode)
  59 {
  60         switch (qcode) {
  61         case QLINK_CMD_RESULT_OK:
  62                 return 0;
  63         case QLINK_CMD_RESULT_INVALID:
  64                 return -EINVAL;
  65         case QLINK_CMD_RESULT_ENOTSUPP:
  66                 return -ENOTSUPP;
  67         case QLINK_CMD_RESULT_ENOTFOUND:
  68                 return -ENOENT;
  69         case QLINK_CMD_RESULT_EALREADY:
  70                 return -EALREADY;
  71         case QLINK_CMD_RESULT_EADDRINUSE:
  72                 return -EADDRINUSE;
  73         case QLINK_CMD_RESULT_EADDRNOTAVAIL:
  74                 return -EADDRNOTAVAIL;
  75         default:
  76                 return -EFAULT;
  77         }
  78 }
  79
  80 static int qtnf_cmd_send_with_reply(struct qtnf_bus *bus,
  81                                     struct sk_buff *cmd_skb,
  82                                     struct sk_buff **response_skb,
  83                                     u16 *result_code,
  84                                     size_t const_resp_size,
  85                                     size_t *var_resp_size)
  86 {
  87         struct qlink_cmd *cmd;
  88         const struct qlink_resp *resp;
  89         struct sk_buff *resp_skb = NULL;
  90         u16 cmd_id;
  91         u8 mac_id, vif_id;
  92         int ret;
  93
  94         cmd = (struct qlink_cmd *)cmd_skb->data;
  95         cmd_id = le16_to_cpu(cmd->cmd_id);
  96         mac_id = cmd->macid;
  97         vif_id = cmd->vifid;
  98         cmd->mhdr.len = cpu_to_le16(cmd_skb->len);
  99
 100         if (unlikely(bus->fw_state != QTNF_FW_STATE_ACTIVE &&
 101                      le16_to_cpu(cmd->cmd_id) != QLINK_CMD_FW_INIT)) {
 102                 pr_warn("VIF%u.%u: drop cmd 0x%.4X in fw state %d\n",
 103                         mac_id, vif_id, le16_to_cpu(cmd->cmd_id),
 104                         bus->fw_state);
 105                 dev_kfree_skb(cmd_skb);
 106                 return -ENODEV;
 107         }
 108
 109         pr_debug("VIF%u.%u cmd=0x%.4X\n", mac_id, vif_id,
 110                  le16_to_cpu(cmd->cmd_id));
 111
 112         ret = qtnf_trans_send_cmd_with_resp(bus, cmd_skb, &resp_skb);
 113
 114         if (unlikely(ret))
 115                 goto out;
 116
 117         resp = (const struct qlink_resp *)resp_skb->data;
 118         ret = qtnf_cmd_check_reply_header(resp, cmd_id, mac_id, vif_id,
 119                                           const_resp_size);
 120
 121         if (unlikely(ret))
 122                 goto out;
 123
 124         if (likely(result_code))
 125                 *result_code = le16_to_cpu(resp->result);
 126
 127         /* Return length of variable part of response */
 128         if (response_skb && var_resp_size)
 129                 *var_resp_size = le16_to_cpu(resp->mhdr.len) - const_resp_size;
 130
 131 out:
 132         if (response_skb)
 133                 *response_skb = resp_skb;
 134         else
 135                 consume_skb(resp_skb);
 136
 137         return ret;
 138 }
 139
 140 static inline int qtnf_cmd_send(struct qtnf_bus *bus,
 141                                 struct sk_buff *cmd_skb,
 142                                 u16 *result_code)
 143 {
 144         return qtnf_cmd_send_with_reply(bus, cmd_skb, NULL, result_code,
 145                                         sizeof(struct qlink_resp), NULL);
 146 }
 147
 148 static struct sk_buff *qtnf_cmd_alloc_new_cmdskb(u8 macid, u8 vifid, u16 cmd_no,
 149                                                  size_t cmd_size)
 150 {
 151         struct qlink_cmd *cmd;
 152         struct sk_buff *cmd_skb;
 153
 154         cmd_skb = __dev_alloc_skb(sizeof(*cmd) +
 155                                   QTNF_MAX_CMD_BUF_SIZE, GFP_KERNEL);
 156         if (unlikely(!cmd_skb)) {
 157                 pr_err("VIF%u.%u CMD %u: alloc failed\n", macid, vifid, cmd_no);
 158                 return NULL;
 159         }
 160
 161         skb_put_zero(cmd_skb, cmd_size);
 162
 163         cmd = (struct qlink_cmd *)cmd_skb->data;
 164         cmd->mhdr.len = cpu_to_le16(cmd_skb->len);
 165         cmd->mhdr.type = cpu_to_le16(QLINK_MSG_TYPE_CMD);
 166         cmd->cmd_id = cpu_to_le16(cmd_no);
 167         cmd->macid = macid;
 168         cmd->vifid = vifid;
 169
 170         return cmd_skb;
 171 }
 172
 173 static void qtnf_cmd_tlv_ie_set_add(struct sk_buff *cmd_skb, u8 frame_type,
 174                                     const u8 *buf, size_t len)
 175 {
 176         struct qlink_tlv_ie_set *tlv;
 177
 178         tlv = (struct qlink_tlv_ie_set *)skb_put(cmd_skb, sizeof(*tlv) + len);
 179         tlv->hdr.type = cpu_to_le16(QTN_TLV_ID_IE_SET);
 180         tlv->hdr.len = cpu_to_le16(len + sizeof(*tlv) - sizeof(tlv->hdr));
 181         tlv->type = frame_type;
 182         tlv->flags = 0;
 183
 184         if (len && buf)
 185                 memcpy(tlv->ie_data, buf, len);
 186 }
 187
 188 static inline size_t qtnf_cmd_acl_data_size(const struct cfg80211_acl_data *acl)
 189 {
 190         size_t size = sizeof(struct qlink_acl_data) +
 191                       acl->n_acl_entries * sizeof(struct qlink_mac_address);
 192
 193         return size;
 194 }
 195
 196 static bool qtnf_cmd_start_ap_can_fit(const struct qtnf_vif *vif,
 197                                       const struct cfg80211_ap_settings *s)
 198 {
 199         unsigned int len = sizeof(struct qlink_cmd_start_ap);
 200
 201         len += s->ssid_len;
 202         len += s->beacon.head_len;
 203         len += s->beacon.tail_len;
 204         len += s->beacon.beacon_ies_len;
 205         len += s->beacon.proberesp_ies_len;
 206         len += s->beacon.assocresp_ies_len;
 207         len += s->beacon.probe_resp_len;
 208
 209         if (cfg80211_chandef_valid(&s->chandef))
 210                 len += sizeof(struct qlink_tlv_chandef);
 211
 212         if (s->acl)
 213                 len += sizeof(struct qlink_tlv_hdr) +
 214                        qtnf_cmd_acl_data_size(s->acl);
 215
 216         if (len > (sizeof(struct qlink_cmd) + QTNF_MAX_CMD_BUF_SIZE)) {
 217                 pr_err("VIF%u.%u: can not fit AP settings: %u\n",
 218                        vif->mac->macid, vif->vifid, len);
 219                 return false;
 220         }
 221
 222         return true;
 223 }
 224
 225 int qtnf_cmd_send_start_ap(struct qtnf_vif *vif,
 226                            const struct cfg80211_ap_settings *s)
 227 {
 228         struct sk_buff *cmd_skb;
 229         struct qlink_cmd_start_ap *cmd;
 230         struct qlink_auth_encr *aen;
 231         u16 res_code = QLINK_CMD_RESULT_OK;
 232         int ret;
 233         int i;
 234
 235         if (!qtnf_cmd_start_ap_can_fit(vif, s))
 236                 return -E2BIG;
 237
 238         cmd_skb = qtnf_cmd_alloc_new_cmdskb(vif->mac->macid, vif->vifid,
 239                                             QLINK_CMD_START_AP,
 240                                             sizeof(*cmd));
 241         if (!cmd_skb)
 242                 return -ENOMEM;
 243
 244         cmd = (struct qlink_cmd_start_ap *)cmd_skb->data;
 245         cmd->dtim_period = s->dtim_period;
 246         cmd->beacon_interval = cpu_to_le16(s->beacon_interval);
 247         cmd->hidden_ssid = qlink_hidden_ssid_nl2q(s->hidden_ssid);
 248         cmd->inactivity_timeout = cpu_to_le16(s->inactivity_timeout);
 249         cmd->smps_mode = s->smps_mode;
 250         cmd->p2p_ctwindow = s->p2p_ctwindow;
 251         cmd->p2p_opp_ps = s->p2p_opp_ps;
 252         cmd->pbss = s->pbss;
 253         cmd->ht_required = s->ht_required;
 254         cmd->vht_required = s->vht_required;
 255
 256         aen = &cmd->aen;
 257         aen->auth_type = s->auth_type;
 258         aen->privacy = !!s->privacy;
 259         aen->wpa_versions = cpu_to_le32(s->crypto.wpa_versions);
 260         aen->cipher_group = cpu_to_le32(s->crypto.cipher_group);
 261         aen->n_ciphers_pairwise = cpu_to_le32(s->crypto.n_ciphers_pairwise);
 262         for (i = 0; i < QLINK_MAX_NR_CIPHER_SUITES; i++)
 263                 aen->ciphers_pairwise[i] =
 264                                 cpu_to_le32(s->crypto.ciphers_pairwise[i]);
 265         aen->n_akm_suites = cpu_to_le32(s->crypto.n_akm_suites);
 266         for (i = 0; i < QLINK_MAX_NR_AKM_SUITES; i++)
 267                 aen->akm_suites[i] = cpu_to_le32(s->crypto.akm_suites[i]);
 268         aen->control_port = s->crypto.control_port;
 269         aen->control_port_no_encrypt = s->crypto.control_port_no_encrypt;
 270         aen->control_port_ethertype =
 271                 cpu_to_le16(be16_to_cpu(s->crypto.control_port_ethertype));
 272
 273         if (s->ssid && s->ssid_len > 0 && s->ssid_len <= IEEE80211_MAX_SSID_LEN)
 274                 qtnf_cmd_skb_put_tlv_arr(cmd_skb, WLAN_EID_SSID, s->ssid,
 275                                          s->ssid_len);
 276
 277         if (cfg80211_chandef_valid(&s->chandef)) {
 278                 struct qlink_tlv_chandef *chtlv =
 279                         (struct qlink_tlv_chandef *)skb_put(cmd_skb,
 280                                                             sizeof(*chtlv));
 281
 282                 chtlv->hdr.type = cpu_to_le16(QTN_TLV_ID_CHANDEF);
 283                 chtlv->hdr.len = cpu_to_le16(sizeof(*chtlv) -
 284                                              sizeof(chtlv->hdr));
 285                 qlink_chandef_cfg2q(&s->chandef, &chtlv->chdef);
 286         }
 287
 288         qtnf_cmd_tlv_ie_set_add(cmd_skb, QLINK_IE_SET_BEACON_HEAD,
 289                                 s->beacon.head, s->beacon.head_len);
 290         qtnf_cmd_tlv_ie_set_add(cmd_skb, QLINK_IE_SET_BEACON_TAIL,
 291                                 s->beacon.tail, s->beacon.tail_len);
 292         qtnf_cmd_tlv_ie_set_add(cmd_skb, QLINK_IE_SET_BEACON_IES,
 293                                 s->beacon.beacon_ies, s->beacon.beacon_ies_len);
 294         qtnf_cmd_tlv_ie_set_add(cmd_skb, QLINK_IE_SET_PROBE_RESP,
 295                                 s->beacon.probe_resp, s->beacon.probe_resp_len);
 296         qtnf_cmd_tlv_ie_set_add(cmd_skb, QLINK_IE_SET_PROBE_RESP_IES,
 297                                 s->beacon.proberesp_ies,
 298                                 s->beacon.proberesp_ies_len);
 299         qtnf_cmd_tlv_ie_set_add(cmd_skb, QLINK_IE_SET_ASSOC_RESP,
 300                                 s->beacon.assocresp_ies,
 301                                 s->beacon.assocresp_ies_len);
 302
 303         if (s->ht_cap) {
 304                 struct qlink_tlv_hdr *tlv = (struct qlink_tlv_hdr *)
 305                         skb_put(cmd_skb, sizeof(*tlv) + sizeof(*s->ht_cap));
 306
 307                 tlv->type = cpu_to_le16(WLAN_EID_HT_CAPABILITY);
 308                 tlv->len = cpu_to_le16(sizeof(*s->ht_cap));
 309                 memcpy(tlv->val, s->ht_cap, sizeof(*s->ht_cap));
 310         }
 311
 312         if (s->vht_cap) {
 313                 struct qlink_tlv_hdr *tlv = (struct qlink_tlv_hdr *)
 314                         skb_put(cmd_skb, sizeof(*tlv) + sizeof(*s->vht_cap));
 315
 316                 tlv->type = cpu_to_le16(WLAN_EID_VHT_CAPABILITY);
 317                 tlv->len = cpu_to_le16(sizeof(*s->vht_cap));
 318                 memcpy(tlv->val, s->vht_cap, sizeof(*s->vht_cap));
 319         }
 320
 321         if (s->acl) {
 322                 size_t acl_size = qtnf_cmd_acl_data_size(s->acl);
 323                 struct qlink_tlv_hdr *tlv =
 324                         skb_put(cmd_skb, sizeof(*tlv) + acl_size);
 325
 326                 tlv->type = cpu_to_le16(QTN_TLV_ID_ACL_DATA);
 327                 tlv->len = cpu_to_le16(acl_size);
 328                 qlink_acl_data_cfg2q(s->acl, (struct qlink_acl_data *)tlv->val);
 329         }
 330
 331         qtnf_bus_lock(vif->mac->bus);
 332
 333         ret = qtnf_cmd_send(vif->mac->bus, cmd_skb, &res_code);
 334
 335         if (unlikely(ret))
 336                 goto out;
 337
 338         if (unlikely(res_code != QLINK_CMD_RESULT_OK)) {
 339                 pr_err("VIF%u.%u: CMD failed: %u\n", vif->mac->macid,
 340                        vif->vifid, res_code);
 341                 ret = -EFAULT;
 342                 goto out;
 343         }
 344
 345         netif_carrier_on(vif->netdev);
 346
 347 out:
 348         qtnf_bus_unlock(vif->mac->bus);
 349         return ret;
 350 }
 351
 352 int qtnf_cmd_send_stop_ap(struct qtnf_vif *vif)
 353 {
 354         struct sk_buff *cmd_skb;
 355         u16 res_code = QLINK_CMD_RESULT_OK;
 356         int ret;
 357
 358         cmd_skb = qtnf_cmd_alloc_new_cmdskb(vif->mac->macid, vif->vifid,
 359                                             QLINK_CMD_STOP_AP,
 360                                             sizeof(struct qlink_cmd));
 361         if (!cmd_skb)
 362                 return -ENOMEM;
 363
 364         qtnf_bus_lock(vif->mac->bus);
 365
 366         ret = qtnf_cmd_send(vif->mac->bus, cmd_skb, &res_code);
 367
 368         if (unlikely(ret))
 369                 goto out;
 370
 371         if (unlikely(res_code != QLINK_CMD_RESULT_OK)) {
 372                 pr_err("VIF%u.%u: CMD failed: %u\n", vif->mac->macid,
 373                        vif->vifid, res_code);
 374                 ret = -EFAULT;
 375                 goto out;
 376         }
 377
 378         netif_carrier_off(vif->netdev);
 379
 380 out:
 381         qtnf_bus_unlock(vif->mac->bus);
 382         return ret;
 383 }
 384
 385 int qtnf_cmd_send_register_mgmt(struct qtnf_vif *vif, u16 frame_type, bool reg)
 386 {
 387         struct sk_buff *cmd_skb;
 388         struct qlink_cmd_mgmt_frame_register *cmd;
 389         u16 res_code = QLINK_CMD_RESULT_OK;
 390         int ret;
 391
 392         cmd_skb = qtnf_cmd_alloc_new_cmdskb(vif->mac->macid, vif->vifid,
 393                                             QLINK_CMD_REGISTER_MGMT,
 394                                             sizeof(*cmd));
 395         if (!cmd_skb)
 396                 return -ENOMEM;
 397
 398         qtnf_bus_lock(vif->mac->bus);
 399
 400         cmd = (struct qlink_cmd_mgmt_frame_register *)cmd_skb->data;
 401         cmd->frame_type = cpu_to_le16(frame_type);
 402         cmd->do_register = reg;
 403
 404         ret = qtnf_cmd_send(vif->mac->bus, cmd_skb, &res_code);
 405
 406         if (unlikely(ret))
 407                 goto out;
 408
 409         if (unlikely(res_code != QLINK_CMD_RESULT_OK)) {
 410                 pr_err("VIF%u.%u: CMD failed: %u\n", vif->mac->macid,
 411                        vif->vifid, res_code);
 412                 ret = -EFAULT;
 413                 goto out;
 414         }
 415
 416 out:
 417         qtnf_bus_unlock(vif->mac->bus);
 418         return ret;
 419 }
 420
 421 int qtnf_cmd_send_mgmt_frame(struct qtnf_vif *vif, u32 cookie, u16 flags,
 422                              u16 freq, const u8 *buf, size_t len)
 423 {
 424         struct sk_buff *cmd_skb;
 425         struct qlink_cmd_mgmt_frame_tx *cmd;
 426         u16 res_code = QLINK_CMD_RESULT_OK;
 427         int ret;
 428
 429         if (sizeof(*cmd) + len > QTNF_MAX_CMD_BUF_SIZE) {
 430                 pr_warn("VIF%u.%u: frame is too big: %zu\n", vif->mac->macid,
 431                         vif->vifid, len);
 432                 return -E2BIG;
 433         }
 434
 435         cmd_skb = qtnf_cmd_alloc_new_cmdskb(vif->mac->macid, vif->vifid,
 436                                             QLINK_CMD_SEND_MGMT_FRAME,
 437                                             sizeof(*cmd));
 438         if (!cmd_skb)
 439                 return -ENOMEM;
 440
 441         qtnf_bus_lock(vif->mac->bus);
 442
 443         cmd = (struct qlink_cmd_mgmt_frame_tx *)cmd_skb->data;
 444         cmd->cookie = cpu_to_le32(cookie);
 445         cmd->freq = cpu_to_le16(freq);
 446         cmd->flags = cpu_to_le16(flags);
 447
 448         if (len && buf)
 449                 qtnf_cmd_skb_put_buffer(cmd_skb, buf, len);
 450
 451         ret = qtnf_cmd_send(vif->mac->bus, cmd_skb, &res_code);
 452
 453         if (unlikely(ret))
 454                 goto out;
 455
 456         if (unlikely(res_code != QLINK_CMD_RESULT_OK)) {
 457                 pr_err("VIF%u.%u: CMD failed: %u\n", vif->mac->macid,
 458                        vif->vifid, res_code);
 459                 ret = -EFAULT;
 460                 goto out;
 461         }
 462
 463 out:
 464         qtnf_bus_unlock(vif->mac->bus);
 465         return ret;
 466 }
 467
 468 int qtnf_cmd_send_mgmt_set_appie(struct qtnf_vif *vif, u8 frame_type,
 469                                  const u8 *buf, size_t len)
 470 {
 471         struct sk_buff *cmd_skb;
 472         u16 res_code = QLINK_CMD_RESULT_OK;
 473         int ret;
 474
 475         if (len > QTNF_MAX_CMD_BUF_SIZE) {
 476                 pr_warn("VIF%u.%u: %u frame is too big: %zu\n", vif->mac->macid,
 477                         vif->vifid, frame_type, len);
 478                 return -E2BIG;
 479         }
 480
 481         cmd_skb = qtnf_cmd_alloc_new_cmdskb(vif->mac->macid, vif->vifid,
 482                                             QLINK_CMD_MGMT_SET_APPIE,
 483                                             sizeof(struct qlink_cmd));
 484         if (!cmd_skb)
 485                 return -ENOMEM;
 486
 487         qtnf_cmd_tlv_ie_set_add(cmd_skb, frame_type, buf, len);
 488
 489         qtnf_bus_lock(vif->mac->bus);
 490
 491         ret = qtnf_cmd_send(vif->mac->bus, cmd_skb, &res_code);
 492
 493         if (unlikely(ret))
 494                 goto out;
 495
 496         if (unlikely(res_code != QLINK_CMD_RESULT_OK)) {
 497                 pr_err("VIF%u.%u frame %u: CMD failed: %u\n", vif->mac->macid,
 498                        vif->vifid, frame_type, res_code);
 499                 ret = -EFAULT;
 500                 goto out;
 501         }
 502
 503 out:
 504         qtnf_bus_unlock(vif->mac->bus);
 505         return ret;
 506 }
 507
 508 static void
 509 qtnf_sta_info_parse_rate(struct rate_info *rate_dst,
 510                          const struct qlink_sta_info_rate *rate_src)
 511 {
 512         rate_dst->legacy = get_unaligned_le16(&rate_src->rate) * 10;
 513
 514         rate_dst->mcs = rate_src->mcs;
 515         rate_dst->nss = rate_src->nss;
 516         rate_dst->flags = 0;
 517
 518         switch (rate_src->bw) {
 519         case QLINK_CHAN_WIDTH_5:
 520                 rate_dst->bw = RATE_INFO_BW_5;
 521                 break;
 522         case QLINK_CHAN_WIDTH_10:
 523                 rate_dst->bw = RATE_INFO_BW_10;
 524                 break;
 525         case QLINK_CHAN_WIDTH_20:
 526         case QLINK_CHAN_WIDTH_20_NOHT:
 527                 rate_dst->bw = RATE_INFO_BW_20;
 528                 break;
 529         case QLINK_CHAN_WIDTH_40:
 530                 rate_dst->bw = RATE_INFO_BW_40;
 531                 break;
 532         case QLINK_CHAN_WIDTH_80:
 533                 rate_dst->bw = RATE_INFO_BW_80;
 534                 break;
 535         case QLINK_CHAN_WIDTH_160:
 536                 rate_dst->bw = RATE_INFO_BW_160;
 537                 break;
 538         default:
 539                 rate_dst->bw = 0;
 540                 break;
 541         }
 542
 543         if (rate_src->flags & QLINK_STA_INFO_RATE_FLAG_HT_MCS)
 544                 rate_dst->flags |= RATE_INFO_FLAGS_MCS;
 545         else if (rate_src->flags & QLINK_STA_INFO_RATE_FLAG_VHT_MCS)
 546                 rate_dst->flags |= RATE_INFO_FLAGS_VHT_MCS;
 547 }
 548
 549 static void
 550 qtnf_sta_info_parse_flags(struct nl80211_sta_flag_update *dst,
 551                           const struct qlink_sta_info_state *src)
 552 {
 553         u32 mask, value;
 554
 555         dst->mask = 0;
 556         dst->set = 0;
 557
 558         mask = le32_to_cpu(src->mask);
 559         value = le32_to_cpu(src->value);
 560
 561         if (mask & QLINK_STA_FLAG_AUTHORIZED) {
 562                 dst->mask |= BIT(NL80211_STA_FLAG_AUTHORIZED);
 563                 if (value & QLINK_STA_FLAG_AUTHORIZED)
 564                         dst->set |= BIT(NL80211_STA_FLAG_AUTHORIZED);
 565         }
 566
 567         if (mask & QLINK_STA_FLAG_SHORT_PREAMBLE) {
 568                 dst->mask |= BIT(NL80211_STA_FLAG_SHORT_PREAMBLE);
 569                 if (value & QLINK_STA_FLAG_SHORT_PREAMBLE)
 570                         dst->set |= BIT(NL80211_STA_FLAG_SHORT_PREAMBLE);
 571         }
 572
 573         if (mask & QLINK_STA_FLAG_WME) {
 574                 dst->mask |= BIT(NL80211_STA_FLAG_WME);
 575                 if (value & QLINK_STA_FLAG_WME)
 576                         dst->set |= BIT(NL80211_STA_FLAG_WME);
 577         }
 578
 579         if (mask & QLINK_STA_FLAG_MFP) {
 580                 dst->mask |= BIT(NL80211_STA_FLAG_MFP);
 581                 if (value & QLINK_STA_FLAG_MFP)
 582                         dst->set |= BIT(NL80211_STA_FLAG_MFP);
 583         }
 584
 585         if (mask & QLINK_STA_FLAG_AUTHENTICATED) {
 586                 dst->mask |= BIT(NL80211_STA_FLAG_AUTHENTICATED);
 587                 if (value & QLINK_STA_FLAG_AUTHENTICATED)
 588                         dst->set |= BIT(NL80211_STA_FLAG_AUTHENTICATED);
 589         }
 590
 591         if (mask & QLINK_STA_FLAG_TDLS_PEER) {
 592                 dst->mask |= BIT(NL80211_STA_FLAG_TDLS_PEER);
 593                 if (value & QLINK_STA_FLAG_TDLS_PEER)
 594                         dst->set |= BIT(NL80211_STA_FLAG_TDLS_PEER);
 595         }
 596
 597         if (mask & QLINK_STA_FLAG_ASSOCIATED) {
 598                 dst->mask |= BIT(NL80211_STA_FLAG_ASSOCIATED);
 599                 if (value & QLINK_STA_FLAG_ASSOCIATED)
 600                         dst->set |= BIT(NL80211_STA_FLAG_ASSOCIATED);
 601         }
 602 }
 603
 604 static void
 605 qtnf_cmd_sta_info_parse(struct station_info *sinfo,
 606                         const struct qlink_tlv_hdr *tlv,
 607                         size_t resp_size)
 608 {
 609         const struct qlink_sta_stats *stats = NULL;
 610         const u8 *map = NULL;
 611         unsigned int map_len = 0;
 612         unsigned int stats_len = 0;
 613         u16 tlv_len;
 614
 615 #define qtnf_sta_stat_avail(stat_name, bitn)    \
 616         (qtnf_utils_is_bit_set(map, bitn, map_len) && \
 617          (offsetofend(struct qlink_sta_stats, stat_name) <= stats_len))
 618
 619         while (resp_size >= sizeof(*tlv)) {
 620                 tlv_len = le16_to_cpu(tlv->len);
 621
 622                 switch (le16_to_cpu(tlv->type)) {
 623                 case QTN_TLV_ID_STA_STATS_MAP:
 624                         map_len = tlv_len;
 625                         map = tlv->val;
 626                         break;
 627                 case QTN_TLV_ID_STA_STATS:
 628                         stats_len = tlv_len;
 629                         stats = (const struct qlink_sta_stats *)tlv->val;
 630                         break;
 631                 default:
 632                         break;
 633                 }
 634
 635                 resp_size -= tlv_len + sizeof(*tlv);
 636                 tlv = (const struct qlink_tlv_hdr *)(tlv->val + tlv_len);
 637         }
 638
 639         if (!map || !stats)
 640                 return;
 641
 642         if (qtnf_sta_stat_avail(inactive_time, QLINK_STA_INFO_INACTIVE_TIME)) {
 643                 sinfo->filled |= BIT(NL80211_STA_INFO_INACTIVE_TIME);
 644                 sinfo->inactive_time = le32_to_cpu(stats->inactive_time);
 645         }
 646
 647         if (qtnf_sta_stat_avail(connected_time,
 648                                 QLINK_STA_INFO_CONNECTED_TIME)) {
 649                 sinfo->filled |= BIT(NL80211_STA_INFO_CONNECTED_TIME);
 650                 sinfo->connected_time = le32_to_cpu(stats->connected_time);
 651         }
 652
 653         if (qtnf_sta_stat_avail(signal, QLINK_STA_INFO_SIGNAL)) {
 654                 sinfo->filled |= BIT(NL80211_STA_INFO_SIGNAL);
 655                 sinfo->signal = stats->signal - QLINK_RSSI_OFFSET;
 656         }
 657
 658         if (qtnf_sta_stat_avail(signal_avg, QLINK_STA_INFO_SIGNAL_AVG)) {
 659                 sinfo->filled |= BIT(NL80211_STA_INFO_SIGNAL_AVG);
 660                 sinfo->signal_avg = stats->signal_avg - QLINK_RSSI_OFFSET;
 661         }
 662
 663         if (qtnf_sta_stat_avail(rxrate, QLINK_STA_INFO_RX_BITRATE)) {
 664                 sinfo->filled |= BIT(NL80211_STA_INFO_RX_BITRATE);
 665                 qtnf_sta_info_parse_rate(&sinfo->rxrate, &stats->rxrate);
 666         }
 667
 668         if (qtnf_sta_stat_avail(txrate, QLINK_STA_INFO_TX_BITRATE)) {
 669                 sinfo->filled |= BIT(NL80211_STA_INFO_TX_BITRATE);
 670                 qtnf_sta_info_parse_rate(&sinfo->txrate, &stats->txrate);
 671         }
 672
 673         if (qtnf_sta_stat_avail(sta_flags, QLINK_STA_INFO_STA_FLAGS)) {
 674                 sinfo->filled |= BIT(NL80211_STA_INFO_STA_FLAGS);
 675                 qtnf_sta_info_parse_flags(&sinfo->sta_flags, &stats->sta_flags);
 676         }
 677
 678         if (qtnf_sta_stat_avail(rx_bytes, QLINK_STA_INFO_RX_BYTES)) {
 679                 sinfo->filled |= BIT(NL80211_STA_INFO_RX_BYTES);
 680                 sinfo->rx_bytes = le64_to_cpu(stats->rx_bytes);
 681         }
 682
 683         if (qtnf_sta_stat_avail(tx_bytes, QLINK_STA_INFO_TX_BYTES)) {
 684                 sinfo->filled |= BIT(NL80211_STA_INFO_TX_BYTES);
 685                 sinfo->tx_bytes = le64_to_cpu(stats->tx_bytes);
 686         }
 687
 688         if (qtnf_sta_stat_avail(rx_bytes, QLINK_STA_INFO_RX_BYTES64)) {
 689                 sinfo->filled |= BIT(NL80211_STA_INFO_RX_BYTES64);
 690                 sinfo->rx_bytes = le64_to_cpu(stats->rx_bytes);
 691         }
 692
 693         if (qtnf_sta_stat_avail(tx_bytes, QLINK_STA_INFO_TX_BYTES64)) {
 694                 sinfo->filled |= BIT(NL80211_STA_INFO_TX_BYTES64);
 695                 sinfo->tx_bytes = le64_to_cpu(stats->tx_bytes);
 696         }
 697
 698         if (qtnf_sta_stat_avail(rx_packets, QLINK_STA_INFO_RX_PACKETS)) {
 699                 sinfo->filled |= BIT(NL80211_STA_INFO_RX_PACKETS);
 700                 sinfo->rx_packets = le32_to_cpu(stats->rx_packets);
 701         }
 702
 703         if (qtnf_sta_stat_avail(tx_packets, QLINK_STA_INFO_TX_PACKETS)) {
 704                 sinfo->filled |= BIT(NL80211_STA_INFO_TX_PACKETS);
 705                 sinfo->tx_packets = le32_to_cpu(stats->tx_packets);
 706         }
 707
 708         if (qtnf_sta_stat_avail(rx_beacon, QLINK_STA_INFO_BEACON_RX)) {
 709                 sinfo->filled |= BIT(NL80211_STA_INFO_BEACON_RX);
 710                 sinfo->rx_beacon = le64_to_cpu(stats->rx_beacon);
 711         }
 712
 713         if (qtnf_sta_stat_avail(rx_dropped_misc, QLINK_STA_INFO_RX_DROP_MISC)) {
 714                 sinfo->filled |= BIT(NL80211_STA_INFO_RX_DROP_MISC);
 715                 sinfo->rx_dropped_misc = le32_to_cpu(stats->rx_dropped_misc);
 716         }
 717
 718         if (qtnf_sta_stat_avail(tx_failed, QLINK_STA_INFO_TX_FAILED)) {
 719                 sinfo->filled |= BIT(NL80211_STA_INFO_TX_FAILED);
 720                 sinfo->tx_failed = le32_to_cpu(stats->tx_failed);
 721         }
 722
 723 #undef qtnf_sta_stat_avail
 724 }
 725
 726 int qtnf_cmd_get_sta_info(struct qtnf_vif *vif, const u8 *sta_mac,
 727                           struct station_info *sinfo)
 728 {
 729         struct sk_buff *cmd_skb, *resp_skb = NULL;
 730         struct qlink_cmd_get_sta_info *cmd;
 731         const struct qlink_resp_get_sta_info *resp;
 732         size_t var_resp_len;
 733         u16 res_code = QLINK_CMD_RESULT_OK;
 734         int ret = 0;
 735
 736         cmd_skb = qtnf_cmd_alloc_new_cmdskb(vif->mac->macid, vif->vifid,
 737                                             QLINK_CMD_GET_STA_INFO,
 738                                             sizeof(*cmd));
 739         if (!cmd_skb)
 740                 return -ENOMEM;
 741
 742         qtnf_bus_lock(vif->mac->bus);
 743
 744         cmd = (struct qlink_cmd_get_sta_info *)cmd_skb->data;
 745         ether_addr_copy(cmd->sta_addr, sta_mac);
 746
 747         ret = qtnf_cmd_send_with_reply(vif->mac->bus, cmd_skb, &resp_skb,
 748                                        &res_code, sizeof(*resp),
 749                                        &var_resp_len);
 750
 751         if (unlikely(ret))
 752                 goto out;
 753
 754         if (unlikely(res_code != QLINK_CMD_RESULT_OK)) {
 755                 switch (res_code) {
 756                 case QLINK_CMD_RESULT_ENOTFOUND:
 757                         pr_warn("VIF%u.%u: %pM STA not found\n",
 758                                 vif->mac->macid, vif->vifid, sta_mac);
 759                         ret = -ENOENT;
 760                         break;
 761                 default:
 762                         pr_err("VIF%u.%u: can't get info for %pM: %u\n",
 763                                vif->mac->macid, vif->vifid, sta_mac, res_code);
 764                         ret = -EFAULT;
 765                         break;
 766                 }
 767                 goto out;
 768         }
 769
 770         resp = (const struct qlink_resp_get_sta_info *)resp_skb->data;
 771
 772         if (unlikely(!ether_addr_equal(sta_mac, resp->sta_addr))) {
 773                 pr_err("VIF%u.%u: wrong mac in reply: %pM != %pM\n",
 774                        vif->mac->macid, vif->vifid, resp->sta_addr, sta_mac);
 775                 ret = -EINVAL;
 776                 goto out;
 777         }
 778
 779         qtnf_cmd_sta_info_parse(sinfo,
 780                                 (const struct qlink_tlv_hdr *)resp->info,
 781                                 var_resp_len);
 782
 783 out:
 784         qtnf_bus_unlock(vif->mac->bus);
 785         consume_skb(resp_skb);
 786
 787         return ret;
 788 }
 789
 790 static int qtnf_cmd_send_add_change_intf(struct qtnf_vif *vif,
 791                                          enum nl80211_iftype iftype,
 792                                          u8 *mac_addr,
 793                                          enum qlink_cmd_type cmd_type)
 794 {
 795         struct sk_buff *cmd_skb, *resp_skb = NULL;
 796         struct qlink_cmd_manage_intf *cmd;
 797         const struct qlink_resp_manage_intf *resp;
 798         u16 res_code = QLINK_CMD_RESULT_OK;
 799         int ret = 0;
 800
 801         cmd_skb = qtnf_cmd_alloc_new_cmdskb(vif->mac->macid, vif->vifid,
 802                                             cmd_type,
 803                                             sizeof(*cmd));
 804         if (!cmd_skb)
 805                 return -ENOMEM;
 806
 807         qtnf_bus_lock(vif->mac->bus);
 808
 809         cmd = (struct qlink_cmd_manage_intf *)cmd_skb->data;
 810
 811         switch (iftype) {
 812         case NL80211_IFTYPE_AP:
 813                 cmd->intf_info.if_type = cpu_to_le16(QLINK_IFTYPE_AP);
 814                 break;
 815         case NL80211_IFTYPE_STATION:
 816                 cmd->intf_info.if_type = cpu_to_le16(QLINK_IFTYPE_STATION);
 817                 break;
 818         default:
 819                 pr_err("VIF%u.%u: unsupported type %d\n", vif->mac->macid,
 820                        vif->vifid, iftype);
 821                 ret = -EINVAL;
 822                 goto out;
 823         }
 824
 825         if (mac_addr)
 826                 ether_addr_copy(cmd->intf_info.mac_addr, mac_addr);
 827         else
 828                 eth_zero_addr(cmd->intf_info.mac_addr);
 829
 830         ret = qtnf_cmd_send_with_reply(vif->mac->bus, cmd_skb, &resp_skb,
 831                                        &res_code, sizeof(*resp), NULL);
 832
 833         if (unlikely(ret))
 834                 goto out;
 835
 836         ret = qtnf_cmd_resp_result_decode(res_code);
 837         if (ret) {
 838                 pr_err("VIF%u.%u: CMD %d failed: %u\n", vif->mac->macid,
 839                        vif->vifid, cmd_type, res_code);
 840                 goto out;
 841         }
 842
 843         resp = (const struct qlink_resp_manage_intf *)resp_skb->data;
 844         ether_addr_copy(vif->mac_addr, resp->intf_info.mac_addr);
 845
 846 out:
 847         qtnf_bus_unlock(vif->mac->bus);
 848         consume_skb(resp_skb);
 849
 850         return ret;
 851 }
 852
 853 int qtnf_cmd_send_add_intf(struct qtnf_vif *vif,
 854                            enum nl80211_iftype iftype, u8 *mac_addr)
 855 {
 856         return qtnf_cmd_send_add_change_intf(vif, iftype, mac_addr,
 857                         QLINK_CMD_ADD_INTF);
 858 }
 859
 860 int qtnf_cmd_send_change_intf_type(struct qtnf_vif *vif,
 861                                    enum nl80211_iftype iftype, u8 *mac_addr)
 862 {
 863         return qtnf_cmd_send_add_change_intf(vif, iftype, mac_addr,
 864                                              QLINK_CMD_CHANGE_INTF);
 865 }
 866
 867 int qtnf_cmd_send_del_intf(struct qtnf_vif *vif)
 868 {
 869         struct sk_buff *cmd_skb;
 870         struct qlink_cmd_manage_intf *cmd;
 871         u16 res_code = QLINK_CMD_RESULT_OK;
 872         int ret = 0;
 873
 874         cmd_skb = qtnf_cmd_alloc_new_cmdskb(vif->mac->macid, vif->vifid,
 875                                             QLINK_CMD_DEL_INTF,
 876                                             sizeof(*cmd));
 877         if (!cmd_skb)
 878                 return -ENOMEM;
 879
 880         qtnf_bus_lock(vif->mac->bus);
 881
 882         cmd = (struct qlink_cmd_manage_intf *)cmd_skb->data;
 883
 884         switch (vif->wdev.iftype) {
 885         case NL80211_IFTYPE_AP:
 886                 cmd->intf_info.if_type = cpu_to_le16(QLINK_IFTYPE_AP);
 887                 break;
 888         case NL80211_IFTYPE_STATION:
 889                 cmd->intf_info.if_type = cpu_to_le16(QLINK_IFTYPE_STATION);
 890                 break;
 891         default:
 892                 pr_warn("VIF%u.%u: unsupported iftype %d\n", vif->mac->macid,
 893                         vif->vifid, vif->wdev.iftype);
 894                 ret = -EINVAL;
 895                 goto out;
 896         }
 897
 898         eth_zero_addr(cmd->intf_info.mac_addr);
 899
 900         ret = qtnf_cmd_send(vif->mac->bus, cmd_skb, &res_code);
 901
 902         if (unlikely(ret))
 903                 goto out;
 904
 905         if (unlikely(res_code != QLINK_CMD_RESULT_OK)) {
 906                 pr_err("VIF%u.%u: CMD failed: %u\n", vif->mac->macid,
 907                        vif->vifid, res_code);
 908                 ret = -EFAULT;
 909                 goto out;
 910         }
 911
 912 out:
 913         qtnf_bus_unlock(vif->mac->bus);
 914         return ret;
 915 }
 916
 917 static u32 qtnf_cmd_resp_reg_rule_flags_parse(u32 qflags)
 918 {
 919         u32 flags = 0;
 920
 921         if (qflags & QLINK_RRF_NO_OFDM)
 922                 flags |= NL80211_RRF_NO_OFDM;
 923
 924         if (qflags & QLINK_RRF_NO_CCK)
 925                 flags |= NL80211_RRF_NO_CCK;
 926
 927         if (qflags & QLINK_RRF_NO_INDOOR)
 928                 flags |= NL80211_RRF_NO_INDOOR;
 929
 930         if (qflags & QLINK_RRF_NO_OUTDOOR)
 931                 flags |= NL80211_RRF_NO_OUTDOOR;
 932
 933         if (qflags & QLINK_RRF_DFS)
 934                 flags |= NL80211_RRF_DFS;
 935
 936         if (qflags & QLINK_RRF_PTP_ONLY)
 937                 flags |= NL80211_RRF_PTP_ONLY;
 938
 939         if (qflags & QLINK_RRF_PTMP_ONLY)
 940                 flags |= NL80211_RRF_PTMP_ONLY;
 941
 942         if (qflags & QLINK_RRF_NO_IR)
 943                 flags |= NL80211_RRF_NO_IR;
 944
 945         if (qflags & QLINK_RRF_AUTO_BW)
 946                 flags |= NL80211_RRF_AUTO_BW;
 947
 948         if (qflags & QLINK_RRF_IR_CONCURRENT)
 949                 flags |= NL80211_RRF_IR_CONCURRENT;
 950
 951         if (qflags & QLINK_RRF_NO_HT40MINUS)
 952                 flags |= NL80211_RRF_NO_HT40MINUS;
 953
 954         if (qflags & QLINK_RRF_NO_HT40PLUS)
 955                 flags |= NL80211_RRF_NO_HT40PLUS;
 956
 957         if (qflags & QLINK_RRF_NO_80MHZ)
 958                 flags |= NL80211_RRF_NO_80MHZ;
 959
 960         if (qflags & QLINK_RRF_NO_160MHZ)
 961                 flags |= NL80211_RRF_NO_160MHZ;
 962
 963         return flags;
 964 }
 965
 966 static int
 967 qtnf_cmd_resp_proc_hw_info(struct qtnf_bus *bus,
 968                            const struct qlink_resp_get_hw_info *resp,
 969                            size_t info_len)
 970 {
 971         struct qtnf_hw_info *hwinfo = &bus->hw_info;
 972         const struct qlink_tlv_hdr *tlv;
 973         const struct qlink_tlv_reg_rule *tlv_rule;
 974         const char *bld_name = NULL;
 975         const char *bld_rev = NULL;
 976         const char *bld_type = NULL;
 977         const char *bld_label = NULL;
 978         u32 bld_tmstamp = 0;
 979         u32 plat_id = 0;
 980         const char *hw_id = NULL;
 981         const char *calibration_ver = NULL;
 982         const char *uboot_ver = NULL;
 983         u32 hw_ver = 0;
 984         struct ieee80211_reg_rule *rule;
 985         u16 tlv_type;
 986         u16 tlv_value_len;
 987         unsigned int rule_idx = 0;
 988
 989         if (WARN_ON(resp->n_reg_rules > NL80211_MAX_SUPP_REG_RULES))
 990                 return -E2BIG;
 991
 992         hwinfo->rd = kzalloc(sizeof(*hwinfo->rd)
 993                              + sizeof(struct ieee80211_reg_rule)
 994                              * resp->n_reg_rules, GFP_KERNEL);
 995
 996         if (!hwinfo->rd)
 997                 return -ENOMEM;
 998
 999         hwinfo->num_mac = resp->num_mac;
1000         hwinfo->mac_bitmap = resp->mac_bitmap;
1001         hwinfo->fw_ver = le32_to_cpu(resp->fw_ver);
1002         hwinfo->ql_proto_ver = le16_to_cpu(resp->ql_proto_ver);
1003         hwinfo->total_tx_chain = resp->total_tx_chain;
1004         hwinfo->total_rx_chain = resp->total_rx_chain;
1005         hwinfo->hw_capab = le32_to_cpu(resp->hw_capab);
1006         hwinfo->rd->n_reg_rules = resp->n_reg_rules;
1007         hwinfo->rd->alpha2[0] = resp->alpha2[0];
1008         hwinfo->rd->alpha2[1] = resp->alpha2[1];
1009
1010         bld_tmstamp = le32_to_cpu(resp->bld_tmstamp);
1011         plat_id = le32_to_cpu(resp->plat_id);
1012         hw_ver = le32_to_cpu(resp->hw_ver);
1013
1014         switch (resp->dfs_region) {
1015         case QLINK_DFS_FCC:
1016                 hwinfo->rd->dfs_region = NL80211_DFS_FCC;
1017                 break;
1018         case QLINK_DFS_ETSI:
1019                 hwinfo->rd->dfs_region = NL80211_DFS_ETSI;
1020                 break;
1021         case QLINK_DFS_JP:
1022                 hwinfo->rd->dfs_region = NL80211_DFS_JP;
1023                 break;
1024         case QLINK_DFS_UNSET:
1025         default:
1026                 hwinfo->rd->dfs_region = NL80211_DFS_UNSET;
1027                 break;
1028         }
1029
1030         tlv = (const struct qlink_tlv_hdr *)resp->info;
1031
1032         while (info_len >= sizeof(*tlv)) {
1033                 tlv_type = le16_to_cpu(tlv->type);
1034                 tlv_value_len = le16_to_cpu(tlv->len);
1035
1036                 if (tlv_value_len + sizeof(*tlv) > info_len) {
1037                         pr_warn("malformed TLV 0x%.2X; LEN: %u\n",
1038                                 tlv_type, tlv_value_len);
1039                         return -EINVAL;
1040                 }
1041
1042                 switch (tlv_type) {
1043                 case QTN_TLV_ID_REG_RULE:
1044                         if (rule_idx >= resp->n_reg_rules) {
1045                                 pr_warn("unexpected number of rules: %u\n",
1046                                         resp->n_reg_rules);
1047                                 return -EINVAL;
1048                         }
1049
1050                         if (tlv_value_len != sizeof(*tlv_rule) - sizeof(*tlv)) {
1051                                 pr_warn("malformed TLV 0x%.2X; LEN: %u\n",
1052                                         tlv_type, tlv_value_len);
1053                                 return -EINVAL;
1054                         }
1055
1056                         tlv_rule = (const struct qlink_tlv_reg_rule *)tlv;
1057                         rule = &hwinfo->rd->reg_rules[rule_idx++];
1058
1059                         rule->freq_range.start_freq_khz =
1060                                 le32_to_cpu(tlv_rule->start_freq_khz);
1061                         rule->freq_range.end_freq_khz =
1062                                 le32_to_cpu(tlv_rule->end_freq_khz);
1063                         rule->freq_range.max_bandwidth_khz =
1064                                 le32_to_cpu(tlv_rule->max_bandwidth_khz);
1065                         rule->power_rule.max_antenna_gain =
1066                                 le32_to_cpu(tlv_rule->max_antenna_gain);
1067                         rule->power_rule.max_eirp =
1068                                 le32_to_cpu(tlv_rule->max_eirp);
1069                         rule->dfs_cac_ms =
1070                                 le32_to_cpu(tlv_rule->dfs_cac_ms);
1071                         rule->flags = qtnf_cmd_resp_reg_rule_flags_parse(
1072                                         le32_to_cpu(tlv_rule->flags));
1073                         break;
1074                 case QTN_TLV_ID_BUILD_NAME:
1075                         bld_name = (const void *)tlv->val;
1076                         break;
1077                 case QTN_TLV_ID_BUILD_REV:
1078                         bld_rev = (const void *)tlv->val;
1079                         break;
1080                 case QTN_TLV_ID_BUILD_TYPE:
1081                         bld_type = (const void *)tlv->val;
1082                         break;
1083                 case QTN_TLV_ID_BUILD_LABEL:
1084                         bld_label = (const void *)tlv->val;
1085                         break;
1086                 case QTN_TLV_ID_HW_ID:
1087                         hw_id = (const void *)tlv->val;
1088                         break;
1089                 case QTN_TLV_ID_CALIBRATION_VER:
1090                         calibration_ver = (const void *)tlv->val;
1091                         break;
1092                 case QTN_TLV_ID_UBOOT_VER:
1093                         uboot_ver = (const void *)tlv->val;
1094                         break;
1095                 default:
1096                         break;
1097                 }
1098
1099                 info_len -= tlv_value_len + sizeof(*tlv);
1100                 tlv = (struct qlink_tlv_hdr *)(tlv->val + tlv_value_len);
1101         }
1102
1103         if (rule_idx != resp->n_reg_rules) {
1104                 pr_warn("unexpected number of rules: expected %u got %u\n",
1105                         resp->n_reg_rules, rule_idx);
1106                 kfree(hwinfo->rd);
1107                 hwinfo->rd = NULL;
1108                 return -EINVAL;
1109         }
1110
1111         pr_info("fw_version=%d, MACs map %#x, alpha2=\"%c%c\", chains Tx=%u Rx=%u, capab=0x%x\n",
1112                 hwinfo->fw_ver, hwinfo->mac_bitmap,
1113                 hwinfo->rd->alpha2[0], hwinfo->rd->alpha2[1],
1114                 hwinfo->total_tx_chain, hwinfo->total_rx_chain,
1115                 hwinfo->hw_capab);
1116
1117         pr_info("\nBuild name:            %s"  \
1118                 "\nBuild revision:        %s"  \
1119                 "\nBuild type:            %s"  \
1120                 "\nBuild label:           %s"  \
1121                 "\nBuild timestamp:       %lu" \
1122                 "\nPlatform ID:           %lu" \
1123                 "\nHardware ID:           %s"  \
1124                 "\nCalibration version:   %s"  \
1125                 "\nU-Boot version:        %s"  \
1126                 "\nHardware version:      0x%08x",
1127                 bld_name, bld_rev, bld_type, bld_label,
1128                 (unsigned long)bld_tmstamp,
1129                 (unsigned long)plat_id,
1130                 hw_id, calibration_ver, uboot_ver, hw_ver);
1131
1132         strlcpy(hwinfo->fw_version, bld_label, sizeof(hwinfo->fw_version));
1133         hwinfo->hw_version = hw_ver;
1134
1135         return 0;
1136 }
1137
1138 static int qtnf_parse_variable_mac_info(struct qtnf_wmac *mac,
1139                                         const u8 *tlv_buf, size_t tlv_buf_size)
1140 {
1141         struct ieee80211_iface_combination *comb = NULL;
1142         size_t n_comb = 0;
1143         struct ieee80211_iface_limit *limits;
1144         const struct qlink_iface_comb_num *comb_num;
1145         const struct qlink_iface_limit_record *rec;
1146         const struct qlink_iface_limit *lim;
1147         u16 rec_len;
1148         u16 tlv_type;
1149         u16 tlv_value_len;
1150         size_t tlv_full_len;
1151         const struct qlink_tlv_hdr *tlv;
1152         u8 *ext_capa = NULL;
1153         u8 *ext_capa_mask = NULL;
1154         u8 ext_capa_len = 0;
1155         u8 ext_capa_mask_len = 0;
1156         int i = 0;
1157
1158         tlv = (const struct qlink_tlv_hdr *)tlv_buf;
1159         while (tlv_buf_size >= sizeof(struct qlink_tlv_hdr)) {
1160                 tlv_type = le16_to_cpu(tlv->type);
1161                 tlv_value_len = le16_to_cpu(tlv->len);
1162                 tlv_full_len = tlv_value_len + sizeof(struct qlink_tlv_hdr);
1163                 if (tlv_full_len > tlv_buf_size) {
1164                         pr_warn("MAC%u: malformed TLV 0x%.2X; LEN: %u\n",
1165                                 mac->macid, tlv_type, tlv_value_len);
1166                         return -EINVAL;
1167                 }
1168
1169                 switch (tlv_type) {
1170                 case QTN_TLV_ID_NUM_IFACE_COMB:
1171                         if (tlv_value_len != sizeof(*comb_num))
1172                                 return -EINVAL;
1173
1174                         comb_num = (void *)tlv->val;
1175
1176                         /* free earlier iface comb memory */
1177                         qtnf_mac_iface_comb_free(mac);
1178
1179                         mac->macinfo.n_if_comb =
1180                                 le32_to_cpu(comb_num->iface_comb_num);
1181
1182                         mac->macinfo.if_comb =
1183                                 kcalloc(mac->macinfo.n_if_comb,
1184                                         sizeof(*mac->macinfo.if_comb),
1185                                         GFP_KERNEL);
1186
1187                         if (!mac->macinfo.if_comb)
1188                                 return -ENOMEM;
1189
1190                         comb = mac->macinfo.if_comb;
1191
1192                         pr_debug("MAC%u: %zu iface combinations\n",
1193                                  mac->macid, mac->macinfo.n_if_comb);
1194
1195                         break;
1196                 case QTN_TLV_ID_IFACE_LIMIT:
1197                         if (unlikely(!comb)) {
1198                                 pr_warn("MAC%u: no combinations advertised\n",
1199                                         mac->macid);
1200                                 return -EINVAL;
1201                         }
1202
1203                         if (n_comb >= mac->macinfo.n_if_comb) {
1204                                 pr_warn("MAC%u: combinations count exceeded\n",
1205                                         mac->macid);
1206                                 n_comb++;
1207                                 break;
1208                         }
1209
1210                         rec = (void *)tlv->val;
1211                         rec_len = sizeof(*rec) + rec->n_limits * sizeof(*lim);
1212
1213                         if (unlikely(tlv_value_len != rec_len)) {
1214                                 pr_warn("MAC%u: record %zu size mismatch\n",
1215                                         mac->macid, n_comb);
1216                                 return -EINVAL;
1217                         }
1218
1219                         limits = kzalloc(sizeof(*limits) * rec->n_limits,
1220                                          GFP_KERNEL);
1221                         if (!limits)
1222                                 return -ENOMEM;
1223
1224                         comb[n_comb].num_different_channels =
1225                                 rec->num_different_channels;
1226                         comb[n_comb].max_interfaces =
1227                                 le16_to_cpu(rec->max_interfaces);
1228                         comb[n_comb].n_limits = rec->n_limits;
1229                         comb[n_comb].limits = limits;
1230
1231                         for (i = 0; i < rec->n_limits; i++) {
1232                                 lim = &rec->limits[i];
1233                                 limits[i].max = le16_to_cpu(lim->max_num);
1234                                 limits[i].types =
1235                                         qlink_iface_type_to_nl_mask(le16_to_cpu(lim->type));
1236                                 pr_debug("MAC%u: comb[%zu]: MAX:%u TYPES:%.4X\n",
1237                                          mac->macid, n_comb,
1238                                          limits[i].max, limits[i].types);
1239                         }
1240
1241                         n_comb++;
1242                         break;
1243                 case WLAN_EID_EXT_CAPABILITY:
1244                         if (unlikely(tlv_value_len > U8_MAX))
1245                                 return -EINVAL;
1246                         ext_capa = (u8 *)tlv->val;
1247                         ext_capa_len = tlv_value_len;
1248                         break;
1249                 case QTN_TLV_ID_EXT_CAPABILITY_MASK:
1250                         if (unlikely(tlv_value_len > U8_MAX))
1251                                 return -EINVAL;
1252                         ext_capa_mask = (u8 *)tlv->val;
1253                         ext_capa_mask_len = tlv_value_len;
1254                         break;
1255                 default:
1256                         break;
1257                 }
1258
1259                 tlv_buf_size -= tlv_full_len;
1260                 tlv = (struct qlink_tlv_hdr *)(tlv->val + tlv_value_len);
1261         }
1262
1263         if (tlv_buf_size) {
1264                 pr_warn("MAC%u: malformed TLV buf; bytes left: %zu\n",
1265                         mac->macid, tlv_buf_size);
1266                 return -EINVAL;
1267         }
1268
1269         if (mac->macinfo.n_if_comb != n_comb) {
1270                 pr_err("MAC%u: combination mismatch: reported=%zu parsed=%zu\n",
1271                        mac->macid, mac->macinfo.n_if_comb, n_comb);
1272                 return -EINVAL;
1273         }
1274
1275         if (ext_capa_len != ext_capa_mask_len) {
1276                 pr_err("MAC%u: ext_capa/_mask lengths mismatch: %u != %u\n",
1277                        mac->macid, ext_capa_len, ext_capa_mask_len);
1278                 return -EINVAL;
1279         }
1280
1281         if (ext_capa_len > 0) {
1282                 ext_capa = kmemdup(ext_capa, ext_capa_len, GFP_KERNEL);
1283                 if (!ext_capa)
1284                         return -ENOMEM;
1285
1286                 ext_capa_mask =
1287                         kmemdup(ext_capa_mask, ext_capa_mask_len, GFP_KERNEL);
1288                 if (!ext_capa_mask) {
1289                         kfree(ext_capa);
1290                         return -ENOMEM;
1291                 }
1292         } else {
1293                 ext_capa = NULL;
1294                 ext_capa_mask = NULL;
1295         }
1296
1297         kfree(mac->macinfo.extended_capabilities);
1298         kfree(mac->macinfo.extended_capabilities_mask);
1299         mac->macinfo.extended_capabilities = ext_capa;
1300         mac->macinfo.extended_capabilities_mask = ext_capa_mask;
1301         mac->macinfo.extended_capabilities_len = ext_capa_len;
1302
1303         return 0;
1304 }
1305
1306 static void
1307 qtnf_cmd_resp_proc_mac_info(struct qtnf_wmac *mac,
1308                             const struct qlink_resp_get_mac_info *resp_info)
1309 {
1310         struct qtnf_mac_info *mac_info;
1311         struct qtnf_vif *vif;
1312
1313         mac_info = &mac->macinfo;
1314
1315         mac_info->bands_cap = resp_info->bands_cap;
1316         memcpy(&mac_info->dev_mac, &resp_info->dev_mac,
1317                sizeof(mac_info->dev_mac));
1318
1319         ether_addr_copy(mac->macaddr, mac_info->dev_mac);
1320
1321         vif = qtnf_mac_get_base_vif(mac);
1322         if (vif)
1323                 ether_addr_copy(vif->mac_addr, mac->macaddr);
1324         else
1325                 pr_err("could not get valid base vif\n");
1326
1327         mac_info->num_tx_chain = resp_info->num_tx_chain;
1328         mac_info->num_rx_chain = resp_info->num_rx_chain;
1329
1330         mac_info->max_ap_assoc_sta = le16_to_cpu(resp_info->max_ap_assoc_sta);
1331         mac_info->radar_detect_widths =
1332                         qlink_chan_width_mask_to_nl(le16_to_cpu(
1333                                         resp_info->radar_detect_widths));
1334         mac_info->max_acl_mac_addrs = le32_to_cpu(resp_info->max_acl_mac_addrs);
1335
1336         memcpy(&mac_info->ht_cap_mod_mask, &resp_info->ht_cap_mod_mask,
1337                sizeof(mac_info->ht_cap_mod_mask));
1338         memcpy(&mac_info->vht_cap_mod_mask, &resp_info->vht_cap_mod_mask,
1339                sizeof(mac_info->vht_cap_mod_mask));
1340 }
1341
1342 static void qtnf_cmd_resp_band_fill_htcap(const u8 *info,
1343                                           struct ieee80211_sta_ht_cap *bcap)
1344 {
1345         const struct ieee80211_ht_cap *ht_cap =
1346                 (const struct ieee80211_ht_cap *)info;
1347
1348         bcap->ht_supported = true;
1349         bcap->cap = le16_to_cpu(ht_cap->cap_info);
1350         bcap->ampdu_factor =
1351                 ht_cap->ampdu_params_info & IEEE80211_HT_AMPDU_PARM_FACTOR;
1352         bcap->ampdu_density =
1353                 (ht_cap->ampdu_params_info & IEEE80211_HT_AMPDU_PARM_DENSITY) >>
1354                 IEEE80211_HT_AMPDU_PARM_DENSITY_SHIFT;
1355         memcpy(&bcap->mcs, &ht_cap->mcs, sizeof(bcap->mcs));
1356 }
1357
1358 static void qtnf_cmd_resp_band_fill_vhtcap(const u8 *info,
1359                                            struct ieee80211_sta_vht_cap *bcap)
1360 {
1361         const struct ieee80211_vht_cap *vht_cap =
1362                 (const struct ieee80211_vht_cap *)info;
1363
1364         bcap->vht_supported = true;
1365         bcap->cap = le32_to_cpu(vht_cap->vht_cap_info);
1366         memcpy(&bcap->vht_mcs, &vht_cap->supp_mcs, sizeof(bcap->vht_mcs));
1367 }
1368
1369 static int
1370 qtnf_cmd_resp_fill_band_info(struct ieee80211_supported_band *band,
1371                              struct qlink_resp_band_info_get *resp,
1372                              size_t payload_len)
1373 {
1374         u16 tlv_type;
1375         size_t tlv_len;
1376         size_t tlv_dlen;
1377         const struct qlink_tlv_hdr *tlv;
1378         const struct qlink_channel *qchan;
1379         struct ieee80211_channel *chan;
1380         unsigned int chidx = 0;
1381         u32 qflags;
1382
1383         memset(&band->ht_cap, 0, sizeof(band->ht_cap));
1384         memset(&band->vht_cap, 0, sizeof(band->vht_cap));
1385
1386         if (band->channels) {
1387                 if (band->n_channels == resp->num_chans) {
1388                         memset(band->channels, 0,
1389                                sizeof(*band->channels) * band->n_channels);
1390                 } else {
1391                         kfree(band->channels);
1392                         band->n_channels = 0;
1393                         band->channels = NULL;
1394                 }
1395         }
1396
1397         band->n_channels = resp->num_chans;
1398         if (band->n_channels == 0)
1399                 return 0;
1400
1401         if (!band->channels)
1402                 band->channels = kcalloc(band->n_channels, sizeof(*chan),
1403                                          GFP_KERNEL);
1404         if (!band->channels) {
1405                 band->n_channels = 0;
1406                 return -ENOMEM;
1407         }
1408
1409         tlv = (struct qlink_tlv_hdr *)resp->info;
1410
1411         while (payload_len >= sizeof(*tlv)) {
1412                 tlv_type = le16_to_cpu(tlv->type);
1413                 tlv_dlen = le16_to_cpu(tlv->len);
1414                 tlv_len = tlv_dlen + sizeof(*tlv);
1415
1416                 if (tlv_len > payload_len) {
1417                         pr_warn("malformed TLV 0x%.2X; LEN: %zu\n",
1418                                 tlv_type, tlv_len);
1419                         goto error_ret;
1420                 }
1421
1422                 switch (tlv_type) {
1423                 case QTN_TLV_ID_CHANNEL:
1424                         if (unlikely(tlv_dlen != sizeof(*qchan))) {
1425                                 pr_err("invalid channel TLV len %zu\n",
1426                                        tlv_len);
1427                                 goto error_ret;
1428                         }
1429
1430                         if (chidx == band->n_channels) {
1431                                 pr_err("too many channel TLVs\n");
1432                                 goto error_ret;
1433                         }
1434
1435                         qchan = (const struct qlink_channel *)tlv->val;
1436                         chan = &band->channels[chidx++];
1437                         qflags = le32_to_cpu(qchan->flags);
1438
1439                         chan->hw_value = le16_to_cpu(qchan->hw_value);
1440                         chan->band = band->band;
1441                         chan->center_freq = le16_to_cpu(qchan->center_freq);
1442                         chan->max_antenna_gain = (int)qchan->max_antenna_gain;
1443                         chan->max_power = (int)qchan->max_power;
1444                         chan->max_reg_power = (int)qchan->max_reg_power;
1445                         chan->beacon_found = qchan->beacon_found;
1446                         chan->dfs_cac_ms = le32_to_cpu(qchan->dfs_cac_ms);
1447                         chan->flags = 0;
1448
1449                         if (qflags & QLINK_CHAN_DISABLED)
1450                                 chan->flags |= IEEE80211_CHAN_DISABLED;
1451
1452                         if (qflags & QLINK_CHAN_NO_IR)
1453                                 chan->flags |= IEEE80211_CHAN_NO_IR;
1454
1455                         if (qflags & QLINK_CHAN_NO_HT40PLUS)
1456                                 chan->flags |= IEEE80211_CHAN_NO_HT40PLUS;
1457
1458                         if (qflags & QLINK_CHAN_NO_HT40MINUS)
1459                                 chan->flags |= IEEE80211_CHAN_NO_HT40MINUS;
1460
1461                         if (qflags & QLINK_CHAN_NO_OFDM)
1462                                 chan->flags |= IEEE80211_CHAN_NO_OFDM;
1463
1464                         if (qflags & QLINK_CHAN_NO_80MHZ)
1465                                 chan->flags |= IEEE80211_CHAN_NO_80MHZ;
1466
1467                         if (qflags & QLINK_CHAN_NO_160MHZ)
1468                                 chan->flags |= IEEE80211_CHAN_NO_160MHZ;
1469
1470                         if (qflags & QLINK_CHAN_INDOOR_ONLY)
1471                                 chan->flags |= IEEE80211_CHAN_INDOOR_ONLY;
1472
1473                         if (qflags & QLINK_CHAN_IR_CONCURRENT)
1474                                 chan->flags |= IEEE80211_CHAN_IR_CONCURRENT;
1475
1476                         if (qflags & QLINK_CHAN_NO_20MHZ)
1477                                 chan->flags |= IEEE80211_CHAN_NO_20MHZ;
1478
1479                         if (qflags & QLINK_CHAN_NO_10MHZ)
1480                                 chan->flags |= IEEE80211_CHAN_NO_10MHZ;
1481
1482                         if (qflags & QLINK_CHAN_RADAR) {
1483                                 chan->flags |= IEEE80211_CHAN_RADAR;
1484                                 chan->dfs_state_entered = jiffies;
1485
1486                                 if (qchan->dfs_state == QLINK_DFS_USABLE)
1487                                         chan->dfs_state = NL80211_DFS_USABLE;
1488                                 else if (qchan->dfs_state ==
1489                                         QLINK_DFS_AVAILABLE)
1490                                         chan->dfs_state = NL80211_DFS_AVAILABLE;
1491                                 else
1492                                         chan->dfs_state =
1493                                                 NL80211_DFS_UNAVAILABLE;
1494                         }
1495
1496                         pr_debug("chan=%d flags=%#x max_pow=%d max_reg_pow=%d\n",
1497                                  chan->hw_value, chan->flags, chan->max_power,
1498                                  chan->max_reg_power);
1499                         break;
1500                 case WLAN_EID_HT_CAPABILITY:
1501                         if (unlikely(tlv_dlen !=
1502                                      sizeof(struct ieee80211_ht_cap))) {
1503                                 pr_err("bad HTCAP TLV len %zu\n", tlv_dlen);
1504                                 goto error_ret;
1505                         }
1506
1507                         qtnf_cmd_resp_band_fill_htcap(tlv->val, &band->ht_cap);
1508                         break;
1509                 case WLAN_EID_VHT_CAPABILITY:
1510                         if (unlikely(tlv_dlen !=
1511                                      sizeof(struct ieee80211_vht_cap))) {
1512                                 pr_err("bad VHTCAP TLV len %zu\n", tlv_dlen);
1513                                 goto error_ret;
1514                         }
1515
1516                         qtnf_cmd_resp_band_fill_vhtcap(tlv->val,
1517                                                        &band->vht_cap);
1518                         break;
1519                 default:
1520                         pr_warn("unknown TLV type: %#x\n", tlv_type);
1521                         break;
1522                 }
1523
1524                 payload_len -= tlv_len;
1525                 tlv = (struct qlink_tlv_hdr *)(tlv->val + tlv_dlen);
1526         }
1527
1528         if (payload_len) {
1529                 pr_err("malformed TLV buf; bytes left: %zu\n", payload_len);
1530                 goto error_ret;
1531         }
1532
1533         if (band->n_channels != chidx) {
1534                 pr_err("channel count mismatch: reported=%d, parsed=%d\n",
1535                        band->n_channels, chidx);
1536                 goto error_ret;
1537         }
1538
1539         return 0;
1540
1541 error_ret:
1542         kfree(band->channels);
1543         band->channels = NULL;
1544         band->n_channels = 0;
1545
1546         return -EINVAL;
1547 }
1548
1549 static int qtnf_cmd_resp_proc_phy_params(struct qtnf_wmac *mac,
1550                                          const u8 *payload, size_t payload_len)
1551 {
1552         struct qtnf_mac_info *mac_info;
1553         struct qlink_tlv_frag_rts_thr *phy_thr;
1554         struct qlink_tlv_rlimit *limit;
1555         struct qlink_tlv_cclass *class;
1556         u16 tlv_type;
1557         u16 tlv_value_len;
1558         size_t tlv_full_len;
1559         const struct qlink_tlv_hdr *tlv;
1560
1561         mac_info = &mac->macinfo;
1562
1563         tlv = (struct qlink_tlv_hdr *)payload;
1564         while (payload_len >= sizeof(struct qlink_tlv_hdr)) {
1565                 tlv_type = le16_to_cpu(tlv->type);
1566                 tlv_value_len = le16_to_cpu(tlv->len);
1567                 tlv_full_len = tlv_value_len + sizeof(struct qlink_tlv_hdr);
1568
1569                 if (tlv_full_len > payload_len) {
1570                         pr_warn("MAC%u: malformed TLV 0x%.2X; LEN: %u\n",
1571                                 mac->macid, tlv_type, tlv_value_len);
1572                         return -EINVAL;
1573                 }
1574
1575                 switch (tlv_type) {
1576                 case QTN_TLV_ID_FRAG_THRESH:
1577                         phy_thr = (void *)tlv;
1578                         mac_info->frag_thr = (u32)le16_to_cpu(phy_thr->thr);
1579                         break;
1580                 case QTN_TLV_ID_RTS_THRESH:
1581                         phy_thr = (void *)tlv;
1582                         mac_info->rts_thr = (u32)le16_to_cpu(phy_thr->thr);
1583                         break;
1584                 case QTN_TLV_ID_SRETRY_LIMIT:
1585                         limit = (void *)tlv;
1586                         mac_info->sretry_limit = limit->rlimit;
1587                         break;
1588                 case QTN_TLV_ID_LRETRY_LIMIT:
1589                         limit = (void *)tlv;
1590                         mac_info->lretry_limit = limit->rlimit;
1591                         break;
1592                 case QTN_TLV_ID_COVERAGE_CLASS:
1593                         class = (void *)tlv;
1594                         mac_info->coverage_class = class->cclass;
1595                         break;
1596                 default:
1597                         pr_err("MAC%u: Unknown TLV type: %#x\n", mac->macid,
1598                                le16_to_cpu(tlv->type));
1599                         break;
1600                 }
1601
1602                 payload_len -= tlv_full_len;
1603                 tlv = (struct qlink_tlv_hdr *)(tlv->val + tlv_value_len);
1604         }
1605
1606         if (payload_len) {
1607                 pr_warn("MAC%u: malformed TLV buf; bytes left: %zu\n",
1608                         mac->macid, payload_len);
1609                 return -EINVAL;
1610         }
1611
1612         return 0;
1613 }
1614
1615 static int
1616 qtnf_cmd_resp_proc_chan_stat_info(struct qtnf_chan_stats *stats,
1617                                   const u8 *payload, size_t payload_len)
1618 {
1619         struct qlink_chan_stats *qlink_stats;
1620         const struct qlink_tlv_hdr *tlv;
1621         size_t tlv_full_len;
1622         u16 tlv_value_len;
1623         u16 tlv_type;
1624
1625         tlv = (struct qlink_tlv_hdr *)payload;
1626         while (payload_len >= sizeof(struct qlink_tlv_hdr)) {
1627                 tlv_type = le16_to_cpu(tlv->type);
1628                 tlv_value_len = le16_to_cpu(tlv->len);
1629                 tlv_full_len = tlv_value_len + sizeof(struct qlink_tlv_hdr);
1630                 if (tlv_full_len > payload_len) {
1631                         pr_warn("malformed TLV 0x%.2X; LEN: %u\n",
1632                                 tlv_type, tlv_value_len);
1633                         return -EINVAL;
1634                 }
1635                 switch (tlv_type) {
1636                 case QTN_TLV_ID_CHANNEL_STATS:
1637                         if (unlikely(tlv_value_len != sizeof(*qlink_stats))) {
1638                                 pr_err("invalid CHANNEL_STATS entry size\n");
1639                                 return -EINVAL;
1640                         }
1641
1642                         qlink_stats = (void *)tlv->val;
1643
1644                         stats->chan_num = le32_to_cpu(qlink_stats->chan_num);
1645                         stats->cca_tx = le32_to_cpu(qlink_stats->cca_tx);
1646                         stats->cca_rx = le32_to_cpu(qlink_stats->cca_rx);
1647                         stats->cca_busy = le32_to_cpu(qlink_stats->cca_busy);
1648                         stats->cca_try = le32_to_cpu(qlink_stats->cca_try);
1649                         stats->chan_noise = qlink_stats->chan_noise;
1650
1651                         pr_debug("chan(%u) try(%u) busy(%u) noise(%d)\n",
1652                                  stats->chan_num, stats->cca_try,
1653                                  stats->cca_busy, stats->chan_noise);
1654                         break;
1655                 default:
1656                         pr_warn("Unknown TLV type: %#x\n",
1657                                 le16_to_cpu(tlv->type));
1658                 }
1659                 payload_len -= tlv_full_len;
1660                 tlv = (struct qlink_tlv_hdr *)(tlv->val + tlv_value_len);
1661         }
1662
1663         if (payload_len) {
1664                 pr_warn("malformed TLV buf; bytes left: %zu\n", payload_len);
1665                 return -EINVAL;
1666         }
1667
1668         return 0;
1669 }
1670
1671 int qtnf_cmd_get_mac_info(struct qtnf_wmac *mac)
1672 {
1673         struct sk_buff *cmd_skb, *resp_skb = NULL;
1674         const struct qlink_resp_get_mac_info *resp;
1675         size_t var_data_len;
1676         u16 res_code = QLINK_CMD_RESULT_OK;
1677         int ret = 0;
1678
1679         cmd_skb = qtnf_cmd_alloc_new_cmdskb(mac->macid, QLINK_VIFID_RSVD,
1680                                             QLINK_CMD_MAC_INFO,
1681                                             sizeof(struct qlink_cmd));
1682         if (!cmd_skb)
1683                 return -ENOMEM;
1684
1685         qtnf_bus_lock(mac->bus);
1686
1687         ret = qtnf_cmd_send_with_reply(mac->bus, cmd_skb, &resp_skb, &res_code,
1688                                        sizeof(*resp), &var_data_len);
1689         if (unlikely(ret))
1690                 goto out;
1691
1692         if (unlikely(res_code != QLINK_CMD_RESULT_OK)) {
1693                 pr_err("MAC%u: CMD failed: %u\n", mac->macid, res_code);
1694                 ret = -EFAULT;
1695                 goto out;
1696         }
1697
1698         resp = (const struct qlink_resp_get_mac_info *)resp_skb->data;
1699         qtnf_cmd_resp_proc_mac_info(mac, resp);
1700         ret = qtnf_parse_variable_mac_info(mac, resp->var_info, var_data_len);
1701
1702 out:
1703         qtnf_bus_unlock(mac->bus);
1704         consume_skb(resp_skb);
1705
1706         return ret;
1707 }
1708
1709 int qtnf_cmd_get_hw_info(struct qtnf_bus *bus)
1710 {
1711         struct sk_buff *cmd_skb, *resp_skb = NULL;
1712         const struct qlink_resp_get_hw_info *resp;
1713         u16 res_code = QLINK_CMD_RESULT_OK;
1714         int ret = 0;
1715         size_t info_len;
1716
1717         cmd_skb = qtnf_cmd_alloc_new_cmdskb(QLINK_MACID_RSVD, QLINK_VIFID_RSVD,
1718                                             QLINK_CMD_GET_HW_INFO,
1719                                             sizeof(struct qlink_cmd));
1720         if (!cmd_skb)
1721                 return -ENOMEM;
1722
1723         qtnf_bus_lock(bus);
1724
1725         ret = qtnf_cmd_send_with_reply(bus, cmd_skb, &resp_skb, &res_code,
1726                                        sizeof(*resp), &info_len);
1727
1728         if (unlikely(ret))
1729                 goto out;
1730
1731         if (unlikely(res_code != QLINK_CMD_RESULT_OK)) {
1732                 pr_err("cmd exec failed: 0x%.4X\n", res_code);
1733                 ret = -EFAULT;
1734                 goto out;
1735         }
1736
1737         resp = (const struct qlink_resp_get_hw_info *)resp_skb->data;
1738         ret = qtnf_cmd_resp_proc_hw_info(bus, resp, info_len);
1739
1740 out:
1741         qtnf_bus_unlock(bus);
1742         consume_skb(resp_skb);
1743
1744         return ret;
1745 }
1746
1747 int qtnf_cmd_band_info_get(struct qtnf_wmac *mac,
1748                            struct ieee80211_supported_band *band)
1749 {
1750         struct sk_buff *cmd_skb, *resp_skb = NULL;
1751         size_t info_len;
1752         struct qlink_cmd_band_info_get *cmd;
1753         struct qlink_resp_band_info_get *resp;
1754         u16 res_code = QLINK_CMD_RESULT_OK;
1755         int ret = 0;
1756         u8 qband;
1757
1758         switch (band->band) {
1759         case NL80211_BAND_2GHZ:
1760                 qband = QLINK_BAND_2GHZ;
1761                 break;
1762         case NL80211_BAND_5GHZ:
1763                 qband = QLINK_BAND_5GHZ;
1764                 break;
1765         case NL80211_BAND_60GHZ:
1766                 qband = QLINK_BAND_60GHZ;
1767                 break;
1768         default:
1769                 return -EINVAL;
1770         }
1771
1772         cmd_skb = qtnf_cmd_alloc_new_cmdskb(mac->macid, 0,
1773                                             QLINK_CMD_BAND_INFO_GET,
1774                                             sizeof(*cmd));
1775         if (!cmd_skb)
1776                 return -ENOMEM;
1777
1778         cmd = (struct qlink_cmd_band_info_get *)cmd_skb->data;
1779         cmd->band = qband;
1780
1781         qtnf_bus_lock(mac->bus);
1782
1783         ret = qtnf_cmd_send_with_reply(mac->bus, cmd_skb, &resp_skb, &res_code,
1784                                        sizeof(*resp), &info_len);
1785
1786         if (unlikely(ret))
1787                 goto out;
1788
1789         if (unlikely(res_code != QLINK_CMD_RESULT_OK)) {
1790                 pr_err("MAC%u: CMD failed: %u\n", mac->macid, res_code);
1791                 ret = -EFAULT;
1792                 goto out;
1793         }
1794
1795         resp = (struct qlink_resp_band_info_get *)resp_skb->data;
1796         if (resp->band != qband) {
1797                 pr_err("MAC%u: reply band %u != cmd band %u\n", mac->macid,
1798                        resp->band, qband);
1799                 ret = -EINVAL;
1800                 goto out;
1801         }
1802
1803         ret = qtnf_cmd_resp_fill_band_info(band, resp, info_len);
1804
1805 out:
1806         qtnf_bus_unlock(mac->bus);
1807         consume_skb(resp_skb);
1808
1809         return ret;
1810 }
1811
1812 int qtnf_cmd_send_get_phy_params(struct qtnf_wmac *mac)
1813 {
1814         struct sk_buff *cmd_skb, *resp_skb = NULL;
1815         size_t response_size;
1816         struct qlink_resp_phy_params *resp;
1817         u16 res_code = QLINK_CMD_RESULT_OK;
1818         int ret = 0;
1819
1820         cmd_skb = qtnf_cmd_alloc_new_cmdskb(mac->macid, 0,
1821                                             QLINK_CMD_PHY_PARAMS_GET,
1822                                             sizeof(struct qlink_cmd));
1823         if (!cmd_skb)
1824                 return -ENOMEM;
1825
1826         qtnf_bus_lock(mac->bus);
1827
1828         ret = qtnf_cmd_send_with_reply(mac->bus, cmd_skb, &resp_skb, &res_code,
1829                                        sizeof(*resp), &response_size);
1830
1831         if (unlikely(ret))
1832                 goto out;
1833
1834         if (unlikely(res_code != QLINK_CMD_RESULT_OK)) {
1835                 pr_err("MAC%u: CMD failed: %u\n", mac->macid, res_code);
1836                 ret = -EFAULT;
1837                 goto out;
1838         }
1839
1840         resp = (struct qlink_resp_phy_params *)resp_skb->data;
1841         ret = qtnf_cmd_resp_proc_phy_params(mac, resp->info, response_size);
1842
1843 out:
1844         qtnf_bus_unlock(mac->bus);
1845         consume_skb(resp_skb);
1846
1847         return ret;
1848 }
1849
1850 int qtnf_cmd_send_update_phy_params(struct qtnf_wmac *mac, u32 changed)
1851 {
1852         struct wiphy *wiphy = priv_to_wiphy(mac);
1853         struct sk_buff *cmd_skb;
1854         u16 res_code = QLINK_CMD_RESULT_OK;
1855         int ret = 0;
1856
1857         cmd_skb = qtnf_cmd_alloc_new_cmdskb(mac->macid, 0,
1858                                             QLINK_CMD_PHY_PARAMS_SET,
1859                                             sizeof(struct qlink_cmd));
1860         if (!cmd_skb)
1861                 return -ENOMEM;
1862
1863         qtnf_bus_lock(mac->bus);
1864
1865         if (changed & WIPHY_PARAM_FRAG_THRESHOLD)
1866                 qtnf_cmd_skb_put_tlv_u16(cmd_skb, QTN_TLV_ID_FRAG_THRESH,
1867                                          wiphy->frag_threshold);
1868         if (changed & WIPHY_PARAM_RTS_THRESHOLD)
1869                 qtnf_cmd_skb_put_tlv_u16(cmd_skb, QTN_TLV_ID_RTS_THRESH,
1870                                          wiphy->rts_threshold);
1871         if (changed & WIPHY_PARAM_COVERAGE_CLASS)
1872                 qtnf_cmd_skb_put_tlv_u8(cmd_skb, QTN_TLV_ID_COVERAGE_CLASS,
1873                                         wiphy->coverage_class);
1874
1875         ret = qtnf_cmd_send(mac->bus, cmd_skb, &res_code);
1876
1877         if (unlikely(ret))
1878                 goto out;
1879
1880         if (unlikely(res_code != QLINK_CMD_RESULT_OK)) {
1881                 pr_err("MAC%u: CMD failed: %u\n", mac->macid, res_code);
1882                 ret = -EFAULT;
1883                 goto out;
1884         }
1885
1886 out:
1887         qtnf_bus_unlock(mac->bus);
1888         return ret;
1889 }
1890
1891 int qtnf_cmd_send_init_fw(struct qtnf_bus *bus)
1892 {
1893         struct sk_buff *cmd_skb;
1894         u16 res_code = QLINK_CMD_RESULT_OK;
1895         int ret = 0;
1896
1897         cmd_skb = qtnf_cmd_alloc_new_cmdskb(QLINK_MACID_RSVD, QLINK_VIFID_RSVD,
1898                                             QLINK_CMD_FW_INIT,
1899                                             sizeof(struct qlink_cmd));
1900         if (!cmd_skb)
1901                 return -ENOMEM;
1902
1903         qtnf_bus_lock(bus);
1904
1905         ret = qtnf_cmd_send(bus, cmd_skb, &res_code);
1906
1907         if (unlikely(ret))
1908                 goto out;
1909
1910         if (unlikely(res_code != QLINK_CMD_RESULT_OK)) {
1911                 pr_err("cmd exec failed: 0x%.4X\n", res_code);
1912                 ret = -EFAULT;
1913                 goto out;
1914         }
1915
1916 out:
1917         qtnf_bus_unlock(bus);
1918         return ret;
1919 }
1920
1921 void qtnf_cmd_send_deinit_fw(struct qtnf_bus *bus)
1922 {
1923         struct sk_buff *cmd_skb;
1924
1925         cmd_skb = qtnf_cmd_alloc_new_cmdskb(QLINK_MACID_RSVD, QLINK_VIFID_RSVD,
1926                                             QLINK_CMD_FW_DEINIT,
1927                                             sizeof(struct qlink_cmd));
1928         if (!cmd_skb)
1929                 return;
1930
1931         qtnf_bus_lock(bus);
1932
1933         qtnf_cmd_send(bus, cmd_skb, NULL);
1934
1935         qtnf_bus_unlock(bus);
1936 }
1937
1938 int qtnf_cmd_send_add_key(struct qtnf_vif *vif, u8 key_index, bool pairwise,
1939                           const u8 *mac_addr, struct key_params *params)
1940 {
1941         struct sk_buff *cmd_skb;
1942         struct qlink_cmd_add_key *cmd;
1943         u16 res_code = QLINK_CMD_RESULT_OK;
1944         int ret = 0;
1945
1946         cmd_skb = qtnf_cmd_alloc_new_cmdskb(vif->mac->macid, vif->vifid,
1947                                             QLINK_CMD_ADD_KEY,
1948                                             sizeof(*cmd));
1949         if (!cmd_skb)
1950                 return -ENOMEM;
1951
1952         qtnf_bus_lock(vif->mac->bus);
1953
1954         cmd = (struct qlink_cmd_add_key *)cmd_skb->data;
1955
1956         if (mac_addr)
1957                 ether_addr_copy(cmd->addr, mac_addr);
1958         else
1959                 eth_broadcast_addr(cmd->addr);
1960
1961         cmd->cipher = cpu_to_le32(params->cipher);
1962         cmd->key_index = key_index;
1963         cmd->pairwise = pairwise;
1964
1965         if (params->key && params->key_len > 0)
1966                 qtnf_cmd_skb_put_tlv_arr(cmd_skb, QTN_TLV_ID_KEY,
1967                                          params->key,
1968                                          params->key_len);
1969
1970         if (params->seq && params->seq_len > 0)
1971                 qtnf_cmd_skb_put_tlv_arr(cmd_skb, QTN_TLV_ID_SEQ,
1972                                          params->seq,
1973                                          params->seq_len);
1974
1975         ret = qtnf_cmd_send(vif->mac->bus, cmd_skb, &res_code);
1976         if (unlikely(ret))
1977                 goto out;
1978
1979         if (unlikely(res_code != QLINK_CMD_RESULT_OK)) {
1980                 pr_err("VIF%u.%u: CMD failed: %u\n",
1981                        vif->mac->macid, vif->vifid, res_code);
1982                 ret = -EFAULT;
1983                 goto out;
1984         }
1985
1986 out:
1987         qtnf_bus_unlock(vif->mac->bus);
1988         return ret;
1989 }
1990
1991 int qtnf_cmd_send_del_key(struct qtnf_vif *vif, u8 key_index, bool pairwise,
1992                           const u8 *mac_addr)
1993 {
1994         struct sk_buff *cmd_skb;
1995         struct qlink_cmd_del_key *cmd;
1996         u16 res_code = QLINK_CMD_RESULT_OK;
1997         int ret = 0;
1998
1999         cmd_skb = qtnf_cmd_alloc_new_cmdskb(vif->mac->macid, vif->vifid,
2000                                             QLINK_CMD_DEL_KEY,
2001                                             sizeof(*cmd));
2002         if (!cmd_skb)
2003                 return -ENOMEM;
2004
2005         qtnf_bus_lock(vif->mac->bus);
2006
2007         cmd = (struct qlink_cmd_del_key *)cmd_skb->data;
2008
2009         if (mac_addr)
2010                 ether_addr_copy(cmd->addr, mac_addr);
2011         else
2012                 eth_broadcast_addr(cmd->addr);
2013
2014         cmd->key_index = key_index;
2015         cmd->pairwise = pairwise;
2016         ret = qtnf_cmd_send(vif->mac->bus, cmd_skb, &res_code);
2017         if (unlikely(ret))
2018                 goto out;
2019
2020         if (unlikely(res_code != QLINK_CMD_RESULT_OK)) {
2021                 pr_err("VIF%u.%u: CMD failed: %u\n",
2022                        vif->mac->macid, vif->vifid, res_code);
2023                 ret = -EFAULT;
2024                 goto out;
2025         }
2026
2027 out:
2028         qtnf_bus_unlock(vif->mac->bus);
2029         return ret;
2030 }
2031
2032 int qtnf_cmd_send_set_default_key(struct qtnf_vif *vif, u8 key_index,
2033                                   bool unicast, bool multicast)
2034 {
2035         struct sk_buff *cmd_skb;
2036         struct qlink_cmd_set_def_key *cmd;
2037         u16 res_code = QLINK_CMD_RESULT_OK;
2038         int ret = 0;
2039
2040         cmd_skb = qtnf_cmd_alloc_new_cmdskb(vif->mac->macid, vif->vifid,
2041                                             QLINK_CMD_SET_DEFAULT_KEY,
2042                                             sizeof(*cmd));
2043         if (!cmd_skb)
2044                 return -ENOMEM;
2045
2046         qtnf_bus_lock(vif->mac->bus);
2047
2048         cmd = (struct qlink_cmd_set_def_key *)cmd_skb->data;
2049         cmd->key_index = key_index;
2050         cmd->unicast = unicast;
2051         cmd->multicast = multicast;
2052         ret = qtnf_cmd_send(vif->mac->bus, cmd_skb, &res_code);
2053         if (unlikely(ret))
2054                 goto out;
2055
2056         if (unlikely(res_code != QLINK_CMD_RESULT_OK)) {
2057                 pr_err("VIF%u.%u: CMD failed: %u\n", vif->mac->macid,
2058                        vif->vifid, res_code);
2059                 ret = -EFAULT;
2060                 goto out;
2061         }
2062
2063 out:
2064         qtnf_bus_unlock(vif->mac->bus);
2065         return ret;
2066 }
2067
2068 int qtnf_cmd_send_set_default_mgmt_key(struct qtnf_vif *vif, u8 key_index)
2069 {
2070         struct sk_buff *cmd_skb;
2071         struct qlink_cmd_set_def_mgmt_key *cmd;
2072         u16 res_code = QLINK_CMD_RESULT_OK;
2073         int ret = 0;
2074
2075         cmd_skb = qtnf_cmd_alloc_new_cmdskb(vif->mac->macid, vif->vifid,
2076                                             QLINK_CMD_SET_DEFAULT_MGMT_KEY,
2077                                             sizeof(*cmd));
2078         if (!cmd_skb)
2079                 return -ENOMEM;
2080
2081         qtnf_bus_lock(vif->mac->bus);
2082
2083         cmd = (struct qlink_cmd_set_def_mgmt_key *)cmd_skb->data;
2084         cmd->key_index = key_index;
2085         ret = qtnf_cmd_send(vif->mac->bus, cmd_skb, &res_code);
2086         if (unlikely(ret))
2087                 goto out;
2088
2089         if (unlikely(res_code != QLINK_CMD_RESULT_OK)) {
2090                 pr_err("VIF%u.%u: CMD failed: %u\n", vif->mac->macid,
2091                        vif->vifid, res_code);
2092                 ret = -EFAULT;
2093                 goto out;
2094         }
2095
2096 out:
2097         qtnf_bus_unlock(vif->mac->bus);
2098         return ret;
2099 }
2100
2101 static u32 qtnf_encode_sta_flags(u32 flags)
2102 {
2103         u32 code = 0;
2104
2105         if (flags & BIT(NL80211_STA_FLAG_AUTHORIZED))
2106                 code |= QLINK_STA_FLAG_AUTHORIZED;
2107         if (flags & BIT(NL80211_STA_FLAG_SHORT_PREAMBLE))
2108                 code |= QLINK_STA_FLAG_SHORT_PREAMBLE;
2109         if (flags & BIT(NL80211_STA_FLAG_WME))
2110                 code |= QLINK_STA_FLAG_WME;
2111         if (flags & BIT(NL80211_STA_FLAG_MFP))
2112                 code |= QLINK_STA_FLAG_MFP;
2113         if (flags & BIT(NL80211_STA_FLAG_AUTHENTICATED))
2114                 code |= QLINK_STA_FLAG_AUTHENTICATED;
2115         if (flags & BIT(NL80211_STA_FLAG_TDLS_PEER))
2116                 code |= QLINK_STA_FLAG_TDLS_PEER;
2117         if (flags & BIT(NL80211_STA_FLAG_ASSOCIATED))
2118                 code |= QLINK_STA_FLAG_ASSOCIATED;
2119         return code;
2120 }
2121
2122 int qtnf_cmd_send_change_sta(struct qtnf_vif *vif, const u8 *mac,
2123                              struct station_parameters *params)
2124 {
2125         struct sk_buff *cmd_skb;
2126         struct qlink_cmd_change_sta *cmd;
2127         u16 res_code = QLINK_CMD_RESULT_OK;
2128         int ret = 0;
2129
2130         cmd_skb = qtnf_cmd_alloc_new_cmdskb(vif->mac->macid, vif->vifid,
2131                                             QLINK_CMD_CHANGE_STA,
2132                                             sizeof(*cmd));
2133         if (!cmd_skb)
2134                 return -ENOMEM;
2135
2136         qtnf_bus_lock(vif->mac->bus);
2137
2138         cmd = (struct qlink_cmd_change_sta *)cmd_skb->data;
2139         ether_addr_copy(cmd->sta_addr, mac);
2140         cmd->flag_update.mask =
2141                 cpu_to_le32(qtnf_encode_sta_flags(params->sta_flags_mask));
2142         cmd->flag_update.value =
2143                 cpu_to_le32(qtnf_encode_sta_flags(params->sta_flags_set));
2144
2145         switch (vif->wdev.iftype) {
2146         case NL80211_IFTYPE_AP:
2147                 cmd->if_type = cpu_to_le16(QLINK_IFTYPE_AP);
2148                 break;
2149         case NL80211_IFTYPE_STATION:
2150                 cmd->if_type = cpu_to_le16(QLINK_IFTYPE_STATION);
2151                 break;
2152         default:
2153                 pr_err("unsupported iftype %d\n", vif->wdev.iftype);
2154                 ret = -EINVAL;
2155                 goto out;
2156         }
2157
2158         ret = qtnf_cmd_send(vif->mac->bus, cmd_skb, &res_code);
2159         if (unlikely(ret))
2160                 goto out;
2161
2162         if (unlikely(res_code != QLINK_CMD_RESULT_OK)) {
2163                 pr_err("VIF%u.%u: CMD failed: %u\n", vif->mac->macid,
2164                        vif->vifid, res_code);
2165                 ret = -EFAULT;
2166                 goto out;
2167         }
2168
2169 out:
2170         qtnf_bus_unlock(vif->mac->bus);
2171         return ret;
2172 }
2173
2174 int qtnf_cmd_send_del_sta(struct qtnf_vif *vif,
2175                           struct station_del_parameters *params)
2176 {
2177         struct sk_buff *cmd_skb;
2178         struct qlink_cmd_del_sta *cmd;
2179         u16 res_code = QLINK_CMD_RESULT_OK;
2180         int ret = 0;
2181
2182         cmd_skb = qtnf_cmd_alloc_new_cmdskb(vif->mac->macid, vif->vifid,
2183                                             QLINK_CMD_DEL_STA,
2184                                             sizeof(*cmd));
2185         if (!cmd_skb)
2186                 return -ENOMEM;
2187
2188         qtnf_bus_lock(vif->mac->bus);
2189
2190         cmd = (struct qlink_cmd_del_sta *)cmd_skb->data;
2191
2192         if (params->mac)
2193                 ether_addr_copy(cmd->sta_addr, params->mac);
2194         else
2195                 eth_broadcast_addr(cmd->sta_addr);      /* flush all stations */
2196
2197         cmd->subtype = params->subtype;
2198         cmd->reason_code = cpu_to_le16(params->reason_code);
2199
2200         ret = qtnf_cmd_send(vif->mac->bus, cmd_skb, &res_code);
2201         if (unlikely(ret))
2202                 goto out;
2203
2204         if (unlikely(res_code != QLINK_CMD_RESULT_OK)) {
2205                 pr_err("VIF%u.%u: CMD failed: %u\n", vif->mac->macid,
2206                        vif->vifid, res_code);
2207                 ret = -EFAULT;
2208                 goto out;
2209         }
2210
2211 out:
2212         qtnf_bus_unlock(vif->mac->bus);
2213         return ret;
2214 }
2215
2216 static void qtnf_cmd_channel_tlv_add(struct sk_buff *cmd_skb,
2217                                      const struct ieee80211_channel *sc)
2218 {
2219         struct qlink_tlv_channel *qchan;
2220         u32 flags = 0;
2221
2222         qchan = skb_put_zero(cmd_skb, sizeof(*qchan));
2223         qchan->hdr.type = cpu_to_le16(QTN_TLV_ID_CHANNEL);
2224         qchan->hdr.len = cpu_to_le16(sizeof(*qchan) - sizeof(qchan->hdr));
2225         qchan->chan.center_freq = cpu_to_le16(sc->center_freq);
2226         qchan->chan.hw_value = cpu_to_le16(sc->hw_value);
2227
2228         if (sc->flags & IEEE80211_CHAN_NO_IR)
2229                 flags |= QLINK_CHAN_NO_IR;
2230
2231         if (sc->flags & IEEE80211_CHAN_RADAR)
2232                 flags |= QLINK_CHAN_RADAR;
2233
2234         qchan->chan.flags = cpu_to_le32(flags);
2235 }
2236
2237 int qtnf_cmd_send_scan(struct qtnf_wmac *mac)
2238 {
2239         struct sk_buff *cmd_skb;
2240         u16 res_code = QLINK_CMD_RESULT_OK;
2241         struct ieee80211_channel *sc;
2242         struct cfg80211_scan_request *scan_req = mac->scan_req;
2243         int n_channels;
2244         int count = 0;
2245         int ret;
2246
2247         if (scan_req->n_ssids > QTNF_MAX_SSID_LIST_LENGTH) {
2248                 pr_err("MAC%u: too many SSIDs in scan request\n", mac->macid);
2249                 return -EINVAL;
2250         }
2251
2252         cmd_skb = qtnf_cmd_alloc_new_cmdskb(mac->macid, QLINK_VIFID_RSVD,
2253                                             QLINK_CMD_SCAN,
2254                                             sizeof(struct qlink_cmd));
2255         if (!cmd_skb)
2256                 return -ENOMEM;
2257
2258         qtnf_bus_lock(mac->bus);
2259
2260         if (scan_req->n_ssids != 0) {
2261                 while (count < scan_req->n_ssids) {
2262                         qtnf_cmd_skb_put_tlv_arr(cmd_skb, WLAN_EID_SSID,
2263                                 scan_req->ssids[count].ssid,
2264                                 scan_req->ssids[count].ssid_len);
2265                         count++;
2266                 }
2267         }
2268
2269         if (scan_req->ie_len != 0)
2270                 qtnf_cmd_tlv_ie_set_add(cmd_skb, QLINK_IE_SET_PROBE_REQ,
2271                                         scan_req->ie, scan_req->ie_len);
2272
2273         if (scan_req->n_channels) {
2274                 n_channels = scan_req->n_channels;
2275                 count = 0;
2276
2277                 while (n_channels != 0) {
2278                         sc = scan_req->channels[count];
2279                         if (sc->flags & IEEE80211_CHAN_DISABLED) {
2280                                 n_channels--;
2281                                 continue;
2282                         }
2283
2284                         pr_debug("MAC%u: scan chan=%d, freq=%d, flags=%#x\n",
2285                                  mac->macid, sc->hw_value, sc->center_freq,
2286                                  sc->flags);
2287
2288                         qtnf_cmd_channel_tlv_add(cmd_skb, sc);
2289                         n_channels--;
2290                         count++;
2291                 }
2292         }
2293
2294         ret = qtnf_cmd_send(mac->bus, cmd_skb, &res_code);
2295
2296         if (unlikely(ret))
2297                 goto out;
2298
2299         pr_debug("MAC%u: scan started\n", mac->macid);
2300
2301         if (unlikely(res_code != QLINK_CMD_RESULT_OK)) {
2302                 pr_err("MAC%u: CMD failed: %u\n", mac->macid, res_code);
2303                 ret = -EFAULT;
2304                 goto out;
2305         }
2306 out:
2307         qtnf_bus_unlock(mac->bus);
2308         return ret;
2309 }
2310
2311 int qtnf_cmd_send_connect(struct qtnf_vif *vif,
2312                           struct cfg80211_connect_params *sme)
2313 {
2314         struct sk_buff *cmd_skb;
2315         struct qlink_cmd_connect *cmd;
2316         struct qlink_auth_encr *aen;
2317         u16 res_code = QLINK_CMD_RESULT_OK;
2318         int ret;
2319         int i;
2320         u32 connect_flags = 0;
2321
2322         cmd_skb = qtnf_cmd_alloc_new_cmdskb(vif->mac->macid, vif->vifid,
2323                                             QLINK_CMD_CONNECT,
2324                                             sizeof(*cmd));
2325         if (!cmd_skb)
2326                 return -ENOMEM;
2327
2328         cmd = (struct qlink_cmd_connect *)cmd_skb->data;
2329
2330         ether_addr_copy(cmd->bssid, vif->bssid);
2331
2332         if (sme->bssid_hint)
2333                 ether_addr_copy(cmd->bssid_hint, sme->bssid_hint);
2334         else
2335                 eth_zero_addr(cmd->bssid_hint);
2336
2337         if (sme->prev_bssid)
2338                 ether_addr_copy(cmd->prev_bssid, sme->prev_bssid);
2339         else
2340                 eth_zero_addr(cmd->prev_bssid);
2341
2342         if ((sme->bg_scan_period >= 0) &&
2343             (sme->bg_scan_period <= SHRT_MAX))
2344                 cmd->bg_scan_period = cpu_to_le16(sme->bg_scan_period);
2345         else
2346                 cmd->bg_scan_period = cpu_to_le16(-1); /* use default value */
2347
2348         if (sme->flags & ASSOC_REQ_DISABLE_HT)
2349                 connect_flags |= QLINK_STA_CONNECT_DISABLE_HT;
2350         if (sme->flags & ASSOC_REQ_DISABLE_VHT)
2351                 connect_flags |= QLINK_STA_CONNECT_DISABLE_VHT;
2352         if (sme->flags & ASSOC_REQ_USE_RRM)
2353                 connect_flags |= QLINK_STA_CONNECT_USE_RRM;
2354
2355         cmd->flags = cpu_to_le32(connect_flags);
2356         memcpy(&cmd->ht_capa, &sme->ht_capa, sizeof(cmd->ht_capa));
2357         memcpy(&cmd->ht_capa_mask, &sme->ht_capa_mask,
2358                sizeof(cmd->ht_capa_mask));
2359         memcpy(&cmd->vht_capa, &sme->vht_capa, sizeof(cmd->vht_capa));
2360         memcpy(&cmd->vht_capa_mask, &sme->vht_capa_mask,
2361                sizeof(cmd->vht_capa_mask));
2362         cmd->pbss = sme->pbss;
2363
2364         aen = &cmd->aen;
2365         aen->auth_type = sme->auth_type;
2366         aen->privacy = !!sme->privacy;
2367         cmd->mfp = sme->mfp;
2368         aen->wpa_versions = cpu_to_le32(sme->crypto.wpa_versions);
2369         aen->cipher_group = cpu_to_le32(sme->crypto.cipher_group);
2370         aen->n_ciphers_pairwise = cpu_to_le32(sme->crypto.n_ciphers_pairwise);
2371
2372         for (i = 0; i < QLINK_MAX_NR_CIPHER_SUITES; i++)
2373                 aen->ciphers_pairwise[i] =
2374                         cpu_to_le32(sme->crypto.ciphers_pairwise[i]);
2375
2376         aen->n_akm_suites = cpu_to_le32(sme->crypto.n_akm_suites);
2377
2378         for (i = 0; i < QLINK_MAX_NR_AKM_SUITES; i++)
2379                 aen->akm_suites[i] = cpu_to_le32(sme->crypto.akm_suites[i]);
2380
2381         aen->control_port = sme->crypto.control_port;
2382         aen->control_port_no_encrypt =
2383                 sme->crypto.control_port_no_encrypt;
2384         aen->control_port_ethertype =
2385                 cpu_to_le16(be16_to_cpu(sme->crypto.control_port_ethertype));
2386
2387         qtnf_cmd_skb_put_tlv_arr(cmd_skb, WLAN_EID_SSID, sme->ssid,
2388                                  sme->ssid_len);
2389
2390         if (sme->ie_len != 0)
2391                 qtnf_cmd_tlv_ie_set_add(cmd_skb, QLINK_IE_SET_ASSOC_REQ,
2392                                         sme->ie, sme->ie_len);
2393
2394         if (sme->channel)
2395                 qtnf_cmd_channel_tlv_add(cmd_skb, sme->channel);
2396
2397         qtnf_bus_lock(vif->mac->bus);
2398
2399         ret = qtnf_cmd_send(vif->mac->bus, cmd_skb, &res_code);
2400
2401         if (unlikely(ret))
2402                 goto out;
2403
2404         if (unlikely(res_code != QLINK_CMD_RESULT_OK)) {
2405                 pr_err("VIF%u.%u: CMD failed: %u\n", vif->mac->macid,
2406                        vif->vifid, res_code);
2407                 ret = -EFAULT;
2408                 goto out;
2409         }
2410 out:
2411         qtnf_bus_unlock(vif->mac->bus);
2412         return ret;
2413 }
2414
2415 int qtnf_cmd_send_disconnect(struct qtnf_vif *vif, u16 reason_code)
2416 {
2417         struct sk_buff *cmd_skb;
2418         struct qlink_cmd_disconnect *cmd;
2419         u16 res_code = QLINK_CMD_RESULT_OK;
2420         int ret;
2421
2422         cmd_skb = qtnf_cmd_alloc_new_cmdskb(vif->mac->macid, vif->vifid,
2423                                             QLINK_CMD_DISCONNECT,
2424                                             sizeof(*cmd));
2425         if (!cmd_skb)
2426                 return -ENOMEM;
2427
2428         qtnf_bus_lock(vif->mac->bus);
2429
2430         cmd = (struct qlink_cmd_disconnect *)cmd_skb->data;
2431         cmd->reason = cpu_to_le16(reason_code);
2432
2433         ret = qtnf_cmd_send(vif->mac->bus, cmd_skb, &res_code);
2434
2435         if (unlikely(ret))
2436                 goto out;
2437
2438         if (unlikely(res_code != QLINK_CMD_RESULT_OK)) {
2439                 pr_err("VIF%u.%u: CMD failed: %u\n", vif->mac->macid,
2440                        vif->vifid, res_code);
2441                 ret = -EFAULT;
2442                 goto out;
2443         }
2444 out:
2445         qtnf_bus_unlock(vif->mac->bus);
2446         return ret;
2447 }
2448
2449 int qtnf_cmd_send_updown_intf(struct qtnf_vif *vif, bool up)
2450 {
2451         struct sk_buff *cmd_skb;
2452         struct qlink_cmd_updown *cmd;
2453         u16 res_code = QLINK_CMD_RESULT_OK;
2454         int ret;
2455
2456         cmd_skb = qtnf_cmd_alloc_new_cmdskb(vif->mac->macid, vif->vifid,
2457                                             QLINK_CMD_UPDOWN_INTF,
2458                                             sizeof(*cmd));
2459         if (!cmd_skb)
2460                 return -ENOMEM;
2461
2462         cmd = (struct qlink_cmd_updown *)cmd_skb->data;
2463         cmd->if_up = !!up;
2464
2465         qtnf_bus_lock(vif->mac->bus);
2466
2467         ret = qtnf_cmd_send(vif->mac->bus, cmd_skb, &res_code);
2468
2469         if (unlikely(ret))
2470                 goto out;
2471
2472         if (unlikely(res_code != QLINK_CMD_RESULT_OK)) {
2473                 pr_err("VIF%u.%u: CMD failed: %u\n", vif->mac->macid,
2474                        vif->vifid, res_code);
2475                 ret = -EFAULT;
2476                 goto out;
2477         }
2478 out:
2479         qtnf_bus_unlock(vif->mac->bus);
2480         return ret;
2481 }
2482
2483 int qtnf_cmd_reg_notify(struct qtnf_bus *bus, struct regulatory_request *req)
2484 {
2485         struct sk_buff *cmd_skb;
2486         int ret;
2487         u16 res_code;
2488         struct qlink_cmd_reg_notify *cmd;
2489
2490         cmd_skb = qtnf_cmd_alloc_new_cmdskb(QLINK_MACID_RSVD, QLINK_VIFID_RSVD,
2491                                             QLINK_CMD_REG_NOTIFY,
2492                                             sizeof(*cmd));
2493         if (!cmd_skb)
2494                 return -ENOMEM;
2495
2496         cmd = (struct qlink_cmd_reg_notify *)cmd_skb->data;
2497         cmd->alpha2[0] = req->alpha2[0];
2498         cmd->alpha2[1] = req->alpha2[1];
2499
2500         switch (req->initiator) {
2501         case NL80211_REGDOM_SET_BY_CORE:
2502                 cmd->initiator = QLINK_REGDOM_SET_BY_CORE;
2503                 break;
2504         case NL80211_REGDOM_SET_BY_USER:
2505                 cmd->initiator = QLINK_REGDOM_SET_BY_USER;
2506                 break;
2507         case NL80211_REGDOM_SET_BY_DRIVER:
2508                 cmd->initiator = QLINK_REGDOM_SET_BY_DRIVER;
2509                 break;
2510         case NL80211_REGDOM_SET_BY_COUNTRY_IE:
2511                 cmd->initiator = QLINK_REGDOM_SET_BY_COUNTRY_IE;
2512                 break;
2513         }
2514
2515         switch (req->user_reg_hint_type) {
2516         case NL80211_USER_REG_HINT_USER:
2517                 cmd->user_reg_hint_type = QLINK_USER_REG_HINT_USER;
2518                 break;
2519         case NL80211_USER_REG_HINT_CELL_BASE:
2520                 cmd->user_reg_hint_type = QLINK_USER_REG_HINT_CELL_BASE;
2521                 break;
2522         case NL80211_USER_REG_HINT_INDOOR:
2523                 cmd->user_reg_hint_type = QLINK_USER_REG_HINT_INDOOR;
2524                 break;
2525         }
2526
2527         qtnf_bus_lock(bus);
2528
2529         ret = qtnf_cmd_send(bus, cmd_skb, &res_code);
2530         if (ret)
2531                 goto out;
2532
2533         switch (res_code) {
2534         case QLINK_CMD_RESULT_ENOTSUPP:
2535                 pr_warn("reg update not supported\n");
2536                 ret = -EOPNOTSUPP;
2537                 break;
2538         case QLINK_CMD_RESULT_EALREADY:
2539                 pr_info("regulatory domain is already set to %c%c",
2540                         req->alpha2[0], req->alpha2[1]);
2541                 ret = -EALREADY;
2542                 break;
2543         case QLINK_CMD_RESULT_OK:
2544                 ret = 0;
2545                 break;
2546         default:
2547                 ret = -EFAULT;
2548                 break;
2549         }
2550
2551 out:
2552         qtnf_bus_unlock(bus);
2553
2554         return ret;
2555 }
2556
2557 int qtnf_cmd_get_chan_stats(struct qtnf_wmac *mac, u16 channel,
2558                             struct qtnf_chan_stats *stats)
2559 {
2560         struct sk_buff *cmd_skb, *resp_skb = NULL;
2561         struct qlink_cmd_get_chan_stats *cmd;
2562         struct qlink_resp_get_chan_stats *resp;
2563         size_t var_data_len;
2564         u16 res_code = QLINK_CMD_RESULT_OK;
2565         int ret = 0;
2566
2567         cmd_skb = qtnf_cmd_alloc_new_cmdskb(mac->macid, QLINK_VIFID_RSVD,
2568                                             QLINK_CMD_CHAN_STATS,
2569                                             sizeof(*cmd));
2570         if (!cmd_skb)
2571                 return -ENOMEM;
2572
2573         qtnf_bus_lock(mac->bus);
2574
2575         cmd = (struct qlink_cmd_get_chan_stats *)cmd_skb->data;
2576         cmd->channel = cpu_to_le16(channel);
2577
2578         ret = qtnf_cmd_send_with_reply(mac->bus, cmd_skb, &resp_skb, &res_code,
2579                                        sizeof(*resp), &var_data_len);
2580         if (unlikely(ret)) {
2581                 qtnf_bus_unlock(mac->bus);
2582                 return ret;
2583         }
2584
2585         if (unlikely(res_code != QLINK_CMD_RESULT_OK)) {
2586                 switch (res_code) {
2587                 case QLINK_CMD_RESULT_ENOTFOUND:
2588                         ret = -ENOENT;
2589                         break;
2590                 default:
2591                         pr_err("cmd exec failed: 0x%.4X\n", res_code);
2592                         ret = -EFAULT;
2593                         break;
2594                 }
2595                 goto out;
2596         }
2597
2598         resp = (struct qlink_resp_get_chan_stats *)resp_skb->data;
2599         ret = qtnf_cmd_resp_proc_chan_stat_info(stats, resp->info,
2600                                                 var_data_len);
2601
2602 out:
2603         qtnf_bus_unlock(mac->bus);
2604         consume_skb(resp_skb);
2605         return ret;
2606 }
2607
2608 int qtnf_cmd_send_chan_switch(struct qtnf_vif *vif,
2609                               struct cfg80211_csa_settings *params)
2610 {
2611         struct qtnf_wmac *mac = vif->mac;
2612         struct qlink_cmd_chan_switch *cmd;
2613         struct sk_buff *cmd_skb;
2614         u16 res_code = QLINK_CMD_RESULT_OK;
2615         int ret;
2616
2617         cmd_skb = qtnf_cmd_alloc_new_cmdskb(mac->macid, vif->vifid,
2618                                             QLINK_CMD_CHAN_SWITCH,
2619                                             sizeof(*cmd));
2620         if (!cmd_skb)
2621                 return -ENOMEM;
2622
2623         qtnf_bus_lock(mac->bus);
2624
2625         cmd = (struct qlink_cmd_chan_switch *)cmd_skb->data;
2626         cmd->channel = cpu_to_le16(params->chandef.chan->hw_value);
2627         cmd->radar_required = params->radar_required;
2628         cmd->block_tx = params->block_tx;
2629         cmd->beacon_count = params->count;
2630
2631         ret = qtnf_cmd_send(mac->bus, cmd_skb, &res_code);
2632
2633         if (unlikely(ret))
2634                 goto out;
2635
2636         switch (res_code) {
2637         case QLINK_CMD_RESULT_OK:
2638                 ret = 0;
2639                 break;
2640         case QLINK_CMD_RESULT_ENOTFOUND:
2641                 ret = -ENOENT;
2642                 break;
2643         case QLINK_CMD_RESULT_ENOTSUPP:
2644                 ret = -EOPNOTSUPP;
2645                 break;
2646         case QLINK_CMD_RESULT_EALREADY:
2647                 ret = -EALREADY;
2648                 break;
2649         case QLINK_CMD_RESULT_INVALID:
2650         default:
2651                 ret = -EFAULT;
2652                 break;
2653         }
2654
2655 out:
2656         qtnf_bus_unlock(mac->bus);
2657         return ret;
2658 }
2659
2660 int qtnf_cmd_get_channel(struct qtnf_vif *vif, struct cfg80211_chan_def *chdef)
2661 {
2662         struct qtnf_bus *bus = vif->mac->bus;
2663         const struct qlink_resp_channel_get *resp;
2664         struct sk_buff *cmd_skb;
2665         struct sk_buff *resp_skb = NULL;
2666         u16 res_code = QLINK_CMD_RESULT_OK;
2667         int ret;
2668
2669         cmd_skb = qtnf_cmd_alloc_new_cmdskb(vif->mac->macid, vif->vifid,
2670                                             QLINK_CMD_CHAN_GET,
2671                                             sizeof(struct qlink_cmd));
2672         if (!cmd_skb)
2673                 return -ENOMEM;
2674
2675         qtnf_bus_lock(bus);
2676
2677         ret = qtnf_cmd_send_with_reply(bus, cmd_skb, &resp_skb, &res_code,
2678                                        sizeof(*resp), NULL);
2679
2680         qtnf_bus_unlock(bus);
2681
2682         if (unlikely(ret))
2683                 goto out;
2684
2685         if (unlikely(res_code != QLINK_CMD_RESULT_OK)) {
2686                 ret = -ENODATA;
2687                 goto out;
2688         }
2689
2690         resp = (const struct qlink_resp_channel_get *)resp_skb->data;
2691         qlink_chandef_q2cfg(priv_to_wiphy(vif->mac), &resp->chan, chdef);
2692
2693 out:
2694         consume_skb(resp_skb);
2695         return ret;
2696 }
2697
2698 int qtnf_cmd_start_cac(const struct qtnf_vif *vif,
2699                        const struct cfg80211_chan_def *chdef,
2700                        u32 cac_time_ms)
2701 {
2702         struct qtnf_bus *bus = vif->mac->bus;
2703         struct sk_buff *cmd_skb;
2704         struct qlink_cmd_start_cac *cmd;
2705         int ret;
2706         u16 res_code;
2707
2708         cmd_skb = qtnf_cmd_alloc_new_cmdskb(vif->mac->macid, vif->vifid,
2709                                             QLINK_CMD_START_CAC,
2710                                             sizeof(*cmd));
2711         if (!cmd_skb)
2712                 return -ENOMEM;
2713
2714         cmd = (struct qlink_cmd_start_cac *)cmd_skb->data;
2715         cmd->cac_time_ms = cpu_to_le32(cac_time_ms);
2716         qlink_chandef_cfg2q(chdef, &cmd->chan);
2717
2718         qtnf_bus_lock(bus);
2719         ret = qtnf_cmd_send(bus, cmd_skb, &res_code);
2720         qtnf_bus_unlock(bus);
2721
2722         if (ret)
2723                 return ret;
2724
2725         switch (res_code) {
2726         case QLINK_CMD_RESULT_OK:
2727                 break;
2728         default:
2729                 ret = -EOPNOTSUPP;
2730                 break;
2731         }
2732
2733         return ret;
2734 }
2735
2736 int qtnf_cmd_set_mac_acl(const struct qtnf_vif *vif,
2737                          const struct cfg80211_acl_data *params)
2738 {
2739         struct qtnf_bus *bus = vif->mac->bus;
2740         struct sk_buff *cmd_skb;
2741         struct qlink_tlv_hdr *tlv;
2742         size_t acl_size = qtnf_cmd_acl_data_size(params);
2743         u16 res_code;
2744         int ret;
2745
2746         cmd_skb = qtnf_cmd_alloc_new_cmdskb(vif->mac->macid, vif->vifid,
2747                                             QLINK_CMD_SET_MAC_ACL,
2748                                             sizeof(struct qlink_cmd));
2749         if (!cmd_skb)
2750                 return -ENOMEM;
2751
2752         tlv = skb_put(cmd_skb, sizeof(*tlv) + acl_size);
2753         tlv->type = cpu_to_le16(QTN_TLV_ID_ACL_DATA);
2754         tlv->len = cpu_to_le16(acl_size);
2755         qlink_acl_data_cfg2q(params, (struct qlink_acl_data *)tlv->val);
2756
2757         qtnf_bus_lock(bus);
2758         ret = qtnf_cmd_send(bus, cmd_skb, &res_code);
2759         qtnf_bus_unlock(bus);
2760
2761         if (unlikely(ret))
2762                 return ret;
2763
2764         switch (res_code) {
2765         case QLINK_CMD_RESULT_OK:
2766                 break;
2767         case QLINK_CMD_RESULT_INVALID:
2768                 ret = -EINVAL;
2769                 break;
2770         default:
2771                 ret = -EOPNOTSUPP;
2772                 break;
2773         }
2774
2775         return ret;
2776 }