3 files changed, 21 insertions, 10 deletions

diff --git a/drivers/infiniband/hw/hfi1/driver.c b/drivers/infiniband/hw/hfi1/driver.c
index 46d1475b2154..bd837a048bf4 100644
--- a/drivers/infiniband/hw/hfi1/driver.c
+++ b/drivers/infiniband/hw/hfi1/driver.c
@@ -433,31 +433,43 @@ void hfi1_process_ecn_slowpath(struct rvt_qp *qp, struct hfi1_packet *pkt,
                                bool do_cnp)
 {
         struct hfi1_ibport *ibp = to_iport(qp->ibqp.device, qp->port_num);
+        struct hfi1_pportdata *ppd = ppd_from_ibp(ibp);
         struct ib_other_headers *ohdr = pkt->ohdr;
         struct ib_grh *grh = pkt->grh;
         u32 rqpn = 0, bth1;
-        u16 pkey, rlid, dlid = ib_get_dlid(pkt->hdr);
+        u16 pkey;
+        u32 rlid, slid, dlid = 0;
         u8 hdr_type, sc, svc_type;
         bool is_mcast = false;
 
+        /* can be called from prescan */
         if (pkt->etype == RHF_RCV_TYPE_BYPASS) {
                 is_mcast = hfi1_is_16B_mcast(dlid);
                 pkey = hfi1_16B_get_pkey(pkt->hdr);
                 sc = hfi1_16B_get_sc(pkt->hdr);
+                dlid = hfi1_16B_get_dlid(pkt->hdr);
+                slid = hfi1_16B_get_slid(pkt->hdr);
                 hdr_type = HFI1_PKT_TYPE_16B;
         } else {
                 is_mcast = (dlid > be16_to_cpu(IB_MULTICAST_LID_BASE)) &&
                            (dlid != be16_to_cpu(IB_LID_PERMISSIVE));
                 pkey = ib_bth_get_pkey(ohdr);
                 sc = hfi1_9B_get_sc5(pkt->hdr, pkt->rhf);
+                dlid = ib_get_dlid(pkt->hdr);
+                slid = ib_get_slid(pkt->hdr);
                 hdr_type = HFI1_PKT_TYPE_9B;
         }
 
         switch (qp->ibqp.qp_type) {
+        case IB_QPT_UD:
+                dlid = ppd->lid;
+                rlid = slid;
+                rqpn = ib_get_sqpn(pkt->ohdr);
+                svc_type = IB_CC_SVCTYPE_UD;
+                break;
         case IB_QPT_SMI:
         case IB_QPT_GSI:
-        case IB_QPT_UD:
-                rlid = ib_get_slid(pkt->hdr);
+                rlid = slid;
                 rqpn = ib_get_sqpn(pkt->ohdr);
                 svc_type = IB_CC_SVCTYPE_UD;
                 break;
@@ -482,7 +494,6 @@ void hfi1_process_ecn_slowpath(struct rvt_qp *qp, struct hfi1_packet *pkt,
                                               dlid, rlid, sc, grh);
 
         if (!is_mcast && (bth1 & IB_BECN_SMASK)) {
-                struct hfi1_pportdata *ppd = ppd_from_ibp(ibp);
                 u32 lqpn = bth1 & RVT_QPN_MASK;
                 u8 sl = ibp->sc_to_sl[sc];
 
diff --git a/drivers/infiniband/hw/hfi1/hfi.h b/drivers/infiniband/hw/hfi1/hfi.h
index 32c48265405e..cac2c62bc42d 100644
--- a/drivers/infiniband/hw/hfi1/hfi.h
+++ b/drivers/infiniband/hw/hfi1/hfi.h
@@ -1537,13 +1537,13 @@ void set_link_ipg(struct hfi1_pportdata *ppd);
 void process_becn(struct hfi1_pportdata *ppd, u8 sl, u32 rlid, u32 lqpn,
                   u32 rqpn, u8 svc_type);
 void return_cnp(struct hfi1_ibport *ibp, struct rvt_qp *qp, u32 remote_qpn,
-                u32 pkey, u32 slid, u32 dlid, u8 sc5,
+                u16 pkey, u32 slid, u32 dlid, u8 sc5,
                 const struct ib_grh *old_grh);
 void return_cnp_16B(struct hfi1_ibport *ibp, struct rvt_qp *qp,
-                    u32 remote_qpn, u32 pkey, u32 slid, u32 dlid,
+                    u32 remote_qpn, u16 pkey, u32 slid, u32 dlid,
                     u8 sc5, const struct ib_grh *old_grh);
 typedef void (*hfi1_handle_cnp)(struct hfi1_ibport *ibp, struct rvt_qp *qp,
-                                u32 remote_qpn, u32 pkey, u32 slid, u32 dlid,
+                                u32 remote_qpn, u16 pkey, u32 slid, u32 dlid,
                                 u8 sc5, const struct ib_grh *old_grh);
 
 #define PKEY_CHECK_INVALID -1
@@ -2437,7 +2437,7 @@ static inline void hfi1_make_16b_hdr(struct hfi1_16b_header *hdr,
                 ((slid >> OPA_16B_SLID_SHIFT) << OPA_16B_SLID_HIGH_SHIFT);
         lrh2 = (lrh2 & ~OPA_16B_DLID_MASK) |
                 ((dlid >> OPA_16B_DLID_SHIFT) << OPA_16B_DLID_HIGH_SHIFT);
-        lrh2 = (lrh2 & ~OPA_16B_PKEY_MASK) | (pkey << OPA_16B_PKEY_SHIFT);
+        lrh2 = (lrh2 & ~OPA_16B_PKEY_MASK) | ((u32)pkey << OPA_16B_PKEY_SHIFT);
         lrh2 = (lrh2 & ~OPA_16B_L4_MASK) | l4;
 
         hdr->lrh[0] = lrh0;
diff --git a/drivers/infiniband/hw/hfi1/ud.c b/drivers/infiniband/hw/hfi1/ud.c
index bcf3b0bebac8..69c17a5ef038 100644
--- a/drivers/infiniband/hw/hfi1/ud.c
+++ b/drivers/infiniband/hw/hfi1/ud.c
@@ -628,7 +628,7 @@ int hfi1_lookup_pkey_idx(struct hfi1_ibport *ibp, u16 pkey)
 }
 
 void return_cnp_16B(struct hfi1_ibport *ibp, struct rvt_qp *qp,
-                    u32 remote_qpn, u32 pkey, u32 slid, u32 dlid,
+                    u32 remote_qpn, u16 pkey, u32 slid, u32 dlid,
                     u8 sc5, const struct ib_grh *old_grh)
 {
         u64 pbc, pbc_flags = 0;
@@ -687,7 +687,7 @@ void return_cnp_16B(struct hfi1_ibport *ibp, struct rvt_qp *qp,
 }
 
 void return_cnp(struct hfi1_ibport *ibp, struct rvt_qp *qp, u32 remote_qpn,
-                u32 pkey, u32 slid, u32 dlid, u8 sc5,
+                u16 pkey, u32 slid, u32 dlid, u8 sc5,
                 const struct ib_grh *old_grh)
 {
         u64 pbc, pbc_flags = 0;