2 files changed, 52 insertions, 6 deletions

diff --git a/drivers/infiniband/hw/ipath/ipath_common.h b/drivers/infiniband/hw/ipath/ipath_common.h
index 12e1349cd03e..f70788c25ea6 100644
--- a/drivers/infiniband/hw/ipath/ipath_common.h
+++ b/drivers/infiniband/hw/ipath/ipath_common.h
@@ -501,13 +501,30 @@ struct __ipath_sendpkt {
         struct ipath_iovec sps_iov[4];
 };
 
-/* Passed into diag data special file's ->write method. */
+/*
+ * diagnostics can send a packet by "writing" one of the following
+ * two structs to diag data special file
+ * The first is the legacy version for backward compatibility
+ */
 struct ipath_diag_pkt {
         __u32 unit;
         __u64 data;
         __u32 len;
 };
 
+/* The second diag_pkt struct is the expanded version that allows
+ * more control over the packet, specifically, by allowing a custom
+ * pbc (+ extra) qword, so that special modes and deliberate
+ * changes to CRCs can be used. The elements were also re-ordered
+ * for better alignment and to avoid padding issues.
+ */
+struct ipath_diag_xpkt {
+        __u64 data;
+        __u64 pbc_wd;
+        __u32 unit;
+        __u32 len;
+};
+
 /*
  * Data layout in I2C flash (for GUID, etc.)
  * All fields are little-endian binary unless otherwise stated
diff --git a/drivers/infiniband/hw/ipath/ipath_diag.c b/drivers/infiniband/hw/ipath/ipath_diag.c
index 63e8368b0e95..aab21c1b822d 100644
--- a/drivers/infiniband/hw/ipath/ipath_diag.c
+++ b/drivers/infiniband/hw/ipath/ipath_diag.c
@@ -323,13 +323,14 @@ static ssize_t ipath_diagpkt_write(struct file *fp,
 {
         u32 __iomem *piobuf;
         u32 plen, clen, pbufn;
-        struct ipath_diag_pkt dp;
+        struct ipath_diag_pkt odp;
+        struct ipath_diag_xpkt dp;
         u32 *tmpbuf = NULL;
         struct ipath_devdata *dd;
         ssize_t ret = 0;
         u64 val;
 
-        if (count < sizeof(dp)) {
+        if (count != sizeof(dp)) {
                 ret = -EINVAL;
                 goto bail;
         }
@@ -339,6 +340,29 @@ static ssize_t ipath_diagpkt_write(struct file *fp,
                 goto bail;
         }
 
+        /*
+         * Due to padding/alignment issues (lessened with new struct)
+         * the old and new structs are the same length. We need to
+         * disambiguate them, which we can do because odp.len has never
+         * been less than the total of LRH+BTH+DETH so far, while
+         * dp.unit (same offset) unit is unlikely to get that high.
+         * Similarly, dp.data, the pointer to user at the same offset
+         * as odp.unit, is almost certainly at least one (512byte)page
+         * "above" NULL. The if-block below can be omitted if compatibility
+         * between a new driver and older diagnostic code is unimportant.
+         * compatibility the other direction (new diags, old driver) is
+         * handled in the diagnostic code, with a warning.
+         */
+        if (dp.unit >= 20 && dp.data < 512) {
+                /* very probable version mismatch. Fix it up */
+                memcpy(&odp, &dp, sizeof(odp));
+                /* We got a legacy dp, copy elements to dp */
+                dp.unit = odp.unit;
+                dp.data = odp.data;
+                dp.len = odp.len;
+                dp.pbc_wd = 0; /* Indicate we need to compute PBC wd */
+        }
+
         /* send count must be an exact number of dwords */
         if (dp.len & 3) {
                 ret = -EINVAL;
@@ -371,9 +395,10 @@ static ssize_t ipath_diagpkt_write(struct file *fp,
                 ret = -ENODEV;
                 goto bail;
         }
+        /* Check link state, but not if we have custom PBC */
         val = dd->ipath_lastibcstat & IPATH_IBSTATE_MASK;
-        if (val != IPATH_IBSTATE_INIT && val != IPATH_IBSTATE_ARM &&
-            val != IPATH_IBSTATE_ACTIVE) {
+        if (!dp.pbc_wd && val != IPATH_IBSTATE_INIT &&
+                val != IPATH_IBSTATE_ARM && val != IPATH_IBSTATE_ACTIVE) {
                 ipath_cdbg(VERBOSE, "unit %u not ready (state %llx)\n",
                            dd->ipath_unit, (unsigned long long) val);
                 ret = -EINVAL;
@@ -419,9 +444,13 @@ static ssize_t ipath_diagpkt_write(struct file *fp,
                 ipath_cdbg(VERBOSE, "unit %u 0x%x+1w pio%d\n",
                            dd->ipath_unit, plen - 1, pbufn);
 
+        if (dp.pbc_wd == 0)
+                /* Legacy operation, use computed pbc_wd */
+                dp.pbc_wd = plen;
+
         /* we have to flush after the PBC for correctness on some cpus
          * or WC buffer can be written out of order */
-        writeq(plen, piobuf);
+        writeq(dp.pbc_wd, piobuf);
         ipath_flush_wc();
         /* copy all by the trigger word, then flush, so it's written
          * to chip before trigger word, then write trigger word, then