[PATCH for-next 2/3] qedr: Consider dscp priority for vlan priority

[Shai Malin <smalin@marvell.com>]

From: Prabhakar Kushwaha <pkushwaha@marvell.com>

if dscp is enabled, consider its priority while deciding
vlan priority.

Signed-off-by: Shai Malin <smalin@marvell.com>
Signed-off-by: Ariel Elior <aelior@marvell.com>
Signed-off-by: Prabhakar Kushwaha <pkushwaha@marvell.com>
---
 drivers/infiniband/hw/qedr/qedr_roce_cm.c  | 11 +++++++++-
 drivers/net/ethernet/qlogic/qed/qed_dcbx.c |  7 ++++++
 drivers/net/ethernet/qlogic/qed/qed_dcbx.h |  5 +++--
 drivers/net/ethernet/qlogic/qed/qed_rdma.c | 25 ++++++++++++++++++++++
 include/linux/qed/qed_rdma_if.h            |  7 ++++++
 5 files changed, 52 insertions(+), 3 deletions(-)

diff --git a/drivers/infiniband/hw/qedr/qedr_roce_cm.c b/drivers/infiniband/hw/qedr/qedr_roce_cm.c
index 13e5e6bbec99..eb7d24742664 100644
--- a/drivers/infiniband/hw/qedr/qedr_roce_cm.c
+++ b/drivers/infiniband/hw/qedr/qedr_roce_cm.c
@@ -389,6 +389,7 @@ static inline int qedr_gsi_build_header(struct qedr_dev *dev,
 	const struct ib_global_route *grh = rdma_ah_read_grh(ah_attr);
 	const struct ib_gid_attr *sgid_attr = grh->sgid_attr;
 	int send_size = 0;
+	u8 vlan_pri, dscp;
 	u16 vlan_id = 0;
 	u16 ether_type;
 	int rc;
@@ -437,8 +438,16 @@ static inline int qedr_gsi_build_header(struct qedr_dev *dev,
 	ether_addr_copy(udh->eth.dmac_h, ah_attr->roce.dmac);
 	ether_addr_copy(udh->eth.smac_h, dev->ndev->dev_addr);
 	if (has_vlan) {
-		udh->eth.type = htons(ETH_P_8021Q);
+		dscp = GET_FIELD(grh->traffic_class, QED_RDMA_TC_TOS_DSCP);
+
+		/* get the vlan priority associated with the dscp value */
+		if (!dev->ops->rdma_get_dscp_priority(dev->rdma_ctx, dscp,
+						      &vlan_pri))
+			vlan_id |= vlan_pri << VLAN_PRIO_SHIFT;
+
 		udh->vlan.tag = htons(vlan_id);
+
+		udh->eth.type = htons(ETH_P_8021Q);
 		udh->vlan.type = htons(ether_type);
 	} else {
 		udh->eth.type = htons(ether_type);
diff --git a/drivers/net/ethernet/qlogic/qed/qed_dcbx.c b/drivers/net/ethernet/qlogic/qed/qed_dcbx.c
index ba9276599e72..cec4b890e69a 100644
--- a/drivers/net/ethernet/qlogic/qed/qed_dcbx.c
+++ b/drivers/net/ethernet/qlogic/qed/qed_dcbx.c
@@ -973,6 +973,13 @@ void qed_dcbx_set_pf_update_params(struct qed_dcbx_results *p_src,
 	qed_dcbx_update_protocol_data(p_dcb_data, p_src, DCBX_PROTOCOL_ETH);
 }
 
+bool qed_dcbx_get_dscp_state(struct qed_hwfn *p_hwfn)
+{
+	struct qed_dcbx_get *p_dcbx_info = &p_hwfn->p_dcbx_info->get;
+
+	return p_dcbx_info->dscp.enabled;
+}
+
 u8 qed_dcbx_get_priority_tc(struct qed_hwfn *p_hwfn, u8 pri)
 {
 	struct qed_dcbx_get *dcbx_info = &p_hwfn->p_dcbx_info->get;
diff --git a/drivers/net/ethernet/qlogic/qed/qed_dcbx.h b/drivers/net/ethernet/qlogic/qed/qed_dcbx.h
index 6d8ce9bb30bd..86a374c3ed86 100644
--- a/drivers/net/ethernet/qlogic/qed/qed_dcbx.h
+++ b/drivers/net/ethernet/qlogic/qed/qed_dcbx.h
@@ -102,8 +102,7 @@ void qed_dcbx_set_pf_update_params(struct qed_dcbx_results *p_src,
 				   struct pf_update_ramrod_data *p_dest);
 
 /* Returns priority value for a given dscp index */
-int qed_dcbx_get_dscp_priority(struct qed_hwfn *p_hwfn, u8 dscp_index,
-			       u8 *p_dscp_pri);
+int qed_dcbx_get_dscp_priority(struct qed_hwfn *p_hwfn, u8 dscp_index, u8 *pri);
 
 /* Sets priority value for a given dscp index */
 int qed_dcbx_set_dscp_priority(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt,
@@ -112,4 +111,6 @@ int qed_dcbx_set_dscp_priority(struct qed_hwfn *p_hwfn, struct qed_ptt *p_ptt,
 #define QED_DCBX_DEFAULT_TC	0
 
 u8 qed_dcbx_get_priority_tc(struct qed_hwfn *p_hwfn, u8 pri);
+
+bool qed_dcbx_get_dscp_state(struct qed_hwfn *p_hwfn);
 #endif
diff --git a/drivers/net/ethernet/qlogic/qed/qed_rdma.c b/drivers/net/ethernet/qlogic/qed/qed_rdma.c
index da864d12916b..e904ba2e6242 100644
--- a/drivers/net/ethernet/qlogic/qed/qed_rdma.c
+++ b/drivers/net/ethernet/qlogic/qed/qed_rdma.c
@@ -32,6 +32,7 @@
 #include "qed_rdma.h"
 #include "qed_roce.h"
 #include "qed_sp.h"
+#include "qed_dcbx.h"
 
 
 int qed_rdma_bmap_alloc(struct qed_hwfn *p_hwfn,
@@ -1965,6 +1966,29 @@ static void qed_rdma_remove_user(void *rdma_cxt, u16 dpi)
 	spin_unlock_bh(&p_hwfn->p_rdma_info->lock);
 }
 
+#ifdef CONFIG_DCB
+static int qed_rdma_get_dscp_priority(void *rdma_cxt, u8 dscp_index, u8 *pri)
+{
+	struct qed_hwfn *p_hwfn = (struct qed_hwfn *)rdma_cxt;
+	int rc = -EINVAL;
+
+	if (qed_dcbx_get_dscp_state(p_hwfn)) {
+		rc = qed_dcbx_get_dscp_priority(p_hwfn, dscp_index, pri);
+		if (rc)
+			DP_INFO(p_hwfn,
+				"Failed to get priority val for dscp idx %d\n",
+				dscp_index);
+	}
+
+	return rc;
+}
+#else
+static int qed_rdma_get_dscp_priority(void *rdma_cxt, u8 dscp_index, u8 *pri)
+{
+	return -EINVAL;
+}
+#endif
+
 static int qed_roce_ll2_set_mac_filter(struct qed_dev *cdev,
 				       u8 *old_mac_address,
 				       u8 *new_mac_address)
@@ -2045,6 +2069,7 @@ static const struct qed_rdma_ops qed_rdma_ops_pass = {
 	.rdma_create_srq = &qed_rdma_create_srq,
 	.rdma_modify_srq = &qed_rdma_modify_srq,
 	.rdma_destroy_srq = &qed_rdma_destroy_srq,
+	.rdma_get_dscp_priority = &qed_rdma_get_dscp_priority,
 	.ll2_acquire_connection = &qed_ll2_acquire_connection,
 	.ll2_establish_connection = &qed_ll2_establish_connection,
 	.ll2_terminate_connection = &qed_ll2_terminate_connection,
diff --git a/include/linux/qed/qed_rdma_if.h b/include/linux/qed/qed_rdma_if.h
index aeb242cefebf..cd98e95ec2e3 100644
--- a/include/linux/qed/qed_rdma_if.h
+++ b/include/linux/qed/qed_rdma_if.h
@@ -363,6 +363,10 @@ struct qed_rdma_modify_qp_in_params {
 	u32 dest_qp;
 	bool lb_indication;
 	u16 mtu;
+#define QED_RDMA_TC_TOS_ECN_SHIFT 0
+#define QED_RDMA_TC_TOS_ECN_MASK 0x3
+#define QED_RDMA_TC_TOS_DSCP_SHIFT 2
+#define QED_RDMA_TC_TOS_DSCP_MASK 0x3f
 	u8 traffic_class_tos;
 	u8 hop_limit_ttl;
 	u32 flow_label;
@@ -639,6 +643,9 @@ struct qed_rdma_ops {
 	int (*rdma_modify_srq)(void *rdma_cxt,
 			       struct qed_rdma_modify_srq_in_params *iparams);
 
+	int (*rdma_get_dscp_priority)(void *rdma_cxt, u8 dscp_index,
+				      u8 *p_dscp_pri);
+
 	int (*ll2_acquire_connection)(void *rdma_cxt,
 				      struct qed_ll2_acquire_data *data);
 
-- 
2.24.1