net/mlx5e: Fix LSO vlan insertion

[karo-tx-linux.git] / drivers / net / ethernet / mellanox / mlx5 / core / en_tx.c

diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en_tx.c b/drivers/net/ethernet/mellanox/mlx5/core/en_tx.c
index b73672f32e2c688169c4e37e44bcf0a5cbf065a7..cd8f85a251d7da5509c0651b6ab781ce1d152084 100644 (file)
--- a/drivers/net/ethernet/mellanox/mlx5/core/en_tx.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en_tx.c
@@ -116,7 +116,7 @@ static inline u16 mlx5e_get_inline_hdr_size(struct mlx5e_sq *sq,
         * headers and occur before the data gather.
         * Therefore these headers must be copied into the WQE
         */
-#define MLX5E_MIN_INLINE (ETH_HLEN + 2/*vlan tag*/)
+#define MLX5E_MIN_INLINE ETH_HLEN
 
        if (bf && (skb_headlen(skb) <= sq->max_inline))
                return skb_headlen(skb);
@@ -124,6 +124,21 @@ static inline u16 mlx5e_get_inline_hdr_size(struct mlx5e_sq *sq,
        return MLX5E_MIN_INLINE;
 }
 
+static inline void mlx5e_insert_vlan(void *start, struct sk_buff *skb, u16 ihs)
+{
+       struct vlan_ethhdr *vhdr = (struct vlan_ethhdr *)start;
+       int cpy1_sz = 2 * ETH_ALEN;
+       int cpy2_sz = ihs - cpy1_sz;
+
+       skb_copy_from_linear_data(skb, vhdr, cpy1_sz);
+       skb_pull_inline(skb, cpy1_sz);
+       vhdr->h_vlan_proto = skb->vlan_proto;
+       vhdr->h_vlan_TCI = cpu_to_be16(skb_vlan_tag_get(skb));
+       skb_copy_from_linear_data(skb, &vhdr->h_vlan_encapsulated_proto,
+                                 cpy2_sz);
+       skb_pull_inline(skb, cpy2_sz);
+}
+
 static netdev_tx_t mlx5e_sq_xmit(struct mlx5e_sq *sq, struct sk_buff *skb)
 {
        struct mlx5_wq_cyc       *wq   = &sq->wq;
@@ -175,8 +190,13 @@ static netdev_tx_t mlx5e_sq_xmit(struct mlx5e_sq *sq, struct sk_buff *skb)
                                                        ETH_ZLEN);
        }
 
-       skb_copy_from_linear_data(skb, eseg->inline_hdr_start, ihs);
-       skb_pull_inline(skb, ihs);
+       if (skb_vlan_tag_present(skb)) {
+               mlx5e_insert_vlan(eseg->inline_hdr_start, skb, ihs);
+               ihs += VLAN_HLEN;
+       } else {
+               skb_copy_from_linear_data(skb, eseg->inline_hdr_start, ihs);
+               skb_pull_inline(skb, ihs);
+       }
 
        eseg->inline_hdr_sz = cpu_to_be16(ihs);