4 files changed, 54 insertions, 0 deletions

diff --git a/sys/dev/mpi3mr/mpi3mr.c b/sys/dev/mpi3mr/mpi3mr.c
index 03fea4bdfcc7..a7bc459c1db8 100644
--- a/sys/dev/mpi3mr/mpi3mr.c
+++ b/sys/dev/mpi3mr/mpi3mr.c
@@ -2177,6 +2177,8 @@ static int mpi3mr_issue_iocinit(struct mpi3mr_softc *sc)
 	time_in_msec = (now.tv_sec * 1000 + now.tv_usec/1000);
 	iocinit_req.TimeStamp = htole64(time_in_msec);
 
+	iocinit_req.MsgFlags |= MPI3_IOCINIT_MSGFLAGS_WRITESAMEDIVERT_SUPPORTED;
+
 	init_completion(&sc->init_cmds.completion);
 	retval = mpi3mr_submit_admin_cmd(sc, &iocinit_req,
 	    sizeof(iocinit_req));
@@ -3340,6 +3342,19 @@ void mpi3mr_update_device(struct mpi3mr_softc *sc,
 		break;
 	}
 
+	switch (flags & MPI3_DEVICE0_FLAGS_MAX_WRITE_SAME_MASK) {
+	case MPI3_DEVICE0_FLAGS_MAX_WRITE_SAME_256_LB:
+		tgtdev->ws_len = 256;
+		break;
+	case MPI3_DEVICE0_FLAGS_MAX_WRITE_SAME_2048_LB:
+		tgtdev->ws_len = 2048;
+		break;
+	case MPI3_DEVICE0_FLAGS_MAX_WRITE_SAME_NO_LIMIT:
+	default:
+		tgtdev->ws_len = 0;
+		break;
+	}
+
 	switch (tgtdev->dev_type) {
 	case MPI3_DEVICE_DEVFORM_SAS_SATA:
 	{
@@ -5649,6 +5664,7 @@ static void mpi3mr_invalidate_devhandles(struct mpi3mr_softc *sc)
 			target->io_throttle_enabled = 0;
 			target->io_divert = 0;
 			target->throttle_group = NULL;
+			target->ws_len = 0;
 		}
 	}
 	mtx_unlock_spin(&sc->target_lock);
diff --git a/sys/dev/mpi3mr/mpi3mr.h b/sys/dev/mpi3mr/mpi3mr.h
index 2f91b0b702dd..fa50ed035fc2 100644
--- a/sys/dev/mpi3mr/mpi3mr.h
+++ b/sys/dev/mpi3mr/mpi3mr.h
@@ -232,6 +232,8 @@ extern char fmt_os_ver[16];
 
 #define MPI3MR_PERIODIC_DELAY	1	/* 1 second heartbeat/watchdog check */
 
+#define	WRITE_SAME_32	0x0d
+
 struct completion {
 	unsigned int done;
 	struct mtx lock;
diff --git a/sys/dev/mpi3mr/mpi3mr_cam.c b/sys/dev/mpi3mr/mpi3mr_cam.c
index d4cb7e9265dd..dca194a5c8cd 100644
--- a/sys/dev/mpi3mr/mpi3mr_cam.c
+++ b/sys/dev/mpi3mr/mpi3mr_cam.c
@@ -83,6 +83,7 @@
 #include "mpi3mr.h"
 #include <sys/time.h>			/* XXX for pcpu.h */
 #include <sys/pcpu.h>			/* XXX for PCPU_GET */
+#include <asm/unaligned.h>
 
 #define	smp_processor_id()  PCPU_GET(cpuid)
 
@@ -102,6 +103,37 @@ extern void mpi3mr_add_sg_single(void *paddr, U8 flags, U32 length,
 
 static U32 event_count;
 
+static
+inline void mpi3mr_divert_ws(Mpi3SCSIIORequest_t *req,
+			     struct ccb_scsiio *csio,
+			     U16 ws_len)
+{
+	U8 unmap = 0, ndob = 0;
+	U32 num_blocks = 0;
+	U8 opcode = scsiio_cdb_ptr(csio)[0];
+	U16 service_action = ((scsiio_cdb_ptr(csio)[8] << 8) | scsiio_cdb_ptr(csio)[9]);
+
+
+	if (opcode == WRITE_SAME_16 ||
+	   (opcode == VARIABLE_LEN_CDB &&
+	    service_action == WRITE_SAME_32)) {
+
+		int unmap_ndob_index = (opcode == WRITE_SAME_16) ? 1 : 10;
+
+		unmap = scsiio_cdb_ptr(csio)[unmap_ndob_index] & 0x08;
+		ndob = scsiio_cdb_ptr(csio)[unmap_ndob_index] & 0x01;
+		num_blocks = get_unaligned_be32(scsiio_cdb_ptr(csio) +
+						((opcode == WRITE_SAME_16) ? 10 : 28));
+
+		/* Check conditions for diversion to firmware */
+		if (unmap && ndob && num_blocks > ws_len) {
+			req->MsgFlags |= MPI3_SCSIIO_MSGFLAGS_DIVERT_TO_FIRMWARE;
+			req->Flags = htole32(le32toh(req->Flags) |
+					     MPI3_SCSIIO_FLAGS_DIVERT_REASON_WRITE_SAME_TOO_LARGE);
+		}
+	}
+}
+
 static void mpi3mr_prepare_sgls(void *arg,
 	bus_dma_segment_t *segs, int nsegs, int error)
 {
@@ -1080,6 +1112,9 @@ mpi3mr_action_scsiio(struct mpi3mr_cam_softc *cam_sc, union ccb *ccb)
 		break;
 	}
 
+	if (targ->ws_len)
+		mpi3mr_divert_ws(req, csio, targ->ws_len);
+
 	req->Flags = htole32(mpi_control);
 
 	if (csio->ccb_h.flags & CAM_CDB_POINTER)
diff --git a/sys/dev/mpi3mr/mpi3mr_cam.h b/sys/dev/mpi3mr/mpi3mr_cam.h
index 3a0526217f86..115ce0c4b8d7 100644
--- a/sys/dev/mpi3mr/mpi3mr_cam.h
+++ b/sys/dev/mpi3mr/mpi3mr_cam.h
@@ -121,6 +121,7 @@ struct mpi3mr_target {
 	struct mpi3mr_throttle_group_info *throttle_group;
 	uint64_t	q_depth;
 	enum mpi3mr_target_state state;
+	uint16_t	ws_len;
 };
 
 struct mpi3mr_cam_softc {