[PATCH] scsi: scsi_debug: Implement support for Receive Diagnostics command

[Martin K. Petersen]

Signed-off-by: Martin K. Petersen <martin.petersen@xxxxxxxxxx>

diff --git a/drivers/scsi/scsi_debug.c b/drivers/scsi/scsi_debug.c
index 2740a90501a0..db8745a7000e 100644
--- a/drivers/scsi/scsi_debug.c
+++ b/drivers/scsi/scsi_debug.c
@@ -356,7 +356,8 @@ enum sdeb_opcode_index {
 	SDEB_I_WRITE_SAME = 26,		/* 10, 16 */
 	SDEB_I_SYNC_CACHE = 27,		/* 10, 16 */
 	SDEB_I_COMP_WRITE = 28,
-	SDEB_I_LAST_ELEMENT = 29,	/* keep this last (previous + 1) */
+	SDEB_I_RECV_DIAG = 29,
+	SDEB_I_LAST_ELEMENT = 30,	/* keep this last (previous + 1) */
 };
 
 
@@ -367,8 +368,8 @@ static const unsigned char opcode_ind_arr[256] = {
 	SDEB_I_READ, 0, SDEB_I_WRITE, 0, 0, 0, 0, 0,
 	0, 0, SDEB_I_INQUIRY, 0, 0, SDEB_I_MODE_SELECT, SDEB_I_RESERVE,
 	    SDEB_I_RELEASE,
-	0, 0, SDEB_I_MODE_SENSE, SDEB_I_START_STOP, 0, SDEB_I_SEND_DIAG,
-	    SDEB_I_ALLOW_REMOVAL, 0,
+	0, 0, SDEB_I_MODE_SENSE, SDEB_I_START_STOP, SDEB_I_RECV_DIAG,
+	SDEB_I_SEND_DIAG, SDEB_I_ALLOW_REMOVAL, 0,
 /* 0x20; 0x20->0x3f: 10 byte cdbs */
 	0, 0, 0, 0, 0, SDEB_I_READ_CAPACITY, 0, 0,
 	SDEB_I_READ, 0, SDEB_I_WRITE, 0, 0, 0, 0, SDEB_I_VERIFY,
@@ -433,6 +434,7 @@ static int resp_write_same_16(struct scsi_cmnd *, struct sdebug_dev_info *);
 static int resp_comp_write(struct scsi_cmnd *, struct sdebug_dev_info *);
 static int resp_write_buffer(struct scsi_cmnd *, struct sdebug_dev_info *);
 static int resp_sync_cache(struct scsi_cmnd *, struct sdebug_dev_info *);
+static int resp_recv_diag(struct scsi_cmnd *, struct sdebug_dev_info *);
 
 /*
  * The following are overflow arrays for cdbs that "hit" the same index in
@@ -613,8 +615,9 @@ static const struct opcode_info_t opcode_info_arr[SDEB_I_LAST_ELEMENT + 1] = {
 	{0, 0x89, 0, F_D_OUT | FF_MEDIA_IO, resp_comp_write, NULL,
 	    {16,  0xf8, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0, 0,
 	     0, 0xff, 0x3f, 0xc7} },		/* COMPARE AND WRITE */
-
-/* 29 */
+	{0, 0x1c, 0, FF_RESPOND | F_D_IN, resp_recv_diag, NULL, /* RECV DIAG */
+	    {6,  0x1, 0xff, 0xff, 0xff, 0xc7, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0} },
+/* 30 */
 	{0xff, 0, 0, 0, NULL, NULL,		/* terminating element */
 	    {0,  0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0} },
 };
@@ -1516,7 +1519,7 @@ static int resp_inquiry(struct scsi_cmnd *scp, struct sdebug_dev_info *devip)
 	arr[5] = (int)have_dif_prot;	/* PROTECT bit */
 	if (sdebug_vpd_use_hostno == 0)
 		arr[5] |= 0x10; /* claim: implicit TPGS */
-	arr[6] = 0x10; /* claim: MultiP */
+	arr[6] = 0x10 | 0x40; /* claim: MultiP */
 	/* arr[6] |= 0x40; ... claim: EncServ (enclosure services) */
 	arr[7] = 0xa; /* claim: LINKED + CMDQUE */
 	memcpy(&arr[8], sdebug_inq_vendor_id, 8);
@@ -3597,6 +3600,36 @@ static int resp_sync_cache(struct scsi_cmnd *scp,
 	return res;
 }
 
+static unsigned char diag0[] = {
+	0x00, 0x00, 0x00, 0x07, 0x00, 0x01, 0x02, 0x06, 0x07, 0x0a, 0xa0, 0x00,
+};
+#define DIAG0_LEN 12
+
+
+static int resp_recv_diag(struct scsi_cmnd *scp, struct sdebug_dev_info *devip)
+{
+	unsigned char *cmd = scp->cmnd;
+
+	switch(cmd[2]) {
+	case 0:
+		return fill_from_dev_buffer(scp, diag0, DIAG0_LEN);
+	case 1:
+		return fill_from_dev_buffer(scp, diag1, DIAG1_LEN);
+	case 2:
+		return fill_from_dev_buffer(scp, diag2, DIAG2_LEN);
+	case 6:
+		return fill_from_dev_buffer(scp, diag6, DIAG6_LEN);
+	case 7:
+		return fill_from_dev_buffer(scp, diag7, DIAG7_LEN);
+	case 0xa:
+		return fill_from_dev_buffer(scp, diaga, DIAGA_LEN);
+	case 0xa0:
+		return fill_from_dev_buffer(scp, diaga0, DIAGA0_LEN);
+	}
+
+	return DID_ERROR << 16;
+}
+
 #define RL_BUCKET_ELEMS 8
 
 /* Even though each pseudo target has a REPORT LUNS "well known logical unit"