iwlwifi: pcie: split tx to amsdu and non amsdu

The code is different enough to justify a split.
Signed-off-by: Sara Sharon <sara.sharon@intel.com>
Signed-off-by: Luca Coelho <luciano.coelho@intel.com>

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

@@ -365,58 +365,89 @@ static int iwl_pcie_gen2_build_amsdu(struct iwl_trans *trans,
 	return -EINVAL;
 }
 
-static
-struct iwl_tfh_tfd *iwl_pcie_gen2_build_tfd(struct iwl_trans *trans,
-					    struct iwl_txq *txq,
-					    struct iwl_device_cmd *dev_cmd,
-					    struct sk_buff *skb,
-					    struct iwl_cmd_meta *out_meta)
+static struct
+iwl_tfh_tfd *iwl_pcie_gen2_build_tx_amsdu(struct iwl_trans *trans,
+					  struct iwl_txq *txq,
+					  struct iwl_device_cmd *dev_cmd,
+					  struct sk_buff *skb,
+					  struct iwl_cmd_meta *out_meta,
+					  int hdr_len,
+					  int tx_cmd_len)
 {
-	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
 	int idx = iwl_pcie_get_cmd_index(txq, txq->write_ptr);
 	struct iwl_tfh_tfd *tfd = iwl_pcie_get_tfd(trans, txq, idx);
 	dma_addr_t tb_phys;
-	bool amsdu;
-	int i, len, tb1_len, tb2_len, hdr_len;
+	int len;
 	void *tb1_addr;
 
-	memset(tfd, 0, sizeof(*tfd));
+	tb_phys = iwl_pcie_get_first_tb_dma(txq, idx);
 
-	amsdu = ieee80211_is_data_qos(hdr->frame_control) &&
-		(*ieee80211_get_qos_ctl(hdr) &
-		 IEEE80211_QOS_CTL_A_MSDU_PRESENT);
+	iwl_pcie_gen2_set_tb(trans, tfd, tb_phys, IWL_FIRST_TB_SIZE);
+
+	/*
+	 * 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)
+	 */
+	len = tx_cmd_len + sizeof(struct iwl_cmd_header) + hdr_len -
+	      IWL_FIRST_TB_SIZE;
+
+	/* do not align A-MSDU to dword as the subframe header aligns it */
+
+	/* map the data for TB1 */
+	tb1_addr = ((u8 *)&dev_cmd->hdr) + IWL_FIRST_TB_SIZE;
+	tb_phys = dma_map_single(trans->dev, tb1_addr, len, DMA_TO_DEVICE);
+	if (unlikely(dma_mapping_error(trans->dev, tb_phys)))
+		goto out_err;
+	iwl_pcie_gen2_set_tb(trans, tfd, tb_phys, len);
+
+	if (iwl_pcie_gen2_build_amsdu(trans, skb, tfd,
+				      len + IWL_FIRST_TB_SIZE,
+				      hdr_len, dev_cmd))
+		goto out_err;
+
+	/* building the A-MSDU might have changed this data, memcpy it now */
+	memcpy(&txq->first_tb_bufs[idx], &dev_cmd->hdr, IWL_FIRST_TB_SIZE);
+	return tfd;
+
+out_err:
+	iwl_pcie_gen2_tfd_unmap(trans, out_meta, tfd);
+	return NULL;
+}
+
+static struct
+iwl_tfh_tfd *iwl_pcie_gen2_build_tx(struct iwl_trans *trans,
+				    struct iwl_txq *txq,
+				    struct iwl_device_cmd *dev_cmd,
+				    struct sk_buff *skb,
+				    struct iwl_cmd_meta *out_meta,
+				    int hdr_len,
+				    int tx_cmd_len)
+{
+	int idx = iwl_pcie_get_cmd_index(txq, txq->write_ptr);
+	struct iwl_tfh_tfd *tfd = iwl_pcie_get_tfd(trans, txq, idx);
+	dma_addr_t tb_phys;
+	int i, len, tb1_len, tb2_len;
+	void *tb1_addr;
 
 	tb_phys = iwl_pcie_get_first_tb_dma(txq, idx);
+
 	/* The first TB points to bi-directional DMA data */
-	if (!amsdu)
-		memcpy(&txq->first_tb_bufs[idx], &dev_cmd->hdr,
-		       IWL_FIRST_TB_SIZE);
+	memcpy(&txq->first_tb_bufs[idx], &dev_cmd->hdr, IWL_FIRST_TB_SIZE);
 
 	iwl_pcie_gen2_set_tb(trans, tfd, tb_phys, IWL_FIRST_TB_SIZE);
 
-	/* there must be data left over for TB1 or this code must be changed */
-	BUILD_BUG_ON(sizeof(struct iwl_tx_cmd_gen2) < IWL_FIRST_TB_SIZE);
-
 	/*
 	 * 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)
 	 */
-	if (trans->cfg->device_family < IWL_DEVICE_FAMILY_22560)
-		len = sizeof(struct iwl_tx_cmd_gen2);
-	else
-		len = sizeof(struct iwl_tx_cmd_gen3);
+	len = tx_cmd_len + sizeof(struct iwl_cmd_header) + hdr_len -
+	      IWL_FIRST_TB_SIZE;
 
-	len += sizeof(struct iwl_cmd_header) +
-		ieee80211_hdrlen(hdr->frame_control) -
-		IWL_FIRST_TB_SIZE;
-
-	/* do not align A-MSDU to dword as the subframe header aligns it */
-	if (amsdu)
-		tb1_len = len;
-	else
-		tb1_len = ALIGN(len, 4);
+	tb1_len = ALIGN(len, 4);
 
 	/* map the data for TB1 */
 	tb1_addr = ((u8 *)&dev_cmd->hdr) + IWL_FIRST_TB_SIZE;
@@ -425,23 +456,6 @@ struct iwl_tfh_tfd *iwl_pcie_gen2_build_tfd(struct iwl_trans *trans,
 		goto out_err;
 	iwl_pcie_gen2_set_tb(trans, tfd, tb_phys, tb1_len);
 
-	hdr_len = ieee80211_hdrlen(hdr->frame_control);
-
-	if (amsdu) {
-		if (iwl_pcie_gen2_build_amsdu(trans, skb, tfd,
-					      tb1_len + IWL_FIRST_TB_SIZE,
-					      hdr_len, dev_cmd))
-			goto out_err;
-
-		/*
-		 * building the A-MSDU might have changed this data, so memcpy
-		 * it now
-		 */
-		memcpy(&txq->first_tb_bufs[idx], &dev_cmd->hdr,
-		       IWL_FIRST_TB_SIZE);
-		return tfd;
-	}
-
 	/* set up TFD's third entry to point to remainder of skb's head */
 	tb2_len = skb_headlen(skb) - hdr_len;
 
@@ -483,6 +497,43 @@ struct iwl_tfh_tfd *iwl_pcie_gen2_build_tfd(struct iwl_trans *trans,
 	return NULL;
 }
 
+static
+struct iwl_tfh_tfd *iwl_pcie_gen2_build_tfd(struct iwl_trans *trans,
+					    struct iwl_txq *txq,
+					    struct iwl_device_cmd *dev_cmd,
+					    struct sk_buff *skb,
+					    struct iwl_cmd_meta *out_meta)
+{
+	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
+	int idx = iwl_pcie_get_cmd_index(txq, txq->write_ptr);
+	struct iwl_tfh_tfd *tfd = iwl_pcie_get_tfd(trans, txq, idx);
+	int len, hdr_len;
+	bool amsdu;
+
+	/* There must be data left over for TB1 or this code must be changed */
+	BUILD_BUG_ON(sizeof(struct iwl_tx_cmd_gen2) < IWL_FIRST_TB_SIZE);
+
+	memset(tfd, 0, sizeof(*tfd));
+
+	if (trans->cfg->device_family < IWL_DEVICE_FAMILY_22560)
+		len = sizeof(struct iwl_tx_cmd_gen2);
+	else
+		len = sizeof(struct iwl_tx_cmd_gen3);
+
+	amsdu = ieee80211_is_data_qos(hdr->frame_control) &&
+			(*ieee80211_get_qos_ctl(hdr) &
+			 IEEE80211_QOS_CTL_A_MSDU_PRESENT);
+
+	hdr_len = ieee80211_hdrlen(hdr->frame_control);
+
+	if (amsdu)
+		return iwl_pcie_gen2_build_tx_amsdu(trans, txq, dev_cmd, skb,
+						    out_meta, hdr_len, len);
+
+	return iwl_pcie_gen2_build_tx(trans, txq, dev_cmd, skb, out_meta,
+				      hdr_len, len);
+}
+
 int iwl_trans_pcie_gen2_tx(struct iwl_trans *trans, struct sk_buff *skb,
 			   struct iwl_device_cmd *dev_cmd, int txq_id)
 {