/**
* iwl_enqueue_hcmd - enqueue a uCode command
* @priv: device private data point
* @cmd: a point to the ucode command structure
*
* The function returns < 0 values to indicate the operation is
* failed. On success, it turns the index (> 0) of command in the
* command queue.
*/
static int iwl_enqueue_hcmd(struct iwl_trans *trans, struct iwl_host_cmd *cmd)
{
struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
struct iwl_tx_queue *txq = &trans_pcie->txq[trans_pcie->cmd_queue];
struct iwl_queue *q = &txq->q;
struct iwl_device_cmd *out_cmd;
struct iwl_cmd_meta *out_meta;
dma_addr_t phys_addr;
u32 idx;
u16 copy_size, cmd_size;
bool had_nocopy = false;
int i;
u8 *cmd_dest;
#ifdef CONFIG_IWLWIFI_DEVICE_TRACING
const void *trace_bufs[IWL_MAX_CMD_TFDS + 1] = {};
int trace_lens[IWL_MAX_CMD_TFDS + 1] = {};
int trace_idx;
#endif
copy_size = sizeof(out_cmd->hdr);
cmd_size = sizeof(out_cmd->hdr);
/* need one for the header if the first is NOCOPY */
BUILD_BUG_ON(IWL_MAX_CMD_TFDS > IWL_NUM_OF_TBS - 1);
for (i = 0; i < IWL_MAX_CMD_TFDS; i++) {
if (!cmd->len[i])
continue;
if (cmd->dataflags[i] & IWL_HCMD_DFL_NOCOPY) {
had_nocopy = true;
} else {
/* NOCOPY must not be followed by normal! */
if (WARN_ON(had_nocopy))
return -EINVAL;
copy_size += cmd->len[i];
}
cmd_size += cmd->len[i];
}
/*
* If any of the command structures end up being larger than
* 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.
*/
if (WARN_ON(copy_size > TFD_MAX_PAYLOAD_SIZE))
return -EINVAL;
spin_lock_bh(&txq->lock);
if (iwl_queue_space(q) < ((cmd->flags & CMD_ASYNC) ? 2 : 1)) {
spin_unlock_bh(&txq->lock);
IWL_ERR(trans, "No space in command queue\n");
iwl_op_mode_cmd_queue_full(trans->op_mode);
return -ENOSPC;
}
idx = get_cmd_index(q, q->write_ptr);
out_cmd = txq->entries[idx].cmd;
out_meta = &txq->entries[idx].meta;
memset(out_meta, 0, sizeof(*out_meta)); /* re-initialize to NULL */
if (cmd->flags & CMD_WANT_SKB)
out_meta->source = cmd;
/* set up the header */
out_cmd->hdr.cmd = cmd->id;
out_cmd->hdr.flags = 0;
out_cmd->hdr.sequence =
cpu_to_le16(QUEUE_TO_SEQ(trans_pcie->cmd_queue) |
INDEX_TO_SEQ(q->write_ptr));
/* and copy the data that needs to be copied */
cmd_dest = out_cmd->payload;
for (i = 0; i < IWL_MAX_CMD_TFDS; i++) {
if (!cmd->len[i])
continue;
if (cmd->dataflags[i] & IWL_HCMD_DFL_NOCOPY)
break;
memcpy(cmd_dest, cmd->data[i], cmd->len[i]);
cmd_dest += cmd->len[i];
}
IWL_DEBUG_HC(trans,
"Sending command %s (#%x), seq: 0x%04X, %d bytes at %d[%d]:%d\n",
trans_pcie_get_cmd_string(trans_pcie, out_cmd->hdr.cmd),
out_cmd->hdr.cmd, le16_to_cpu(out_cmd->hdr.sequence),
cmd_size, q->write_ptr, idx, trans_pcie->cmd_queue);
phys_addr = dma_map_single(trans->dev, &out_cmd->hdr, copy_size,
DMA_BIDIRECTIONAL);
if (unlikely(dma_mapping_error(trans->dev, phys_addr))) {
idx = -ENOMEM;
goto out;
}
dma_unmap_addr_set(out_meta, mapping, phys_addr);
dma_unmap_len_set(out_meta, len, copy_size);
iwlagn_txq_attach_buf_to_tfd(trans, txq, phys_addr, copy_size, 1);
#ifdef CONFIG_IWLWIFI_DEVICE_TRACING
trace_bufs[0] = &out_cmd->hdr;
trace_lens[0] = copy_size;
trace_idx = 1;
#endif
for (i = 0; i < IWL_MAX_CMD_TFDS; i++) {
if (!cmd->len[i])
continue;
if (!(cmd->dataflags[i] & IWL_HCMD_DFL_NOCOPY))
continue;
phys_addr = dma_map_single(trans->dev, (void *)cmd->data[i],
cmd->len[i], DMA_BIDIRECTIONAL);
if (dma_mapping_error(trans->dev, phys_addr)) {
iwl_unmap_tfd(trans, out_meta,
&txq->tfds[q->write_ptr],
DMA_BIDIRECTIONAL);
idx = -ENOMEM;
goto out;
}
iwlagn_txq_attach_buf_to_tfd(trans, txq, phys_addr,
cmd->len[i], 0);
#ifdef CONFIG_IWLWIFI_DEVICE_TRACING
trace_bufs[trace_idx] = cmd->data[i];
trace_lens[trace_idx] = cmd->len[i];
trace_idx++;
#endif
}
out_meta->flags = cmd->flags;
txq->need_update = 1;
/* check that tracing gets all possible blocks */
BUILD_BUG_ON(IWL_MAX_CMD_TFDS + 1 != 3);
#ifdef CONFIG_IWLWIFI_DEVICE_TRACING
trace_iwlwifi_dev_hcmd(trans->dev, cmd->flags,
trace_bufs[0], trace_lens[0],
trace_bufs[1], trace_lens[1],
trace_bufs[2], trace_lens[2]);
#endif
/* start timer if queue currently empty */
if (q->read_ptr == q->write_ptr && trans_pcie->wd_timeout)
mod_timer(&txq->stuck_timer, jiffies + trans_pcie->wd_timeout);
/* Increment and update queue's write index */
q->write_ptr = iwl_queue_inc_wrap(q->write_ptr, q->n_bd);
iwl_txq_update_write_ptr(trans, txq);
out:
spin_unlock_bh(&txq->lock);
return idx;
}
/**
* iwl_enqueue_hcmd - enqueue a uCode command
* @priv: device private data point
* @cmd: a point to the ucode command structure
*
* The function returns < 0 values to indicate the operation is
* failed. On success, it turns the index (> 0) of command in the
* command queue.
*/
static int iwl_enqueue_hcmd(struct iwl_priv *priv, struct iwl_host_cmd *cmd)
{
struct iwl_tx_queue *txq = &priv->txq[priv->cmd_queue];
struct iwl_queue *q = &txq->q;
struct iwl_device_cmd *out_cmd;
struct iwl_cmd_meta *out_meta;
dma_addr_t phys_addr;
unsigned long flags;
u32 idx;
u16 copy_size, cmd_size;
bool is_ct_kill = false;
bool had_nocopy = false;
int i;
u8 *cmd_dest;
#ifdef CONFIG_IWLWIFI_DEVICE_TRACING
const void *trace_bufs[IWL_MAX_CMD_TFDS + 1] = {};
int trace_lens[IWL_MAX_CMD_TFDS + 1] = {};
int trace_idx;
#endif
if (test_bit(STATUS_FW_ERROR, &priv->status)) {
IWL_WARN(priv, "fw recovery, no hcmd send\n");
return -EIO;
}
if ((priv->ucode_owner == IWL_OWNERSHIP_TM) &&
!(cmd->flags & CMD_ON_DEMAND)) {
IWL_DEBUG_HC(priv, "tm own the uCode, no regular hcmd send\n");
return -EIO;
}
copy_size = sizeof(out_cmd->hdr);
cmd_size = sizeof(out_cmd->hdr);
/* need one for the header if the first is NOCOPY */
BUILD_BUG_ON(IWL_MAX_CMD_TFDS > IWL_NUM_OF_TBS - 1);
for (i = 0; i < IWL_MAX_CMD_TFDS; i++) {
if (!cmd->len[i])
continue;
if (cmd->dataflags[i] & IWL_HCMD_DFL_NOCOPY) {
had_nocopy = true;
} else {
/* NOCOPY must not be followed by normal! */
if (WARN_ON(had_nocopy))
return -EINVAL;
copy_size += cmd->len[i];
}
cmd_size += cmd->len[i];
}
/*
* If any of the command structures end up being larger than
* 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.
*/
if (WARN_ON(copy_size > TFD_MAX_PAYLOAD_SIZE))
return -EINVAL;
if (iwl_is_rfkill(priv) || iwl_is_ctkill(priv)) {
IWL_WARN(priv, "Not sending command - %s KILL\n",
iwl_is_rfkill(priv) ? "RF" : "CT");
return -EIO;
}
spin_lock_irqsave(&priv->hcmd_lock, flags);
if (iwl_queue_space(q) < ((cmd->flags & CMD_ASYNC) ? 2 : 1)) {
spin_unlock_irqrestore(&priv->hcmd_lock, flags);
IWL_ERR(priv, "No space in command queue\n");
is_ct_kill = iwl_check_for_ct_kill(priv);
if (!is_ct_kill) {
IWL_ERR(priv, "Restarting adapter due to queue full\n");
iwlagn_fw_error(priv, false);
}
return -ENOSPC;
}
idx = get_cmd_index(q, q->write_ptr);
out_cmd = txq->cmd[idx];
out_meta = &txq->meta[idx];
memset(out_meta, 0, sizeof(*out_meta)); /* re-initialize to NULL */
if (cmd->flags & CMD_WANT_SKB)
out_meta->source = cmd;
if (cmd->flags & CMD_ASYNC)
out_meta->callback = cmd->callback;
/* set up the header */
out_cmd->hdr.cmd = cmd->id;
out_cmd->hdr.flags = 0;
out_cmd->hdr.sequence = cpu_to_le16(QUEUE_TO_SEQ(priv->cmd_queue) |
INDEX_TO_SEQ(q->write_ptr));
/* and copy the data that needs to be copied */
cmd_dest = &out_cmd->cmd.payload[0];
for (i = 0; i < IWL_MAX_CMD_TFDS; i++) {
if (!cmd->len[i])
continue;
if (cmd->dataflags[i] & IWL_HCMD_DFL_NOCOPY)
break;
memcpy(cmd_dest, cmd->data[i], cmd->len[i]);
cmd_dest += cmd->len[i];
}
IWL_DEBUG_HC(priv, "Sending command %s (#%x), seq: 0x%04X, "
"%d bytes at %d[%d]:%d\n",
get_cmd_string(out_cmd->hdr.cmd),
out_cmd->hdr.cmd,
le16_to_cpu(out_cmd->hdr.sequence), cmd_size,
q->write_ptr, idx, priv->cmd_queue);
phys_addr = dma_map_single(priv->bus->dev, &out_cmd->hdr, copy_size,
DMA_BIDIRECTIONAL);
if (unlikely(dma_mapping_error(priv->bus->dev, phys_addr))) {
idx = -ENOMEM;
goto out;
}
dma_unmap_addr_set(out_meta, mapping, phys_addr);
dma_unmap_len_set(out_meta, len, copy_size);
iwlagn_txq_attach_buf_to_tfd(priv, txq, phys_addr, copy_size, 1);
#ifdef CONFIG_IWLWIFI_DEVICE_TRACING
trace_bufs[0] = &out_cmd->hdr;
trace_lens[0] = copy_size;
trace_idx = 1;
#endif
for (i = 0; i < IWL_MAX_CMD_TFDS; i++) {
if (!cmd->len[i])
continue;
if (!(cmd->dataflags[i] & IWL_HCMD_DFL_NOCOPY))
continue;
phys_addr = dma_map_single(priv->bus->dev, (void *)cmd->data[i],
cmd->len[i], DMA_BIDIRECTIONAL);
if (dma_mapping_error(priv->bus->dev, phys_addr)) {
iwlagn_unmap_tfd(priv, out_meta,
&txq->tfds[q->write_ptr],
DMA_BIDIRECTIONAL);
idx = -ENOMEM;
goto out;
}
iwlagn_txq_attach_buf_to_tfd(priv, txq, phys_addr,
cmd->len[i], 0);
#ifdef CONFIG_IWLWIFI_DEVICE_TRACING
trace_bufs[trace_idx] = cmd->data[i];
trace_lens[trace_idx] = cmd->len[i];
trace_idx++;
#endif
}
out_meta->flags = cmd->flags;
txq->need_update = 1;
/* check that tracing gets all possible blocks */
BUILD_BUG_ON(IWL_MAX_CMD_TFDS + 1 != 3);
#ifdef CONFIG_IWLWIFI_DEVICE_TRACING
trace_iwlwifi_dev_hcmd(priv, cmd->flags,
trace_bufs[0], trace_lens[0],
trace_bufs[1], trace_lens[1],
trace_bufs[2], trace_lens[2]);
#endif
/* Increment and update queue's write index */
q->write_ptr = iwl_queue_inc_wrap(q->write_ptr, q->n_bd);
iwl_txq_update_write_ptr(priv, txq);
out:
spin_unlock_irqrestore(&priv->hcmd_lock, flags);
return idx;
}
static void iwl5000_rx_reply_tx(struct iwl_priv *priv,
struct iwl_rx_mem_buffer *rxb)
{
struct iwl_rx_packet *pkt = (struct iwl_rx_packet *)rxb->skb->data;
u16 sequence = le16_to_cpu(pkt->hdr.sequence);
int txq_id = SEQ_TO_QUEUE(sequence);
int index = SEQ_TO_INDEX(sequence);
struct iwl_tx_queue *txq = &priv->txq[txq_id];
struct ieee80211_tx_info *info;
struct iwl5000_tx_resp *tx_resp = (void *)&pkt->u.raw[0];
u32 status = le16_to_cpu(tx_resp->status.status);
int tid;
int sta_id;
int freed;
if ((index >= txq->q.n_bd) || (iwl_queue_used(&txq->q, index) == 0)) {
IWL_ERR(priv, "Read index for DMA queue txq_id (%d) index %d "
"is out of range [0-%d] %d %d\n", txq_id,
index, txq->q.n_bd, txq->q.write_ptr,
txq->q.read_ptr);
return;
}
info = IEEE80211_SKB_CB(txq->txb[txq->q.read_ptr].skb[0]);
memset(&info->status, 0, sizeof(info->status));
tid = (tx_resp->ra_tid & IWL50_TX_RES_TID_MSK) >> IWL50_TX_RES_TID_POS;
sta_id = (tx_resp->ra_tid & IWL50_TX_RES_RA_MSK) >> IWL50_TX_RES_RA_POS;
if (txq->sched_retry) {
const u32 scd_ssn = iwl5000_get_scd_ssn(tx_resp);
struct iwl_ht_agg *agg = NULL;
agg = &priv->stations[sta_id].tid[tid].agg;
iwl5000_tx_status_reply_tx(priv, agg, tx_resp, txq_id, index);
/* check if BAR is needed */
if ((tx_resp->frame_count == 1) && !iwl_is_tx_success(status))
info->flags |= IEEE80211_TX_STAT_AMPDU_NO_BACK;
if (txq->q.read_ptr != (scd_ssn & 0xff)) {
index = iwl_queue_dec_wrap(scd_ssn & 0xff, txq->q.n_bd);
IWL_DEBUG_TX_REPLY(priv, "Retry scheduler reclaim "
"scd_ssn=%d idx=%d txq=%d swq=%d\n",
scd_ssn , index, txq_id, txq->swq_id);
freed = iwl_tx_queue_reclaim(priv, txq_id, index);
priv->stations[sta_id].tid[tid].tfds_in_queue -= freed;
if (priv->mac80211_registered &&
(iwl_queue_space(&txq->q) > txq->q.low_mark) &&
(agg->state != IWL_EMPTYING_HW_QUEUE_DELBA)) {
if (agg->state == IWL_AGG_OFF)
iwl_wake_queue(priv, txq_id);
else
iwl_wake_queue(priv, txq->swq_id);
}
}
} else {
BUG_ON(txq_id != txq->swq_id);
info->status.rates[0].count = tx_resp->failure_frame + 1;
info->flags |= iwl_is_tx_success(status) ?
IEEE80211_TX_STAT_ACK : 0;
iwl_hwrate_to_tx_control(priv,
le32_to_cpu(tx_resp->rate_n_flags),
info);
IWL_DEBUG_TX_REPLY(priv, "TXQ %d status %s (0x%08x) rate_n_flags "
"0x%x retries %d\n",
txq_id,
iwl_get_tx_fail_reason(status), status,
le32_to_cpu(tx_resp->rate_n_flags),
tx_resp->failure_frame);
freed = iwl_tx_queue_reclaim(priv, txq_id, index);
if (ieee80211_is_data_qos(tx_resp->frame_ctrl))
priv->stations[sta_id].tid[tid].tfds_in_queue -= freed;
if (priv->mac80211_registered &&
(iwl_queue_space(&txq->q) > txq->q.low_mark))
iwl_wake_queue(priv, txq_id);
}
if (ieee80211_is_data_qos(tx_resp->frame_ctrl))
iwl_txq_check_empty(priv, sta_id, tid, txq_id);
if (iwl_check_bits(status, TX_ABORT_REQUIRED_MSK))
IWL_ERR(priv, "TODO: Implement Tx ABORT REQUIRED!!!\n");
}
/**
* iwl_enqueue_hcmd - enqueue a uCode command
* @priv: device private data point
* @cmd: a point to the ucode command structure
*
* The function returns < 0 values to indicate the operation is
* failed. On success, it turns the index (> 0) of command in the
* command queue.
*/
static int iwl_enqueue_hcmd(struct iwl_trans *trans, struct iwl_host_cmd *cmd)
{
struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
struct iwl_tx_queue *txq = &trans_pcie->txq[trans_pcie->cmd_queue];
struct iwl_queue *q = &txq->q;
struct iwl_device_cmd *out_cmd;
struct iwl_cmd_meta *out_meta;
dma_addr_t phys_addr;
u32 idx;
u16 copy_size, cmd_size, dma_size;
bool had_nocopy = false;
int i;
u8 *cmd_dest;
const u8 *cmddata[IWL_MAX_CMD_TFDS];
u16 cmdlen[IWL_MAX_CMD_TFDS];
#ifdef CONFIG_IWLWIFI_DEVICE_TRACING
const void *trace_bufs[IWL_MAX_CMD_TFDS + 1] = {};
int trace_lens[IWL_MAX_CMD_TFDS + 1] = {};
int trace_idx;
#endif
if (test_bit(STATUS_FW_ERROR, &trans->shrd->status)) {
IWL_WARN(trans, "fw recovery, no hcmd send\n");
return -EIO;
}
copy_size = sizeof(out_cmd->hdr);
cmd_size = sizeof(out_cmd->hdr);
/* need one for the header if the first is NOCOPY */
BUILD_BUG_ON(IWL_MAX_CMD_TFDS > IWL_NUM_OF_TBS - 1);
for (i = 0; i < IWL_MAX_CMD_TFDS; i++) {
cmddata[i] = cmd->data[i];
cmdlen[i] = cmd->len[i];
if (!cmd->len[i])
continue;
/* need at least IWL_HCMD_MIN_COPY_SIZE copied */
if (copy_size < IWL_HCMD_MIN_COPY_SIZE) {
int copy = IWL_HCMD_MIN_COPY_SIZE - copy_size;
if (copy > cmdlen[i])
copy = cmdlen[i];
cmdlen[i] -= copy;
cmddata[i] += copy;
copy_size += copy;
}
if (cmd->dataflags[i] & IWL_HCMD_DFL_NOCOPY) {
had_nocopy = true;
} else {
/* NOCOPY must not be followed by normal! */
if (WARN_ON(had_nocopy))
return -EINVAL;
copy_size += cmdlen[i];
}
cmd_size += cmd->len[i];
}
/*
* If any of the command structures end up being larger than
* 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.
*/
if (WARN_ON(copy_size > TFD_MAX_PAYLOAD_SIZE))
return -EINVAL;
spin_lock_bh(&txq->lock);
if (iwl_queue_space(q) < ((cmd->flags & CMD_ASYNC) ? 2 : 1)) {
spin_unlock_bh(&txq->lock);
IWL_ERR(trans, "No space in command queue\n");
iwl_op_mode_cmd_queue_full(trans->op_mode);
return -ENOSPC;
}
idx = get_cmd_index(q, q->write_ptr);
out_cmd = txq->cmd[idx];
out_meta = &txq->meta[idx];
memset(out_meta, 0, sizeof(*out_meta)); /* re-initialize to NULL */
if (cmd->flags & CMD_WANT_SKB)
out_meta->source = cmd;
/* set up the header */
out_cmd->hdr.cmd = cmd->id;
out_cmd->hdr.flags = 0;
out_cmd->hdr.sequence =
cpu_to_le16(QUEUE_TO_SEQ(trans_pcie->cmd_queue) |
INDEX_TO_SEQ(q->write_ptr));
/* and copy the data that needs to be copied */
cmd_dest = out_cmd->payload;
copy_size = sizeof(out_cmd->hdr);
for (i = 0; i < IWL_MAX_CMD_TFDS; i++) {
int copy = 0;
if (!cmd->len)
continue;
/* need at least IWL_HCMD_MIN_COPY_SIZE copied */
if (copy_size < IWL_HCMD_MIN_COPY_SIZE) {
copy = IWL_HCMD_MIN_COPY_SIZE - copy_size;
if (copy > cmd->len[i])
copy = cmd->len[i];
}
/* copy everything if not nocopy/dup */
if (!(cmd->dataflags[i] & IWL_HCMD_DFL_NOCOPY))
copy = cmd->len[i];
if (copy) {
memcpy(cmd_dest, cmd->data[i], copy);
cmd_dest += copy;
copy_size += copy;
}
}
IWL_DEBUG_HC(trans, "Sending command %s (#%x), seq: 0x%04X, "
"%d bytes at %d[%d]:%d\n",
get_cmd_string(out_cmd->hdr.cmd),
out_cmd->hdr.cmd,
le16_to_cpu(out_cmd->hdr.sequence), cmd_size,
q->write_ptr, idx, trans_pcie->cmd_queue);
/*
* If the entire command is smaller than IWL_HCMD_MIN_COPY_SIZE, we must
* still map at least that many bytes for the hardware to write back to.
* We have enough space, so that's not a problem.
*/
dma_size = max_t(u16, copy_size, IWL_HCMD_MIN_COPY_SIZE);
phys_addr = dma_map_single(trans->dev, &out_cmd->hdr, dma_size,
DMA_BIDIRECTIONAL);
if (unlikely(dma_mapping_error(trans->dev, phys_addr))) {
idx = -ENOMEM;
goto out;
}
dma_unmap_addr_set(out_meta, mapping, phys_addr);
dma_unmap_len_set(out_meta, len, dma_size);
iwlagn_txq_attach_buf_to_tfd(trans, txq,
phys_addr, copy_size, 1);
#ifdef CONFIG_IWLWIFI_DEVICE_TRACING
trace_bufs[0] = &out_cmd->hdr;
trace_lens[0] = copy_size;
trace_idx = 1;
#endif
/* map the remaining (adjusted) nocopy/dup fragments */
for (i = 0; i < IWL_MAX_CMD_TFDS; i++) {
if (!cmdlen[i])
continue;
if (!(cmd->dataflags[i] & IWL_HCMD_DFL_NOCOPY))
continue;
phys_addr = dma_map_single(trans->dev,
(void *)cmddata[i],
cmdlen[i], DMA_BIDIRECTIONAL);
if (dma_mapping_error(trans->dev, phys_addr)) {
iwlagn_unmap_tfd(trans, out_meta,
&txq->tfds[q->write_ptr],
DMA_BIDIRECTIONAL);
idx = -ENOMEM;
goto out;
}
iwlagn_txq_attach_buf_to_tfd(trans, txq, phys_addr,
cmdlen[i], 0);
#ifdef CONFIG_IWLWIFI_DEVICE_TRACING
trace_bufs[trace_idx] = cmddata[i];
trace_lens[trace_idx] = cmdlen[i];
trace_idx++;
#endif
}
out_meta->flags = cmd->flags;
txq->need_update = 1;
/* check that tracing gets all possible blocks */
BUILD_BUG_ON(IWL_MAX_CMD_TFDS + 1 != 3);
#ifdef CONFIG_IWLWIFI_DEVICE_TRACING
trace_iwlwifi_dev_hcmd(trans->dev, cmd->flags,
trace_bufs[0], trace_lens[0],
trace_bufs[1], trace_lens[1],
trace_bufs[2], trace_lens[2]);
#endif
/* Increment and update queue's write index */
q->write_ptr = iwl_queue_inc_wrap(q->write_ptr, q->n_bd);
iwl_txq_update_write_ptr(trans, txq);
out:
spin_unlock_bh(&txq->lock);
return idx;
}
static int iwl_enqueue_hcmd(struct iwl_trans *trans, struct iwl_host_cmd *cmd)
{
struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
struct iwl_tx_queue *txq = &trans_pcie->txq[trans_pcie->cmd_queue];
struct iwl_queue *q = &txq->q;
struct iwl_device_cmd *out_cmd;
struct iwl_cmd_meta *out_meta;
dma_addr_t phys_addr;
u32 idx;
u16 copy_size, cmd_size;
bool had_nocopy = false;
int i;
u8 *cmd_dest;
#ifdef CONFIG_IWLWIFI_DEVICE_TRACING
const void *trace_bufs[IWL_MAX_CMD_TFDS + 1] = {};
int trace_lens[IWL_MAX_CMD_TFDS + 1] = {};
int trace_idx;
#endif
if (test_bit(STATUS_FW_ERROR, &trans->shrd->status)) {
IWL_WARN(trans, "fw recovery, no hcmd send\n");
return -EIO;
}
copy_size = sizeof(out_cmd->hdr);
cmd_size = sizeof(out_cmd->hdr);
BUILD_BUG_ON(IWL_MAX_CMD_TFDS > IWL_NUM_OF_TBS - 1);
for (i = 0; i < IWL_MAX_CMD_TFDS; i++) {
if (!cmd->len[i])
continue;
if (cmd->dataflags[i] & IWL_HCMD_DFL_NOCOPY) {
had_nocopy = true;
} else {
if (WARN_ON(had_nocopy))
return -EINVAL;
copy_size += cmd->len[i];
}
cmd_size += cmd->len[i];
}
if (WARN_ON(copy_size > TFD_MAX_PAYLOAD_SIZE))
return -EINVAL;
spin_lock_bh(&txq->lock);
if (iwl_queue_space(q) < ((cmd->flags & CMD_ASYNC) ? 2 : 1)) {
spin_unlock_bh(&txq->lock);
IWL_ERR(trans, "No space in command queue\n");
iwl_op_mode_cmd_queue_full(trans->op_mode);
return -ENOSPC;
}
idx = get_cmd_index(q, q->write_ptr);
out_cmd = txq->cmd[idx];
out_meta = &txq->meta[idx];
memset(out_meta, 0, sizeof(*out_meta));
if (cmd->flags & CMD_WANT_SKB)
out_meta->source = cmd;
out_cmd->hdr.cmd = cmd->id;
out_cmd->hdr.flags = 0;
out_cmd->hdr.sequence =
cpu_to_le16(QUEUE_TO_SEQ(trans_pcie->cmd_queue) |
INDEX_TO_SEQ(q->write_ptr));
cmd_dest = out_cmd->payload;
for (i = 0; i < IWL_MAX_CMD_TFDS; i++) {
if (!cmd->len[i])
continue;
if (cmd->dataflags[i] & IWL_HCMD_DFL_NOCOPY)
break;
memcpy(cmd_dest, cmd->data[i], cmd->len[i]);
cmd_dest += cmd->len[i];
}
IWL_DEBUG_HC(trans, "Sending command %s (#%x), seq: 0x%04X, "
"%d bytes at %d[%d]:%d\n",
get_cmd_string(out_cmd->hdr.cmd),
out_cmd->hdr.cmd,
le16_to_cpu(out_cmd->hdr.sequence), cmd_size,
q->write_ptr, idx, trans_pcie->cmd_queue);
phys_addr = dma_map_single(trans->dev, &out_cmd->hdr, copy_size,
DMA_BIDIRECTIONAL);
if (unlikely(dma_mapping_error(trans->dev, phys_addr))) {
idx = -ENOMEM;
goto out;
}
dma_unmap_addr_set(out_meta, mapping, phys_addr);
dma_unmap_len_set(out_meta, len, copy_size);
iwlagn_txq_attach_buf_to_tfd(trans, txq,
phys_addr, copy_size, 1);
#ifdef CONFIG_IWLWIFI_DEVICE_TRACING
trace_bufs[0] = &out_cmd->hdr;
trace_lens[0] = copy_size;
trace_idx = 1;
#endif
for (i = 0; i < IWL_MAX_CMD_TFDS; i++) {
if (!cmd->len[i])
continue;
if (!(cmd->dataflags[i] & IWL_HCMD_DFL_NOCOPY))
continue;
phys_addr = dma_map_single(trans->dev,
(void *)cmd->data[i],
cmd->len[i], DMA_BIDIRECTIONAL);
if (dma_mapping_error(trans->dev, phys_addr)) {
iwlagn_unmap_tfd(trans, out_meta,
&txq->tfds[q->write_ptr],
DMA_BIDIRECTIONAL);
idx = -ENOMEM;
goto out;
}
iwlagn_txq_attach_buf_to_tfd(trans, txq, phys_addr,
cmd->len[i], 0);
#ifdef CONFIG_IWLWIFI_DEVICE_TRACING
trace_bufs[trace_idx] = cmd->data[i];
trace_lens[trace_idx] = cmd->len[i];
trace_idx++;
#endif
}
out_meta->flags = cmd->flags;
txq->need_update = 1;
BUILD_BUG_ON(IWL_MAX_CMD_TFDS + 1 != 3);
#ifdef CONFIG_IWLWIFI_DEVICE_TRACING
trace_iwlwifi_dev_hcmd(trans->dev, cmd->flags,
trace_bufs[0], trace_lens[0],
trace_bufs[1], trace_lens[1],
trace_bufs[2], trace_lens[2]);
#endif
q->write_ptr = iwl_queue_inc_wrap(q->write_ptr, q->n_bd);
iwl_txq_update_write_ptr(trans, txq);
out:
spin_unlock_bh(&txq->lock);
return idx;
}
/*
* iwl_pcie_gen2_enqueue_hcmd - enqueue a uCode command
* @priv: device private data point
* @cmd: a pointer to the ucode command structure
*
* The function returns < 0 values to indicate the operation
* failed. On success, it returns the index (>= 0) of command in the
* command queue.
*/
static int iwl_pcie_gen2_enqueue_hcmd(struct iwl_trans *trans,
struct iwl_host_cmd *cmd)
{
struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
struct iwl_txq *txq = trans_pcie->txq[trans_pcie->cmd_queue];
struct iwl_device_cmd *out_cmd;
struct iwl_cmd_meta *out_meta;
unsigned long flags;
void *dup_buf = NULL;
dma_addr_t phys_addr;
int i, cmd_pos, idx;
u16 copy_size, cmd_size, tb0_size;
bool had_nocopy = false;
u8 group_id = iwl_cmd_groupid(cmd->id);
const u8 *cmddata[IWL_MAX_CMD_TBS_PER_TFD];
u16 cmdlen[IWL_MAX_CMD_TBS_PER_TFD];
struct iwl_tfh_tfd *tfd;
copy_size = sizeof(struct iwl_cmd_header_wide);
cmd_size = sizeof(struct iwl_cmd_header_wide);
for (i = 0; i < IWL_MAX_CMD_TBS_PER_TFD; i++) {
cmddata[i] = cmd->data[i];
cmdlen[i] = cmd->len[i];
if (!cmd->len[i])
continue;
/* need at least IWL_FIRST_TB_SIZE copied */
if (copy_size < IWL_FIRST_TB_SIZE) {
int copy = IWL_FIRST_TB_SIZE - copy_size;
if (copy > cmdlen[i])
copy = cmdlen[i];
cmdlen[i] -= copy;
cmddata[i] += copy;
copy_size += copy;
}
if (cmd->dataflags[i] & IWL_HCMD_DFL_NOCOPY) {
had_nocopy = true;
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;
}
dup_buf = kmemdup(cmddata[i], cmdlen[i],
GFP_ATOMIC);
if (!dup_buf)
return -ENOMEM;
} else {
/* NOCOPY must not be followed by normal! */
if (WARN_ON(had_nocopy)) {
idx = -EINVAL;
goto free_dup_buf;
}
copy_size += cmdlen[i];
}
cmd_size += cmd->len[i];
}
/*
* If any of the command structures end up being larger than 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
*/
if (WARN(copy_size > TFD_MAX_PAYLOAD_SIZE,
"Command %s (%#x) is too large (%d bytes)\n",
iwl_get_cmd_string(trans, cmd->id), cmd->id, copy_size)) {
idx = -EINVAL;
goto free_dup_buf;
}
spin_lock_bh(&txq->lock);
idx = iwl_pcie_get_cmd_index(txq, txq->write_ptr);
tfd = iwl_pcie_get_tfd(trans, txq, txq->write_ptr);
memset(tfd, 0, sizeof(*tfd));
if (iwl_queue_space(trans, txq) < ((cmd->flags & CMD_ASYNC) ? 2 : 1)) {
spin_unlock_bh(&txq->lock);
IWL_ERR(trans, "No space in command queue\n");
iwl_op_mode_cmd_queue_full(trans->op_mode);
idx = -ENOSPC;
goto free_dup_buf;
}
out_cmd = txq->entries[idx].cmd;
out_meta = &txq->entries[idx].meta;
/* re-initialize to NULL */
memset(out_meta, 0, sizeof(*out_meta));
if (cmd->flags & CMD_WANT_SKB)
out_meta->source = cmd;
/* set up the header */
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(txq->write_ptr));
cmd_pos = sizeof(struct iwl_cmd_header_wide);
copy_size = sizeof(struct iwl_cmd_header_wide);
/* and copy the data that needs to be copied */
for (i = 0; i < IWL_MAX_CMD_TBS_PER_TFD; i++) {
int copy;
if (!cmd->len[i])
continue;
/* copy everything if not nocopy/dup */
if (!(cmd->dataflags[i] & (IWL_HCMD_DFL_NOCOPY |
IWL_HCMD_DFL_DUP))) {
copy = cmd->len[i];
memcpy((u8 *)out_cmd + cmd_pos, cmd->data[i], copy);
cmd_pos += copy;
copy_size += copy;
continue;
}
/*
* Otherwise we need at least IWL_FIRST_TB_SIZE copied
* in total (for bi-directional DMA), 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_FIRST_TB_SIZE) {
copy = IWL_FIRST_TB_SIZE - copy_size;
if (copy > cmd->len[i])
copy = cmd->len[i];
copy_size += copy;
}
}
IWL_DEBUG_HC(trans,
"Sending command %s (%.2x.%.2x), seq: 0x%04X, %d bytes at %d[%d]:%d\n",
iwl_get_cmd_string(trans, cmd->id), group_id,
out_cmd->hdr.cmd, le16_to_cpu(out_cmd->hdr.sequence),
cmd_size, txq->write_ptr, idx, trans_pcie->cmd_queue);
/* start the TFD with the minimum copy bytes */
tb0_size = min_t(int, copy_size, IWL_FIRST_TB_SIZE);
memcpy(&txq->first_tb_bufs[idx], &out_cmd->hdr, tb0_size);
iwl_pcie_gen2_set_tb(trans, tfd, iwl_pcie_get_first_tb_dma(txq, idx),
tb0_size);
/* map first command fragment, if any remains */
if (copy_size > tb0_size) {
phys_addr = dma_map_single(trans->dev,
((u8 *)&out_cmd->hdr) + tb0_size,
copy_size - tb0_size,
DMA_TO_DEVICE);
if (dma_mapping_error(trans->dev, phys_addr)) {
idx = -ENOMEM;
iwl_pcie_gen2_tfd_unmap(trans, out_meta, tfd);
goto out;
}
iwl_pcie_gen2_set_tb(trans, tfd, phys_addr,
copy_size - tb0_size);
}
/* map the remaining (adjusted) nocopy/dup fragments */
for (i = 0; i < IWL_MAX_CMD_TBS_PER_TFD; i++) {
const void *data = cmddata[i];
if (!cmdlen[i])
continue;
if (!(cmd->dataflags[i] & (IWL_HCMD_DFL_NOCOPY |
IWL_HCMD_DFL_DUP)))
continue;
if (cmd->dataflags[i] & IWL_HCMD_DFL_DUP)
data = dup_buf;
phys_addr = dma_map_single(trans->dev, (void *)data,
cmdlen[i], DMA_TO_DEVICE);
if (dma_mapping_error(trans->dev, phys_addr)) {
idx = -ENOMEM;
iwl_pcie_gen2_tfd_unmap(trans, out_meta, tfd);
goto out;
}
iwl_pcie_gen2_set_tb(trans, tfd, phys_addr, cmdlen[i]);
}
BUILD_BUG_ON(IWL_TFH_NUM_TBS > sizeof(out_meta->tbs) * BITS_PER_BYTE);
out_meta->flags = cmd->flags;
if (WARN_ON_ONCE(txq->entries[idx].free_buf))
kzfree(txq->entries[idx].free_buf);
txq->entries[idx].free_buf = dup_buf;
trace_iwlwifi_dev_hcmd(trans->dev, cmd, cmd_size, &out_cmd->hdr_wide);
/* start timer if queue currently empty */
if (txq->read_ptr == txq->write_ptr && txq->wd_timeout)
mod_timer(&txq->stuck_timer, jiffies + txq->wd_timeout);
spin_lock_irqsave(&trans_pcie->reg_lock, flags);
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_ref(trans);
}
/* Increment and update queue's write index */
txq->write_ptr = iwl_queue_inc_wrap(trans, txq->write_ptr);
iwl_pcie_gen2_txq_inc_wr_ptr(trans, txq);
spin_unlock_irqrestore(&trans_pcie->reg_lock, flags);
out:
spin_unlock_bh(&txq->lock);
free_dup_buf:
if (idx < 0)
kfree(dup_buf);
return idx;
}
int iwl_trans_pcie_gen2_tx(struct iwl_trans *trans, struct sk_buff *skb,
struct iwl_device_cmd *dev_cmd, int txq_id)
{
struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
struct iwl_cmd_meta *out_meta;
struct iwl_txq *txq = trans_pcie->txq[txq_id];
u16 cmd_len;
int idx;
void *tfd;
if (WARN_ONCE(!test_bit(txq_id, trans_pcie->queue_used),
"TX on unused queue %d\n", txq_id))
return -EINVAL;
if (skb_is_nonlinear(skb) &&
skb_shinfo(skb)->nr_frags > IWL_PCIE_MAX_FRAGS(trans_pcie) &&
__skb_linearize(skb))
return -ENOMEM;
spin_lock(&txq->lock);
if (iwl_queue_space(trans, txq) < txq->high_mark) {
iwl_stop_queue(trans, txq);
/* don't put the packet on the ring, if there is no room */
if (unlikely(iwl_queue_space(trans, txq) < 3)) {
struct iwl_device_cmd **dev_cmd_ptr;
dev_cmd_ptr = (void *)((u8 *)skb->cb +
trans_pcie->dev_cmd_offs);
*dev_cmd_ptr = dev_cmd;
__skb_queue_tail(&txq->overflow_q, skb);
spin_unlock(&txq->lock);
return 0;
}
}
idx = iwl_pcie_get_cmd_index(txq, txq->write_ptr);
/* Set up driver data for this TFD */
txq->entries[idx].skb = skb;
txq->entries[idx].cmd = dev_cmd;
dev_cmd->hdr.sequence =
cpu_to_le16((u16)(QUEUE_TO_SEQ(txq_id) |
INDEX_TO_SEQ(idx)));
/* Set up first empty entry in queue's array of Tx/cmd buffers */
out_meta = &txq->entries[idx].meta;
out_meta->flags = 0;
tfd = iwl_pcie_gen2_build_tfd(trans, txq, dev_cmd, skb, out_meta);
if (!tfd) {
spin_unlock(&txq->lock);
return -1;
}
if (trans->cfg->device_family >= IWL_DEVICE_FAMILY_22560) {
struct iwl_tx_cmd_gen3 *tx_cmd_gen3 =
(void *)dev_cmd->payload;
cmd_len = le16_to_cpu(tx_cmd_gen3->len);
} else {
struct iwl_tx_cmd_gen2 *tx_cmd_gen2 =
(void *)dev_cmd->payload;
cmd_len = le16_to_cpu(tx_cmd_gen2->len);
}
/* Set up entry for this TFD in Tx byte-count array */
iwl_pcie_gen2_update_byte_tbl(trans_pcie, txq, cmd_len,
iwl_pcie_gen2_get_num_tbs(trans, tfd));
/* start timer if queue currently empty */
if (txq->read_ptr == txq->write_ptr) {
if (txq->wd_timeout)
mod_timer(&txq->stuck_timer, jiffies + txq->wd_timeout);
IWL_DEBUG_RPM(trans, "Q: %d first tx - take ref\n", txq->id);
iwl_trans_ref(trans);
}
/* Tell device the write index *just past* this latest filled TFD */
txq->write_ptr = iwl_queue_inc_wrap(trans, txq->write_ptr);
iwl_pcie_gen2_txq_inc_wr_ptr(trans, txq);
/*
* At this point the frame is "transmitted" successfully
* and we will get a TX status notification eventually.
*/
spin_unlock(&txq->lock);
return 0;
}
