drivers/net/wireless/intel/iwlwifi/pcie/tx-gen2.c

   1 /******************************************************************************
   2  *
   3  * This file is provided under a dual BSD/GPLv2 license.  When using or
   4  * redistributing this file, you may do so under either license.
   5  *
   6  * GPL LICENSE SUMMARY
   7  *
   8  * Copyright(c) 2017 Intel Deutschland GmbH
   9  * Copyright(c) 2018 - 2019 Intel Corporation
  10  *
  11  * This program is free software; you can redistribute it and/or modify
  12  * it under the terms of version 2 of the GNU General Public License as
  13  * published by the Free Software Foundation.
  14  *
  15  * This program is distributed in the hope that it will be useful, but
  16  * WITHOUT ANY WARRANTY; without even the implied warranty of
  17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  18  * General Public License for more details.
  19  *
  20  * BSD LICENSE
  21  *
  22  * Copyright(c) 2017 Intel Deutschland GmbH
  23  * Copyright(c) 2018 - 2019 Intel Corporation
  24  * All rights reserved.
  25  *
  26  * Redistribution and use in source and binary forms, with or without
  27  * modification, are permitted provided that the following conditions
  28  * are met:
  29  *
  30  *  * Redistributions of source code must retain the above copyright
  31  *    notice, this list of conditions and the following disclaimer.
  32  *  * Redistributions in binary form must reproduce the above copyright
  33  *    notice, this list of conditions and the following disclaimer in
  34  *    the documentation and/or other materials provided with the
  35  *    distribution.
  36  *  * Neither the name Intel Corporation nor the names of its
  37  *    contributors may be used to endorse or promote products derived
  38  *    from this software without specific prior written permission.
  39  *
  40  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  41  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
  42  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
  43  * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
  44  * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
  45  * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
  46  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
  47  * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
  48  * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
  49  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
  50  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  51  *
  52  *****************************************************************************/
  53 #include <net/tso.h>
  54 #include <linux/tcp.h>
  55
  56 #include "iwl-debug.h"
  57 #include "iwl-csr.h"
  58 #include "iwl-io.h"
  59 #include "internal.h"
  60 #include "fw/api/tx.h"
  61
  62  /*
  63  * iwl_pcie_gen2_tx_stop - Stop all Tx DMA channels
  64  */
  65 void iwl_pcie_gen2_tx_stop(struct iwl_trans *trans)
  66 {
  67         struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
  68         int txq_id;
  69
  70         /*
  71          * This function can be called before the op_mode disabled the
  72          * queues. This happens when we have an rfkill interrupt.
  73          * Since we stop Tx altogether - mark the queues as stopped.
  74          */
  75         memset(trans_pcie->queue_stopped, 0, sizeof(trans_pcie->queue_stopped));
  76         memset(trans_pcie->queue_used, 0, sizeof(trans_pcie->queue_used));
  77
  78         /* Unmap DMA from host system and free skb's */
  79         for (txq_id = 0; txq_id < ARRAY_SIZE(trans_pcie->txq); txq_id++) {
  80                 if (!trans_pcie->txq[txq_id])
  81                         continue;
  82                 iwl_pcie_gen2_txq_unmap(trans, txq_id);
  83         }
  84 }
  85
  86 /*
  87  * iwl_pcie_txq_update_byte_tbl - Set up entry in Tx byte-count array
  88  */
  89 static void iwl_pcie_gen2_update_byte_tbl(struct iwl_trans_pcie *trans_pcie,
  90                                           struct iwl_txq *txq, u16 byte_cnt,
  91                                           int num_tbs)
  92 {
  93         struct iwlagn_scd_bc_tbl *scd_bc_tbl = txq->bc_tbl.addr;
  94         struct iwl_trans *trans = iwl_trans_pcie_get_trans(trans_pcie);
  95         struct iwl_gen3_bc_tbl *scd_bc_tbl_gen3 = txq->bc_tbl.addr;
  96         int idx = iwl_pcie_get_cmd_index(txq, txq->write_ptr);
  97         u8 filled_tfd_size, num_fetch_chunks;
  98         u16 len = byte_cnt;
  99         __le16 bc_ent;
 100
 101         if (WARN(idx >= txq->n_window, "%d >= %d\n", idx, txq->n_window))
 102                 return;
 103
 104         filled_tfd_size = offsetof(struct iwl_tfh_tfd, tbs) +
 105                                    num_tbs * sizeof(struct iwl_tfh_tb);
 106         /*
 107          * filled_tfd_size contains the number of filled bytes in the TFD.
 108          * Dividing it by 64 will give the number of chunks to fetch
 109          * to SRAM- 0 for one chunk, 1 for 2 and so on.
 110          * If, for example, TFD contains only 3 TBs then 32 bytes
 111          * of the TFD are used, and only one chunk of 64 bytes should
 112          * be fetched
 113          */
 114         num_fetch_chunks = DIV_ROUND_UP(filled_tfd_size, 64) - 1;
 115
 116         if (trans->trans_cfg->device_family >= IWL_DEVICE_FAMILY_AX210) {
 117                 /* Starting from AX210, the HW expects bytes */
 118                 WARN_ON(trans_pcie->bc_table_dword);
 119                 WARN_ON(len > 0x3FFF);
 120                 bc_ent = cpu_to_le16(len | (num_fetch_chunks << 14));
 121                 scd_bc_tbl_gen3->tfd_offset[idx] = bc_ent;
 122         } else {
 123                 /* Before AX210, the HW expects DW */
 124                 WARN_ON(!trans_pcie->bc_table_dword);
 125                 len = DIV_ROUND_UP(len, 4);
 126                 WARN_ON(len > 0xFFF);
 127                 bc_ent = cpu_to_le16(len | (num_fetch_chunks << 12));
 128                 scd_bc_tbl->tfd_offset[idx] = bc_ent;
 129         }
 130 }
 131
 132 /*
 133  * iwl_pcie_gen2_txq_inc_wr_ptr - Send new write index to hardware
 134  */
 135 void iwl_pcie_gen2_txq_inc_wr_ptr(struct iwl_trans *trans,
 136                                   struct iwl_txq *txq)
 137 {
 138         lockdep_assert_held(&txq->lock);
 139
 140         IWL_DEBUG_TX(trans, "Q:%d WR: 0x%x\n", txq->id, txq->write_ptr);
 141
 142         /*
 143          * if not in power-save mode, uCode will never sleep when we're
 144          * trying to tx (during RFKILL, we're not trying to tx).
 145          */
 146         iwl_write32(trans, HBUS_TARG_WRPTR, txq->write_ptr | (txq->id << 16));
 147 }
 148
 149 static u8 iwl_pcie_gen2_get_num_tbs(struct iwl_trans *trans,
 150                                     struct iwl_tfh_tfd *tfd)
 151 {
 152         return le16_to_cpu(tfd->num_tbs) & 0x1f;
 153 }
 154
 155 static void iwl_pcie_gen2_tfd_unmap(struct iwl_trans *trans,
 156                                     struct iwl_cmd_meta *meta,
 157                                     struct iwl_tfh_tfd *tfd)
 158 {
 159         struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
 160         int i, num_tbs;
 161
 162         /* Sanity check on number of chunks */
 163         num_tbs = iwl_pcie_gen2_get_num_tbs(trans, tfd);
 164
 165         if (num_tbs > trans_pcie->max_tbs) {
 166                 IWL_ERR(trans, "Too many chunks: %i\n", num_tbs);
 167                 return;
 168         }
 169
 170         /* first TB is never freed - it's the bidirectional DMA data */
 171         for (i = 1; i < num_tbs; i++) {
 172                 if (meta->tbs & BIT(i))
 173                         dma_unmap_page(trans->dev,
 174                                        le64_to_cpu(tfd->tbs[i].addr),
 175                                        le16_to_cpu(tfd->tbs[i].tb_len),
 176                                        DMA_TO_DEVICE);
 177                 else
 178                         dma_unmap_single(trans->dev,
 179                                          le64_to_cpu(tfd->tbs[i].addr),
 180                                          le16_to_cpu(tfd->tbs[i].tb_len),
 181                                          DMA_TO_DEVICE);
 182         }
 183
 184         tfd->num_tbs = 0;
 185 }
 186
 187 static void iwl_pcie_gen2_free_tfd(struct iwl_trans *trans, struct iwl_txq *txq)
 188 {
 189         /* rd_ptr is bounded by TFD_QUEUE_SIZE_MAX and
 190          * idx is bounded by n_window
 191          */
 192         int idx = iwl_pcie_get_cmd_index(txq, txq->read_ptr);
 193
 194         lockdep_assert_held(&txq->lock);
 195
 196         iwl_pcie_gen2_tfd_unmap(trans, &txq->entries[idx].meta,
 197                                 iwl_pcie_get_tfd(trans, txq, idx));
 198
 199         /* free SKB */
 200         if (txq->entries) {
 201                 struct sk_buff *skb;
 202
 203                 skb = txq->entries[idx].skb;
 204
 205                 /* Can be called from irqs-disabled context
 206                  * If skb is not NULL, it means that the whole queue is being
 207                  * freed and that the queue is not empty - free the skb
 208                  */
 209                 if (skb) {
 210                         iwl_op_mode_free_skb(trans->op_mode, skb);
 211                         txq->entries[idx].skb = NULL;
 212                 }
 213         }
 214 }
 215
 216 static int iwl_pcie_gen2_set_tb(struct iwl_trans *trans,
 217                                 struct iwl_tfh_tfd *tfd, dma_addr_t addr,
 218                                 u16 len)
 219 {
 220         struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
 221         int idx = iwl_pcie_gen2_get_num_tbs(trans, tfd);
 222         struct iwl_tfh_tb *tb;
 223
 224         if (WARN_ON(idx >= IWL_TFH_NUM_TBS))
 225                 return -EINVAL;
 226         tb = &tfd->tbs[idx];
 227
 228         /* Each TFD can point to a maximum max_tbs Tx buffers */
 229         if (le16_to_cpu(tfd->num_tbs) >= trans_pcie->max_tbs) {
 230                 IWL_ERR(trans, "Error can not send more than %d chunks\n",
 231                         trans_pcie->max_tbs);
 232                 return -EINVAL;
 233         }
 234
 235         put_unaligned_le64(addr, &tb->addr);
 236         tb->tb_len = cpu_to_le16(len);
 237
 238         tfd->num_tbs = cpu_to_le16(idx + 1);
 239
 240         return idx;
 241 }
 242
 243 static int iwl_pcie_gen2_build_amsdu(struct iwl_trans *trans,
 244                                      struct sk_buff *skb,
 245                                      struct iwl_tfh_tfd *tfd, int start_len,
 246                                      u8 hdr_len, struct iwl_device_cmd *dev_cmd)
 247 {
 248 #ifdef CONFIG_INET
 249         struct iwl_tx_cmd_gen2 *tx_cmd = (void *)dev_cmd->payload;
 250         struct ieee80211_hdr *hdr = (void *)skb->data;
 251         unsigned int snap_ip_tcp_hdrlen, ip_hdrlen, total_len, hdr_room;
 252         unsigned int mss = skb_shinfo(skb)->gso_size;
 253         u16 length, amsdu_pad;
 254         u8 *start_hdr;
 255         struct iwl_tso_hdr_page *hdr_page;
 256         struct tso_t tso;
 257
 258         trace_iwlwifi_dev_tx(trans->dev, skb, tfd, sizeof(*tfd),
 259                              &dev_cmd->hdr, start_len, 0);
 260
 261         ip_hdrlen = skb_transport_header(skb) - skb_network_header(skb);
 262         snap_ip_tcp_hdrlen = 8 + ip_hdrlen + tcp_hdrlen(skb);
 263         total_len = skb->len - snap_ip_tcp_hdrlen - hdr_len;
 264         amsdu_pad = 0;
 265
 266         /* total amount of header we may need for this A-MSDU */
 267         hdr_room = DIV_ROUND_UP(total_len, mss) *
 268                 (3 + snap_ip_tcp_hdrlen + sizeof(struct ethhdr));
 269
 270         /* Our device supports 9 segments at most, it will fit in 1 page */
 271         hdr_page = get_page_hdr(trans, hdr_room, skb);
 272         if (!hdr_page)
 273                 return -ENOMEM;
 274
 275         start_hdr = hdr_page->pos;
 276
 277         /*
 278          * Pull the ieee80211 header to be able to use TSO core,
 279          * we will restore it for the tx_status flow.
 280          */
 281         skb_pull(skb, hdr_len);
 282
 283         /*
 284          * Remove the length of all the headers that we don't actually
 285          * have in the MPDU by themselves, but that we duplicate into
 286          * all the different MSDUs inside the A-MSDU.
 287          */
 288         le16_add_cpu(&tx_cmd->len, -snap_ip_tcp_hdrlen);
 289
 290         tso_start(skb, &tso);
 291
 292         while (total_len) {
 293                 /* this is the data left for this subframe */
 294                 unsigned int data_left = min_t(unsigned int, mss, total_len);
 295                 struct sk_buff *csum_skb = NULL;
 296                 unsigned int tb_len;
 297                 dma_addr_t tb_phys;
 298                 u8 *subf_hdrs_start = hdr_page->pos;
 299
 300                 total_len -= data_left;
 301
 302                 memset(hdr_page->pos, 0, amsdu_pad);
 303                 hdr_page->pos += amsdu_pad;
 304                 amsdu_pad = (4 - (sizeof(struct ethhdr) + snap_ip_tcp_hdrlen +
 305                                   data_left)) & 0x3;
 306                 ether_addr_copy(hdr_page->pos, ieee80211_get_DA(hdr));
 307                 hdr_page->pos += ETH_ALEN;
 308                 ether_addr_copy(hdr_page->pos, ieee80211_get_SA(hdr));
 309                 hdr_page->pos += ETH_ALEN;
 310
 311                 length = snap_ip_tcp_hdrlen + data_left;
 312                 *((__be16 *)hdr_page->pos) = cpu_to_be16(length);
 313                 hdr_page->pos += sizeof(length);
 314
 315                 /*
 316                  * This will copy the SNAP as well which will be considered
 317                  * as MAC header.
 318                  */
 319                 tso_build_hdr(skb, hdr_page->pos, &tso, data_left, !total_len);
 320
 321                 hdr_page->pos += snap_ip_tcp_hdrlen;
 322
 323                 tb_len = hdr_page->pos - start_hdr;
 324                 tb_phys = dma_map_single(trans->dev, start_hdr,
 325                                          tb_len, DMA_TO_DEVICE);
 326                 if (unlikely(dma_mapping_error(trans->dev, tb_phys))) {
 327                         dev_kfree_skb(csum_skb);
 328                         goto out_err;
 329                 }
 330                 iwl_pcie_gen2_set_tb(trans, tfd, tb_phys, tb_len);
 331                 trace_iwlwifi_dev_tx_tb(trans->dev, skb, start_hdr,
 332                                         tb_phys, tb_len);
 333                 /* add this subframe's headers' length to the tx_cmd */
 334                 le16_add_cpu(&tx_cmd->len, hdr_page->pos - subf_hdrs_start);
 335
 336                 /* prepare the start_hdr for the next subframe */
 337                 start_hdr = hdr_page->pos;
 338
 339                 /* put the payload */
 340                 while (data_left) {
 341                         tb_len = min_t(unsigned int, tso.size, data_left);
 342                         tb_phys = dma_map_single(trans->dev, tso.data,
 343                                                  tb_len, DMA_TO_DEVICE);
 344                         if (unlikely(dma_mapping_error(trans->dev, tb_phys))) {
 345                                 dev_kfree_skb(csum_skb);
 346                                 goto out_err;
 347                         }
 348                         iwl_pcie_gen2_set_tb(trans, tfd, tb_phys, tb_len);
 349                         trace_iwlwifi_dev_tx_tb(trans->dev, skb, tso.data,
 350                                                 tb_phys, tb_len);
 351
 352                         data_left -= tb_len;
 353                         tso_build_data(skb, &tso, tb_len);
 354                 }
 355         }
 356
 357         /* re -add the WiFi header */
 358         skb_push(skb, hdr_len);
 359
 360         return 0;
 361
 362 out_err:
 363 #endif
 364         return -EINVAL;
 365 }
 366
 367 static struct
 368 iwl_tfh_tfd *iwl_pcie_gen2_build_tx_amsdu(struct iwl_trans *trans,
 369                                           struct iwl_txq *txq,
 370                                           struct iwl_device_cmd *dev_cmd,
 371                                           struct sk_buff *skb,
 372                                           struct iwl_cmd_meta *out_meta,
 373                                           int hdr_len,
 374                                           int tx_cmd_len)
 375 {
 376         int idx = iwl_pcie_get_cmd_index(txq, txq->write_ptr);
 377         struct iwl_tfh_tfd *tfd = iwl_pcie_get_tfd(trans, txq, idx);
 378         dma_addr_t tb_phys;
 379         int len;
 380         void *tb1_addr;
 381
 382         tb_phys = iwl_pcie_get_first_tb_dma(txq, idx);
 383
 384         iwl_pcie_gen2_set_tb(trans, tfd, tb_phys, IWL_FIRST_TB_SIZE);
 385
 386         /*
 387          * The second TB (tb1) points to the remainder of the TX command
 388          * and the 802.11 header - dword aligned size
 389          * (This calculation modifies the TX command, so do it before the
 390          * setup of the first TB)
 391          */
 392         len = tx_cmd_len + sizeof(struct iwl_cmd_header) + hdr_len -
 393               IWL_FIRST_TB_SIZE;
 394
 395         /* do not align A-MSDU to dword as the subframe header aligns it */
 396
 397         /* map the data for TB1 */
 398         tb1_addr = ((u8 *)&dev_cmd->hdr) + IWL_FIRST_TB_SIZE;
 399         tb_phys = dma_map_single(trans->dev, tb1_addr, len, DMA_TO_DEVICE);
 400         if (unlikely(dma_mapping_error(trans->dev, tb_phys)))
 401                 goto out_err;
 402         iwl_pcie_gen2_set_tb(trans, tfd, tb_phys, len);
 403
 404         if (iwl_pcie_gen2_build_amsdu(trans, skb, tfd,
 405                                       len + IWL_FIRST_TB_SIZE,
 406                                       hdr_len, dev_cmd))
 407                 goto out_err;
 408
 409         /* building the A-MSDU might have changed this data, memcpy it now */
 410         memcpy(&txq->first_tb_bufs[idx], dev_cmd, IWL_FIRST_TB_SIZE);
 411         return tfd;
 412
 413 out_err:
 414         iwl_pcie_gen2_tfd_unmap(trans, out_meta, tfd);
 415         return NULL;
 416 }
 417
 418 static int iwl_pcie_gen2_tx_add_frags(struct iwl_trans *trans,
 419                                       struct sk_buff *skb,
 420                                       struct iwl_tfh_tfd *tfd,
 421                                       struct iwl_cmd_meta *out_meta)
 422 {
 423         int i;
 424
 425         for (i = 0; i < skb_shinfo(skb)->nr_frags; i++) {
 426                 const skb_frag_t *frag = &skb_shinfo(skb)->frags[i];
 427                 dma_addr_t tb_phys;
 428                 int tb_idx;
 429
 430                 if (!skb_frag_size(frag))
 431                         continue;
 432
 433                 tb_phys = skb_frag_dma_map(trans->dev, frag, 0,
 434                                            skb_frag_size(frag), DMA_TO_DEVICE);
 435
 436                 if (unlikely(dma_mapping_error(trans->dev, tb_phys)))
 437                         return -ENOMEM;
 438                 tb_idx = iwl_pcie_gen2_set_tb(trans, tfd, tb_phys,
 439                                               skb_frag_size(frag));
 440                 trace_iwlwifi_dev_tx_tb(trans->dev, skb, skb_frag_address(frag),
 441                                         tb_phys, skb_frag_size(frag));
 442                 if (tb_idx < 0)
 443                         return tb_idx;
 444
 445                 out_meta->tbs |= BIT(tb_idx);
 446         }
 447
 448         return 0;
 449 }
 450
 451 static struct
 452 iwl_tfh_tfd *iwl_pcie_gen2_build_tx(struct iwl_trans *trans,
 453                                     struct iwl_txq *txq,
 454                                     struct iwl_device_cmd *dev_cmd,
 455                                     struct sk_buff *skb,
 456                                     struct iwl_cmd_meta *out_meta,
 457                                     int hdr_len,
 458                                     int tx_cmd_len,
 459                                     bool pad)
 460 {
 461         int idx = iwl_pcie_get_cmd_index(txq, txq->write_ptr);
 462         struct iwl_tfh_tfd *tfd = iwl_pcie_get_tfd(trans, txq, idx);
 463         dma_addr_t tb_phys;
 464         int len, tb1_len, tb2_len;
 465         void *tb1_addr;
 466         struct sk_buff *frag;
 467
 468         tb_phys = iwl_pcie_get_first_tb_dma(txq, idx);
 469
 470         /* The first TB points to bi-directional DMA data */
 471         memcpy(&txq->first_tb_bufs[idx], dev_cmd, IWL_FIRST_TB_SIZE);
 472
 473         iwl_pcie_gen2_set_tb(trans, tfd, tb_phys, IWL_FIRST_TB_SIZE);
 474
 475         /*
 476          * The second TB (tb1) points to the remainder of the TX command
 477          * and the 802.11 header - dword aligned size
 478          * (This calculation modifies the TX command, so do it before the
 479          * setup of the first TB)
 480          */
 481         len = tx_cmd_len + sizeof(struct iwl_cmd_header) + hdr_len -
 482               IWL_FIRST_TB_SIZE;
 483
 484         if (pad)
 485                 tb1_len = ALIGN(len, 4);
 486         else
 487                 tb1_len = len;
 488
 489         /* map the data for TB1 */
 490         tb1_addr = ((u8 *)&dev_cmd->hdr) + IWL_FIRST_TB_SIZE;
 491         tb_phys = dma_map_single(trans->dev, tb1_addr, tb1_len, DMA_TO_DEVICE);
 492         if (unlikely(dma_mapping_error(trans->dev, tb_phys)))
 493                 goto out_err;
 494         iwl_pcie_gen2_set_tb(trans, tfd, tb_phys, tb1_len);
 495         trace_iwlwifi_dev_tx(trans->dev, skb, tfd, sizeof(*tfd), &dev_cmd->hdr,
 496                              IWL_FIRST_TB_SIZE + tb1_len, hdr_len);
 497
 498         /* set up TFD's third entry to point to remainder of skb's head */
 499         tb2_len = skb_headlen(skb) - hdr_len;
 500
 501         if (tb2_len > 0) {
 502                 tb_phys = dma_map_single(trans->dev, skb->data + hdr_len,
 503                                          tb2_len, DMA_TO_DEVICE);
 504                 if (unlikely(dma_mapping_error(trans->dev, tb_phys)))
 505                         goto out_err;
 506                 iwl_pcie_gen2_set_tb(trans, tfd, tb_phys, tb2_len);
 507                 trace_iwlwifi_dev_tx_tb(trans->dev, skb, skb->data + hdr_len,
 508                                         tb_phys, tb2_len);
 509         }
 510
 511         if (iwl_pcie_gen2_tx_add_frags(trans, skb, tfd, out_meta))
 512                 goto out_err;
 513
 514         skb_walk_frags(skb, frag) {
 515                 tb_phys = dma_map_single(trans->dev, frag->data,
 516                                          skb_headlen(frag), DMA_TO_DEVICE);
 517                 if (unlikely(dma_mapping_error(trans->dev, tb_phys)))
 518                         goto out_err;
 519                 iwl_pcie_gen2_set_tb(trans, tfd, tb_phys, skb_headlen(frag));
 520                 trace_iwlwifi_dev_tx_tb(trans->dev, skb, frag->data,
 521                                         tb_phys, skb_headlen(frag));
 522                 if (iwl_pcie_gen2_tx_add_frags(trans, frag, tfd, out_meta))
 523                         goto out_err;
 524         }
 525
 526         return tfd;
 527
 528 out_err:
 529         iwl_pcie_gen2_tfd_unmap(trans, out_meta, tfd);
 530         return NULL;
 531 }
 532
 533 static
 534 struct iwl_tfh_tfd *iwl_pcie_gen2_build_tfd(struct iwl_trans *trans,
 535                                             struct iwl_txq *txq,
 536                                             struct iwl_device_cmd *dev_cmd,
 537                                             struct sk_buff *skb,
 538                                             struct iwl_cmd_meta *out_meta)
 539 {
 540         struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
 541         int idx = iwl_pcie_get_cmd_index(txq, txq->write_ptr);
 542         struct iwl_tfh_tfd *tfd = iwl_pcie_get_tfd(trans, txq, idx);
 543         int len, hdr_len;
 544         bool amsdu;
 545
 546         /* There must be data left over for TB1 or this code must be changed */
 547         BUILD_BUG_ON(sizeof(struct iwl_tx_cmd_gen2) < IWL_FIRST_TB_SIZE);
 548
 549         memset(tfd, 0, sizeof(*tfd));
 550
 551         if (trans->trans_cfg->device_family < IWL_DEVICE_FAMILY_AX210)
 552                 len = sizeof(struct iwl_tx_cmd_gen2);
 553         else
 554                 len = sizeof(struct iwl_tx_cmd_gen3);
 555
 556         amsdu = ieee80211_is_data_qos(hdr->frame_control) &&
 557                         (*ieee80211_get_qos_ctl(hdr) &
 558                          IEEE80211_QOS_CTL_A_MSDU_PRESENT);
 559
 560         hdr_len = ieee80211_hdrlen(hdr->frame_control);
 561
 562         /*
 563          * Only build A-MSDUs here if doing so by GSO, otherwise it may be
 564          * an A-MSDU for other reasons, e.g. NAN or an A-MSDU having been
 565          * built in the higher layers already.
 566          */
 567         if (amsdu && skb_shinfo(skb)->gso_size)
 568                 return iwl_pcie_gen2_build_tx_amsdu(trans, txq, dev_cmd, skb,
 569                                                     out_meta, hdr_len, len);
 570
 571         return iwl_pcie_gen2_build_tx(trans, txq, dev_cmd, skb, out_meta,
 572                                       hdr_len, len, !amsdu);
 573 }
 574
 575 int iwl_trans_pcie_gen2_tx(struct iwl_trans *trans, struct sk_buff *skb,
 576                            struct iwl_device_cmd *dev_cmd, int txq_id)
 577 {
 578         struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
 579         struct iwl_cmd_meta *out_meta;
 580         struct iwl_txq *txq = trans_pcie->txq[txq_id];
 581         u16 cmd_len;
 582         int idx;
 583         void *tfd;
 584
 585         if (WARN_ONCE(!test_bit(txq_id, trans_pcie->queue_used),
 586                       "TX on unused queue %d\n", txq_id))
 587                 return -EINVAL;
 588
 589         if (skb_is_nonlinear(skb) &&
 590             skb_shinfo(skb)->nr_frags > IWL_PCIE_MAX_FRAGS(trans_pcie) &&
 591             __skb_linearize(skb))
 592                 return -ENOMEM;
 593
 594         spin_lock(&txq->lock);
 595
 596         if (iwl_queue_space(trans, txq) < txq->high_mark) {
 597                 iwl_stop_queue(trans, txq);
 598
 599                 /* don't put the packet on the ring, if there is no room */
 600                 if (unlikely(iwl_queue_space(trans, txq) < 3)) {
 601                         struct iwl_device_cmd **dev_cmd_ptr;
 602
 603                         dev_cmd_ptr = (void *)((u8 *)skb->cb +
 604                                                trans_pcie->dev_cmd_offs);
 605
 606                         *dev_cmd_ptr = dev_cmd;
 607                         __skb_queue_tail(&txq->overflow_q, skb);
 608                         spin_unlock(&txq->lock);
 609                         return 0;
 610                 }
 611         }
 612
 613         idx = iwl_pcie_get_cmd_index(txq, txq->write_ptr);
 614
 615         /* Set up driver data for this TFD */
 616         txq->entries[idx].skb = skb;
 617         txq->entries[idx].cmd = dev_cmd;
 618
 619         dev_cmd->hdr.sequence =
 620                 cpu_to_le16((u16)(QUEUE_TO_SEQ(txq_id) |
 621                             INDEX_TO_SEQ(idx)));
 622
 623         /* Set up first empty entry in queue's array of Tx/cmd buffers */
 624         out_meta = &txq->entries[idx].meta;
 625         out_meta->flags = 0;
 626
 627         tfd = iwl_pcie_gen2_build_tfd(trans, txq, dev_cmd, skb, out_meta);
 628         if (!tfd) {
 629                 spin_unlock(&txq->lock);
 630                 return -1;
 631         }
 632
 633         if (trans->trans_cfg->device_family >= IWL_DEVICE_FAMILY_AX210) {
 634                 struct iwl_tx_cmd_gen3 *tx_cmd_gen3 =
 635                         (void *)dev_cmd->payload;
 636
 637                 cmd_len = le16_to_cpu(tx_cmd_gen3->len);
 638         } else {
 639                 struct iwl_tx_cmd_gen2 *tx_cmd_gen2 =
 640                         (void *)dev_cmd->payload;
 641
 642                 cmd_len = le16_to_cpu(tx_cmd_gen2->len);
 643         }
 644
 645         /* Set up entry for this TFD in Tx byte-count array */
 646         iwl_pcie_gen2_update_byte_tbl(trans_pcie, txq, cmd_len,
 647                                       iwl_pcie_gen2_get_num_tbs(trans, tfd));
 648
 649         /* start timer if queue currently empty */
 650         if (txq->read_ptr == txq->write_ptr && txq->wd_timeout)
 651                 mod_timer(&txq->stuck_timer, jiffies + txq->wd_timeout);
 652
 653         /* Tell device the write index *just past* this latest filled TFD */
 654         txq->write_ptr = iwl_queue_inc_wrap(trans, txq->write_ptr);
 655         iwl_pcie_gen2_txq_inc_wr_ptr(trans, txq);
 656         /*
 657          * At this point the frame is "transmitted" successfully
 658          * and we will get a TX status notification eventually.
 659          */
 660         spin_unlock(&txq->lock);
 661         return 0;
 662 }
 663
 664 /*************** HOST COMMAND QUEUE FUNCTIONS   *****/
 665
 666 /*
 667  * iwl_pcie_gen2_enqueue_hcmd - enqueue a uCode command
 668  * @priv: device private data point
 669  * @cmd: a pointer to the ucode command structure
 670  *
 671  * The function returns < 0 values to indicate the operation
 672  * failed. On success, it returns the index (>= 0) of command in the
 673  * command queue.
 674  */
 675 static int iwl_pcie_gen2_enqueue_hcmd(struct iwl_trans *trans,
 676                                       struct iwl_host_cmd *cmd)
 677 {
 678         struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
 679         struct iwl_txq *txq = trans_pcie->txq[trans_pcie->cmd_queue];
 680         struct iwl_device_cmd *out_cmd;
 681         struct iwl_cmd_meta *out_meta;
 682         unsigned long flags;
 683         void *dup_buf = NULL;
 684         dma_addr_t phys_addr;
 685         int i, cmd_pos, idx;
 686         u16 copy_size, cmd_size, tb0_size;
 687         bool had_nocopy = false;
 688         u8 group_id = iwl_cmd_groupid(cmd->id);
 689         const u8 *cmddata[IWL_MAX_CMD_TBS_PER_TFD];
 690         u16 cmdlen[IWL_MAX_CMD_TBS_PER_TFD];
 691         struct iwl_tfh_tfd *tfd;
 692
 693         copy_size = sizeof(struct iwl_cmd_header_wide);
 694         cmd_size = sizeof(struct iwl_cmd_header_wide);
 695
 696         for (i = 0; i < IWL_MAX_CMD_TBS_PER_TFD; i++) {
 697                 cmddata[i] = cmd->data[i];
 698                 cmdlen[i] = cmd->len[i];
 699
 700                 if (!cmd->len[i])
 701                         continue;
 702
 703                 /* need at least IWL_FIRST_TB_SIZE copied */
 704                 if (copy_size < IWL_FIRST_TB_SIZE) {
 705                         int copy = IWL_FIRST_TB_SIZE - copy_size;
 706
 707                         if (copy > cmdlen[i])
 708                                 copy = cmdlen[i];
 709                         cmdlen[i] -= copy;
 710                         cmddata[i] += copy;
 711                         copy_size += copy;
 712                 }
 713
 714                 if (cmd->dataflags[i] & IWL_HCMD_DFL_NOCOPY) {
 715                         had_nocopy = true;
 716                         if (WARN_ON(cmd->dataflags[i] & IWL_HCMD_DFL_DUP)) {
 717                                 idx = -EINVAL;
 718                                 goto free_dup_buf;
 719                         }
 720                 } else if (cmd->dataflags[i] & IWL_HCMD_DFL_DUP) {
 721                         /*
 722                          * This is also a chunk that isn't copied
 723                          * to the static buffer so set had_nocopy.
 724                          */
 725                         had_nocopy = true;
 726
 727                         /* only allowed once */
 728                         if (WARN_ON(dup_buf)) {
 729                                 idx = -EINVAL;
 730                                 goto free_dup_buf;
 731                         }
 732
 733                         dup_buf = kmemdup(cmddata[i], cmdlen[i],
 734                                           GFP_ATOMIC);
 735                         if (!dup_buf)
 736                                 return -ENOMEM;
 737                 } else {
 738                         /* NOCOPY must not be followed by normal! */
 739                         if (WARN_ON(had_nocopy)) {
 740                                 idx = -EINVAL;
 741                                 goto free_dup_buf;
 742                         }
 743                         copy_size += cmdlen[i];
 744                 }
 745                 cmd_size += cmd->len[i];
 746         }
 747
 748         /*
 749          * If any of the command structures end up being larger than the
 750          * TFD_MAX_PAYLOAD_SIZE and they aren't dynamically allocated into
 751          * separate TFDs, then we will need to increase the size of the buffers
 752          */
 753         if (WARN(copy_size > TFD_MAX_PAYLOAD_SIZE,
 754                  "Command %s (%#x) is too large (%d bytes)\n",
 755                  iwl_get_cmd_string(trans, cmd->id), cmd->id, copy_size)) {
 756                 idx = -EINVAL;
 757                 goto free_dup_buf;
 758         }
 759
 760         spin_lock_bh(&txq->lock);
 761
 762         idx = iwl_pcie_get_cmd_index(txq, txq->write_ptr);
 763         tfd = iwl_pcie_get_tfd(trans, txq, txq->write_ptr);
 764         memset(tfd, 0, sizeof(*tfd));
 765
 766         if (iwl_queue_space(trans, txq) < ((cmd->flags & CMD_ASYNC) ? 2 : 1)) {
 767                 spin_unlock_bh(&txq->lock);
 768
 769                 IWL_ERR(trans, "No space in command queue\n");
 770                 iwl_op_mode_cmd_queue_full(trans->op_mode);
 771                 idx = -ENOSPC;
 772                 goto free_dup_buf;
 773         }
 774
 775         out_cmd = txq->entries[idx].cmd;
 776         out_meta = &txq->entries[idx].meta;
 777
 778         /* re-initialize to NULL */
 779         memset(out_meta, 0, sizeof(*out_meta));
 780         if (cmd->flags & CMD_WANT_SKB)
 781                 out_meta->source = cmd;
 782
 783         /* set up the header */
 784         out_cmd->hdr_wide.cmd = iwl_cmd_opcode(cmd->id);
 785         out_cmd->hdr_wide.group_id = group_id;
 786         out_cmd->hdr_wide.version = iwl_cmd_version(cmd->id);
 787         out_cmd->hdr_wide.length =
 788                 cpu_to_le16(cmd_size - sizeof(struct iwl_cmd_header_wide));
 789         out_cmd->hdr_wide.reserved = 0;
 790         out_cmd->hdr_wide.sequence =
 791                 cpu_to_le16(QUEUE_TO_SEQ(trans_pcie->cmd_queue) |
 792                                          INDEX_TO_SEQ(txq->write_ptr));
 793
 794         cmd_pos = sizeof(struct iwl_cmd_header_wide);
 795         copy_size = sizeof(struct iwl_cmd_header_wide);
 796
 797         /* and copy the data that needs to be copied */
 798         for (i = 0; i < IWL_MAX_CMD_TBS_PER_TFD; i++) {
 799                 int copy;
 800
 801                 if (!cmd->len[i])
 802                         continue;
 803
 804                 /* copy everything if not nocopy/dup */
 805                 if (!(cmd->dataflags[i] & (IWL_HCMD_DFL_NOCOPY |
 806                                            IWL_HCMD_DFL_DUP))) {
 807                         copy = cmd->len[i];
 808
 809                         memcpy((u8 *)out_cmd + cmd_pos, cmd->data[i], copy);
 810                         cmd_pos += copy;
 811                         copy_size += copy;
 812                         continue;
 813                 }
 814
 815                 /*
 816                  * Otherwise we need at least IWL_FIRST_TB_SIZE copied
 817                  * in total (for bi-directional DMA), but copy up to what
 818                  * we can fit into the payload for debug dump purposes.
 819                  */
 820                 copy = min_t(int, TFD_MAX_PAYLOAD_SIZE - cmd_pos, cmd->len[i]);
 821
 822                 memcpy((u8 *)out_cmd + cmd_pos, cmd->data[i], copy);
 823                 cmd_pos += copy;
 824
 825                 /* However, treat copy_size the proper way, we need it below */
 826                 if (copy_size < IWL_FIRST_TB_SIZE) {
 827                         copy = IWL_FIRST_TB_SIZE - copy_size;
 828
 829                         if (copy > cmd->len[i])
 830                                 copy = cmd->len[i];
 831                         copy_size += copy;
 832                 }
 833         }
 834
 835         IWL_DEBUG_HC(trans,
 836                      "Sending command %s (%.2x.%.2x), seq: 0x%04X, %d bytes at %d[%d]:%d\n",
 837                      iwl_get_cmd_string(trans, cmd->id), group_id,
 838                      out_cmd->hdr.cmd, le16_to_cpu(out_cmd->hdr.sequence),
 839                      cmd_size, txq->write_ptr, idx, trans_pcie->cmd_queue);
 840
 841         /* start the TFD with the minimum copy bytes */
 842         tb0_size = min_t(int, copy_size, IWL_FIRST_TB_SIZE);
 843         memcpy(&txq->first_tb_bufs[idx], out_cmd, tb0_size);
 844         iwl_pcie_gen2_set_tb(trans, tfd, iwl_pcie_get_first_tb_dma(txq, idx),
 845                              tb0_size);
 846
 847         /* map first command fragment, if any remains */
 848         if (copy_size > tb0_size) {
 849                 phys_addr = dma_map_single(trans->dev,
 850                                            (u8 *)out_cmd + tb0_size,
 851                                            copy_size - tb0_size,
 852                                            DMA_TO_DEVICE);
 853                 if (dma_mapping_error(trans->dev, phys_addr)) {
 854                         idx = -ENOMEM;
 855                         iwl_pcie_gen2_tfd_unmap(trans, out_meta, tfd);
 856                         goto out;
 857                 }
 858                 iwl_pcie_gen2_set_tb(trans, tfd, phys_addr,
 859                                      copy_size - tb0_size);
 860         }
 861
 862         /* map the remaining (adjusted) nocopy/dup fragments */
 863         for (i = 0; i < IWL_MAX_CMD_TBS_PER_TFD; i++) {
 864                 const void *data = cmddata[i];
 865
 866                 if (!cmdlen[i])
 867                         continue;
 868                 if (!(cmd->dataflags[i] & (IWL_HCMD_DFL_NOCOPY |
 869                                            IWL_HCMD_DFL_DUP)))
 870                         continue;
 871                 if (cmd->dataflags[i] & IWL_HCMD_DFL_DUP)
 872                         data = dup_buf;
 873                 phys_addr = dma_map_single(trans->dev, (void *)data,
 874                                            cmdlen[i], DMA_TO_DEVICE);
 875                 if (dma_mapping_error(trans->dev, phys_addr)) {
 876                         idx = -ENOMEM;
 877                         iwl_pcie_gen2_tfd_unmap(trans, out_meta, tfd);
 878                         goto out;
 879                 }
 880                 iwl_pcie_gen2_set_tb(trans, tfd, phys_addr, cmdlen[i]);
 881         }
 882
 883         BUILD_BUG_ON(IWL_TFH_NUM_TBS > sizeof(out_meta->tbs) * BITS_PER_BYTE);
 884         out_meta->flags = cmd->flags;
 885         if (WARN_ON_ONCE(txq->entries[idx].free_buf))
 886                 kzfree(txq->entries[idx].free_buf);
 887         txq->entries[idx].free_buf = dup_buf;
 888
 889         trace_iwlwifi_dev_hcmd(trans->dev, cmd, cmd_size, &out_cmd->hdr_wide);
 890
 891         /* start timer if queue currently empty */
 892         if (txq->read_ptr == txq->write_ptr && txq->wd_timeout)
 893                 mod_timer(&txq->stuck_timer, jiffies + txq->wd_timeout);
 894
 895         spin_lock_irqsave(&trans_pcie->reg_lock, flags);
 896         /* Increment and update queue's write index */
 897         txq->write_ptr = iwl_queue_inc_wrap(trans, txq->write_ptr);
 898         iwl_pcie_gen2_txq_inc_wr_ptr(trans, txq);
 899         spin_unlock_irqrestore(&trans_pcie->reg_lock, flags);
 900
 901 out:
 902         spin_unlock_bh(&txq->lock);
 903 free_dup_buf:
 904         if (idx < 0)
 905                 kfree(dup_buf);
 906         return idx;
 907 }
 908
 909 #define HOST_COMPLETE_TIMEOUT   (2 * HZ)
 910
 911 static int iwl_pcie_gen2_send_hcmd_sync(struct iwl_trans *trans,
 912                                         struct iwl_host_cmd *cmd)
 913 {
 914         struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
 915         const char *cmd_str = iwl_get_cmd_string(trans, cmd->id);
 916         struct iwl_txq *txq = trans_pcie->txq[trans_pcie->cmd_queue];
 917         int cmd_idx;
 918         int ret;
 919
 920         IWL_DEBUG_INFO(trans, "Attempting to send sync command %s\n", cmd_str);
 921
 922         if (WARN(test_and_set_bit(STATUS_SYNC_HCMD_ACTIVE,
 923                                   &trans->status),
 924                  "Command %s: a command is already active!\n", cmd_str))
 925                 return -EIO;
 926
 927         IWL_DEBUG_INFO(trans, "Setting HCMD_ACTIVE for command %s\n", cmd_str);
 928
 929         cmd_idx = iwl_pcie_gen2_enqueue_hcmd(trans, cmd);
 930         if (cmd_idx < 0) {
 931                 ret = cmd_idx;
 932                 clear_bit(STATUS_SYNC_HCMD_ACTIVE, &trans->status);
 933                 IWL_ERR(trans, "Error sending %s: enqueue_hcmd failed: %d\n",
 934                         cmd_str, ret);
 935                 return ret;
 936         }
 937
 938         ret = wait_event_timeout(trans_pcie->wait_command_queue,
 939                                  !test_bit(STATUS_SYNC_HCMD_ACTIVE,
 940                                            &trans->status),
 941                                  HOST_COMPLETE_TIMEOUT);
 942         if (!ret) {
 943                 IWL_ERR(trans, "Error sending %s: time out after %dms.\n",
 944                         cmd_str, jiffies_to_msecs(HOST_COMPLETE_TIMEOUT));
 945
 946                 IWL_ERR(trans, "Current CMD queue read_ptr %d write_ptr %d\n",
 947                         txq->read_ptr, txq->write_ptr);
 948
 949                 clear_bit(STATUS_SYNC_HCMD_ACTIVE, &trans->status);
 950                 IWL_DEBUG_INFO(trans, "Clearing HCMD_ACTIVE for command %s\n",
 951                                cmd_str);
 952                 ret = -ETIMEDOUT;
 953
 954                 iwl_trans_pcie_sync_nmi(trans);
 955                 goto cancel;
 956         }
 957
 958         if (test_bit(STATUS_FW_ERROR, &trans->status)) {
 959                 IWL_ERR(trans, "FW error in SYNC CMD %s\n", cmd_str);
 960                 dump_stack();
 961                 ret = -EIO;
 962                 goto cancel;
 963         }
 964
 965         if (!(cmd->flags & CMD_SEND_IN_RFKILL) &&
 966             test_bit(STATUS_RFKILL_OPMODE, &trans->status)) {
 967                 IWL_DEBUG_RF_KILL(trans, "RFKILL in SYNC CMD... no rsp\n");
 968                 ret = -ERFKILL;
 969                 goto cancel;
 970         }
 971
 972         if ((cmd->flags & CMD_WANT_SKB) && !cmd->resp_pkt) {
 973                 IWL_ERR(trans, "Error: Response NULL in '%s'\n", cmd_str);
 974                 ret = -EIO;
 975                 goto cancel;
 976         }
 977
 978         return 0;
 979
 980 cancel:
 981         if (cmd->flags & CMD_WANT_SKB) {
 982                 /*
 983                  * Cancel the CMD_WANT_SKB flag for the cmd in the
 984                  * TX cmd queue. Otherwise in case the cmd comes
 985                  * in later, it will possibly set an invalid
 986                  * address (cmd->meta.source).
 987                  */
 988                 txq->entries[cmd_idx].meta.flags &= ~CMD_WANT_SKB;
 989         }
 990
 991         if (cmd->resp_pkt) {
 992                 iwl_free_resp(cmd);
 993                 cmd->resp_pkt = NULL;
 994         }
 995
 996         return ret;
 997 }
 998
 999 int iwl_trans_pcie_gen2_send_hcmd(struct iwl_trans *trans,
1000                                   struct iwl_host_cmd *cmd)
1001 {
1002         if (!(cmd->flags & CMD_SEND_IN_RFKILL) &&
1003             test_bit(STATUS_RFKILL_OPMODE, &trans->status)) {
1004                 IWL_DEBUG_RF_KILL(trans, "Dropping CMD 0x%x: RF KILL\n",
1005                                   cmd->id);
1006                 return -ERFKILL;
1007         }
1008
1009         if (cmd->flags & CMD_ASYNC) {
1010                 int ret;
1011
1012                 /* An asynchronous command can not expect an SKB to be set. */
1013                 if (WARN_ON(cmd->flags & CMD_WANT_SKB))
1014                         return -EINVAL;
1015
1016                 ret = iwl_pcie_gen2_enqueue_hcmd(trans, cmd);
1017                 if (ret < 0) {
1018                         IWL_ERR(trans,
1019                                 "Error sending %s: enqueue_hcmd failed: %d\n",
1020                                 iwl_get_cmd_string(trans, cmd->id), ret);
1021                         return ret;
1022                 }
1023                 return 0;
1024         }
1025
1026         return iwl_pcie_gen2_send_hcmd_sync(trans, cmd);
1027 }
1028
1029 /*
1030  * iwl_pcie_gen2_txq_unmap -  Unmap any remaining DMA mappings and free skb's
1031  */
1032 void iwl_pcie_gen2_txq_unmap(struct iwl_trans *trans, int txq_id)
1033 {
1034         struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
1035         struct iwl_txq *txq = trans_pcie->txq[txq_id];
1036
1037         spin_lock_bh(&txq->lock);
1038         while (txq->write_ptr != txq->read_ptr) {
1039                 IWL_DEBUG_TX_REPLY(trans, "Q %d Free %d\n",
1040                                    txq_id, txq->read_ptr);
1041
1042                 if (txq_id != trans_pcie->cmd_queue) {
1043                         int idx = iwl_pcie_get_cmd_index(txq, txq->read_ptr);
1044                         struct sk_buff *skb = txq->entries[idx].skb;
1045
1046                         if (WARN_ON_ONCE(!skb))
1047                                 continue;
1048
1049                         iwl_pcie_free_tso_page(trans_pcie, skb);
1050                 }
1051                 iwl_pcie_gen2_free_tfd(trans, txq);
1052                 txq->read_ptr = iwl_queue_inc_wrap(trans, txq->read_ptr);
1053         }
1054
1055         while (!skb_queue_empty(&txq->overflow_q)) {
1056                 struct sk_buff *skb = __skb_dequeue(&txq->overflow_q);
1057
1058                 iwl_op_mode_free_skb(trans->op_mode, skb);
1059         }
1060
1061         spin_unlock_bh(&txq->lock);
1062
1063         /* just in case - this queue may have been stopped */
1064         iwl_wake_queue(trans, txq);
1065 }
1066
1067 void iwl_pcie_gen2_txq_free_memory(struct iwl_trans *trans,
1068                                    struct iwl_txq *txq)
1069 {
1070         struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
1071         struct device *dev = trans->dev;
1072
1073         /* De-alloc circular buffer of TFDs */
1074         if (txq->tfds) {
1075                 dma_free_coherent(dev,
1076                                   trans_pcie->tfd_size * txq->n_window,
1077                                   txq->tfds, txq->dma_addr);
1078                 dma_free_coherent(dev,
1079                                   sizeof(*txq->first_tb_bufs) * txq->n_window,
1080                                   txq->first_tb_bufs, txq->first_tb_dma);
1081         }
1082
1083         kfree(txq->entries);
1084         iwl_pcie_free_dma_ptr(trans, &txq->bc_tbl);
1085         kfree(txq);
1086 }
1087
1088 /*
1089  * iwl_pcie_txq_free - Deallocate DMA queue.
1090  * @txq: Transmit queue to deallocate.
1091  *
1092  * Empty queue by removing and destroying all BD's.
1093  * Free all buffers.
1094  * 0-fill, but do not free "txq" descriptor structure.
1095  */
1096 static void iwl_pcie_gen2_txq_free(struct iwl_trans *trans, int txq_id)
1097 {
1098         struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
1099         struct iwl_txq *txq = trans_pcie->txq[txq_id];
1100         int i;
1101
1102         if (WARN_ON(!txq))
1103                 return;
1104
1105         iwl_pcie_gen2_txq_unmap(trans, txq_id);
1106
1107         /* De-alloc array of command/tx buffers */
1108         if (txq_id == trans_pcie->cmd_queue)
1109                 for (i = 0; i < txq->n_window; i++) {
1110                         kzfree(txq->entries[i].cmd);
1111                         kzfree(txq->entries[i].free_buf);
1112                 }
1113         del_timer_sync(&txq->stuck_timer);
1114
1115         iwl_pcie_gen2_txq_free_memory(trans, txq);
1116
1117         trans_pcie->txq[txq_id] = NULL;
1118
1119         clear_bit(txq_id, trans_pcie->queue_used);
1120 }
1121
1122 int iwl_trans_pcie_dyn_txq_alloc_dma(struct iwl_trans *trans,
1123                                      struct iwl_txq **intxq, int size,
1124                                      unsigned int timeout)
1125 {
1126         int ret;
1127
1128         struct iwl_txq *txq;
1129         txq = kzalloc(sizeof(*txq), GFP_KERNEL);
1130         if (!txq)
1131                 return -ENOMEM;
1132         ret = iwl_pcie_alloc_dma_ptr(trans, &txq->bc_tbl,
1133                                      (trans->trans_cfg->device_family >=
1134                                       IWL_DEVICE_FAMILY_AX210) ?
1135                                      sizeof(struct iwl_gen3_bc_tbl) :
1136                                      sizeof(struct iwlagn_scd_bc_tbl));
1137         if (ret) {
1138                 IWL_ERR(trans, "Scheduler BC Table allocation failed\n");
1139                 kfree(txq);
1140                 return -ENOMEM;
1141         }
1142
1143         ret = iwl_pcie_txq_alloc(trans, txq, size, false);
1144         if (ret) {
1145                 IWL_ERR(trans, "Tx queue alloc failed\n");
1146                 goto error;
1147         }
1148         ret = iwl_pcie_txq_init(trans, txq, size, false);
1149         if (ret) {
1150                 IWL_ERR(trans, "Tx queue init failed\n");
1151                 goto error;
1152         }
1153
1154         txq->wd_timeout = msecs_to_jiffies(timeout);
1155
1156         *intxq = txq;
1157         return 0;
1158
1159 error:
1160         iwl_pcie_gen2_txq_free_memory(trans, txq);
1161         return ret;
1162 }
1163
1164 int iwl_trans_pcie_txq_alloc_response(struct iwl_trans *trans,
1165                                       struct iwl_txq *txq,
1166                                       struct iwl_host_cmd *hcmd)
1167 {
1168         struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
1169         struct iwl_tx_queue_cfg_rsp *rsp;
1170         int ret, qid;
1171         u32 wr_ptr;
1172
1173         if (WARN_ON(iwl_rx_packet_payload_len(hcmd->resp_pkt) !=
1174                     sizeof(*rsp))) {
1175                 ret = -EINVAL;
1176                 goto error_free_resp;
1177         }
1178
1179         rsp = (void *)hcmd->resp_pkt->data;
1180         qid = le16_to_cpu(rsp->queue_number);
1181         wr_ptr = le16_to_cpu(rsp->write_pointer);
1182
1183         if (qid >= ARRAY_SIZE(trans_pcie->txq)) {
1184                 WARN_ONCE(1, "queue index %d unsupported", qid);
1185                 ret = -EIO;
1186                 goto error_free_resp;
1187         }
1188
1189         if (test_and_set_bit(qid, trans_pcie->queue_used)) {
1190                 WARN_ONCE(1, "queue %d already used", qid);
1191                 ret = -EIO;
1192                 goto error_free_resp;
1193         }
1194
1195         txq->id = qid;
1196         trans_pcie->txq[qid] = txq;
1197         wr_ptr &= (trans->trans_cfg->base_params->max_tfd_queue_size - 1);
1198
1199         /* Place first TFD at index corresponding to start sequence number */
1200         txq->read_ptr = wr_ptr;
1201         txq->write_ptr = wr_ptr;
1202
1203         IWL_DEBUG_TX_QUEUES(trans, "Activate queue %d\n", qid);
1204
1205         iwl_free_resp(hcmd);
1206         return qid;
1207
1208 error_free_resp:
1209         iwl_free_resp(hcmd);
1210         iwl_pcie_gen2_txq_free_memory(trans, txq);
1211         return ret;
1212 }
1213
1214 int iwl_trans_pcie_dyn_txq_alloc(struct iwl_trans *trans,
1215                                  __le16 flags, u8 sta_id, u8 tid,
1216                                  int cmd_id, int size,
1217                                  unsigned int timeout)
1218 {
1219         struct iwl_txq *txq = NULL;
1220         struct iwl_tx_queue_cfg_cmd cmd = {
1221                 .flags = flags,
1222                 .sta_id = sta_id,
1223                 .tid = tid,
1224         };
1225         struct iwl_host_cmd hcmd = {
1226                 .id = cmd_id,
1227                 .len = { sizeof(cmd) },
1228                 .data = { &cmd, },
1229                 .flags = CMD_WANT_SKB,
1230         };
1231         int ret;
1232
1233         ret = iwl_trans_pcie_dyn_txq_alloc_dma(trans, &txq, size, timeout);
1234         if (ret)
1235                 return ret;
1236
1237         cmd.tfdq_addr = cpu_to_le64(txq->dma_addr);
1238         cmd.byte_cnt_addr = cpu_to_le64(txq->bc_tbl.dma);
1239         cmd.cb_size = cpu_to_le32(TFD_QUEUE_CB_SIZE(size));
1240
1241         ret = iwl_trans_send_cmd(trans, &hcmd);
1242         if (ret)
1243                 goto error;
1244
1245         return iwl_trans_pcie_txq_alloc_response(trans, txq, &hcmd);
1246
1247 error:
1248         iwl_pcie_gen2_txq_free_memory(trans, txq);
1249         return ret;
1250 }
1251
1252 void iwl_trans_pcie_dyn_txq_free(struct iwl_trans *trans, int queue)
1253 {
1254         struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
1255
1256         /*
1257          * Upon HW Rfkill - we stop the device, and then stop the queues
1258          * in the op_mode. Just for the sake of the simplicity of the op_mode,
1259          * allow the op_mode to call txq_disable after it already called
1260          * stop_device.
1261          */
1262         if (!test_and_clear_bit(queue, trans_pcie->queue_used)) {
1263                 WARN_ONCE(test_bit(STATUS_DEVICE_ENABLED, &trans->status),
1264                           "queue %d not used", queue);
1265                 return;
1266         }
1267
1268         iwl_pcie_gen2_txq_unmap(trans, queue);
1269
1270         IWL_DEBUG_TX_QUEUES(trans, "Deactivate queue %d\n", queue);
1271 }
1272
1273 void iwl_pcie_gen2_tx_free(struct iwl_trans *trans)
1274 {
1275         struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
1276         int i;
1277
1278         memset(trans_pcie->queue_used, 0, sizeof(trans_pcie->queue_used));
1279
1280         /* Free all TX queues */
1281         for (i = 0; i < ARRAY_SIZE(trans_pcie->txq); i++) {
1282                 if (!trans_pcie->txq[i])
1283                         continue;
1284
1285                 iwl_pcie_gen2_txq_free(trans, i);
1286         }
1287 }
1288
1289 int iwl_pcie_gen2_tx_init(struct iwl_trans *trans, int txq_id, int queue_size)
1290 {
1291         struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
1292         struct iwl_txq *queue;
1293         int ret;
1294
1295         /* alloc and init the tx queue */
1296         if (!trans_pcie->txq[txq_id]) {
1297                 queue = kzalloc(sizeof(*queue), GFP_KERNEL);
1298                 if (!queue) {
1299                         IWL_ERR(trans, "Not enough memory for tx queue\n");
1300                         return -ENOMEM;
1301                 }
1302                 trans_pcie->txq[txq_id] = queue;
1303                 ret = iwl_pcie_txq_alloc(trans, queue, queue_size, true);
1304                 if (ret) {
1305                         IWL_ERR(trans, "Tx %d queue init failed\n", txq_id);
1306                         goto error;
1307                 }
1308         } else {
1309                 queue = trans_pcie->txq[txq_id];
1310         }
1311
1312         ret = iwl_pcie_txq_init(trans, queue, queue_size,
1313                                 (txq_id == trans_pcie->cmd_queue));
1314         if (ret) {
1315                 IWL_ERR(trans, "Tx %d queue alloc failed\n", txq_id);
1316                 goto error;
1317         }
1318         trans_pcie->txq[txq_id]->id = txq_id;
1319         set_bit(txq_id, trans_pcie->queue_used);
1320
1321         return 0;
1322
1323 error:
1324         iwl_pcie_gen2_tx_free(trans);
1325         return ret;
1326 }
1327