MFC r310524: Improve length handling when writing sense data.

 - Allow maximal sense size limitation via Control Extension mode page.
 - When sense size limited, include descriptors atomically: whole or none.
 - Set new SDAT_OVFL bit if some descriptors don't fit the limit.
 - Report real written sense length instead of static maximal 252 bytes.

6 files changed, 313 insertions, 328 deletions

diff --git a/sys/cam/ctl/ctl.c b/sys/cam/ctl/ctl.c
index ed9cbfb..1a24641 100644
--- a/sys/cam/ctl/ctl.c
+++ b/sys/cam/ctl/ctl.c
@@ -278,7 +278,7 @@ const static struct scsi_control_ext_page control_ext_page_changeable = {
 	/*page_length*/{CTL_CEM_LEN >> 8, CTL_CEM_LEN},
 	/*flags*/0,
 	/*prio*/0,
-	/*max_sense*/0
+	/*max_sense*/0xff
 };
 
 const static struct scsi_info_exceptions_page ie_page_default = {
@@ -9211,6 +9211,7 @@ ctl_request_sense(struct ctl_scsiio *ctsio)
 	struct ctl_lun *lun;
 	uint32_t initidx;
 	int have_error;
+	u_int sense_len = SSD_FULL_SIZE;
 	scsi_sense_data_type sense_format;
 	ctl_ua_type ua_type;
 	uint8_t asc = 0, ascq = 0;
@@ -9254,7 +9255,7 @@ ctl_request_sense(struct ctl_scsiio *ctsio)
 	    ((lun->flags & CTL_LUN_PRIMARY_SC) == 0 &&
 	     softc->ha_link < CTL_HA_LINK_UNKNOWN)) {
 		/* "Logical unit not supported" */
-		ctl_set_sense_data(sense_ptr, NULL, sense_format,
+		ctl_set_sense_data(sense_ptr, &sense_len, NULL, sense_format,
 		    /*current_error*/ 1,
 		    /*sense_key*/ SSD_KEY_ILLEGAL_REQUEST,
 		    /*asc*/ 0x25,
@@ -9310,7 +9311,8 @@ ctl_request_sense(struct ctl_scsiio *ctsio)
 	} else
 #endif
 	if (have_error == 0) {
-		ua_type = ctl_build_ua(lun, initidx, sense_ptr, sense_format);
+		ua_type = ctl_build_ua(lun, initidx, sense_ptr, &sense_len,
+		    sense_format);
 		if (ua_type != CTL_UA_NONE)
 			have_error = 1;
 	}
@@ -9322,7 +9324,7 @@ ctl_request_sense(struct ctl_scsiio *ctsio)
 			asc = lun->ie_asc;
 			ascq = lun->ie_ascq;
 		}
-		ctl_set_sense_data(sense_ptr, lun, sense_format,
+		ctl_set_sense_data(sense_ptr, &sense_len, lun, sense_format,
 		    /*current_error*/ 1,
 		    /*sense_key*/ SSD_KEY_NO_SENSE,
 		    /*asc*/ asc,
@@ -11626,14 +11628,15 @@ ctl_scsiio_precheck(struct ctl_softc *softc, struct ctl_scsiio *ctsio)
 	 */
 	if ((entry->flags & CTL_CMD_FLAG_NO_SENSE) == 0) {
 		ctl_ua_type ua_type;
+		u_int sense_len = 0;
 
 		ua_type = ctl_build_ua(lun, initidx, &ctsio->sense_data,
-		    SSD_TYPE_NONE);
+		    &sense_len, SSD_TYPE_NONE);
 		if (ua_type != CTL_UA_NONE) {
 			mtx_unlock(&lun->lun_lock);
 			ctsio->scsi_status = SCSI_STATUS_CHECK_COND;
 			ctsio->io_hdr.status = CTL_SCSI_ERROR | CTL_AUTOSENSE;
-			ctsio->sense_len = SSD_FULL_SIZE;
+			ctsio->sense_len = sense_len;
 			ctl_done((union ctl_io *)ctsio);
 			return (retval);
 		}
diff --git a/sys/cam/ctl/ctl_error.c b/sys/cam/ctl/ctl_error.c
index 4e75a5c..02330a1 100644
--- a/sys/cam/ctl/ctl_error.c
+++ b/sys/cam/ctl/ctl_error.c
@@ -65,9 +65,9 @@ __FBSDID("$FreeBSD$");
 #include <cam/ctl/ctl_private.h>
 
 void
-ctl_set_sense_data_va(struct scsi_sense_data *sense_data, void *lunptr,
-		      scsi_sense_data_type sense_format, int current_error,
-		      int sense_key, int asc, int ascq, va_list ap) 
+ctl_set_sense_data_va(struct scsi_sense_data *sense_data, u_int *sense_len,
+    void *lunptr, scsi_sense_data_type sense_format, int current_error,
+    int sense_key, int asc, int ascq, va_list ap)
 {
 	struct ctl_lun *lun;
 
@@ -89,20 +89,30 @@ ctl_set_sense_data_va(struct scsi_sense_data *sense_data, void *lunptr,
 			sense_format = SSD_TYPE_FIXED;
 	}
 
-	scsi_set_sense_data_va(sense_data, sense_format, current_error,
-			       sense_key, asc, ascq, ap);
+	/*
+	 * Determine maximum sense data length to return.
+	 */
+	if (*sense_len == 0) {
+		if ((lun != NULL) && (lun->MODE_CTRLE.max_sense != 0))
+			*sense_len = lun->MODE_CTRLE.max_sense;
+		else
+			*sense_len = SSD_FULL_SIZE;
+	}
+
+	scsi_set_sense_data_va(sense_data, sense_len, sense_format,
+	    current_error, sense_key, asc, ascq, ap);
 }
 
 void
-ctl_set_sense_data(struct scsi_sense_data *sense_data, void *lunptr,
-		   scsi_sense_data_type sense_format, int current_error,
-		   int sense_key, int asc, int ascq, ...) 
+ctl_set_sense_data(struct scsi_sense_data *sense_data, u_int *sense_len,
+    void *lunptr, scsi_sense_data_type sense_format, int current_error,
+    int sense_key, int asc, int ascq, ...)
 {
 	va_list ap;
 
 	va_start(ap, ascq);
-	ctl_set_sense_data_va(sense_data, lunptr, sense_format, current_error,
-			      sense_key, asc, ascq, ap);
+	ctl_set_sense_data_va(sense_data, sense_len, lunptr, sense_format,
+	    current_error, sense_key, asc, ascq, ap);
 	va_end(ap);
 }
 
@@ -112,6 +122,7 @@ ctl_set_sense(struct ctl_scsiio *ctsio, int current_error, int sense_key,
 {
 	va_list ap;
 	struct ctl_lun *lun;
+	u_int sense_len;
 
 	/*
 	 * The LUN can't go away until all of the commands have been
@@ -121,7 +132,8 @@ ctl_set_sense(struct ctl_scsiio *ctsio, int current_error, int sense_key,
 	lun = (struct ctl_lun *)ctsio->io_hdr.ctl_private[CTL_PRIV_LUN].ptr;
 
 	va_start(ap, ascq);
-	ctl_set_sense_data_va(&ctsio->sense_data,
+	sense_len = 0;
+	ctl_set_sense_data_va(&ctsio->sense_data, &sense_len,
 			      lun,
 			      SSD_TYPE_NONE,
 			      current_error,
@@ -132,7 +144,7 @@ ctl_set_sense(struct ctl_scsiio *ctsio, int current_error, int sense_key,
 	va_end(ap);
 
 	ctsio->scsi_status = SCSI_STATUS_CHECK_COND;
-	ctsio->sense_len = SSD_FULL_SIZE;
+	ctsio->sense_len = sense_len;
 	ctsio->io_hdr.status = CTL_SCSI_ERROR | CTL_AUTOSENSE;
 }
 
@@ -148,6 +160,7 @@ ctl_sense_to_desc(struct scsi_sense_data_fixed *sense_src,
 {
 	struct scsi_sense_stream stream_sense;
 	int current_error;
+	u_int sense_len;
 	uint8_t stream_bits;
 
 	bzero(sense_dest, sizeof(*sense_dest));
@@ -173,7 +186,8 @@ ctl_sense_to_desc(struct scsi_sense_data_fixed *sense_src,
 	 * value is set in the fixed sense data, set it in the descriptor
 	 * data.  Otherwise, skip it.
 	 */
-	ctl_set_sense_data((struct scsi_sense_data *)sense_dest,
+	sense_len = SSD_FULL_SIZE;
+	ctl_set_sense_data((struct scsi_sense_data *)sense_dest, &sense_len,
 			   /*lun*/ NULL,
 			   /*sense_format*/ SSD_TYPE_DESC,
 			   current_error,
@@ -233,6 +247,7 @@ ctl_sense_to_fixed(struct scsi_sense_data_desc *sense_src,
 	int info_size = 0, cmd_size = 0, fru_size = 0;
 	int sks_size = 0, stream_size = 0;
 	int pos;
+	u_int sense_len;
 
 	if ((sense_src->error_code & SSD_ERRCODE) == SSD_DESC_CURRENT_ERROR)
 		current_error = 1;
@@ -318,7 +333,8 @@ ctl_sense_to_fixed(struct scsi_sense_data_desc *sense_src,
 		}
 	}
 
-	ctl_set_sense_data((struct scsi_sense_data *)sense_dest,
+	sense_len = SSD_FULL_SIZE;
+	ctl_set_sense_data((struct scsi_sense_data *)sense_dest, &sense_len,
 			   /*lun*/ NULL,
 			   /*sense_format*/ SSD_TYPE_FIXED,
 			   current_error,
@@ -501,12 +517,13 @@ ctl_build_qae(struct ctl_lun *lun, uint32_t initidx, uint8_t *resp)
 		resp[0] |= 0x20;
 	resp[1] = asc;
 	resp[2] = ascq;
-	return (ua);
+	return (ua_to_build);
 }
 
 ctl_ua_type
 ctl_build_ua(struct ctl_lun *lun, uint32_t initidx,
-    struct scsi_sense_data *sense, scsi_sense_data_type sense_format)
+    struct scsi_sense_data *sense, u_int *sense_len,
+    scsi_sense_data_type sense_format)
 {
 	ctl_ua_type *ua;
 	ctl_ua_type ua_to_build, ua_to_clear;
@@ -540,7 +557,7 @@ ctl_build_ua(struct ctl_lun *lun, uint32_t initidx,
 	info = NULL;
 	ctl_ua_to_ascq(lun, ua_to_build, &asc, &ascq, &ua_to_clear, &info);
 
-	ctl_set_sense_data(sense, lun, sense_format, /*current_error*/ 1,
+	ctl_set_sense_data(sense, sense_len, lun, sense_format, 1,
 	    /*sense_key*/ SSD_KEY_UNIT_ATTENTION, asc, ascq,
 	    ((info != NULL) ? SSD_ELEM_INFO : SSD_ELEM_SKIP), 8, info,
 	    SSD_ELEM_NONE);
diff --git a/sys/cam/ctl/ctl_error.h b/sys/cam/ctl/ctl_error.h
index 1922289..d4cdbb3 100644
--- a/sys/cam/ctl/ctl_error.h
+++ b/sys/cam/ctl/ctl_error.h
@@ -45,12 +45,12 @@
 
 struct ctl_lun;
 
-void ctl_set_sense_data_va(struct scsi_sense_data *sense_data, void *lun,
-			   scsi_sense_data_type sense_format, int current_error,
-			   int sense_key, int asc, int ascq, va_list ap); 
-void ctl_set_sense_data(struct scsi_sense_data *sense_data, void *lun,
-			scsi_sense_data_type sense_format, int current_error,
-			int sense_key, int asc, int ascq, ...); 
+void ctl_set_sense_data_va(struct scsi_sense_data *sense_data, u_int *sense_len,
+    void *lun, scsi_sense_data_type sense_format, int current_error,
+    int sense_key, int asc, int ascq, va_list ap);
+void ctl_set_sense_data(struct scsi_sense_data *sense_data, u_int *sense_len,
+    void *lun, scsi_sense_data_type sense_format, int current_error,
+    int sense_key, int asc, int ascq, ...);
 void ctl_set_sense(struct ctl_scsiio *ctsio, int current_error, int sense_key,
 		   int asc, int ascq, ...);
 void ctl_sense_to_desc(struct scsi_sense_data_fixed *sense_src,
@@ -60,7 +60,8 @@ void ctl_sense_to_fixed(struct scsi_sense_data_desc *sense_src,
 void ctl_set_ua(struct ctl_scsiio *ctsio, int asc, int ascq);
 ctl_ua_type ctl_build_qae(struct ctl_lun *lun, uint32_t initidx, uint8_t *resp);
 ctl_ua_type ctl_build_ua(struct ctl_lun *lun, uint32_t initidx,
-    struct scsi_sense_data *sense, scsi_sense_data_type sense_format);
+    struct scsi_sense_data *sense, u_int *sense_len,
+    scsi_sense_data_type sense_format);
 void ctl_set_overlapped_cmd(struct ctl_scsiio *ctsio);
 void ctl_set_overlapped_tag(struct ctl_scsiio *ctsio, uint8_t tag);
 void ctl_set_invalid_field(struct ctl_scsiio *ctsio, int sks_valid, int command,
diff --git a/sys/cam/ctl/ctl_private.h b/sys/cam/ctl/ctl_private.h
index b8ad726..e118343 100644
--- a/sys/cam/ctl/ctl_private.h
+++ b/sys/cam/ctl/ctl_private.h
@@ -285,7 +285,7 @@ static const struct ctl_page_index page_index_template[] = {
 	 CTL_PAGE_FLAG_ALL, NULL, ctl_default_page_handler},
 	{SMS_CONTROL_MODE_PAGE | SMPH_SPF, 0x01,
 	 sizeof(struct scsi_control_ext_page), NULL,
-	 CTL_PAGE_FLAG_ALL, NULL, NULL},
+	 CTL_PAGE_FLAG_ALL, NULL, ctl_default_page_handler},
 	{SMS_INFO_EXCEPTIONS_PAGE, 0, sizeof(struct scsi_info_exceptions_page), NULL,
 	 CTL_PAGE_FLAG_ALL, NULL, ctl_ie_page_handler},
 	{SMS_INFO_EXCEPTIONS_PAGE | SMPH_SPF, 0x02,
diff --git a/sys/cam/scsi/scsi_all.c b/sys/cam/scsi/scsi_all.c
index 82232c1..78ddb12 100644
--- a/sys/cam/scsi/scsi_all.c
+++ b/sys/cam/scsi/scsi_all.c
@@ -3741,341 +3741,300 @@ scsi_find_desc(struct scsi_sense_data_desc *sense, u_int sense_len,
 }
 
 /*
- * Fill in SCSI sense data with the specified parameters.  This routine can
- * fill in either fixed or descriptor type sense data.
+ * Fill in SCSI descriptor sense data with the specified parameters.
  */
-void
-scsi_set_sense_data_va(struct scsi_sense_data *sense_data,
-		      scsi_sense_data_type sense_format, int current_error,
-		      int sense_key, int asc, int ascq, va_list ap) 
+static void
+scsi_set_sense_data_desc_va(struct scsi_sense_data *sense_data,
+    u_int *sense_len, scsi_sense_data_type sense_format, int current_error,
+    int sense_key, int asc, int ascq, va_list ap)
 {
-	int descriptor_sense;
+	struct scsi_sense_data_desc *sense;
 	scsi_sense_elem_type elem_type;
+	int space, len;
+	uint8_t *desc, *data;
 
-	/*
-	 * Determine whether to return fixed or descriptor format sense
-	 * data.  If the user specifies SSD_TYPE_NONE for some reason,
-	 * they'll just get fixed sense data.
-	 */
-	if (sense_format == SSD_TYPE_DESC)
-		descriptor_sense = 1;
-	else
-		descriptor_sense = 0;
-
-	/*
-	 * Zero the sense data, so that we don't pass back any garbage data
-	 * to the user.
-	 */
 	memset(sense_data, 0, sizeof(*sense_data));
+	sense = (struct scsi_sense_data_desc *)sense_data;
+	if (current_error != 0)
+		sense->error_code = SSD_DESC_CURRENT_ERROR;
+	else
+		sense->error_code = SSD_DESC_DEFERRED_ERROR;
+	sense->sense_key = sense_key;
+	sense->add_sense_code = asc;
+	sense->add_sense_code_qual = ascq;
+	sense->flags = 0;
+
+	desc = &sense->sense_desc[0];
+	space = *sense_len - offsetof(struct scsi_sense_data_desc, sense_desc);
+	while ((elem_type = va_arg(ap, scsi_sense_elem_type)) !=
+	    SSD_ELEM_NONE) {
+		if (elem_type >= SSD_ELEM_MAX) {
+			printf("%s: invalid sense type %d\n", __func__,
+			       elem_type);
+			break;
+		}
+		len = va_arg(ap, int);
+		data = va_arg(ap, uint8_t *);
 
-	if (descriptor_sense != 0) {
-		struct scsi_sense_data_desc *sense;
-
-		sense = (struct scsi_sense_data_desc *)sense_data;
-		/*
-		 * The descriptor sense format eliminates the use of the
-		 * valid bit.
-		 */
-		if (current_error != 0)
-			sense->error_code = SSD_DESC_CURRENT_ERROR;
-		else
-			sense->error_code = SSD_DESC_DEFERRED_ERROR;
-		sense->sense_key = sense_key;
-		sense->add_sense_code = asc;
-		sense->add_sense_code_qual = ascq;
-		/*
-		 * Start off with no extra length, since the above data
-		 * fits in the standard descriptor sense information.
-		 */
-		sense->extra_len = 0;
-		while ((elem_type = (scsi_sense_elem_type)va_arg(ap,
-			scsi_sense_elem_type)) != SSD_ELEM_NONE) {
-			int sense_len, len_to_copy;
-			uint8_t *data;
-
-			if (elem_type >= SSD_ELEM_MAX) {
-				printf("%s: invalid sense type %d\n", __func__,
-				       elem_type);
+		switch (elem_type) {
+		case SSD_ELEM_SKIP:
+			break;
+		case SSD_ELEM_DESC:
+			if (space < len) {
+				sense->flags |= SSDD_SDAT_OVFL;
 				break;
 			}
+			bcopy(data, desc, len);
+			desc += len;
+			space -= len;
+			break;
+		case SSD_ELEM_SKS: {
+			struct scsi_sense_sks *sks = (void *)desc;
 
-			sense_len = (int)va_arg(ap, int);
-			len_to_copy = MIN(sense_len, SSD_EXTRA_MAX -
-					  sense->extra_len);
-			data = (uint8_t *)va_arg(ap, uint8_t *);
-
-			/*
-			 * We've already consumed the arguments for this one.
-			 */
-			if (elem_type == SSD_ELEM_SKIP)
-				continue;
-
-			switch (elem_type) {
-			case SSD_ELEM_DESC: {
-
-				/*
-				 * This is a straight descriptor.  All we
-				 * need to do is copy the data in.
-				 */
-				bcopy(data, &sense->sense_desc[
-				      sense->extra_len], len_to_copy);
-				sense->extra_len += len_to_copy;
+			if (len > sizeof(sks->sense_key_spec))
+				break;
+			if (space < sizeof(*sks)) {
+				sense->flags |= SSDD_SDAT_OVFL;
 				break;
 			}
-			case SSD_ELEM_SKS: {
-				struct scsi_sense_sks sks;
-
-				bzero(&sks, sizeof(sks));
+			sks->desc_type = SSD_DESC_SKS;
+			sks->length = sizeof(*sks) -
+			    (offsetof(struct scsi_sense_sks, length) + 1);
+			bcopy(data, &sks->sense_key_spec, len);
+			desc += sizeof(*sks);
+			space -= sizeof(*sks);
+			break;
+		}
+		case SSD_ELEM_COMMAND: {
+			struct scsi_sense_command *cmd = (void *)desc;
 
-				/*
-				 * This is already-formatted sense key
-				 * specific data.  We just need to fill out
-				 * the header and copy everything in.
-				 */
-				bcopy(data, &sks.sense_key_spec,
-				      MIN(len_to_copy,
-				          sizeof(sks.sense_key_spec)));
-
-				sks.desc_type = SSD_DESC_SKS;
-				sks.length = sizeof(sks) -
-				    offsetof(struct scsi_sense_sks, reserved1);
-				bcopy(&sks,&sense->sense_desc[sense->extra_len],
-				      sizeof(sks));
-				sense->extra_len += sizeof(sks);
+			if (len > sizeof(cmd->command_info))
+				break;
+			if (space < sizeof(*cmd)) {
+				sense->flags |= SSDD_SDAT_OVFL;
 				break;
 			}
-			case SSD_ELEM_INFO:
-			case SSD_ELEM_COMMAND: {
-				struct scsi_sense_command cmd;
-				struct scsi_sense_info info;
-				uint8_t *data_dest;
-				uint8_t *descriptor;
-				int descriptor_size, i, copy_len;
-
-				bzero(&cmd, sizeof(cmd));
-				bzero(&info, sizeof(info));
-
-				/*
-				 * Command or information data.  The
-				 * operate in pretty much the same way.
-				 */
-				if (elem_type == SSD_ELEM_COMMAND) {
-					len_to_copy = MIN(len_to_copy,
-					    sizeof(cmd.command_info));
-					descriptor = (uint8_t *)&cmd;
-					descriptor_size  = sizeof(cmd);
-					data_dest =(uint8_t *)&cmd.command_info;
-					cmd.desc_type = SSD_DESC_COMMAND;
-					cmd.length = sizeof(cmd) -
-					    offsetof(struct scsi_sense_command,
-						     reserved);
-				} else {
-					len_to_copy = MIN(len_to_copy,
-					    sizeof(info.info));
-					descriptor = (uint8_t *)&info;
-					descriptor_size = sizeof(cmd);
-					data_dest = (uint8_t *)&info.info;
-					info.desc_type = SSD_DESC_INFO;
-					info.byte2 = SSD_INFO_VALID;
-					info.length = sizeof(info) -
-					    offsetof(struct scsi_sense_info,
-						     byte2);
-				}
-
-				/*
-				 * Copy this in reverse because the spec
-				 * (SPC-4) says that when 4 byte quantities
-				 * are stored in this 8 byte field, the
-				 * first four bytes shall be 0.
-				 *
-				 * So we fill the bytes in from the end, and
-				 * if we have less than 8 bytes to copy,
-				 * the initial, most significant bytes will
-				 * be 0.
-				 */
-				for (i = sense_len - 1; i >= 0 &&
-				     len_to_copy > 0; i--, len_to_copy--)
-					data_dest[len_to_copy - 1] = data[i];
+			cmd->desc_type = SSD_DESC_COMMAND;
+			cmd->length = sizeof(*cmd) -
+			    (offsetof(struct scsi_sense_command, length) + 1);
+			bcopy(data, &cmd->command_info[
+			    sizeof(cmd->command_info) - len], len);
+			desc += sizeof(*cmd);
+			space -= sizeof(*cmd);
+			break;
+		}
+		case SSD_ELEM_INFO: {
+			struct scsi_sense_info *info = (void *)desc;
 
-				/*
-				 * This calculation looks much like the
-				 * initial len_to_copy calculation, but
-				 * we have to do it again here, because
-				 * we're looking at a larger amount that
-				 * may or may not fit.  It's not only the
-				 * data the user passed in, but also the
-				 * rest of the descriptor.
-				 */
-				copy_len = MIN(descriptor_size,
-				    SSD_EXTRA_MAX - sense->extra_len);
-				bcopy(descriptor, &sense->sense_desc[
-				      sense->extra_len], copy_len);
-				sense->extra_len += copy_len;
+			if (len > sizeof(info->info))
 				break;
-			}
-			case SSD_ELEM_FRU: {
-				struct scsi_sense_fru fru;
-				int copy_len;
-
-				bzero(&fru, sizeof(fru));
-
-				fru.desc_type = SSD_DESC_FRU;
-				fru.length = sizeof(fru) -
-				    offsetof(struct scsi_sense_fru, reserved);
-				fru.fru = *data;
-
-				copy_len = MIN(sizeof(fru), SSD_EXTRA_MAX -
-					       sense->extra_len);
-				bcopy(&fru, &sense->sense_desc[
-				      sense->extra_len], copy_len);
-				sense->extra_len += copy_len;
+			if (space < sizeof(*info)) {
+				sense->flags |= SSDD_SDAT_OVFL;
 				break;
 			}
-			case SSD_ELEM_STREAM: {
-				struct scsi_sense_stream stream_sense;
-				int copy_len;
-
-				bzero(&stream_sense, sizeof(stream_sense));
-				stream_sense.desc_type = SSD_DESC_STREAM;
-				stream_sense.length = sizeof(stream_sense) -
-				   offsetof(struct scsi_sense_stream, reserved);
-				stream_sense.byte3 = *data;
-
-				copy_len = MIN(sizeof(stream_sense),
-				    SSD_EXTRA_MAX - sense->extra_len);
-				bcopy(&stream_sense, &sense->sense_desc[
-				      sense->extra_len], copy_len);
-				sense->extra_len += copy_len;
+			info->desc_type = SSD_DESC_INFO;
+			info->length = sizeof(*info) -
+			    (offsetof(struct scsi_sense_info, length) + 1);
+			info->byte2 = SSD_INFO_VALID;
+			bcopy(data, &info->info[sizeof(info->info) - len], len);
+			desc += sizeof(*info);
+			space -= sizeof(*info);
+			break;
+		}
+		case SSD_ELEM_FRU: {
+			struct scsi_sense_fru *fru = (void *)desc;
+
+			if (len > sizeof(fru->fru))
 				break;
-			}
-			default:
-				/*
-				 * We shouldn't get here, but if we do, do
-				 * nothing.  We've already consumed the
-				 * arguments above.
-				 */
+			if (space < sizeof(*fru)) {
+				sense->flags |= SSDD_SDAT_OVFL;
 				break;
 			}
+			fru->desc_type = SSD_DESC_FRU;
+			fru->length = sizeof(*fru) -
+			    (offsetof(struct scsi_sense_fru, length) + 1);
+			fru->fru = *data;
+			desc += sizeof(*fru);
+			space -= sizeof(*fru);
+			break;
 		}
-	} else {
-		struct scsi_sense_data_fixed *sense;
-
-		sense = (struct scsi_sense_data_fixed *)sense_data;
-
-		if (current_error != 0)
-			sense->error_code = SSD_CURRENT_ERROR;
-		else
-			sense->error_code = SSD_DEFERRED_ERROR;
-
-		sense->flags = sense_key;
-		sense->add_sense_code = asc;
-		sense->add_sense_code_qual = ascq;
-		/*
-		 * We've set the ASC and ASCQ, so we have 6 more bytes of
-		 * valid data.  If we wind up setting any of the other
-		 * fields, we'll bump this to 10 extra bytes.
-		 */
-		sense->extra_len = 6;
-
-		while ((elem_type = (scsi_sense_elem_type)va_arg(ap,
-			scsi_sense_elem_type)) != SSD_ELEM_NONE) {
-			int sense_len, len_to_copy;
-			uint8_t *data;
+		case SSD_ELEM_STREAM: {
+			struct scsi_sense_stream *stream = (void *)desc;
 
-			if (elem_type >= SSD_ELEM_MAX) {
-				printf("%s: invalid sense type %d\n", __func__,
-				       elem_type);
+			if (len > sizeof(stream->byte3))
+				break;
+			if (space < sizeof(*stream)) {
+				sense->flags |= SSDD_SDAT_OVFL;
 				break;
 			}
+			stream->desc_type = SSD_DESC_STREAM;
+			stream->length = sizeof(*stream) -
+			    (offsetof(struct scsi_sense_stream, length) + 1);
+			stream->byte3 = *data;
+			desc += sizeof(*stream);
+			space -= sizeof(*stream);
+			break;
+		}
+		default:
 			/*
-			 * If we get in here, just bump the extra length to
-			 * 10 bytes.  That will encompass anything we're
-			 * going to set here.
+			 * We shouldn't get here, but if we do, do nothing.
+			 * We've already consumed the arguments above.
 			 */
-			sense->extra_len = 10;
-			sense_len = (int)va_arg(ap, int);
-			data = (uint8_t *)va_arg(ap, uint8_t *);
+			break;
+		}
+	}
+	sense->extra_len = desc - &sense->sense_desc[0];
+	*sense_len = offsetof(struct scsi_sense_data_desc, extra_len) + 1 +
+	    sense->extra_len;
+}
 
-			switch (elem_type) {
-			case SSD_ELEM_SKS:
-				/*
-				 * The user passed in pre-formatted sense
-				 * key specific data.
-				 */
-				bcopy(data, &sense->sense_key_spec[0],
-				      MIN(sizeof(sense->sense_key_spec),
-				      sense_len));
-				break;
-			case SSD_ELEM_INFO:
-			case SSD_ELEM_COMMAND: {
-				uint8_t *data_dest;
-				int i;
-
-				if (elem_type == SSD_ELEM_COMMAND) {
-					data_dest = &sense->cmd_spec_info[0];
-					len_to_copy = MIN(sense_len,
-					    sizeof(sense->cmd_spec_info));
-				} else {
-					data_dest = &sense->info[0];
-					len_to_copy = MIN(sense_len,
-					    sizeof(sense->info));
-
-					/* Set VALID bit only if no overflow. */
-					for (i = 0; i < sense_len - len_to_copy;
-					    i++) {
-						if (data[i] != 0)
-							break;
-					}
-					if (i >= sense_len - len_to_copy) {
-						sense->error_code |=
-						    SSD_ERRCODE_VALID;
-					}
-				}
+/*
+ * Fill in SCSI fixed sense data with the specified parameters.
+ */
+static void
+scsi_set_sense_data_fixed_va(struct scsi_sense_data *sense_data,
+    u_int *sense_len, scsi_sense_data_type sense_format, int current_error,
+    int sense_key, int asc, int ascq, va_list ap)
+{
+	struct scsi_sense_data_fixed *sense;
+	scsi_sense_elem_type elem_type;
+	uint8_t *data;
+	int len;
 
-				/*
-			 	 * Copy this in reverse so that if we have
-				 * less than 4 bytes to fill, the least
-				 * significant bytes will be at the end.
-				 * If we have more than 4 bytes, only the
-				 * least significant bytes will be included.
-				 */
-				for (i = sense_len - 1; i >= 0 &&
-				     len_to_copy > 0; i--, len_to_copy--)
-					data_dest[len_to_copy - 1] = data[i];
+	memset(sense_data, 0, sizeof(*sense_data));
+	sense = (struct scsi_sense_data_fixed *)sense_data;
+	if (current_error != 0)
+		sense->error_code = SSD_CURRENT_ERROR;
+	else
+		sense->error_code = SSD_DEFERRED_ERROR;
+	sense->flags = sense_key & SSD_KEY;
+	sense->extra_len = 0;
+	if (*sense_len >= 13) {
+		sense->add_sense_code = asc;
+		sense->extra_len = MAX(sense->extra_len, 5);
+	} else
+		sense->flags |= SSD_SDAT_OVFL;
+	if (*sense_len >= 14) {
+		sense->add_sense_code_qual = ascq;
+		sense->extra_len = MAX(sense->extra_len, 6);
+	} else
+		sense->flags |= SSD_SDAT_OVFL;
 
+	while ((elem_type = va_arg(ap, scsi_sense_elem_type)) !=
+	    SSD_ELEM_NONE) {
+		if (elem_type >= SSD_ELEM_MAX) {
+			printf("%s: invalid sense type %d\n", __func__,
+			       elem_type);
+			break;
+		}
+		len = va_arg(ap, int);
+		data = va_arg(ap, uint8_t *);
+
+		switch (elem_type) {
+		case SSD_ELEM_SKIP:
+			break;
+		case SSD_ELEM_SKS:
+			if (len > sizeof(sense->sense_key_spec))
 				break;
-			}
-			case SSD_ELEM_FRU:
-				sense->fru = *data;
+			if (*sense_len < 18) {
+				sense->flags |= SSD_SDAT_OVFL;
 				break;
-			case SSD_ELEM_STREAM:
-				sense->flags |= *data;
+			}
+			bcopy(data, &sense->sense_key_spec[0], len);
+			sense->extra_len = MAX(sense->extra_len, 10);
+			break;
+		case SSD_ELEM_COMMAND:
+			if (*sense_len < 12) {
+				sense->flags |= SSD_SDAT_OVFL;
 				break;
-			case SSD_ELEM_DESC:
-			default:
-
-				/*
-				 * If the user passes in descriptor sense,
-				 * we can't handle that in fixed format.
-				 * So just skip it, and any unknown argument
-				 * types.
-				 */
+			}
+			if (len > sizeof(sense->cmd_spec_info)) {
+				data += len - sizeof(sense->cmd_spec_info);
+				len -= len - sizeof(sense->cmd_spec_info);
+			}
+			bcopy(data, &sense->cmd_spec_info[
+			    sizeof(sense->cmd_spec_info) - len], len);
+			sense->extra_len = MAX(sense->extra_len, 4);
+			break;
+		case SSD_ELEM_INFO:
+			/* Set VALID bit only if no overflow. */
+			sense->error_code |= SSD_ERRCODE_VALID;
+			while (len > sizeof(sense->info)) {
+				if (data[0] != 0)
+					sense->error_code &= ~SSD_ERRCODE_VALID;
+				data ++;
+				len --;
+			}
+			bcopy(data, &sense->info[sizeof(sense->info) - len], len);
+			break;
+		case SSD_ELEM_FRU:
+			if (*sense_len < 15) {
+				sense->flags |= SSD_SDAT_OVFL;
 				break;
 			}
+			sense->fru = *data;
+			sense->extra_len = MAX(sense->extra_len, 7);
+			break;
+		case SSD_ELEM_STREAM:
+			sense->flags |= *data &
+			    (SSD_ILI | SSD_EOM | SSD_FILEMARK);
+			break;
+		default:
+
+			/*
+			 * We can't handle that in fixed format.  Skip it.
+			 */
+			break;
 		}
 	}
+	*sense_len = offsetof(struct scsi_sense_data_fixed, extra_len) + 1 +
+	    sense->extra_len;
+}
+
+/*
+ * Fill in SCSI sense data with the specified parameters.  This routine can
+ * fill in either fixed or descriptor type sense data.
+ */
+void
+scsi_set_sense_data_va(struct scsi_sense_data *sense_data, u_int *sense_len,
+		      scsi_sense_data_type sense_format, int current_error,
+		      int sense_key, int asc, int ascq, va_list ap)
+{
+
+	if (*sense_len > SSD_FULL_SIZE)
+		*sense_len = SSD_FULL_SIZE;
+	if (sense_format == SSD_TYPE_DESC)
+		scsi_set_sense_data_desc_va(sense_data, sense_len,
+		    sense_format, current_error, sense_key, asc, ascq, ap);
+	else
+		scsi_set_sense_data_fixed_va(sense_data, sense_len,
+		    sense_format, current_error, sense_key, asc, ascq, ap);
+}
+
+void
+scsi_set_sense_data(struct scsi_sense_data *sense_data,
+		    scsi_sense_data_type sense_format, int current_error,
+		    int sense_key, int asc, int ascq, ...)
+{
+	va_list ap;
+	u_int	sense_len = SSD_FULL_SIZE;
+
+	va_start(ap, ascq);
+	scsi_set_sense_data_va(sense_data, &sense_len, sense_format,
+	    current_error, sense_key, asc, ascq, ap);
+	va_end(ap);
 }
 
 void
-scsi_set_sense_data(struct scsi_sense_data *sense_data, 
+scsi_set_sense_data_len(struct scsi_sense_data *sense_data, u_int *sense_len,
 		    scsi_sense_data_type sense_format, int current_error,
-		    int sense_key, int asc, int ascq, ...) 
+		    int sense_key, int asc, int ascq, ...)
 {
 	va_list ap;
 
 	va_start(ap, ascq);
-	scsi_set_sense_data_va(sense_data, sense_format, current_error,
-			       sense_key, asc, ascq, ap);
+	scsi_set_sense_data_va(sense_data, sense_len, sense_format,
+	    current_error, sense_key, asc, ascq, ap);
 	va_end(ap);
 }
 
diff --git a/sys/cam/scsi/scsi_all.h b/sys/cam/scsi/scsi_all.h
index a018fe9..c156536 100644
--- a/sys/cam/scsi/scsi_all.h
+++ b/sys/cam/scsi/scsi_all.h
@@ -3113,11 +3113,12 @@ struct scsi_sense_data_fixed
 #define		SSD_KEY_BLANK_CHECK	0x08
 #define		SSD_KEY_Vendor_Specific	0x09
 #define		SSD_KEY_COPY_ABORTED	0x0a
-#define		SSD_KEY_ABORTED_COMMAND	0x0b		
+#define		SSD_KEY_ABORTED_COMMAND	0x0b
 #define		SSD_KEY_EQUAL		0x0c
 #define		SSD_KEY_VOLUME_OVERFLOW	0x0d
 #define		SSD_KEY_MISCOMPARE	0x0e
-#define		SSD_KEY_COMPLETED	0x0f			
+#define		SSD_KEY_COMPLETED	0x0f
+#define	SSD_SDAT_OVFL	0x10
 #define	SSD_ILI		0x20
 #define	SSD_EOM		0x40
 #define	SSD_FILEMARK	0x80
@@ -3155,7 +3156,9 @@ struct scsi_sense_data_desc
 	uint8_t sense_key;
 	uint8_t	add_sense_code;
 	uint8_t	add_sense_code_qual;
-	uint8_t	reserved[3];
+	uint8_t	flags;
+#define	SSDD_SDAT_OVFL		0x80
+	uint8_t	reserved[2];
 	/*
 	 * Note that SPC-4, section 4.5.2.1 says that the extra_len field
 	 * must be less than or equal to 244.
@@ -3600,13 +3603,15 @@ void scsi_desc_iterate(struct scsi_sense_data_desc *sense, u_int sense_len,
 					void *), void *arg);
 uint8_t *scsi_find_desc(struct scsi_sense_data_desc *sense, u_int sense_len,
 			uint8_t desc_type);
-void scsi_set_sense_data(struct scsi_sense_data *sense_data, 
+void scsi_set_sense_data(struct scsi_sense_data *sense_data,
 			 scsi_sense_data_type sense_format, int current_error,
 			 int sense_key, int asc, int ascq, ...) ;
+void scsi_set_sense_data_len(struct scsi_sense_data *sense_data,
+    u_int *sense_len, scsi_sense_data_type sense_format, int current_error,
+    int sense_key, int asc, int ascq, ...) ;
 void scsi_set_sense_data_va(struct scsi_sense_data *sense_data,
-			    scsi_sense_data_type sense_format,
-			    int current_error, int sense_key, int asc,
-			    int ascq, va_list ap);
+    u_int *sense_len, scsi_sense_data_type sense_format,
+    int current_error, int sense_key, int asc, int ascq, va_list ap);
 int scsi_get_sense_info(struct scsi_sense_data *sense_data, u_int sense_len,
 			uint8_t info_type, uint64_t *info,
 			int64_t *signed_info);