ice: Add priority information into VLAN header

This patch introduces a new function ice_tx_prepare_vlan_flags_dcb to
insert 802.1p priority information into the VLAN header

Signed-off-by: Anirudh Venkataramanan <anirudh.venkataramanan@intel.com>
Tested-by: Andrew Bowers <andrewx.bowers@intel.com>
Signed-off-by: Jeff Kirsher <jeffrey.t.kirsher@intel.com>

1 files changed, 39 insertions, 0 deletions

diff --git a/drivers/net/ethernet/intel/ice/ice_dcb_lib.c b/drivers/net/ethernet/intel/ice/ice_dcb_lib.c
index aabba91189bd..de1e33db761a 100644
--- a/drivers/net/ethernet/intel/ice/ice_dcb_lib.c
+++ b/drivers/net/ethernet/intel/ice/ice_dcb_lib.c
@@ -360,6 +360,45 @@ dcb_init_err:
 }
 
 /**
+ * ice_tx_prepare_vlan_flags_dcb - prepare VLAN tagging for DCB
+ * @tx_ring: ring to send buffer on
+ * @first: pointer to struct ice_tx_buf
+ */
+int
+ice_tx_prepare_vlan_flags_dcb(struct ice_ring *tx_ring,
+                              struct ice_tx_buf *first)
+{
+        struct sk_buff *skb = first->skb;
+
+        if (!test_bit(ICE_FLAG_DCB_ENA, tx_ring->vsi->back->flags))
+                return 0;
+
+        /* Insert 802.1p priority into VLAN header */
+        if ((first->tx_flags & (ICE_TX_FLAGS_HW_VLAN | ICE_TX_FLAGS_SW_VLAN)) ||
+            skb->priority != TC_PRIO_CONTROL) {
+                first->tx_flags &= ~ICE_TX_FLAGS_VLAN_PR_M;
+                /* Mask the lower 3 bits to set the 802.1p priority */
+                first->tx_flags |= (skb->priority & 0x7) <<
+                                   ICE_TX_FLAGS_VLAN_PR_S;
+                if (first->tx_flags & ICE_TX_FLAGS_SW_VLAN) {
+                        struct vlan_ethhdr *vhdr;
+                        int rc;
+
+                        rc = skb_cow_head(skb, 0);
+                        if (rc < 0)
+                                return rc;
+                        vhdr = (struct vlan_ethhdr *)skb->data;
+                        vhdr->h_vlan_TCI = htons(first->tx_flags >>
+                                                 ICE_TX_FLAGS_VLAN_S);
+                } else {
+                        first->tx_flags |= ICE_TX_FLAGS_HW_VLAN;
+                }
+        }
+
+        return 0;
+}
+
+/**
  * ice_dcb_process_lldp_set_mib_change - Process MIB change
  * @pf: ptr to ice_pf
  * @event: pointer to the admin queue receive event