tx.c 55.4 KB
Newer Older
1 2
/******************************************************************************
 *
3
 * Copyright(c) 2003 - 2014 Intel Corporation. All rights reserved.
4
 * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25
 *
 * Portions of this file are derived from the ipw3945 project, as well
 * as portions of the ieee80211 subsystem header files.
 *
 * This program is free software; you can redistribute it and/or modify it
 * under the terms of version 2 of the GNU General Public License as
 * published by the Free Software Foundation.
 *
 * This program is distributed in the hope that it will be useful, but WITHOUT
 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
 * more details.
 *
 * You should have received a copy of the GNU General Public License along with
 * this program; if not, write to the Free Software Foundation, Inc.,
 * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
 *
 * The full GNU General Public License is included in this distribution in the
 * file called LICENSE.
 *
 * Contact Information:
26
 *  Intel Linux Wireless <ilw@linux.intel.com>
27 28 29
 * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
 *
 *****************************************************************************/
30
#include <linux/etherdevice.h>
31
#include <linux/slab.h>
32 33
#include <linux/sched.h>

34 35 36
#include "iwl-debug.h"
#include "iwl-csr.h"
#include "iwl-prph.h"
37
#include "iwl-io.h"
38
#include "iwl-scd.h"
39
#include "iwl-op-mode.h"
40
#include "internal.h"
41
/* FIXME: need to abstract out TX command (once we know what it looks like) */
42
#include "dvm/commands.h"
43

44 45 46
#define IWL_TX_CRC_SIZE 4
#define IWL_TX_DELIMITER_SIZE 4

47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69
/*************** DMA-QUEUE-GENERAL-FUNCTIONS  *****
 * DMA services
 *
 * Theory of operation
 *
 * A Tx or Rx queue resides in host DRAM, and is comprised of a circular buffer
 * of buffer descriptors, each of which points to one or more data buffers for
 * the device to read from or fill.  Driver and device exchange status of each
 * queue via "read" and "write" pointers.  Driver keeps minimum of 2 empty
 * entries in each circular buffer, to protect against confusing empty and full
 * queue states.
 *
 * The device reads or writes the data in the queues via the device's several
 * DMA/FIFO channels.  Each queue is mapped to a single DMA channel.
 *
 * For Tx queue, there are low mark and high mark limits. If, after queuing
 * the packet for Tx, free space become < low mark, Tx queue stopped. When
 * reclaiming packets (on 'tx done IRQ), if free space become > high mark,
 * Tx queue resumed.
 *
 ***************************************************/
static int iwl_queue_space(const struct iwl_queue *q)
{
70 71
	unsigned int max;
	unsigned int used;
72

73 74
	/*
	 * To avoid ambiguity between empty and completely full queues, there
75 76 77
	 * should always be less than TFD_QUEUE_SIZE_MAX elements in the queue.
	 * If q->n_window is smaller than TFD_QUEUE_SIZE_MAX, there is no need
	 * to reserve any queue entries for this purpose.
78
	 */
79
	if (q->n_window < TFD_QUEUE_SIZE_MAX)
80 81
		max = q->n_window;
	else
82
		max = TFD_QUEUE_SIZE_MAX - 1;
83

84
	/*
85 86
	 * TFD_QUEUE_SIZE_MAX is a power of 2, so the following is equivalent to
	 * modulo by TFD_QUEUE_SIZE_MAX and is well defined.
87
	 */
88
	used = (q->write_ptr - q->read_ptr) & (TFD_QUEUE_SIZE_MAX - 1);
89 90 91 92 93

	if (WARN_ON(used > max))
		return 0;

	return max - used;
94 95 96 97 98
}

/*
 * iwl_queue_init - Initialize queue's high/low-water and read/write indexes
 */
99
static int iwl_queue_init(struct iwl_queue *q, int slots_num, u32 id)
100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165
{
	q->n_window = slots_num;
	q->id = id;

	/* slots_num must be power-of-two size, otherwise
	 * get_cmd_index is broken. */
	if (WARN_ON(!is_power_of_2(slots_num)))
		return -EINVAL;

	q->low_mark = q->n_window / 4;
	if (q->low_mark < 4)
		q->low_mark = 4;

	q->high_mark = q->n_window / 8;
	if (q->high_mark < 2)
		q->high_mark = 2;

	q->write_ptr = 0;
	q->read_ptr = 0;

	return 0;
}

static int iwl_pcie_alloc_dma_ptr(struct iwl_trans *trans,
				  struct iwl_dma_ptr *ptr, size_t size)
{
	if (WARN_ON(ptr->addr))
		return -EINVAL;

	ptr->addr = dma_alloc_coherent(trans->dev, size,
				       &ptr->dma, GFP_KERNEL);
	if (!ptr->addr)
		return -ENOMEM;
	ptr->size = size;
	return 0;
}

static void iwl_pcie_free_dma_ptr(struct iwl_trans *trans,
				  struct iwl_dma_ptr *ptr)
{
	if (unlikely(!ptr->addr))
		return;

	dma_free_coherent(trans->dev, ptr->size, ptr->addr, ptr->dma);
	memset(ptr, 0, sizeof(*ptr));
}

static void iwl_pcie_txq_stuck_timer(unsigned long data)
{
	struct iwl_txq *txq = (void *)data;
	struct iwl_trans_pcie *trans_pcie = txq->trans_pcie;
	struct iwl_trans *trans = iwl_trans_pcie_get_trans(trans_pcie);
	u32 scd_sram_addr = trans_pcie->scd_base_addr +
				SCD_TX_STTS_QUEUE_OFFSET(txq->q.id);
	u8 buf[16];
	int i;

	spin_lock(&txq->lock);
	/* check if triggered erroneously */
	if (txq->q.read_ptr == txq->q.write_ptr) {
		spin_unlock(&txq->lock);
		return;
	}
	spin_unlock(&txq->lock);

	IWL_ERR(trans, "Queue %d stuck for %u ms.\n", txq->q.id,
166
		jiffies_to_msecs(txq->wd_timeout));
167 168 169
	IWL_ERR(trans, "Current SW read_ptr %d write_ptr %d\n",
		txq->q.read_ptr, txq->q.write_ptr);

170
	iwl_trans_read_mem_bytes(trans, scd_sram_addr, buf, sizeof(buf));
171 172 173 174 175 176 177 178 179 180 181 182

	iwl_print_hex_error(trans, buf, sizeof(buf));

	for (i = 0; i < FH_TCSR_CHNL_NUM; i++)
		IWL_ERR(trans, "FH TRBs(%d) = 0x%08x\n", i,
			iwl_read_direct32(trans, FH_TX_TRB_REG(i)));

	for (i = 0; i < trans->cfg->base_params->num_of_queues; i++) {
		u32 status = iwl_read_prph(trans, SCD_QUEUE_STATUS_BITS(i));
		u8 fifo = (status >> SCD_QUEUE_STTS_REG_POS_TXF) & 0x7;
		bool active = !!(status & BIT(SCD_QUEUE_STTS_REG_POS_ACTIVE));
		u32 tbl_dw =
183 184 185
			iwl_trans_read_mem32(trans,
					     trans_pcie->scd_base_addr +
					     SCD_TRANS_TBL_OFFSET_QUEUE(i));
186 187 188 189 190 191 192 193 194

		if (i & 0x1)
			tbl_dw = (tbl_dw & 0xFFFF0000) >> 16;
		else
			tbl_dw = tbl_dw & 0x0000FFFF;

		IWL_ERR(trans,
			"Q %d is %sactive and mapped to fifo %d ra_tid 0x%04x [%d,%d]\n",
			i, active ? "" : "in", fifo, tbl_dw,
195 196
			iwl_read_prph(trans, SCD_QUEUE_RDPTR(i)) &
				(TFD_QUEUE_SIZE_MAX - 1),
197 198 199
			iwl_read_prph(trans, SCD_QUEUE_WRPTR(i)));
	}

L
Liad Kaufman 已提交
200
	iwl_force_nmi(trans);
201 202
}

203 204
/*
 * iwl_pcie_txq_update_byte_cnt_tbl - Set up entry in Tx byte-count array
205
 */
206 207
static void iwl_pcie_txq_update_byte_cnt_tbl(struct iwl_trans *trans,
					     struct iwl_txq *txq, u16 byte_cnt)
208
{
209
	struct iwlagn_scd_bc_tbl *scd_bc_tbl;
210
	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
211 212 213 214 215 216
	int write_ptr = txq->q.write_ptr;
	int txq_id = txq->q.id;
	u8 sec_ctl = 0;
	u8 sta_id = 0;
	u16 len = byte_cnt + IWL_TX_CRC_SIZE + IWL_TX_DELIMITER_SIZE;
	__le16 bc_ent;
217
	struct iwl_tx_cmd *tx_cmd =
218
		(void *) txq->entries[txq->q.write_ptr].cmd->payload;
219

220 221
	scd_bc_tbl = trans_pcie->scd_bc_tbls.addr;

222 223
	sta_id = tx_cmd->sta_id;
	sec_ctl = tx_cmd->sec_ctl;
224 225 226

	switch (sec_ctl & TX_CMD_SEC_MSK) {
	case TX_CMD_SEC_CCM:
227
		len += IEEE80211_CCMP_MIC_LEN;
228 229
		break;
	case TX_CMD_SEC_TKIP:
230
		len += IEEE80211_TKIP_ICV_LEN;
231 232
		break;
	case TX_CMD_SEC_WEP:
233
		len += IEEE80211_WEP_IV_LEN + IEEE80211_WEP_ICV_LEN;
234 235 236
		break;
	}

237 238 239
	if (trans_pcie->bc_table_dword)
		len = DIV_ROUND_UP(len, 4);

240 241 242
	if (WARN_ON(len > 0xFFF || write_ptr >= TFD_QUEUE_SIZE_MAX))
		return;

243
	bc_ent = cpu_to_le16(len | (sta_id << 12));
244 245 246 247 248 249 250 251

	scd_bc_tbl[txq_id].tfd_offset[write_ptr] = bc_ent;

	if (write_ptr < TFD_QUEUE_SIZE_BC_DUP)
		scd_bc_tbl[txq_id].
			tfd_offset[TFD_QUEUE_SIZE_MAX + write_ptr] = bc_ent;
}

252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277
static void iwl_pcie_txq_inval_byte_cnt_tbl(struct iwl_trans *trans,
					    struct iwl_txq *txq)
{
	struct iwl_trans_pcie *trans_pcie =
		IWL_TRANS_GET_PCIE_TRANS(trans);
	struct iwlagn_scd_bc_tbl *scd_bc_tbl = trans_pcie->scd_bc_tbls.addr;
	int txq_id = txq->q.id;
	int read_ptr = txq->q.read_ptr;
	u8 sta_id = 0;
	__le16 bc_ent;
	struct iwl_tx_cmd *tx_cmd =
		(void *)txq->entries[txq->q.read_ptr].cmd->payload;

	WARN_ON(read_ptr >= TFD_QUEUE_SIZE_MAX);

	if (txq_id != trans_pcie->cmd_queue)
		sta_id = tx_cmd->sta_id;

	bc_ent = cpu_to_le16(1 | (sta_id << 12));
	scd_bc_tbl[txq_id].tfd_offset[read_ptr] = bc_ent;

	if (read_ptr < TFD_QUEUE_SIZE_BC_DUP)
		scd_bc_tbl[txq_id].
			tfd_offset[TFD_QUEUE_SIZE_MAX + read_ptr] = bc_ent;
}

278 279
/*
 * iwl_pcie_txq_inc_wr_ptr - Send new write index to hardware
280
 */
281 282
static void iwl_pcie_txq_inc_wr_ptr(struct iwl_trans *trans,
				    struct iwl_txq *txq)
283
{
284
	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
285 286 287
	u32 reg = 0;
	int txq_id = txq->q.id;

288
	lockdep_assert_held(&txq->lock);
289

290 291 292 293 294 295 296 297 298
	/*
	 * explicitly wake up the NIC if:
	 * 1. shadow registers aren't enabled
	 * 2. NIC is woken up for CMD regardless of shadow outside this function
	 * 3. there is a chance that the NIC is asleep
	 */
	if (!trans->cfg->base_params->shadow_reg_enable &&
	    txq_id != trans_pcie->cmd_queue &&
	    test_bit(STATUS_TPOWER_PMI, &trans->status)) {
W
Wey-Yi Guy 已提交
299
		/*
300 301 302
		 * wake up nic if it's powered down ...
		 * uCode will wake up, and interrupt us again, so next
		 * time we'll skip this part.
W
Wey-Yi Guy 已提交
303
		 */
304 305 306 307 308 309 310
		reg = iwl_read32(trans, CSR_UCODE_DRV_GP1);

		if (reg & CSR_UCODE_DRV_GP1_BIT_MAC_SLEEP) {
			IWL_DEBUG_INFO(trans, "Tx queue %d requesting wakeup, GP1 = 0x%x\n",
				       txq_id, reg);
			iwl_set_bit(trans, CSR_GP_CNTRL,
				    CSR_GP_CNTRL_REG_FLAG_MAC_ACCESS_REQ);
311
			txq->need_update = true;
312 313
			return;
		}
W
Wey-Yi Guy 已提交
314
	}
315 316 317 318 319 320 321

	/*
	 * if not in power-save mode, uCode will never sleep when we're
	 * trying to tx (during RFKILL, we're not trying to tx).
	 */
	IWL_DEBUG_TX(trans, "Q:%d WR: 0x%x\n", txq_id, txq->q.write_ptr);
	iwl_write32(trans, HBUS_TARG_WRPTR, txq->q.write_ptr | (txq_id << 8));
322
}
323

324 325 326 327 328 329 330 331
void iwl_pcie_txq_check_wrptrs(struct iwl_trans *trans)
{
	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
	int i;

	for (i = 0; i < trans->cfg->base_params->num_of_queues; i++) {
		struct iwl_txq *txq = &trans_pcie->txq[i];

332
		spin_lock_bh(&txq->lock);
333 334 335 336
		if (trans_pcie->txq[i].need_update) {
			iwl_pcie_txq_inc_wr_ptr(trans, txq);
			trans_pcie->txq[i].need_update = false;
		}
337
		spin_unlock_bh(&txq->lock);
338
	}
339 340
}

341
static inline dma_addr_t iwl_pcie_tfd_tb_get_addr(struct iwl_tfd *tfd, u8 idx)
J
Johannes Berg 已提交
342 343 344 345 346 347 348 349 350 351 352
{
	struct iwl_tfd_tb *tb = &tfd->tbs[idx];

	dma_addr_t addr = get_unaligned_le32(&tb->lo);
	if (sizeof(dma_addr_t) > sizeof(u32))
		addr |=
		((dma_addr_t)(le16_to_cpu(tb->hi_n_len) & 0xF) << 16) << 16;

	return addr;
}

353 354
static inline void iwl_pcie_tfd_set_tb(struct iwl_tfd *tfd, u8 idx,
				       dma_addr_t addr, u16 len)
J
Johannes Berg 已提交
355 356 357 358 359 360 361 362 363 364 365 366 367
{
	struct iwl_tfd_tb *tb = &tfd->tbs[idx];
	u16 hi_n_len = len << 4;

	put_unaligned_le32(addr, &tb->lo);
	if (sizeof(dma_addr_t) > sizeof(u32))
		hi_n_len |= ((addr >> 16) >> 16) & 0xF;

	tb->hi_n_len = cpu_to_le16(hi_n_len);

	tfd->num_tbs = idx + 1;
}

368
static inline u8 iwl_pcie_tfd_get_num_tbs(struct iwl_tfd *tfd)
J
Johannes Berg 已提交
369 370 371 372
{
	return tfd->num_tbs & 0x1f;
}

373
static void iwl_pcie_tfd_unmap(struct iwl_trans *trans,
374 375
			       struct iwl_cmd_meta *meta,
			       struct iwl_tfd *tfd)
J
Johannes Berg 已提交
376 377 378 379 380
{
	int i;
	int num_tbs;

	/* Sanity check on number of chunks */
381
	num_tbs = iwl_pcie_tfd_get_num_tbs(tfd);
J
Johannes Berg 已提交
382 383

	if (num_tbs >= IWL_NUM_OF_TBS) {
384
		IWL_ERR(trans, "Too many chunks: %i\n", num_tbs);
J
Johannes Berg 已提交
385 386 387 388
		/* @todo issue fatal error, it is quite serious situation */
		return;
	}

389
	/* first TB is never freed - it's the scratchbuf data */
J
Johannes Berg 已提交
390

J
Johannes Berg 已提交
391 392 393 394 395 396 397 398 399 400 401 402
	for (i = 1; i < num_tbs; i++) {
		if (meta->flags & BIT(i + CMD_TB_BITMAP_POS))
			dma_unmap_page(trans->dev,
				       iwl_pcie_tfd_tb_get_addr(tfd, i),
				       iwl_pcie_tfd_tb_get_len(tfd, i),
				       DMA_TO_DEVICE);
		else
			dma_unmap_single(trans->dev,
					 iwl_pcie_tfd_tb_get_addr(tfd, i),
					 iwl_pcie_tfd_tb_get_len(tfd, i),
					 DMA_TO_DEVICE);
	}
403
	tfd->num_tbs = 0;
404 405
}

406 407
/*
 * iwl_pcie_txq_free_tfd - Free all chunks referenced by TFD [txq->q.read_ptr]
408
 * @trans - transport private data
409
 * @txq - tx queue
410
 * @dma_dir - the direction of the DMA mapping
411 412 413 414
 *
 * Does NOT advance any TFD circular buffer read/write indexes
 * Does NOT free the TFD itself (which is within circular buffer)
 */
415
static void iwl_pcie_txq_free_tfd(struct iwl_trans *trans, struct iwl_txq *txq)
416 417 418
{
	struct iwl_tfd *tfd_tmp = txq->tfds;

419 420 421
	/* rd_ptr is bounded by TFD_QUEUE_SIZE_MAX and
	 * idx is bounded by n_window
	 */
422 423 424
	int rd_ptr = txq->q.read_ptr;
	int idx = get_cmd_index(&txq->q, rd_ptr);

425 426
	lockdep_assert_held(&txq->lock);

427 428 429
	/* We have only q->n_window txq->entries, but we use
	 * TFD_QUEUE_SIZE_MAX tfds
	 */
430
	iwl_pcie_tfd_unmap(trans, &txq->entries[idx].meta, &tfd_tmp[rd_ptr]);
J
Johannes Berg 已提交
431 432

	/* free SKB */
433
	if (txq->entries) {
J
Johannes Berg 已提交
434 435
		struct sk_buff *skb;

436
		skb = txq->entries[idx].skb;
J
Johannes Berg 已提交
437

438 439 440 441
		/* Can be called from irqs-disabled context
		 * If skb is not NULL, it means that the whole queue is being
		 * freed and that the queue is not empty - free the skb
		 */
J
Johannes Berg 已提交
442
		if (skb) {
443
			iwl_op_mode_free_skb(trans->op_mode, skb);
444
			txq->entries[idx].skb = NULL;
J
Johannes Berg 已提交
445 446 447 448
		}
	}
}

449
static int iwl_pcie_txq_build_tfd(struct iwl_trans *trans, struct iwl_txq *txq,
450
				  dma_addr_t addr, u16 len, bool reset)
J
Johannes Berg 已提交
451 452 453 454 455 456
{
	struct iwl_queue *q;
	struct iwl_tfd *tfd, *tfd_tmp;
	u32 num_tbs;

	q = &txq->q;
457
	tfd_tmp = txq->tfds;
J
Johannes Berg 已提交
458 459
	tfd = &tfd_tmp[q->write_ptr];

460 461 462 463 464 465 466 467 468 469 470 471
	if (reset)
		memset(tfd, 0, sizeof(*tfd));

	num_tbs = iwl_pcie_tfd_get_num_tbs(tfd);

	/* Each TFD can point to a maximum 20 Tx buffers */
	if (num_tbs >= IWL_NUM_OF_TBS) {
		IWL_ERR(trans, "Error can not send more than %d chunks\n",
			IWL_NUM_OF_TBS);
		return -EINVAL;
	}

472 473
	if (WARN(addr & ~IWL_TX_DMA_MASK,
		 "Unaligned address = %llx\n", (unsigned long long)addr))
474 475 476 477
		return -EINVAL;

	iwl_pcie_tfd_set_tb(tfd, num_tbs, addr, len);

J
Johannes Berg 已提交
478
	return num_tbs;
479 480 481 482 483 484 485 486
}

static int iwl_pcie_txq_alloc(struct iwl_trans *trans,
			       struct iwl_txq *txq, int slots_num,
			       u32 txq_id)
{
	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
	size_t tfd_sz = sizeof(struct iwl_tfd) * TFD_QUEUE_SIZE_MAX;
487
	size_t scratchbuf_sz;
488 489 490 491 492 493 494 495 496 497 498 499 500 501 502 503 504 505 506 507 508 509 510 511 512 513 514 515 516 517 518
	int i;

	if (WARN_ON(txq->entries || txq->tfds))
		return -EINVAL;

	setup_timer(&txq->stuck_timer, iwl_pcie_txq_stuck_timer,
		    (unsigned long)txq);
	txq->trans_pcie = trans_pcie;

	txq->q.n_window = slots_num;

	txq->entries = kcalloc(slots_num,
			       sizeof(struct iwl_pcie_txq_entry),
			       GFP_KERNEL);

	if (!txq->entries)
		goto error;

	if (txq_id == trans_pcie->cmd_queue)
		for (i = 0; i < slots_num; i++) {
			txq->entries[i].cmd =
				kmalloc(sizeof(struct iwl_device_cmd),
					GFP_KERNEL);
			if (!txq->entries[i].cmd)
				goto error;
		}

	/* Circular buffer of transmit frame descriptors (TFDs),
	 * shared with device */
	txq->tfds = dma_alloc_coherent(trans->dev, tfd_sz,
				       &txq->q.dma_addr, GFP_KERNEL);
519
	if (!txq->tfds)
520
		goto error;
521 522 523 524 525 526 527 528 529 530 531 532 533 534

	BUILD_BUG_ON(IWL_HCMD_SCRATCHBUF_SIZE != sizeof(*txq->scratchbufs));
	BUILD_BUG_ON(offsetof(struct iwl_pcie_txq_scratch_buf, scratch) !=
			sizeof(struct iwl_cmd_header) +
			offsetof(struct iwl_tx_cmd, scratch));

	scratchbuf_sz = sizeof(*txq->scratchbufs) * slots_num;

	txq->scratchbufs = dma_alloc_coherent(trans->dev, scratchbuf_sz,
					      &txq->scratchbufs_dma,
					      GFP_KERNEL);
	if (!txq->scratchbufs)
		goto err_free_tfds;

535 536 537
	txq->q.id = txq_id;

	return 0;
538 539
err_free_tfds:
	dma_free_coherent(trans->dev, tfd_sz, txq->tfds, txq->q.dma_addr);
540 541 542 543 544 545 546 547 548 549 550 551 552 553 554 555
error:
	if (txq->entries && txq_id == trans_pcie->cmd_queue)
		for (i = 0; i < slots_num; i++)
			kfree(txq->entries[i].cmd);
	kfree(txq->entries);
	txq->entries = NULL;

	return -ENOMEM;

}

static int iwl_pcie_txq_init(struct iwl_trans *trans, struct iwl_txq *txq,
			      int slots_num, u32 txq_id)
{
	int ret;

556
	txq->need_update = false;
557 558 559 560 561 562

	/* TFD_QUEUE_SIZE_MAX must be power-of-two size, otherwise
	 * iwl_queue_inc_wrap and iwl_queue_dec_wrap are broken. */
	BUILD_BUG_ON(TFD_QUEUE_SIZE_MAX & (TFD_QUEUE_SIZE_MAX - 1));

	/* Initialize queue's high/low-water marks, and head/tail indexes */
563
	ret = iwl_queue_init(&txq->q, slots_num, txq_id);
564 565 566 567 568 569 570 571 572 573 574 575 576 577 578 579 580 581 582 583 584 585 586 587 588 589
	if (ret)
		return ret;

	spin_lock_init(&txq->lock);

	/*
	 * Tell nic where to find circular buffer of Tx Frame Descriptors for
	 * given Tx queue, and enable the DMA channel used for that queue.
	 * Circular buffer (TFD queue in DRAM) physical base address */
	iwl_write_direct32(trans, FH_MEM_CBBC_QUEUE(txq_id),
			   txq->q.dma_addr >> 8);

	return 0;
}

/*
 * iwl_pcie_txq_unmap -  Unmap any remaining DMA mappings and free skb's
 */
static void iwl_pcie_txq_unmap(struct iwl_trans *trans, int txq_id)
{
	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
	struct iwl_txq *txq = &trans_pcie->txq[txq_id];
	struct iwl_queue *q = &txq->q;

	spin_lock_bh(&txq->lock);
	while (q->write_ptr != q->read_ptr) {
590 591
		IWL_DEBUG_TX_REPLY(trans, "Q %d Free %d\n",
				   txq_id, q->read_ptr);
592
		iwl_pcie_txq_free_tfd(trans, txq);
593
		q->read_ptr = iwl_queue_inc_wrap(q->read_ptr);
594
	}
595
	txq->active = false;
596
	spin_unlock_bh(&txq->lock);
597 598 599

	/* just in case - this queue may have been stopped */
	iwl_wake_queue(trans, txq);
600 601 602 603 604 605 606 607 608 609 610 611 612 613 614 615 616 617 618 619 620 621 622 623 624
}

/*
 * iwl_pcie_txq_free - Deallocate DMA queue.
 * @txq: Transmit queue to deallocate.
 *
 * Empty queue by removing and destroying all BD's.
 * Free all buffers.
 * 0-fill, but do not free "txq" descriptor structure.
 */
static void iwl_pcie_txq_free(struct iwl_trans *trans, int txq_id)
{
	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
	struct iwl_txq *txq = &trans_pcie->txq[txq_id];
	struct device *dev = trans->dev;
	int i;

	if (WARN_ON(!txq))
		return;

	iwl_pcie_txq_unmap(trans, txq_id);

	/* De-alloc array of command/tx buffers */
	if (txq_id == trans_pcie->cmd_queue)
		for (i = 0; i < txq->q.n_window; i++) {
625 626
			kzfree(txq->entries[i].cmd);
			kzfree(txq->entries[i].free_buf);
627 628 629
		}

	/* De-alloc circular buffer of TFDs */
630 631 632 633
	if (txq->tfds) {
		dma_free_coherent(dev,
				  sizeof(struct iwl_tfd) * TFD_QUEUE_SIZE_MAX,
				  txq->tfds, txq->q.dma_addr);
634
		txq->q.dma_addr = 0;
635
		txq->tfds = NULL;
636 637 638 639

		dma_free_coherent(dev,
				  sizeof(*txq->scratchbufs) * txq->q.n_window,
				  txq->scratchbufs, txq->scratchbufs_dma);
640 641 642 643 644 645 646 647 648 649 650 651 652 653
	}

	kfree(txq->entries);
	txq->entries = NULL;

	del_timer_sync(&txq->stuck_timer);

	/* 0-fill queue descriptor structure */
	memset(txq, 0, sizeof(*txq));
}

void iwl_pcie_tx_start(struct iwl_trans *trans, u32 scd_base_addr)
{
	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
654
	int nq = trans->cfg->base_params->num_of_queues;
655 656
	int chan;
	u32 reg_val;
657 658
	int clear_dwords = (SCD_TRANS_TBL_OFFSET_QUEUE(nq) -
				SCD_CONTEXT_MEM_LOWER_BOUND) / sizeof(u32);
659 660 661 662 663 664 665 666 667 668 669

	/* make sure all queue are not stopped/used */
	memset(trans_pcie->queue_stopped, 0, sizeof(trans_pcie->queue_stopped));
	memset(trans_pcie->queue_used, 0, sizeof(trans_pcie->queue_used));

	trans_pcie->scd_base_addr =
		iwl_read_prph(trans, SCD_SRAM_BASE_ADDR);

	WARN_ON(scd_base_addr != 0 &&
		scd_base_addr != trans_pcie->scd_base_addr);

670 671 672 673
	/* reset context data, TX status and translation data */
	iwl_trans_write_mem(trans, trans_pcie->scd_base_addr +
				   SCD_CONTEXT_MEM_LOWER_BOUND,
			    NULL, clear_dwords);
674 675 676 677 678 679 680

	iwl_write_prph(trans, SCD_DRAM_BASE_ADDR,
		       trans_pcie->scd_bc_tbls.dma >> 10);

	/* The chain extension of the SCD doesn't work well. This feature is
	 * enabled by default by the HW, so we need to disable it manually.
	 */
681 682
	if (trans->cfg->base_params->scd_chain_ext_wa)
		iwl_write_prph(trans, SCD_CHAINEXT_EN, 0);
683 684

	iwl_trans_ac_txq_enable(trans, trans_pcie->cmd_queue,
685 686
				trans_pcie->cmd_fifo,
				trans_pcie->cmd_q_wdg_timeout);
687 688

	/* Activate all Tx DMA/FIFO channels */
689
	iwl_scd_activate_fifos(trans);
690 691 692 693 694 695 696 697 698 699 700 701 702

	/* Enable DMA channel */
	for (chan = 0; chan < FH_TCSR_CHNL_NUM; chan++)
		iwl_write_direct32(trans, FH_TCSR_CHNL_TX_CONFIG_REG(chan),
				   FH_TCSR_TX_CONFIG_REG_VAL_DMA_CHNL_ENABLE |
				   FH_TCSR_TX_CONFIG_REG_VAL_DMA_CREDIT_ENABLE);

	/* Update FH chicken bits */
	reg_val = iwl_read_direct32(trans, FH_TX_CHICKEN_BITS_REG);
	iwl_write_direct32(trans, FH_TX_CHICKEN_BITS_REG,
			   reg_val | FH_TX_CHICKEN_BITS_SCD_AUTO_RETRY_EN);

	/* Enable L1-Active */
703 704 705
	if (trans->cfg->device_family != IWL_DEVICE_FAMILY_8000)
		iwl_clear_bits_prph(trans, APMG_PCIDEV_STT_REG,
				    APMG_PCIDEV_STT_VAL_L1_ACT_DIS);
706 707
}

708 709 710 711 712 713 714 715 716 717 718 719 720 721 722 723 724 725 726 727
void iwl_trans_pcie_tx_reset(struct iwl_trans *trans)
{
	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
	int txq_id;

	for (txq_id = 0; txq_id < trans->cfg->base_params->num_of_queues;
	     txq_id++) {
		struct iwl_txq *txq = &trans_pcie->txq[txq_id];

		iwl_write_direct32(trans, FH_MEM_CBBC_QUEUE(txq_id),
				   txq->q.dma_addr >> 8);
		iwl_pcie_txq_unmap(trans, txq_id);
		txq->q.read_ptr = 0;
		txq->q.write_ptr = 0;
	}

	/* Tell NIC where to find the "keep warm" buffer */
	iwl_write_direct32(trans, FH_KW_MEM_ADDR_REG,
			   trans_pcie->kw.dma >> 4);

728 729 730 731 732 733
	/*
	 * Send 0 as the scd_base_addr since the device may have be reset
	 * while we were in WoWLAN in which case SCD_SRAM_BASE_ADDR will
	 * contain garbage.
	 */
	iwl_pcie_tx_start(trans, 0);
734 735
}

736 737 738 739 740 741 742 743 744 745 746 747 748 749 750 751 752 753 754 755 756 757 758 759 760 761 762 763 764 765 766
static void iwl_pcie_tx_stop_fh(struct iwl_trans *trans)
{
	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
	unsigned long flags;
	int ch, ret;
	u32 mask = 0;

	spin_lock(&trans_pcie->irq_lock);

	if (!iwl_trans_grab_nic_access(trans, false, &flags))
		goto out;

	/* Stop each Tx DMA channel */
	for (ch = 0; ch < FH_TCSR_CHNL_NUM; ch++) {
		iwl_write32(trans, FH_TCSR_CHNL_TX_CONFIG_REG(ch), 0x0);
		mask |= FH_TSSR_TX_STATUS_REG_MSK_CHNL_IDLE(ch);
	}

	/* Wait for DMA channels to be idle */
	ret = iwl_poll_bit(trans, FH_TSSR_TX_STATUS_REG, mask, mask, 5000);
	if (ret < 0)
		IWL_ERR(trans,
			"Failing on timeout while stopping DMA channel %d [0x%08x]\n",
			ch, iwl_read32(trans, FH_TSSR_TX_STATUS_REG));

	iwl_trans_release_nic_access(trans, &flags);

out:
	spin_unlock(&trans_pcie->irq_lock);
}

767 768 769 770 771 772
/*
 * iwl_pcie_tx_stop - Stop all Tx DMA channels
 */
int iwl_pcie_tx_stop(struct iwl_trans *trans)
{
	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
773
	int txq_id;
774 775

	/* Turn off all Tx DMA fifos */
776
	iwl_scd_deactivate_fifos(trans);
777

778 779
	/* Turn off all Tx DMA channels */
	iwl_pcie_tx_stop_fh(trans);
780

781 782 783 784 785 786 787 788 789 790
	/*
	 * This function can be called before the op_mode disabled the
	 * queues. This happens when we have an rfkill interrupt.
	 * Since we stop Tx altogether - mark the queues as stopped.
	 */
	memset(trans_pcie->queue_stopped, 0, sizeof(trans_pcie->queue_stopped));
	memset(trans_pcie->queue_used, 0, sizeof(trans_pcie->queue_used));

	/* This can happen: start_hw, stop_device */
	if (!trans_pcie->txq)
791 792 793 794 795 796 797 798 799 800 801 802 803 804 805 806 807 808 809 810 811 812 813 814 815 816 817 818 819 820 821 822 823 824 825 826 827 828 829 830 831 832 833 834 835 836 837 838 839 840 841 842 843 844 845 846 847 848 849 850 851 852 853 854 855 856 857 858 859 860 861 862 863
		return 0;

	/* Unmap DMA from host system and free skb's */
	for (txq_id = 0; txq_id < trans->cfg->base_params->num_of_queues;
	     txq_id++)
		iwl_pcie_txq_unmap(trans, txq_id);

	return 0;
}

/*
 * iwl_trans_tx_free - Free TXQ Context
 *
 * Destroy all TX DMA queues and structures
 */
void iwl_pcie_tx_free(struct iwl_trans *trans)
{
	int txq_id;
	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);

	/* Tx queues */
	if (trans_pcie->txq) {
		for (txq_id = 0;
		     txq_id < trans->cfg->base_params->num_of_queues; txq_id++)
			iwl_pcie_txq_free(trans, txq_id);
	}

	kfree(trans_pcie->txq);
	trans_pcie->txq = NULL;

	iwl_pcie_free_dma_ptr(trans, &trans_pcie->kw);

	iwl_pcie_free_dma_ptr(trans, &trans_pcie->scd_bc_tbls);
}

/*
 * iwl_pcie_tx_alloc - allocate TX context
 * Allocate all Tx DMA structures and initialize them
 */
static int iwl_pcie_tx_alloc(struct iwl_trans *trans)
{
	int ret;
	int txq_id, slots_num;
	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);

	u16 scd_bc_tbls_size = trans->cfg->base_params->num_of_queues *
			sizeof(struct iwlagn_scd_bc_tbl);

	/*It is not allowed to alloc twice, so warn when this happens.
	 * We cannot rely on the previous allocation, so free and fail */
	if (WARN_ON(trans_pcie->txq)) {
		ret = -EINVAL;
		goto error;
	}

	ret = iwl_pcie_alloc_dma_ptr(trans, &trans_pcie->scd_bc_tbls,
				   scd_bc_tbls_size);
	if (ret) {
		IWL_ERR(trans, "Scheduler BC Table allocation failed\n");
		goto error;
	}

	/* Alloc keep-warm buffer */
	ret = iwl_pcie_alloc_dma_ptr(trans, &trans_pcie->kw, IWL_KW_SIZE);
	if (ret) {
		IWL_ERR(trans, "Keep Warm allocation failed\n");
		goto error;
	}

	trans_pcie->txq = kcalloc(trans->cfg->base_params->num_of_queues,
				  sizeof(struct iwl_txq), GFP_KERNEL);
	if (!trans_pcie->txq) {
		IWL_ERR(trans, "Not enough memory for txq\n");
864
		ret = -ENOMEM;
865 866 867 868 869 870 871 872 873 874 875 876 877 878 879 880 881 882 883 884 885 886 887 888 889 890 891 892 893 894 895 896 897 898 899 900 901
		goto error;
	}

	/* Alloc and init all Tx queues, including the command queue (#4/#9) */
	for (txq_id = 0; txq_id < trans->cfg->base_params->num_of_queues;
	     txq_id++) {
		slots_num = (txq_id == trans_pcie->cmd_queue) ?
					TFD_CMD_SLOTS : TFD_TX_CMD_SLOTS;
		ret = iwl_pcie_txq_alloc(trans, &trans_pcie->txq[txq_id],
					  slots_num, txq_id);
		if (ret) {
			IWL_ERR(trans, "Tx %d queue alloc failed\n", txq_id);
			goto error;
		}
	}

	return 0;

error:
	iwl_pcie_tx_free(trans);

	return ret;
}
int iwl_pcie_tx_init(struct iwl_trans *trans)
{
	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
	int ret;
	int txq_id, slots_num;
	bool alloc = false;

	if (!trans_pcie->txq) {
		ret = iwl_pcie_tx_alloc(trans);
		if (ret)
			goto error;
		alloc = true;
	}

902
	spin_lock(&trans_pcie->irq_lock);
903 904

	/* Turn off all Tx DMA fifos */
905
	iwl_scd_deactivate_fifos(trans);
906 907 908 909 910

	/* Tell NIC where to find the "keep warm" buffer */
	iwl_write_direct32(trans, FH_KW_MEM_ADDR_REG,
			   trans_pcie->kw.dma >> 4);

911
	spin_unlock(&trans_pcie->irq_lock);
912 913 914 915 916 917 918 919 920 921 922 923 924 925

	/* Alloc and init all Tx queues, including the command queue (#4/#9) */
	for (txq_id = 0; txq_id < trans->cfg->base_params->num_of_queues;
	     txq_id++) {
		slots_num = (txq_id == trans_pcie->cmd_queue) ?
					TFD_CMD_SLOTS : TFD_TX_CMD_SLOTS;
		ret = iwl_pcie_txq_init(trans, &trans_pcie->txq[txq_id],
					 slots_num, txq_id);
		if (ret) {
			IWL_ERR(trans, "Tx %d queue init failed\n", txq_id);
			goto error;
		}
	}

926
	iwl_set_bits_prph(trans, SCD_GP_CTRL, SCD_GP_CTRL_AUTO_ACTIVE_MODE);
927 928 929 930
	if (trans->cfg->base_params->num_of_queues > 20)
		iwl_set_bits_prph(trans, SCD_GP_CTRL,
				  SCD_GP_CTRL_ENABLE_31_QUEUES);

931 932 933 934 935 936 937 938
	return 0;
error:
	/*Upon error, free only if we allocated something */
	if (alloc)
		iwl_pcie_tx_free(trans);
	return ret;
}

939
static inline void iwl_pcie_txq_progress(struct iwl_txq *txq)
940
{
941 942
	lockdep_assert_held(&txq->lock);

943
	if (!txq->wd_timeout)
944 945
		return;

946 947 948 949 950 951 952
	/*
	 * station is asleep and we send data - that must
	 * be uAPSD or PS-Poll. Don't rearm the timer.
	 */
	if (txq->frozen)
		return;

953 954 955 956 957 958 959
	/*
	 * if empty delete timer, otherwise move timer forward
	 * since we're making progress on this queue
	 */
	if (txq->q.read_ptr == txq->q.write_ptr)
		del_timer(&txq->stuck_timer);
	else
960
		mod_timer(&txq->stuck_timer, jiffies + txq->wd_timeout);
961 962 963
}

/* Frees buffers until index _not_ inclusive */
964 965
void iwl_trans_pcie_reclaim(struct iwl_trans *trans, int txq_id, int ssn,
			    struct sk_buff_head *skbs)
966 967 968
{
	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
	struct iwl_txq *txq = &trans_pcie->txq[txq_id];
969
	int tfd_num = ssn & (TFD_QUEUE_SIZE_MAX - 1);
970 971 972 973 974
	struct iwl_queue *q = &txq->q;
	int last_to_free;

	/* This function is not meant to release cmd queue*/
	if (WARN_ON(txq_id == trans_pcie->cmd_queue))
975
		return;
J
Johannes Berg 已提交
976

977
	spin_lock_bh(&txq->lock);
978

979 980 981 982 983 984
	if (!txq->active) {
		IWL_DEBUG_TX_QUEUES(trans, "Q %d inactive - ignoring idx %d\n",
				    txq_id, ssn);
		goto out;
	}

985 986 987 988 989
	if (txq->q.read_ptr == tfd_num)
		goto out;

	IWL_DEBUG_TX_REPLY(trans, "[Q %d] %d -> %d (%d)\n",
			   txq_id, txq->q.read_ptr, tfd_num, ssn);
J
Johannes Berg 已提交
990

991 992
	/*Since we free until index _not_ inclusive, the one before index is
	 * the last we will free. This one must be used */
993
	last_to_free = iwl_queue_dec_wrap(tfd_num);
994

995
	if (!iwl_queue_used(q, last_to_free)) {
996 997
		IWL_ERR(trans,
			"%s: Read index for DMA queue txq id (%d), last_to_free %d is out of range [0-%d] %d %d.\n",
998
			__func__, txq_id, last_to_free, TFD_QUEUE_SIZE_MAX,
999
			q->write_ptr, q->read_ptr);
1000
		goto out;
J
Johannes Berg 已提交
1001 1002
	}

1003
	if (WARN_ON(!skb_queue_empty(skbs)))
1004
		goto out;
J
Johannes Berg 已提交
1005

1006
	for (;
1007
	     q->read_ptr != tfd_num;
1008
	     q->read_ptr = iwl_queue_inc_wrap(q->read_ptr)) {
J
Johannes Berg 已提交
1009

1010 1011
		if (WARN_ON_ONCE(txq->entries[txq->q.read_ptr].skb == NULL))
			continue;
J
Johannes Berg 已提交
1012

1013
		__skb_queue_tail(skbs, txq->entries[txq->q.read_ptr].skb);
J
Johannes Berg 已提交
1014

1015
		txq->entries[txq->q.read_ptr].skb = NULL;
1016

1017
		iwl_pcie_txq_inval_byte_cnt_tbl(trans, txq);
1018

1019
		iwl_pcie_txq_free_tfd(trans, txq);
1020
	}
1021

1022
	iwl_pcie_txq_progress(txq);
1023

1024 1025
	if (iwl_queue_space(&txq->q) > txq->q.low_mark)
		iwl_wake_queue(trans, txq);
1026 1027 1028 1029 1030 1031

	if (q->read_ptr == q->write_ptr) {
		IWL_DEBUG_RPM(trans, "Q %d - last tx reclaimed\n", q->id);
		iwl_trans_pcie_unref(trans);
	}

1032
out:
1033
	spin_unlock_bh(&txq->lock);
1034 1035
}

1036 1037
static int iwl_pcie_set_cmd_in_flight(struct iwl_trans *trans,
				      const struct iwl_host_cmd *cmd)
1038 1039 1040 1041 1042 1043
{
	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
	int ret;

	lockdep_assert_held(&trans_pcie->reg_lock);

1044 1045 1046 1047 1048 1049 1050
	if (!(cmd->flags & CMD_SEND_IN_IDLE) &&
	    !trans_pcie->ref_cmd_in_flight) {
		trans_pcie->ref_cmd_in_flight = true;
		IWL_DEBUG_RPM(trans, "set ref_cmd_in_flight - ref\n");
		iwl_trans_pcie_ref(trans);
	}

1051 1052 1053 1054 1055 1056
	/*
	 * wake up the NIC to make sure that the firmware will see the host
	 * command - we will let the NIC sleep once all the host commands
	 * returned. This needs to be done only on NICs that have
	 * apmg_wake_up_wa set.
	 */
1057 1058
	if (trans->cfg->base_params->apmg_wake_up_wa &&
	    !trans_pcie->cmd_hold_nic_awake) {
1059 1060 1061 1062 1063 1064 1065 1066 1067 1068 1069 1070 1071 1072
		__iwl_trans_pcie_set_bit(trans, CSR_GP_CNTRL,
					 CSR_GP_CNTRL_REG_FLAG_MAC_ACCESS_REQ);

		ret = iwl_poll_bit(trans, CSR_GP_CNTRL,
				   CSR_GP_CNTRL_REG_VAL_MAC_ACCESS_EN,
				   (CSR_GP_CNTRL_REG_FLAG_MAC_CLOCK_READY |
				    CSR_GP_CNTRL_REG_FLAG_GOING_TO_SLEEP),
				   15000);
		if (ret < 0) {
			__iwl_trans_pcie_clear_bit(trans, CSR_GP_CNTRL,
					CSR_GP_CNTRL_REG_FLAG_MAC_ACCESS_REQ);
			IWL_ERR(trans, "Failed to wake NIC for hcmd\n");
			return -EIO;
		}
1073
		trans_pcie->cmd_hold_nic_awake = true;
1074 1075 1076 1077 1078 1079 1080 1081 1082 1083 1084
	}

	return 0;
}

static int iwl_pcie_clear_cmd_in_flight(struct iwl_trans *trans)
{
	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);

	lockdep_assert_held(&trans_pcie->reg_lock);

1085 1086 1087 1088 1089 1090
	if (trans_pcie->ref_cmd_in_flight) {
		trans_pcie->ref_cmd_in_flight = false;
		IWL_DEBUG_RPM(trans, "clear ref_cmd_in_flight - unref\n");
		iwl_trans_pcie_unref(trans);
	}

1091 1092 1093
	if (trans->cfg->base_params->apmg_wake_up_wa) {
		if (WARN_ON(!trans_pcie->cmd_hold_nic_awake))
			return 0;
1094

1095
		trans_pcie->cmd_hold_nic_awake = false;
1096
		__iwl_trans_pcie_clear_bit(trans, CSR_GP_CNTRL,
1097 1098
					   CSR_GP_CNTRL_REG_FLAG_MAC_ACCESS_REQ);
	}
1099 1100 1101
	return 0;
}

1102 1103 1104 1105 1106 1107 1108 1109
/*
 * iwl_pcie_cmdq_reclaim - Reclaim TX command queue entries already Tx'd
 *
 * When FW advances 'R' index, all entries between old and new 'R' index
 * need to be reclaimed. As result, some free space forms.  If there is
 * enough free space (> low mark), wake the stack that feeds us.
 */
static void iwl_pcie_cmdq_reclaim(struct iwl_trans *trans, int txq_id, int idx)
1110
{
1111 1112 1113
	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
	struct iwl_txq *txq = &trans_pcie->txq[txq_id];
	struct iwl_queue *q = &txq->q;
1114
	unsigned long flags;
1115
	int nfreed = 0;
1116

1117
	lockdep_assert_held(&txq->lock);
1118

1119
	if ((idx >= TFD_QUEUE_SIZE_MAX) || (!iwl_queue_used(q, idx))) {
1120 1121
		IWL_ERR(trans,
			"%s: Read index for DMA queue txq id (%d), index %d is out of range [0-%d] %d %d.\n",
1122
			__func__, txq_id, idx, TFD_QUEUE_SIZE_MAX,
1123 1124 1125
			q->write_ptr, q->read_ptr);
		return;
	}
1126

1127 1128
	for (idx = iwl_queue_inc_wrap(idx); q->read_ptr != idx;
	     q->read_ptr = iwl_queue_inc_wrap(q->read_ptr)) {
1129

1130 1131 1132
		if (nfreed++ > 0) {
			IWL_ERR(trans, "HCMD skipped: index (%d) %d %d\n",
				idx, q->write_ptr, q->read_ptr);
L
Liad Kaufman 已提交
1133
			iwl_force_nmi(trans);
1134 1135 1136
		}
	}

1137
	if (q->read_ptr == q->write_ptr) {
1138
		spin_lock_irqsave(&trans_pcie->reg_lock, flags);
1139
		iwl_pcie_clear_cmd_in_flight(trans);
1140 1141 1142
		spin_unlock_irqrestore(&trans_pcie->reg_lock, flags);
	}

1143
	iwl_pcie_txq_progress(txq);
1144 1145
}

1146
static int iwl_pcie_txq_set_ratid_map(struct iwl_trans *trans, u16 ra_tid,
1147
				 u16 txq_id)
1148
{
1149
	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
1150 1151 1152 1153 1154 1155
	u32 tbl_dw_addr;
	u32 tbl_dw;
	u16 scd_q2ratid;

	scd_q2ratid = ra_tid & SCD_QUEUE_RA_TID_MAP_RATID_MSK;

1156
	tbl_dw_addr = trans_pcie->scd_base_addr +
1157 1158
			SCD_TRANS_TBL_OFFSET_QUEUE(txq_id);

1159
	tbl_dw = iwl_trans_read_mem32(trans, tbl_dw_addr);
1160 1161 1162 1163 1164 1165

	if (txq_id & 0x1)
		tbl_dw = (scd_q2ratid << 16) | (tbl_dw & 0x0000FFFF);
	else
		tbl_dw = scd_q2ratid | (tbl_dw & 0xFFFF0000);

1166
	iwl_trans_write_mem32(trans, tbl_dw_addr, tbl_dw);
1167 1168 1169 1170

	return 0;
}

1171 1172 1173 1174
/* Receiver address (actually, Rx station's index into station table),
 * combined with Traffic ID (QOS priority), in format used by Tx Scheduler */
#define BUILD_RAxTID(sta_id, tid)	(((sta_id) << 4) + (tid))

1175
void iwl_trans_pcie_txq_enable(struct iwl_trans *trans, int txq_id, u16 ssn,
1176 1177
			       const struct iwl_trans_txq_scd_cfg *cfg,
			       unsigned int wdg_timeout)
1178
{
1179
	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
1180
	struct iwl_txq *txq = &trans_pcie->txq[txq_id];
1181
	int fifo = -1;
1182

1183 1184
	if (test_and_set_bit(txq_id, trans_pcie->queue_used))
		WARN_ONCE(1, "queue %d already used - expect issues", txq_id);
1185

1186 1187
	txq->wd_timeout = msecs_to_jiffies(wdg_timeout);

1188 1189
	if (cfg) {
		fifo = cfg->fifo;
1190

1191
		/* Disable the scheduler prior configuring the cmd queue */
1192 1193
		if (txq_id == trans_pcie->cmd_queue &&
		    trans_pcie->scd_set_active)
1194 1195
			iwl_scd_enable_set_active(trans, 0);

1196 1197
		/* Stop this Tx queue before configuring it */
		iwl_scd_txq_set_inactive(trans, txq_id);
1198

1199 1200 1201
		/* Set this queue as a chain-building queue unless it is CMD */
		if (txq_id != trans_pcie->cmd_queue)
			iwl_scd_txq_set_chain(trans, txq_id);
1202

1203
		if (cfg->aggregate) {
1204
			u16 ra_tid = BUILD_RAxTID(cfg->sta_id, cfg->tid);
1205

1206 1207
			/* Map receiver-address / traffic-ID to this queue */
			iwl_pcie_txq_set_ratid_map(trans, ra_tid, txq_id);
1208

1209 1210
			/* enable aggregations for the queue */
			iwl_scd_txq_enable_agg(trans, txq_id);
1211
			txq->ampdu = true;
1212 1213 1214 1215 1216 1217 1218 1219
		} else {
			/*
			 * disable aggregations for the queue, this will also
			 * make the ra_tid mapping configuration irrelevant
			 * since it is now a non-AGG queue.
			 */
			iwl_scd_txq_disable_agg(trans, txq_id);

1220
			ssn = txq->q.read_ptr;
1221
		}
1222
	}
1223 1224 1225

	/* Place first TFD at index corresponding to start sequence number.
	 * Assumes that ssn_idx is valid (!= 0xFFF) */
1226 1227
	txq->q.read_ptr = (ssn & 0xff);
	txq->q.write_ptr = (ssn & 0xff);
1228 1229
	iwl_write_direct32(trans, HBUS_TARG_WRPTR,
			   (ssn & 0xff) | (txq_id << 8));
1230

1231 1232
	if (cfg) {
		u8 frame_limit = cfg->frame_limit;
1233

1234 1235 1236 1237 1238 1239 1240
		iwl_write_prph(trans, SCD_QUEUE_RDPTR(txq_id), ssn);

		/* Set up Tx window size and frame limit for this queue */
		iwl_trans_write_mem32(trans, trans_pcie->scd_base_addr +
				SCD_CONTEXT_QUEUE_OFFSET(txq_id), 0);
		iwl_trans_write_mem32(trans,
			trans_pcie->scd_base_addr +
1241 1242
			SCD_CONTEXT_QUEUE_OFFSET(txq_id) + sizeof(u32),
			((frame_limit << SCD_QUEUE_CTX_REG2_WIN_SIZE_POS) &
1243
					SCD_QUEUE_CTX_REG2_WIN_SIZE_MSK) |
1244
			((frame_limit << SCD_QUEUE_CTX_REG2_FRAME_LIMIT_POS) &
1245 1246 1247 1248 1249 1250 1251 1252
					SCD_QUEUE_CTX_REG2_FRAME_LIMIT_MSK));

		/* Set up status area in SRAM, map to Tx DMA/FIFO, activate */
		iwl_write_prph(trans, SCD_QUEUE_STATUS_BITS(txq_id),
			       (1 << SCD_QUEUE_STTS_REG_POS_ACTIVE) |
			       (cfg->fifo << SCD_QUEUE_STTS_REG_POS_TXF) |
			       (1 << SCD_QUEUE_STTS_REG_POS_WSL) |
			       SCD_QUEUE_STTS_REG_MSK);
1253 1254

		/* enable the scheduler for this queue (only) */
1255 1256
		if (txq_id == trans_pcie->cmd_queue &&
		    trans_pcie->scd_set_active)
1257
			iwl_scd_enable_set_active(trans, BIT(txq_id));
1258 1259 1260 1261 1262 1263 1264 1265

		IWL_DEBUG_TX_QUEUES(trans,
				    "Activate queue %d on FIFO %d WrPtr: %d\n",
				    txq_id, fifo, ssn & 0xff);
	} else {
		IWL_DEBUG_TX_QUEUES(trans,
				    "Activate queue %d WrPtr: %d\n",
				    txq_id, ssn & 0xff);
1266 1267
	}

1268
	txq->active = true;
1269 1270
}

1271 1272
void iwl_trans_pcie_txq_disable(struct iwl_trans *trans, int txq_id,
				bool configure_scd)
1273
{
1274
	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
1275 1276 1277
	u32 stts_addr = trans_pcie->scd_base_addr +
			SCD_TX_STTS_QUEUE_OFFSET(txq_id);
	static const u32 zero_val[4] = {};
1278

1279 1280 1281
	trans_pcie->txq[txq_id].frozen_expiry_remainder = 0;
	trans_pcie->txq[txq_id].frozen = false;

1282 1283 1284 1285 1286 1287
	/*
	 * Upon HW Rfkill - we stop the device, and then stop the queues
	 * in the op_mode. Just for the sake of the simplicity of the op_mode,
	 * allow the op_mode to call txq_disable after it already called
	 * stop_device.
	 */
1288
	if (!test_and_clear_bit(txq_id, trans_pcie->queue_used)) {
1289 1290
		WARN_ONCE(test_bit(STATUS_DEVICE_ENABLED, &trans->status),
			  "queue %d not used", txq_id);
1291
		return;
1292 1293
	}

1294 1295
	if (configure_scd) {
		iwl_scd_txq_set_inactive(trans, txq_id);
1296

1297 1298 1299
		iwl_trans_write_mem(trans, stts_addr, (void *)zero_val,
				    ARRAY_SIZE(zero_val));
	}
1300

1301
	iwl_pcie_txq_unmap(trans, txq_id);
1302
	trans_pcie->txq[txq_id].ampdu = false;
1303

1304
	IWL_DEBUG_TX_QUEUES(trans, "Deactivate queue %d\n", txq_id);
1305 1306
}

1307 1308
/*************** HOST COMMAND QUEUE FUNCTIONS   *****/

1309
/*
1310
 * iwl_pcie_enqueue_hcmd - enqueue a uCode command
1311
 * @priv: device private data point
1312
 * @cmd: a pointer to the ucode command structure
1313
 *
1314 1315
 * The function returns < 0 values to indicate the operation
 * failed. On success, it returns the index (>= 0) of command in the
1316 1317
 * command queue.
 */
1318 1319
static int iwl_pcie_enqueue_hcmd(struct iwl_trans *trans,
				 struct iwl_host_cmd *cmd)
1320
{
1321
	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
1322
	struct iwl_txq *txq = &trans_pcie->txq[trans_pcie->cmd_queue];
1323
	struct iwl_queue *q = &txq->q;
J
Johannes Berg 已提交
1324 1325
	struct iwl_device_cmd *out_cmd;
	struct iwl_cmd_meta *out_meta;
1326
	unsigned long flags;
1327
	void *dup_buf = NULL;
1328
	dma_addr_t phys_addr;
1329
	int idx;
1330
	u16 copy_size, cmd_size, scratch_size;
1331
	bool had_nocopy = false;
1332
	u8 group_id = iwl_cmd_groupid(cmd->id);
1333
	int i, ret;
1334
	u32 cmd_pos;
1335 1336
	const u8 *cmddata[IWL_MAX_CMD_TBS_PER_TFD];
	u16 cmdlen[IWL_MAX_CMD_TBS_PER_TFD];
1337

1338 1339
	if (WARN(!trans_pcie->wide_cmd_header &&
		 group_id > IWL_ALWAYS_LONG_GROUP,
1340 1341 1342 1343 1344 1345 1346 1347 1348 1349
		 "unsupported wide command %#x\n", cmd->id))
		return -EINVAL;

	if (group_id != 0) {
		copy_size = sizeof(struct iwl_cmd_header_wide);
		cmd_size = sizeof(struct iwl_cmd_header_wide);
	} else {
		copy_size = sizeof(struct iwl_cmd_header);
		cmd_size = sizeof(struct iwl_cmd_header);
	}
1350 1351

	/* need one for the header if the first is NOCOPY */
1352
	BUILD_BUG_ON(IWL_MAX_CMD_TBS_PER_TFD > IWL_NUM_OF_TBS - 1);
1353

1354
	for (i = 0; i < IWL_MAX_CMD_TBS_PER_TFD; i++) {
1355 1356 1357
		cmddata[i] = cmd->data[i];
		cmdlen[i] = cmd->len[i];

1358 1359
		if (!cmd->len[i])
			continue;
1360

1361 1362 1363
		/* need at least IWL_HCMD_SCRATCHBUF_SIZE copied */
		if (copy_size < IWL_HCMD_SCRATCHBUF_SIZE) {
			int copy = IWL_HCMD_SCRATCHBUF_SIZE - copy_size;
1364 1365 1366 1367 1368 1369 1370 1371

			if (copy > cmdlen[i])
				copy = cmdlen[i];
			cmdlen[i] -= copy;
			cmddata[i] += copy;
			copy_size += copy;
		}

1372 1373
		if (cmd->dataflags[i] & IWL_HCMD_DFL_NOCOPY) {
			had_nocopy = true;
1374 1375 1376 1377 1378 1379 1380 1381 1382 1383 1384 1385 1386 1387 1388 1389 1390
			if (WARN_ON(cmd->dataflags[i] & IWL_HCMD_DFL_DUP)) {
				idx = -EINVAL;
				goto free_dup_buf;
			}
		} else if (cmd->dataflags[i] & IWL_HCMD_DFL_DUP) {
			/*
			 * This is also a chunk that isn't copied
			 * to the static buffer so set had_nocopy.
			 */
			had_nocopy = true;

			/* only allowed once */
			if (WARN_ON(dup_buf)) {
				idx = -EINVAL;
				goto free_dup_buf;
			}

1391
			dup_buf = kmemdup(cmddata[i], cmdlen[i],
1392 1393 1394
					  GFP_ATOMIC);
			if (!dup_buf)
				return -ENOMEM;
1395 1396
		} else {
			/* NOCOPY must not be followed by normal! */
1397 1398 1399 1400
			if (WARN_ON(had_nocopy)) {
				idx = -EINVAL;
				goto free_dup_buf;
			}
1401
			copy_size += cmdlen[i];
1402 1403 1404
		}
		cmd_size += cmd->len[i];
	}
1405

1406 1407
	/*
	 * If any of the command structures end up being larger than
1408 1409 1410
	 * the TFD_MAX_PAYLOAD_SIZE and they aren't dynamically
	 * allocated into separate TFDs, then we will need to
	 * increase the size of the buffers.
1411
	 */
1412 1413
	if (WARN(copy_size > TFD_MAX_PAYLOAD_SIZE,
		 "Command %s (%#x) is too large (%d bytes)\n",
1414
		 get_cmd_string(trans_pcie, cmd->id), cmd->id, copy_size)) {
1415 1416 1417
		idx = -EINVAL;
		goto free_dup_buf;
	}
1418

1419
	spin_lock_bh(&txq->lock);
1420

J
Johannes Berg 已提交
1421
	if (iwl_queue_space(q) < ((cmd->flags & CMD_ASYNC) ? 2 : 1)) {
1422
		spin_unlock_bh(&txq->lock);
1423

1424
		IWL_ERR(trans, "No space in command queue\n");
1425
		iwl_op_mode_cmd_queue_full(trans->op_mode);
1426 1427
		idx = -ENOSPC;
		goto free_dup_buf;
1428 1429
	}

1430
	idx = get_cmd_index(q, q->write_ptr);
1431 1432
	out_cmd = txq->entries[idx].cmd;
	out_meta = &txq->entries[idx].meta;
J
Johannes Berg 已提交
1433

1434
	memset(out_meta, 0, sizeof(*out_meta));	/* re-initialize to NULL */
J
Johannes Berg 已提交
1435 1436
	if (cmd->flags & CMD_WANT_SKB)
		out_meta->source = cmd;
1437

1438
	/* set up the header */
1439 1440 1441 1442 1443 1444 1445 1446 1447 1448 1449 1450 1451 1452 1453 1454 1455 1456 1457 1458 1459 1460 1461 1462
	if (group_id != 0) {
		out_cmd->hdr_wide.cmd = iwl_cmd_opcode(cmd->id);
		out_cmd->hdr_wide.group_id = group_id;
		out_cmd->hdr_wide.version = iwl_cmd_version(cmd->id);
		out_cmd->hdr_wide.length =
			cpu_to_le16(cmd_size -
				    sizeof(struct iwl_cmd_header_wide));
		out_cmd->hdr_wide.reserved = 0;
		out_cmd->hdr_wide.sequence =
			cpu_to_le16(QUEUE_TO_SEQ(trans_pcie->cmd_queue) |
						 INDEX_TO_SEQ(q->write_ptr));

		cmd_pos = sizeof(struct iwl_cmd_header_wide);
		copy_size = sizeof(struct iwl_cmd_header_wide);
	} else {
		out_cmd->hdr.cmd = iwl_cmd_opcode(cmd->id);
		out_cmd->hdr.sequence =
			cpu_to_le16(QUEUE_TO_SEQ(trans_pcie->cmd_queue) |
						 INDEX_TO_SEQ(q->write_ptr));
		out_cmd->hdr.group_id = 0;

		cmd_pos = sizeof(struct iwl_cmd_header);
		copy_size = sizeof(struct iwl_cmd_header);
	}
1463 1464

	/* and copy the data that needs to be copied */
1465
	for (i = 0; i < IWL_MAX_CMD_TBS_PER_TFD; i++) {
1466
		int copy;
1467

1468
		if (!cmd->len[i])
1469
			continue;
1470 1471 1472

		/* copy everything if not nocopy/dup */
		if (!(cmd->dataflags[i] & (IWL_HCMD_DFL_NOCOPY |
1473
					   IWL_HCMD_DFL_DUP))) {
1474 1475 1476 1477 1478
			copy = cmd->len[i];

			memcpy((u8 *)out_cmd + cmd_pos, cmd->data[i], copy);
			cmd_pos += copy;
			copy_size += copy;
1479 1480 1481 1482 1483 1484 1485 1486 1487 1488 1489 1490 1491 1492 1493 1494 1495 1496 1497 1498
			continue;
		}

		/*
		 * Otherwise we need at least IWL_HCMD_SCRATCHBUF_SIZE copied
		 * in total (for the scratchbuf handling), but copy up to what
		 * we can fit into the payload for debug dump purposes.
		 */
		copy = min_t(int, TFD_MAX_PAYLOAD_SIZE - cmd_pos, cmd->len[i]);

		memcpy((u8 *)out_cmd + cmd_pos, cmd->data[i], copy);
		cmd_pos += copy;

		/* However, treat copy_size the proper way, we need it below */
		if (copy_size < IWL_HCMD_SCRATCHBUF_SIZE) {
			copy = IWL_HCMD_SCRATCHBUF_SIZE - copy_size;

			if (copy > cmd->len[i])
				copy = cmd->len[i];
			copy_size += copy;
1499
		}
1500 1501
	}

J
Johannes Berg 已提交
1502
	IWL_DEBUG_HC(trans,
1503
		     "Sending command %s (%.2x.%.2x), seq: 0x%04X, %d bytes at %d[%d]:%d\n",
1504
		     get_cmd_string(trans_pcie, out_cmd->hdr.cmd),
1505 1506
		     group_id, out_cmd->hdr.cmd,
		     le16_to_cpu(out_cmd->hdr.sequence),
1507
		     cmd_size, q->write_ptr, idx, trans_pcie->cmd_queue);
1508

1509 1510 1511 1512 1513
	/* start the TFD with the scratchbuf */
	scratch_size = min_t(int, copy_size, IWL_HCMD_SCRATCHBUF_SIZE);
	memcpy(&txq->scratchbufs[q->write_ptr], &out_cmd->hdr, scratch_size);
	iwl_pcie_txq_build_tfd(trans, txq,
			       iwl_pcie_get_scratchbuf_dma(txq, q->write_ptr),
1514
			       scratch_size, true);
1515 1516 1517 1518 1519 1520 1521 1522 1523 1524 1525 1526 1527

	/* map first command fragment, if any remains */
	if (copy_size > scratch_size) {
		phys_addr = dma_map_single(trans->dev,
					   ((u8 *)&out_cmd->hdr) + scratch_size,
					   copy_size - scratch_size,
					   DMA_TO_DEVICE);
		if (dma_mapping_error(trans->dev, phys_addr)) {
			iwl_pcie_tfd_unmap(trans, out_meta,
					   &txq->tfds[q->write_ptr]);
			idx = -ENOMEM;
			goto out;
		}
1528

1529
		iwl_pcie_txq_build_tfd(trans, txq, phys_addr,
1530
				       copy_size - scratch_size, false);
J
Johannes Berg 已提交
1531 1532
	}

1533
	/* map the remaining (adjusted) nocopy/dup fragments */
1534
	for (i = 0; i < IWL_MAX_CMD_TBS_PER_TFD; i++) {
1535
		const void *data = cmddata[i];
1536

1537
		if (!cmdlen[i])
1538
			continue;
1539 1540
		if (!(cmd->dataflags[i] & (IWL_HCMD_DFL_NOCOPY |
					   IWL_HCMD_DFL_DUP)))
1541
			continue;
1542 1543 1544
		if (cmd->dataflags[i] & IWL_HCMD_DFL_DUP)
			data = dup_buf;
		phys_addr = dma_map_single(trans->dev, (void *)data,
1545
					   cmdlen[i], DMA_TO_DEVICE);
1546
		if (dma_mapping_error(trans->dev, phys_addr)) {
1547
			iwl_pcie_tfd_unmap(trans, out_meta,
1548
					   &txq->tfds[q->write_ptr]);
1549 1550 1551 1552
			idx = -ENOMEM;
			goto out;
		}

1553
		iwl_pcie_txq_build_tfd(trans, txq, phys_addr, cmdlen[i], false);
1554
	}
R
Reinette Chatre 已提交
1555

J
Johannes Berg 已提交
1556 1557
	BUILD_BUG_ON(IWL_NUM_OF_TBS + CMD_TB_BITMAP_POS >
		     sizeof(out_meta->flags) * BITS_PER_BYTE);
1558
	out_meta->flags = cmd->flags;
1559
	if (WARN_ON_ONCE(txq->entries[idx].free_buf))
1560
		kzfree(txq->entries[idx].free_buf);
1561
	txq->entries[idx].free_buf = dup_buf;
J
Johannes Berg 已提交
1562

1563
	trace_iwlwifi_dev_hcmd(trans->dev, cmd, cmd_size, &out_cmd->hdr_wide);
R
Reinette Chatre 已提交
1564

1565
	/* start timer if queue currently empty */
1566 1567
	if (q->read_ptr == q->write_ptr && txq->wd_timeout)
		mod_timer(&txq->stuck_timer, jiffies + txq->wd_timeout);
1568

1569
	spin_lock_irqsave(&trans_pcie->reg_lock, flags);
1570
	ret = iwl_pcie_set_cmd_in_flight(trans, cmd);
1571 1572 1573 1574
	if (ret < 0) {
		idx = ret;
		spin_unlock_irqrestore(&trans_pcie->reg_lock, flags);
		goto out;
1575 1576
	}

1577
	/* Increment and update queue's write index */
1578
	q->write_ptr = iwl_queue_inc_wrap(q->write_ptr);
1579
	iwl_pcie_txq_inc_wr_ptr(trans, txq);
1580

1581 1582
	spin_unlock_irqrestore(&trans_pcie->reg_lock, flags);

J
Johannes Berg 已提交
1583
 out:
1584
	spin_unlock_bh(&txq->lock);
1585 1586 1587
 free_dup_buf:
	if (idx < 0)
		kfree(dup_buf);
1588
	return idx;
1589 1590
}

1591 1592
/*
 * iwl_pcie_hcmd_complete - Pull unused buffers off the queue and reclaim them
1593 1594 1595 1596 1597 1598
 * @rxb: Rx buffer to reclaim
 *
 * If an Rx buffer has an async callback associated with it the callback
 * will be executed.  The attached skb (if present) will only be freed
 * if the callback returns 1
 */
1599
void iwl_pcie_hcmd_complete(struct iwl_trans *trans,
1600
			    struct iwl_rx_cmd_buffer *rxb)
1601
{
Z
Zhu Yi 已提交
1602
	struct iwl_rx_packet *pkt = rxb_addr(rxb);
1603 1604 1605 1606
	u16 sequence = le16_to_cpu(pkt->hdr.sequence);
	int txq_id = SEQ_TO_QUEUE(sequence);
	int index = SEQ_TO_INDEX(sequence);
	int cmd_index;
J
Johannes Berg 已提交
1607 1608
	struct iwl_device_cmd *cmd;
	struct iwl_cmd_meta *meta;
1609
	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
1610
	struct iwl_txq *txq = &trans_pcie->txq[trans_pcie->cmd_queue];
1611 1612 1613 1614

	/* If a Tx command is being handled and it isn't in the actual
	 * command queue then there a command routing bug has been introduced
	 * in the queue management code. */
1615
	if (WARN(txq_id != trans_pcie->cmd_queue,
1616
		 "wrong command queue %d (should be %d), sequence 0x%X readp=%d writep=%d\n",
1617 1618 1619
		 txq_id, trans_pcie->cmd_queue, sequence,
		 trans_pcie->txq[trans_pcie->cmd_queue].q.read_ptr,
		 trans_pcie->txq[trans_pcie->cmd_queue].q.write_ptr)) {
1620
		iwl_print_hex_error(trans, pkt, 32);
1621
		return;
1622
	}
1623

1624
	spin_lock_bh(&txq->lock);
1625

1626
	cmd_index = get_cmd_index(&txq->q, index);
1627 1628
	cmd = txq->entries[cmd_index].cmd;
	meta = &txq->entries[cmd_index].meta;
1629

1630
	iwl_pcie_tfd_unmap(trans, meta, &txq->tfds[index]);
R
Reinette Chatre 已提交
1631

1632
	/* Input error checking is done when commands are added to queue. */
J
Johannes Berg 已提交
1633
	if (meta->flags & CMD_WANT_SKB) {
1634
		struct page *p = rxb_steal_page(rxb);
1635 1636 1637

		meta->source->resp_pkt = pkt;
		meta->source->_rx_page_addr = (unsigned long)page_address(p);
1638
		meta->source->_rx_page_order = trans_pcie->rx_page_order;
1639
	}
1640

1641
	iwl_pcie_cmdq_reclaim(trans, txq_id, index);
1642

J
Johannes Berg 已提交
1643
	if (!(meta->flags & CMD_ASYNC)) {
1644
		if (!test_bit(STATUS_SYNC_HCMD_ACTIVE, &trans->status)) {
1645 1646
			IWL_WARN(trans,
				 "HCMD_ACTIVE already clear for command %s\n",
1647
				 get_cmd_string(trans_pcie, cmd->hdr.cmd));
1648
		}
1649
		clear_bit(STATUS_SYNC_HCMD_ACTIVE, &trans->status);
1650
		IWL_DEBUG_INFO(trans, "Clearing HCMD_ACTIVE for command %s\n",
1651
			       get_cmd_string(trans_pcie, cmd->hdr.cmd));
1652
		wake_up(&trans_pcie->wait_command_queue);
1653
	}
1654

Z
Zhu Yi 已提交
1655
	meta->flags = 0;
1656

1657
	spin_unlock_bh(&txq->lock);
1658
}
1659

1660
#define HOST_COMPLETE_TIMEOUT	(2 * HZ)
1661

1662 1663
static int iwl_pcie_send_hcmd_async(struct iwl_trans *trans,
				    struct iwl_host_cmd *cmd)
1664
{
J
Johannes Berg 已提交
1665
	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
1666 1667 1668 1669 1670 1671
	int ret;

	/* An asynchronous command can not expect an SKB to be set. */
	if (WARN_ON(cmd->flags & CMD_WANT_SKB))
		return -EINVAL;

1672
	ret = iwl_pcie_enqueue_hcmd(trans, cmd);
1673
	if (ret < 0) {
1674
		IWL_ERR(trans,
1675
			"Error sending %s: enqueue_hcmd failed: %d\n",
1676
			get_cmd_string(trans_pcie, cmd->id), ret);
1677 1678 1679 1680 1681
		return ret;
	}
	return 0;
}

1682 1683
static int iwl_pcie_send_hcmd_sync(struct iwl_trans *trans,
				   struct iwl_host_cmd *cmd)
1684
{
1685
	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
1686 1687 1688
	int cmd_idx;
	int ret;

1689
	IWL_DEBUG_INFO(trans, "Attempting to send sync command %s\n",
1690
		       get_cmd_string(trans_pcie, cmd->id));
1691

1692 1693
	if (WARN(test_and_set_bit(STATUS_SYNC_HCMD_ACTIVE,
				  &trans->status),
1694 1695
		 "Command %s: a command is already active!\n",
		 get_cmd_string(trans_pcie, cmd->id)))
1696 1697
		return -EIO;

1698
	IWL_DEBUG_INFO(trans, "Setting HCMD_ACTIVE for command %s\n",
1699
		       get_cmd_string(trans_pcie, cmd->id));
1700

1701
	cmd_idx = iwl_pcie_enqueue_hcmd(trans, cmd);
1702 1703
	if (cmd_idx < 0) {
		ret = cmd_idx;
1704
		clear_bit(STATUS_SYNC_HCMD_ACTIVE, &trans->status);
1705
		IWL_ERR(trans,
1706
			"Error sending %s: enqueue_hcmd failed: %d\n",
1707
			get_cmd_string(trans_pcie, cmd->id), ret);
1708 1709 1710
		return ret;
	}

1711 1712 1713 1714
	ret = wait_event_timeout(trans_pcie->wait_command_queue,
				 !test_bit(STATUS_SYNC_HCMD_ACTIVE,
					   &trans->status),
				 HOST_COMPLETE_TIMEOUT);
1715
	if (!ret) {
1716 1717
		struct iwl_txq *txq = &trans_pcie->txq[trans_pcie->cmd_queue];
		struct iwl_queue *q = &txq->q;
1718

1719 1720 1721
		IWL_ERR(trans, "Error sending %s: time out after %dms.\n",
			get_cmd_string(trans_pcie, cmd->id),
			jiffies_to_msecs(HOST_COMPLETE_TIMEOUT));
1722

1723 1724
		IWL_ERR(trans, "Current CMD queue read_ptr %d write_ptr %d\n",
			q->read_ptr, q->write_ptr);
1725

1726
		clear_bit(STATUS_SYNC_HCMD_ACTIVE, &trans->status);
1727 1728 1729
		IWL_DEBUG_INFO(trans, "Clearing HCMD_ACTIVE for command %s\n",
			       get_cmd_string(trans_pcie, cmd->id));
		ret = -ETIMEDOUT;
1730

L
Liad Kaufman 已提交
1731
		iwl_force_nmi(trans);
1732
		iwl_trans_fw_error(trans);
1733

1734
		goto cancel;
1735 1736
	}

1737
	if (test_bit(STATUS_FW_ERROR, &trans->status)) {
1738
		IWL_ERR(trans, "FW error in SYNC CMD %s\n",
1739
			get_cmd_string(trans_pcie, cmd->id));
1740
		dump_stack();
1741 1742 1743 1744
		ret = -EIO;
		goto cancel;
	}

1745
	if (!(cmd->flags & CMD_SEND_IN_RFKILL) &&
1746
	    test_bit(STATUS_RFKILL, &trans->status)) {
1747 1748 1749 1750 1751
		IWL_DEBUG_RF_KILL(trans, "RFKILL in SYNC CMD... no rsp\n");
		ret = -ERFKILL;
		goto cancel;
	}

1752
	if ((cmd->flags & CMD_WANT_SKB) && !cmd->resp_pkt) {
1753
		IWL_ERR(trans, "Error: Response NULL in '%s'\n",
1754
			get_cmd_string(trans_pcie, cmd->id));
1755 1756 1757 1758 1759 1760 1761 1762 1763 1764 1765 1766 1767 1768
		ret = -EIO;
		goto cancel;
	}

	return 0;

cancel:
	if (cmd->flags & CMD_WANT_SKB) {
		/*
		 * Cancel the CMD_WANT_SKB flag for the cmd in the
		 * TX cmd queue. Otherwise in case the cmd comes
		 * in later, it will possibly set an invalid
		 * address (cmd->meta.source).
		 */
1769 1770
		trans_pcie->txq[trans_pcie->cmd_queue].
			entries[cmd_idx].meta.flags &= ~CMD_WANT_SKB;
1771
	}
1772

1773 1774 1775
	if (cmd->resp_pkt) {
		iwl_free_resp(cmd);
		cmd->resp_pkt = NULL;
1776 1777 1778 1779 1780
	}

	return ret;
}

1781
int iwl_trans_pcie_send_hcmd(struct iwl_trans *trans, struct iwl_host_cmd *cmd)
1782
{
1783
	if (!(cmd->flags & CMD_SEND_IN_RFKILL) &&
1784
	    test_bit(STATUS_RFKILL, &trans->status)) {
1785 1786
		IWL_DEBUG_RF_KILL(trans, "Dropping CMD 0x%x: RF KILL\n",
				  cmd->id);
1787
		return -ERFKILL;
1788
	}
1789

1790
	if (cmd->flags & CMD_ASYNC)
1791
		return iwl_pcie_send_hcmd_async(trans, cmd);
1792

1793
	/* We still can fail on RFKILL that can be asserted while we wait */
1794
	return iwl_pcie_send_hcmd_sync(trans, cmd);
1795 1796
}

1797 1798
int iwl_trans_pcie_tx(struct iwl_trans *trans, struct sk_buff *skb,
		      struct iwl_device_cmd *dev_cmd, int txq_id)
1799
{
1800
	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
J
Johannes Berg 已提交
1801
	struct ieee80211_hdr *hdr;
1802 1803 1804 1805
	struct iwl_tx_cmd *tx_cmd = (struct iwl_tx_cmd *)dev_cmd->payload;
	struct iwl_cmd_meta *out_meta;
	struct iwl_txq *txq;
	struct iwl_queue *q;
1806 1807 1808
	dma_addr_t tb0_phys, tb1_phys, scratch_phys;
	void *tb1_addr;
	u16 len, tb1_len, tb2_len;
1809
	bool wait_write_ptr;
J
Johannes Berg 已提交
1810 1811
	__le16 fc;
	u8 hdr_len;
1812
	u16 wifi_seq;
J
Johannes Berg 已提交
1813
	int i;
1814 1815 1816

	txq = &trans_pcie->txq[txq_id];
	q = &txq->q;
1817

1818 1819
	if (WARN_ONCE(!test_bit(txq_id, trans_pcie->queue_used),
		      "TX on unused queue %d\n", txq_id))
1820
		return -EINVAL;
1821

J
Johannes Berg 已提交
1822 1823 1824 1825 1826 1827 1828 1829 1830 1831 1832 1833
	if (skb_is_nonlinear(skb) &&
	    skb_shinfo(skb)->nr_frags > IWL_PCIE_MAX_FRAGS &&
	    __skb_linearize(skb))
		return -ENOMEM;

	/* mac80211 always puts the full header into the SKB's head,
	 * so there's no need to check if it's readable there
	 */
	hdr = (struct ieee80211_hdr *)skb->data;
	fc = hdr->frame_control;
	hdr_len = ieee80211_hdrlen(fc);

1834
	spin_lock(&txq->lock);
1835

1836 1837 1838 1839 1840
	/* In AGG mode, the index in the ring must correspond to the WiFi
	 * sequence number. This is a HW requirements to help the SCD to parse
	 * the BA.
	 * Check here that the packets are in the right place on the ring.
	 */
1841
	wifi_seq = IEEE80211_SEQ_TO_SN(le16_to_cpu(hdr->seq_ctrl));
1842
	WARN_ONCE(txq->ampdu &&
1843
		  (wifi_seq & 0xff) != q->write_ptr,
1844 1845 1846 1847 1848 1849 1850 1851 1852 1853 1854
		  "Q: %d WiFi Seq %d tfdNum %d",
		  txq_id, wifi_seq, q->write_ptr);

	/* Set up driver data for this TFD */
	txq->entries[q->write_ptr].skb = skb;
	txq->entries[q->write_ptr].cmd = dev_cmd;

	dev_cmd->hdr.sequence =
		cpu_to_le16((u16)(QUEUE_TO_SEQ(txq_id) |
			    INDEX_TO_SEQ(q->write_ptr)));

1855 1856 1857 1858 1859 1860 1861
	tb0_phys = iwl_pcie_get_scratchbuf_dma(txq, q->write_ptr);
	scratch_phys = tb0_phys + sizeof(struct iwl_cmd_header) +
		       offsetof(struct iwl_tx_cmd, scratch);

	tx_cmd->dram_lsb_ptr = cpu_to_le32(scratch_phys);
	tx_cmd->dram_msb_ptr = iwl_get_dma_hi_addr(scratch_phys);

1862 1863
	/* Set up first empty entry in queue's array of Tx/cmd buffers */
	out_meta = &txq->entries[q->write_ptr].meta;
J
Johannes Berg 已提交
1864
	out_meta->flags = 0;
1865

1866
	/*
1867 1868 1869 1870
	 * The second TB (tb1) points to the remainder of the TX command
	 * and the 802.11 header - dword aligned size
	 * (This calculation modifies the TX command, so do it before the
	 * setup of the first TB)
1871
	 */
1872 1873
	len = sizeof(struct iwl_tx_cmd) + sizeof(struct iwl_cmd_header) +
	      hdr_len - IWL_HCMD_SCRATCHBUF_SIZE;
1874
	tb1_len = ALIGN(len, 4);
1875 1876

	/* Tell NIC about any 2-byte padding after MAC header */
1877
	if (tb1_len != len)
1878 1879
		tx_cmd->tx_flags |= TX_CMD_FLG_MH_PAD_MSK;

1880 1881 1882 1883
	/* The first TB points to the scratchbuf data - min_copy bytes */
	memcpy(&txq->scratchbufs[q->write_ptr], &dev_cmd->hdr,
	       IWL_HCMD_SCRATCHBUF_SIZE);
	iwl_pcie_txq_build_tfd(trans, txq, tb0_phys,
1884
			       IWL_HCMD_SCRATCHBUF_SIZE, true);
1885

1886 1887 1888 1889 1890 1891 1892 1893
	/* there must be data left over for TB1 or this code must be changed */
	BUILD_BUG_ON(sizeof(struct iwl_tx_cmd) < IWL_HCMD_SCRATCHBUF_SIZE);

	/* map the data for TB1 */
	tb1_addr = ((u8 *)&dev_cmd->hdr) + IWL_HCMD_SCRATCHBUF_SIZE;
	tb1_phys = dma_map_single(trans->dev, tb1_addr, tb1_len, DMA_TO_DEVICE);
	if (unlikely(dma_mapping_error(trans->dev, tb1_phys)))
		goto out_err;
1894
	iwl_pcie_txq_build_tfd(trans, txq, tb1_phys, tb1_len, false);
1895

1896 1897
	/*
	 * Set up TFD's third entry to point directly to remainder
J
Johannes Berg 已提交
1898
	 * of skb's head, if any
1899
	 */
J
Johannes Berg 已提交
1900
	tb2_len = skb_headlen(skb) - hdr_len;
1901 1902 1903 1904 1905 1906 1907
	if (tb2_len > 0) {
		dma_addr_t tb2_phys = dma_map_single(trans->dev,
						     skb->data + hdr_len,
						     tb2_len, DMA_TO_DEVICE);
		if (unlikely(dma_mapping_error(trans->dev, tb2_phys))) {
			iwl_pcie_tfd_unmap(trans, out_meta,
					   &txq->tfds[q->write_ptr]);
1908 1909
			goto out_err;
		}
1910
		iwl_pcie_txq_build_tfd(trans, txq, tb2_phys, tb2_len, false);
1911
	}
1912

J
Johannes Berg 已提交
1913 1914 1915 1916 1917 1918 1919 1920 1921 1922 1923 1924 1925 1926 1927 1928 1929 1930 1931 1932 1933 1934 1935
	/* set up the remaining entries to point to the data */
	for (i = 0; i < skb_shinfo(skb)->nr_frags; i++) {
		const skb_frag_t *frag = &skb_shinfo(skb)->frags[i];
		dma_addr_t tb_phys;
		int tb_idx;

		if (!skb_frag_size(frag))
			continue;

		tb_phys = skb_frag_dma_map(trans->dev, frag, 0,
					   skb_frag_size(frag), DMA_TO_DEVICE);

		if (unlikely(dma_mapping_error(trans->dev, tb_phys))) {
			iwl_pcie_tfd_unmap(trans, out_meta,
					   &txq->tfds[q->write_ptr]);
			goto out_err;
		}
		tb_idx = iwl_pcie_txq_build_tfd(trans, txq, tb_phys,
						skb_frag_size(frag), false);

		out_meta->flags |= BIT(tb_idx + CMD_TB_BITMAP_POS);
	}

1936 1937
	/* Set up entry for this TFD in Tx byte-count array */
	iwl_pcie_txq_update_byte_cnt_tbl(trans, txq, le16_to_cpu(tx_cmd->len));
1938

1939 1940 1941
	trace_iwlwifi_dev_tx(trans->dev, skb,
			     &txq->tfds[txq->q.write_ptr],
			     sizeof(struct iwl_tfd),
1942 1943
			     &dev_cmd->hdr, IWL_HCMD_SCRATCHBUF_SIZE + tb1_len,
			     skb->data + hdr_len, tb2_len);
1944
	trace_iwlwifi_dev_tx_data(trans->dev, skb,
J
Johannes Berg 已提交
1945
				  hdr_len, skb->len - hdr_len);
1946

1947
	wait_write_ptr = ieee80211_has_morefrags(fc);
1948

1949
	/* start timer if queue currently empty */
1950
	if (q->read_ptr == q->write_ptr) {
1951 1952 1953 1954 1955 1956 1957 1958 1959 1960 1961 1962 1963
		if (txq->wd_timeout) {
			/*
			 * If the TXQ is active, then set the timer, if not,
			 * set the timer in remainder so that the timer will
			 * be armed with the right value when the station will
			 * wake up.
			 */
			if (!txq->frozen)
				mod_timer(&txq->stuck_timer,
					  jiffies + txq->wd_timeout);
			else
				txq->frozen_expiry_remainder = txq->wd_timeout;
		}
1964 1965 1966
		IWL_DEBUG_RPM(trans, "Q: %d first tx - take ref\n", q->id);
		iwl_trans_pcie_ref(trans);
	}
1967 1968

	/* Tell device the write index *just past* this latest filled TFD */
1969
	q->write_ptr = iwl_queue_inc_wrap(q->write_ptr);
1970 1971
	if (!wait_write_ptr)
		iwl_pcie_txq_inc_wr_ptr(trans, txq);
1972 1973 1974

	/*
	 * At this point the frame is "transmitted" successfully
1975
	 * and we will get a TX status notification eventually.
1976 1977
	 */
	if (iwl_queue_space(q) < q->high_mark) {
1978
		if (wait_write_ptr)
1979
			iwl_pcie_txq_inc_wr_ptr(trans, txq);
1980
		else
1981 1982 1983 1984 1985 1986 1987
			iwl_stop_queue(trans, txq);
	}
	spin_unlock(&txq->lock);
	return 0;
out_err:
	spin_unlock(&txq->lock);
	return -1;
1988
}