diff --git a/drivers/infiniband/hw/bnxt_re/main.c b/drivers/infiniband/hw/bnxt_re/main.c
index 4aa3442..b9e2f89 100644
--- a/drivers/infiniband/hw/bnxt_re/main.c
+++ b/drivers/infiniband/hw/bnxt_re/main.c
@@ -1332,6 +1332,27 @@ static int bnxt_re_add_device(struct auxiliary_device *adev, u8 wqe_mode)
 	return rc;
 }
 
+static void bnxt_re_setup_cc(struct bnxt_re_dev *rdev, bool enable)
+{
+	struct bnxt_qplib_cc_param cc_param = {};
+
+	/* Currently enabling only for GenP5 adapters */
+	if (!bnxt_qplib_is_chip_gen_p5(rdev->chip_ctx))
+		return;
+
+	if (enable) {
+		cc_param.enable  = 1;
+		cc_param.cc_mode = CMDQ_MODIFY_ROCE_CC_CC_MODE_PROBABILISTIC_CC_MODE;
+	}
+
+	cc_param.mask = (CMDQ_MODIFY_ROCE_CC_MODIFY_MASK_CC_MODE |
+			 CMDQ_MODIFY_ROCE_CC_MODIFY_MASK_ENABLE_CC |
+			 CMDQ_MODIFY_ROCE_CC_MODIFY_MASK_TOS_ECN);
+
+	if (bnxt_qplib_modify_cc(&rdev->qplib_res, &cc_param))
+		ibdev_err(&rdev->ibdev, "Failed to setup CC enable = %d\n", enable);
+}
+
 /*
  * "Notifier chain callback can be invoked for the same chain from
  * different CPUs at the same time".
@@ -1400,7 +1421,7 @@ static void bnxt_re_remove(struct auxiliary_device *adev)
 		 */
 		goto skip_remove;
 	}
-
+	bnxt_re_setup_cc(rdev, false);
 	ib_unregister_device(&rdev->ibdev);
 	ib_dealloc_device(&rdev->ibdev);
 	bnxt_re_dev_uninit(rdev);
@@ -1432,6 +1453,7 @@ static int bnxt_re_probe(struct auxiliary_device *adev,
 		goto err;
 	}
 
+	bnxt_re_setup_cc(rdev, true);
 	mutex_unlock(&bnxt_re_mutex);
 	return 0;
 
diff --git a/drivers/infiniband/hw/bnxt_re/qplib_rcfw.c b/drivers/infiniband/hw/bnxt_re/qplib_rcfw.c
index 06979f7..de90691 100644
--- a/drivers/infiniband/hw/bnxt_re/qplib_rcfw.c
+++ b/drivers/infiniband/hw/bnxt_re/qplib_rcfw.c
@@ -96,7 +96,7 @@ static int __send_message(struct bnxt_qplib_rcfw *rcfw,
 	u32 sw_prod, cmdq_prod;
 	struct pci_dev *pdev;
 	unsigned long flags;
-	u32 size, opcode;
+	u32 bsize, opcode;
 	u16 cookie, cbit;
 	u8 *preq;
 
@@ -145,15 +145,14 @@ static int __send_message(struct bnxt_qplib_rcfw *rcfw,
 		return -EBUSY;
 	}
 
-	size = msg->req->cmd_size;
 	/* change the cmd_size to the number of 16byte cmdq unit.
 	 * req->cmd_size is modified here
 	 */
-	bnxt_qplib_set_cmd_slots(msg->req);
+	bsize = bnxt_qplib_set_cmd_slots(msg->req);
 
 	memset(msg->resp, 0, sizeof(*msg->resp));
 	crsqe->resp = (struct creq_qp_event *)msg->resp;
-	crsqe->resp->cookie = msg->req->cookie;
+	crsqe->resp->cookie = cpu_to_le16(cookie);
 	crsqe->req_size = __get_cmdq_base_cmd_size(msg->req, msg->req_sz);
 	if (__get_cmdq_base_resp_size(msg->req, msg->req_sz) && msg->sb) {
 		struct bnxt_qplib_rcfw_sbuf *sbuf = msg->sb;
@@ -174,11 +173,11 @@ static int __send_message(struct bnxt_qplib_rcfw *rcfw,
 		}
 		/* Copy a segment of the req cmd to the cmdq */
 		memset(cmdqe, 0, sizeof(*cmdqe));
-		memcpy(cmdqe, preq, min_t(u32, size, sizeof(*cmdqe)));
-		preq += min_t(u32, size, sizeof(*cmdqe));
-		size -= min_t(u32, size, sizeof(*cmdqe));
+		memcpy(cmdqe, preq, min_t(u32, bsize, sizeof(*cmdqe)));
+		preq += min_t(u32, bsize, sizeof(*cmdqe));
+		bsize -= min_t(u32, bsize, sizeof(*cmdqe));
 		hwq->prod++;
-	} while (size > 0);
+	} while (bsize > 0);
 	cmdq->seq_num++;
 
 	cmdq_prod = hwq->prod;
diff --git a/drivers/infiniband/hw/bnxt_re/qplib_rcfw.h b/drivers/infiniband/hw/bnxt_re/qplib_rcfw.h
index 5d619ce..dd56514 100644
--- a/drivers/infiniband/hw/bnxt_re/qplib_rcfw.h
+++ b/drivers/infiniband/hw/bnxt_re/qplib_rcfw.h
@@ -39,6 +39,8 @@
 #ifndef __BNXT_QPLIB_RCFW_H__
 #define __BNXT_QPLIB_RCFW_H__
 
+#include "qplib_tlv.h"
+
 #define RCFW_CMDQ_TRIG_VAL		1
 #define RCFW_COMM_PCI_BAR_REGION	0
 #define RCFW_COMM_CONS_PCI_BAR_REGION	2
@@ -87,11 +89,21 @@ static inline u32 bnxt_qplib_cmdqe_page_size(u32 depth)
 	return (bnxt_qplib_cmdqe_npages(depth) * PAGE_SIZE);
 }
 
-/* Set the cmd_size to a factor of CMDQE unit */
-static inline void bnxt_qplib_set_cmd_slots(struct cmdq_base *req)
+static inline u32 bnxt_qplib_set_cmd_slots(struct cmdq_base *req)
 {
-	req->cmd_size = (req->cmd_size + BNXT_QPLIB_CMDQE_UNITS - 1) /
-			 BNXT_QPLIB_CMDQE_UNITS;
+	u32 cmd_byte = 0;
+
+	if (HAS_TLV_HEADER(req)) {
+		struct roce_tlv *tlv_req = (struct roce_tlv *)req;
+
+		cmd_byte = tlv_req->total_size * BNXT_QPLIB_CMDQE_UNITS;
+	} else {
+		cmd_byte = req->cmd_size;
+		req->cmd_size = (req->cmd_size + BNXT_QPLIB_CMDQE_UNITS - 1) /
+				 BNXT_QPLIB_CMDQE_UNITS;
+	}
+
+	return cmd_byte;
 }
 
 #define RCFW_MAX_COOKIE_VALUE		0x7FFF
diff --git a/drivers/infiniband/hw/bnxt_re/qplib_sp.c b/drivers/infiniband/hw/bnxt_re/qplib_sp.c
index 54c26c5..1714a1e 100644
--- a/drivers/infiniband/hw/bnxt_re/qplib_sp.c
+++ b/drivers/infiniband/hw/bnxt_re/qplib_sp.c
@@ -48,6 +48,7 @@
 #include "qplib_res.h"
 #include "qplib_rcfw.h"
 #include "qplib_sp.h"
+#include "qplib_tlv.h"
 
 const struct bnxt_qplib_gid bnxt_qplib_gid_zero = {{ 0, 0, 0, 0, 0, 0, 0, 0,
 						     0, 0, 0, 0, 0, 0, 0, 0 } };
@@ -849,3 +850,111 @@ int bnxt_qplib_qext_stat(struct bnxt_qplib_rcfw *rcfw, u32 fid,
 	bnxt_qplib_rcfw_free_sbuf(rcfw, sbuf);
 	return rc;
 }
+
+static void bnxt_qplib_fill_cc_gen1(struct cmdq_modify_roce_cc_gen1_tlv *ext_req,
+				    struct bnxt_qplib_cc_param_ext *cc_ext)
+{
+	ext_req->modify_mask = cpu_to_le64(cc_ext->ext_mask);
+	cc_ext->ext_mask = 0;
+	ext_req->inactivity_th_hi = cpu_to_le16(cc_ext->inact_th_hi);
+	ext_req->min_time_between_cnps = cpu_to_le16(cc_ext->min_delta_cnp);
+	ext_req->init_cp = cpu_to_le16(cc_ext->init_cp);
+	ext_req->tr_update_mode = cc_ext->tr_update_mode;
+	ext_req->tr_update_cycles = cc_ext->tr_update_cyls;
+	ext_req->fr_num_rtts = cc_ext->fr_rtt;
+	ext_req->ai_rate_increase = cc_ext->ai_rate_incr;
+	ext_req->reduction_relax_rtts_th = cpu_to_le16(cc_ext->rr_rtt_th);
+	ext_req->additional_relax_cr_th = cpu_to_le16(cc_ext->ar_cr_th);
+	ext_req->cr_min_th = cpu_to_le16(cc_ext->cr_min_th);
+	ext_req->bw_avg_weight = cc_ext->bw_avg_weight;
+	ext_req->actual_cr_factor = cc_ext->cr_factor;
+	ext_req->max_cp_cr_th = cpu_to_le16(cc_ext->cr_th_max_cp);
+	ext_req->cp_bias_en = cc_ext->cp_bias_en;
+	ext_req->cp_bias = cc_ext->cp_bias;
+	ext_req->cnp_ecn = cc_ext->cnp_ecn;
+	ext_req->rtt_jitter_en = cc_ext->rtt_jitter_en;
+	ext_req->link_bytes_per_usec = cpu_to_le16(cc_ext->bytes_per_usec);
+	ext_req->reset_cc_cr_th = cpu_to_le16(cc_ext->cc_cr_reset_th);
+	ext_req->cr_width = cc_ext->cr_width;
+	ext_req->quota_period_min = cc_ext->min_quota;
+	ext_req->quota_period_max = cc_ext->max_quota;
+	ext_req->quota_period_abs_max = cc_ext->abs_max_quota;
+	ext_req->tr_lower_bound = cpu_to_le16(cc_ext->tr_lb);
+	ext_req->cr_prob_factor = cc_ext->cr_prob_fac;
+	ext_req->tr_prob_factor = cc_ext->tr_prob_fac;
+	ext_req->fairness_cr_th = cpu_to_le16(cc_ext->fair_cr_th);
+	ext_req->red_div = cc_ext->red_div;
+	ext_req->cnp_ratio_th = cc_ext->cnp_ratio_th;
+	ext_req->exp_ai_rtts = cpu_to_le16(cc_ext->ai_ext_rtt);
+	ext_req->exp_ai_cr_cp_ratio = cc_ext->exp_crcp_ratio;
+	ext_req->use_rate_table = cc_ext->low_rate_en;
+	ext_req->cp_exp_update_th = cpu_to_le16(cc_ext->cpcr_update_th);
+	ext_req->high_exp_ai_rtts_th1 = cpu_to_le16(cc_ext->ai_rtt_th1);
+	ext_req->high_exp_ai_rtts_th2 = cpu_to_le16(cc_ext->ai_rtt_th2);
+	ext_req->actual_cr_cong_free_rtts_th = cpu_to_le16(cc_ext->cf_rtt_th);
+	ext_req->severe_cong_cr_th1 = cpu_to_le16(cc_ext->sc_cr_th1);
+	ext_req->severe_cong_cr_th2 = cpu_to_le16(cc_ext->sc_cr_th2);
+	ext_req->link64B_per_rtt = cpu_to_le32(cc_ext->l64B_per_rtt);
+	ext_req->cc_ack_bytes = cc_ext->cc_ack_bytes;
+}
+
+int bnxt_qplib_modify_cc(struct bnxt_qplib_res *res,
+			 struct bnxt_qplib_cc_param *cc_param)
+{
+	struct bnxt_qplib_tlv_modify_cc_req tlv_req = {};
+	struct creq_modify_roce_cc_resp resp = {};
+	struct bnxt_qplib_cmdqmsg msg = {};
+	struct cmdq_modify_roce_cc *req;
+	int req_size;
+	void *cmd;
+	int rc;
+
+	/* Prepare the older base command */
+	req = &tlv_req.base_req;
+	cmd = req;
+	req_size = sizeof(*req);
+	bnxt_qplib_rcfw_cmd_prep((struct cmdq_base *)req, CMDQ_BASE_OPCODE_MODIFY_ROCE_CC,
+				 sizeof(*req));
+	req->modify_mask = cpu_to_le32(cc_param->mask);
+	req->enable_cc = cc_param->enable;
+	req->g = cc_param->g;
+	req->num_phases_per_state = cc_param->nph_per_state;
+	req->time_per_phase = cc_param->time_pph;
+	req->pkts_per_phase = cc_param->pkts_pph;
+	req->init_cr = cpu_to_le16(cc_param->init_cr);
+	req->init_tr = cpu_to_le16(cc_param->init_tr);
+	req->tos_dscp_tos_ecn = (cc_param->tos_dscp << CMDQ_MODIFY_ROCE_CC_TOS_DSCP_SFT) |
+				(cc_param->tos_ecn & CMDQ_MODIFY_ROCE_CC_TOS_ECN_MASK);
+	req->alt_vlan_pcp = cc_param->alt_vlan_pcp;
+	req->alt_tos_dscp = cpu_to_le16(cc_param->alt_tos_dscp);
+	req->rtt = cpu_to_le16(cc_param->rtt);
+	req->tcp_cp = cpu_to_le16(cc_param->tcp_cp);
+	req->cc_mode = cc_param->cc_mode;
+	req->inactivity_th = cpu_to_le16(cc_param->inact_th);
+
+	/* For chip gen P5 onwards fill extended cmd and header */
+	if (bnxt_qplib_is_chip_gen_p5(res->cctx)) {
+		struct roce_tlv *hdr;
+		u32 payload;
+		u32 chunks;
+
+		cmd = &tlv_req;
+		req_size = sizeof(tlv_req);
+		/* Prepare primary tlv header */
+		hdr = &tlv_req.tlv_hdr;
+		chunks = CHUNKS(sizeof(struct bnxt_qplib_tlv_modify_cc_req));
+		payload = sizeof(struct cmdq_modify_roce_cc);
+		__roce_1st_tlv_prep(hdr, chunks, payload, true);
+		/* Prepare secondary tlv header */
+		hdr = (struct roce_tlv *)&tlv_req.ext_req;
+		payload = sizeof(struct cmdq_modify_roce_cc_gen1_tlv) -
+			  sizeof(struct roce_tlv);
+		__roce_ext_tlv_prep(hdr, TLV_TYPE_MODIFY_ROCE_CC_GEN1, payload, false, true);
+		bnxt_qplib_fill_cc_gen1(&tlv_req.ext_req, &cc_param->cc_ext);
+	}
+
+	bnxt_qplib_fill_cmdqmsg(&msg, cmd, &resp, NULL, req_size,
+				sizeof(resp), 0);
+	rc = bnxt_qplib_rcfw_send_message(res->rcfw, &msg);
+	return rc;
+}
diff --git a/drivers/infiniband/hw/bnxt_re/qplib_sp.h b/drivers/infiniband/hw/bnxt_re/qplib_sp.h
index 96e61db..5de87465 100644
--- a/drivers/infiniband/hw/bnxt_re/qplib_sp.h
+++ b/drivers/infiniband/hw/bnxt_re/qplib_sp.h
@@ -244,6 +244,71 @@ struct bnxt_qplib_ext_stat {
 	u64  rx_ecn_marked;
 };
 
+struct bnxt_qplib_cc_param_ext {
+	u64 ext_mask;
+	u16 inact_th_hi;
+	u16 min_delta_cnp;
+	u16 init_cp;
+	u8 tr_update_mode;
+	u8 tr_update_cyls;
+	u8 fr_rtt;
+	u8 ai_rate_incr;
+	u16 rr_rtt_th;
+	u16 ar_cr_th;
+	u16 cr_min_th;
+	u8 bw_avg_weight;
+	u8 cr_factor;
+	u16 cr_th_max_cp;
+	u8 cp_bias_en;
+	u8 cp_bias;
+	u8 cnp_ecn;
+	u8 rtt_jitter_en;
+	u16 bytes_per_usec;
+	u16 cc_cr_reset_th;
+	u8 cr_width;
+	u8 min_quota;
+	u8 max_quota;
+	u8 abs_max_quota;
+	u16 tr_lb;
+	u8 cr_prob_fac;
+	u8 tr_prob_fac;
+	u16 fair_cr_th;
+	u8 red_div;
+	u8 cnp_ratio_th;
+	u16 ai_ext_rtt;
+	u8 exp_crcp_ratio;
+	u8 low_rate_en;
+	u16 cpcr_update_th;
+	u16 ai_rtt_th1;
+	u16 ai_rtt_th2;
+	u16 cf_rtt_th;
+	u16 sc_cr_th1; /* severe congestion cr threshold 1 */
+	u16 sc_cr_th2; /* severe congestion cr threshold 2 */
+	u32 l64B_per_rtt;
+	u8 cc_ack_bytes;
+	u16 reduce_cf_rtt_th;
+};
+
+struct bnxt_qplib_cc_param {
+	u8 alt_vlan_pcp;
+	u16 alt_tos_dscp;
+	u8 cc_mode;
+	u8 enable;
+	u16 inact_th;
+	u16 init_cr;
+	u16 init_tr;
+	u16 rtt;
+	u8 g;
+	u8 nph_per_state;
+	u8 time_pph;
+	u8 pkts_pph;
+	u8 tos_ecn;
+	u8 tos_dscp;
+	u16 tcp_cp;
+	struct bnxt_qplib_cc_param_ext cc_ext;
+	u32 mask;
+};
+
 int bnxt_qplib_get_sgid(struct bnxt_qplib_res *res,
 			struct bnxt_qplib_sgid_tbl *sgid_tbl, int index,
 			struct bnxt_qplib_gid *gid);
@@ -281,5 +346,7 @@ int bnxt_qplib_get_roce_stats(struct bnxt_qplib_rcfw *rcfw,
 			      struct bnxt_qplib_roce_stats *stats);
 int bnxt_qplib_qext_stat(struct bnxt_qplib_rcfw *rcfw, u32 fid,
 			 struct bnxt_qplib_ext_stat *estat);
+int bnxt_qplib_modify_cc(struct bnxt_qplib_res *res,
+			 struct bnxt_qplib_cc_param *cc_param);
 
 #endif /* __BNXT_QPLIB_SP_H__*/
