scsi: add scsi_set_sense_field_pointer()

Add a function to set the field pointer for SCSI sense codes.

Signed-off-by: Hannes Reinecke <hare@suse.com>
Signed-off-by: Tejun Heo <tj@kernel.org>

2 files changed, 54 insertions, 0 deletions

diff --git a/drivers/scsi/scsi_common.c b/drivers/scsi/scsi_common.c
index ce79de822e46..b1383a71400e 100644
--- a/drivers/scsi/scsi_common.c
+++ b/drivers/scsi/scsi_common.c
@@ -293,3 +293,56 @@ int scsi_set_sense_information(u8 *buf, int buf_len, u64 info)
         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);
diff --git a/include/scsi/scsi_common.h b/include/scsi/scsi_common.h
index 11571b2a831e..20bf7eaef05a 100644
--- a/include/scsi/scsi_common.h
+++ b/include/scsi/scsi_common.h
@@ -63,6 +63,7 @@ extern bool scsi_normalize_sense(const u8 *sense_buffer, int sb_len,
 
 extern void scsi_build_sense_buffer(int desc, u8 *buf, u8 key, u8 asc, u8 ascq);
 int scsi_set_sense_information(u8 *buf, int buf_len, u64 info);
+int scsi_set_sense_field_pointer(u8 *buf, int buf_len, u16 fp, u8 bp, bool cd);
 extern const u8 * scsi_sense_desc_find(const u8 * sense_buffer, int sb_len,
                                        int desc_type);