scsi: add scsi_set_sense_field_pointer()

[cascardo/linux.git] / drivers / scsi / scsi_common.c

diff --git a/drivers/scsi/scsi_common.c b/drivers/scsi/scsi_common.c
index c126966..b1383a7 100644 (file)
--- a/drivers/scsi/scsi_common.c
+++ b/drivers/scsi/scsi_common.c
@@ -278,10 +278,71 @@ int scsi_set_sense_information(u8 *buf, int buf_len, u64 info)
                ucp[3] = 0;
                put_unaligned_be64(info, &ucp[4]);
        } else if ((buf[0] & 0x7f) == 0x70) {
-               buf[0] |= 0x80;
-               put_unaligned_be64(info, &buf[3]);
+               /*
+                * Only set the 'VALID' bit if we can represent the value
+                * correctly; otherwise just fill out the lower bytes and
+                * clear the 'VALID' flag.
+                */
+               if (info <= 0xffffffffUL)
+                       buf[0] |= 0x80;
+               else
+                       buf[0] &= 0x7f;
+               put_unaligned_be32((u32)info, &buf[3]);
        }
 
        return 0;
 }
 EXPORT_SYMBOL(scsi_set_sense_information);
+
+/**
+ * scsi_set_sense_field_pointer - set the field pointer sense key
+ *             specific information in a formatted sense data buffer
+ * @buf:       Where to build sense data
+ * @buf_len:    buffer length
+ * @fp:                field pointer to be set
+ * @bp:                bit pointer to be set
+ * @cd:                command/data bit
+ *
+ * Return value:
+ *     0 on success or EINVAL for invalid sense buffer length
+ */
+int scsi_set_sense_field_pointer(u8 *buf, int buf_len, u16 fp, u8 bp, bool cd)
+{
+       u8 *ucp, len;
+
+       if ((buf[0] & 0x7f) == 0x72) {
+               len = buf[7];
+               ucp = (char *)scsi_sense_desc_find(buf, len + 8, 2);
+               if (!ucp) {
+                       buf[7] = len + 8;
+                       ucp = buf + 8 + len;
+               }
+
+               if (buf_len < len + 8)
+                       /* Not enough room for info */
+                       return -EINVAL;
+
+               ucp[0] = 2;
+               ucp[1] = 6;
+               ucp[4] = 0x80; /* Valid bit */
+               if (cd)
+                       ucp[4] |= 0x40;
+               if (bp < 0x8)
+                       ucp[4] |= 0x8 | bp;
+               put_unaligned_be16(fp, &ucp[5]);
+       } else if ((buf[0] & 0x7f) == 0x70) {
+               len = buf[7];
+               if (len < 18)
+                       buf[7] = 18;
+
+               buf[15] = 0x80;
+               if (cd)
+                       buf[15] |= 0x40;
+               if (bp < 0x8)
+                       buf[15] |= 0x8 | bp;
+               put_unaligned_be16(fp, &buf[16]);
+       }
+
+       return 0;
+}
+EXPORT_SYMBOL(scsi_set_sense_field_pointer);