1 files changed, 118 insertions, 56 deletions

diff --git a/drivers/scsi/scsi_debug.c b/drivers/scsi/scsi_debug.c
index 82c06f0a9d02..1541c174937a 100644
--- a/drivers/scsi/scsi_debug.c
+++ b/drivers/scsi/scsi_debug.c
@@ -280,6 +280,8 @@ static int resp_write(struct scsi_cmnd * SCpnt, unsigned long long lba,
                       unsigned int num, struct sdebug_dev_info * devip);
 static int resp_report_luns(struct scsi_cmnd * SCpnt,
                             struct sdebug_dev_info * devip);
+static int resp_xdwriteread(struct scsi_cmnd *scp, unsigned long long lba,
+                            unsigned int num, struct sdebug_dev_info *devip);
 static int fill_from_dev_buffer(struct scsi_cmnd * scp, unsigned char * arr,
                                 int arr_len);
 static int fetch_to_dev_buffer(struct scsi_cmnd * scp, unsigned char * arr,
@@ -311,12 +313,48 @@ static void sdebug_max_tgts_luns(void);
 static struct device pseudo_primary;
 static struct bus_type pseudo_lld_bus;
 
+static void get_data_transfer_info(unsigned char *cmd,
+                                   unsigned long long *lba, unsigned int *num)
+{
+        int i;
+
+        switch (*cmd) {
+        case WRITE_16:
+        case READ_16:
+                for (*lba = 0, i = 0; i < 8; ++i) {
+                        if (i > 0)
+                                *lba <<= 8;
+                        *lba += cmd[2 + i];
+                }
+                *num = cmd[13] + (cmd[12] << 8) +
+                        (cmd[11] << 16) + (cmd[10] << 24);
+                break;
+        case WRITE_12:
+        case READ_12:
+                *lba = cmd[5] + (cmd[4] << 8) + (cmd[3] << 16) + (cmd[2] << 24);
+                *num = cmd[9] + (cmd[8] << 8) + (cmd[7] << 16) + (cmd[6] << 24);
+                break;
+        case WRITE_10:
+        case READ_10:
+        case XDWRITEREAD_10:
+                *lba = cmd[5] + (cmd[4] << 8) + (cmd[3] << 16) + (cmd[2] << 24);
+                *num = cmd[8] + (cmd[7] << 8);
+                break;
+        case WRITE_6:
+        case READ_6:
+                *lba = cmd[3] + (cmd[2] << 8) + ((cmd[1] & 0x1f) << 16);
+                *num = (0 == cmd[4]) ? 256 : cmd[4];
+                break;
+        default:
+                break;
+        }
+}
 
 static
 int scsi_debug_queuecommand(struct scsi_cmnd * SCpnt, done_funct_t done)
 {
         unsigned char *cmd = (unsigned char *) SCpnt->cmnd;
-        int len, k, j;
+        int len, k;
         unsigned int num;
         unsigned long long lba;
         int errsts = 0;
@@ -452,28 +490,7 @@ int scsi_debug_queuecommand(struct scsi_cmnd * SCpnt, done_funct_t done)
                         break;
                 if (scsi_debug_fake_rw)
                         break;
-                if ((*cmd) == READ_16) {
-                        for (lba = 0, j = 0; j < 8; ++j) {
-                                if (j > 0)
-                                        lba <<= 8;
-                                lba += cmd[2 + j];
-                        }
-                        num = cmd[13] + (cmd[12] << 8) +
-                                (cmd[11] << 16) + (cmd[10] << 24);
-                } else if ((*cmd) == READ_12) {
-                        lba = cmd[5] + (cmd[4] << 8) +
-                                (cmd[3] << 16) + (cmd[2] << 24);
-                        num = cmd[9] + (cmd[8] << 8) +
-                                (cmd[7] << 16) + (cmd[6] << 24);
-                } else if ((*cmd) == READ_10) {
-                        lba = cmd[5] + (cmd[4] << 8) +
-                                (cmd[3] << 16) + (cmd[2] << 24);
-                        num = cmd[8] + (cmd[7] << 8);
-                } else {        /* READ (6) */
-                        lba = cmd[3] + (cmd[2] << 8) +
-                                ((cmd[1] & 0x1f) << 16);
-                        num = (0 == cmd[4]) ? 256 : cmd[4];
-                }
+                get_data_transfer_info(cmd, &lba, &num);
                 errsts = resp_read(SCpnt, lba, num, devip);
                 if (inj_recovered && (0 == errsts)) {
                         mk_sense_buffer(devip, RECOVERED_ERROR,
@@ -500,28 +517,7 @@ int scsi_debug_queuecommand(struct scsi_cmnd * SCpnt, done_funct_t done)
                         break;
                 if (scsi_debug_fake_rw)
                         break;
-                if ((*cmd) == WRITE_16) {
-                        for (lba = 0, j = 0; j < 8; ++j) {
-                                if (j > 0)
-                                        lba <<= 8;
-                                lba += cmd[2 + j];
-                        }
-                        num = cmd[13] + (cmd[12] << 8) +
-                                (cmd[11] << 16) + (cmd[10] << 24);
-                } else if ((*cmd) == WRITE_12) {
-                        lba = cmd[5] + (cmd[4] << 8) +
-                                (cmd[3] << 16) + (cmd[2] << 24);
-                        num = cmd[9] + (cmd[8] << 8) +
-                                (cmd[7] << 16) + (cmd[6] << 24);
-                } else if ((*cmd) == WRITE_10) {
-                        lba = cmd[5] + (cmd[4] << 8) +
-                                (cmd[3] << 16) + (cmd[2] << 24);
-                        num = cmd[8] + (cmd[7] << 8);
-                } else {        /* WRITE (6) */
-                        lba = cmd[3] + (cmd[2] << 8) +
-                                ((cmd[1] & 0x1f) << 16);
-                        num = (0 == cmd[4]) ? 256 : cmd[4];
-                }
+                get_data_transfer_info(cmd, &lba, &num);
                 errsts = resp_write(SCpnt, lba, num, devip);
                 if (inj_recovered && (0 == errsts)) {
                         mk_sense_buffer(devip, RECOVERED_ERROR,
@@ -549,6 +545,28 @@ int scsi_debug_queuecommand(struct scsi_cmnd * SCpnt, done_funct_t done)
         case WRITE_BUFFER:
                 errsts = check_readiness(SCpnt, 1, devip);
                 break;
+        case XDWRITEREAD_10:
+                if (!scsi_bidi_cmnd(SCpnt)) {
+                        mk_sense_buffer(devip, ILLEGAL_REQUEST,
+                                        INVALID_FIELD_IN_CDB, 0);
+                        errsts = check_condition_result;
+                        break;
+                }
+
+                errsts = check_readiness(SCpnt, 0, devip);
+                if (errsts)
+                        break;
+                if (scsi_debug_fake_rw)
+                        break;
+                get_data_transfer_info(cmd, &lba, &num);
+                errsts = resp_read(SCpnt, lba, num, devip);
+                if (errsts)
+                        break;
+                errsts = resp_write(SCpnt, lba, num, devip);
+                if (errsts)
+                        break;
+                errsts = resp_xdwriteread(SCpnt, lba, num, devip);
+                break;
         default:
                 if (SCSI_DEBUG_OPT_NOISE & scsi_debug_opts)
                         printk(KERN_INFO "scsi_debug: Opcode: 0x%x not "
@@ -601,18 +619,18 @@ static int fill_from_dev_buffer(struct scsi_cmnd * scp, unsigned char * arr,
         int k, req_len, act_len, len, active;
         void * kaddr;
         void * kaddr_off;
-        struct scatterlist * sg;
+        struct scatterlist *sg;
+        struct scsi_data_buffer *sdb = scsi_in(scp);
 
-        if (0 == scsi_bufflen(scp))
+        if (!sdb->length)
                 return 0;
-        if (NULL == scsi_sglist(scp))
+        if (!sdb->table.sgl)
                 return (DID_ERROR << 16);
-        if (! ((scp->sc_data_direction == DMA_BIDIRECTIONAL) ||
-              (scp->sc_data_direction == DMA_FROM_DEVICE)))
+        if (!(scsi_bidi_cmnd(scp) || scp->sc_data_direction == DMA_FROM_DEVICE))
                 return (DID_ERROR << 16);
         active = 1;
         req_len = act_len = 0;
-        scsi_for_each_sg(scp, sg, scsi_sg_count(scp), k) {
+        for_each_sg(sdb->table.sgl, sg, sdb->table.nents, k) {
                 if (active) {
                         kaddr = (unsigned char *)
                                 kmap_atomic(sg_page(sg), KM_USER0);
@@ -630,10 +648,10 @@ static int fill_from_dev_buffer(struct scsi_cmnd * scp, unsigned char * arr,
                 }
                 req_len += sg->length;
         }
-        if (scsi_get_resid(scp))
-                scsi_set_resid(scp, scsi_get_resid(scp) - act_len);
+        if (sdb->resid)
+                sdb->resid -= act_len;
         else
-                scsi_set_resid(scp, req_len - act_len);
+                sdb->resid = req_len - act_len;
         return 0;
 }
 
@@ -650,8 +668,7 @@ static int fetch_to_dev_buffer(struct scsi_cmnd * scp, unsigned char * arr,
                 return 0;
         if (NULL == scsi_sglist(scp))
                 return -1;
-        if (! ((scp->sc_data_direction == DMA_BIDIRECTIONAL) ||
-              (scp->sc_data_direction == DMA_TO_DEVICE)))
+        if (!(scsi_bidi_cmnd(scp) || scp->sc_data_direction == DMA_TO_DEVICE))
                 return -1;
         req_len = fin = 0;
         scsi_for_each_sg(scp, sg, scsi_sg_count(scp), k) {
@@ -1956,6 +1973,50 @@ static int resp_report_luns(struct scsi_cmnd * scp,
                                     min((int)alloc_len, SDEBUG_RLUN_ARR_SZ));
 }
 
+static int resp_xdwriteread(struct scsi_cmnd *scp, unsigned long long lba,
+                            unsigned int num, struct sdebug_dev_info *devip)
+{
+        int i, j, ret = -1;
+        unsigned char *kaddr, *buf;
+        unsigned int offset;
+        struct scatterlist *sg;
+        struct scsi_data_buffer *sdb = scsi_in(scp);
+
+        /* better not to use temporary buffer. */
+        buf = kmalloc(scsi_bufflen(scp), GFP_ATOMIC);
+        if (!buf)
+                return ret;
+
+        offset = 0;
+        scsi_for_each_sg(scp, sg, scsi_sg_count(scp), i) {
+                kaddr = (unsigned char *)kmap_atomic(sg_page(sg), KM_USER0);
+                if (!kaddr)
+                        goto out;
+
+                memcpy(buf + offset, kaddr + sg->offset, sg->length);
+                offset += sg->length;
+                kunmap_atomic(kaddr, KM_USER0);
+        }
+
+        offset = 0;
+        for_each_sg(sdb->table.sgl, sg, sdb->table.nents, i) {
+                kaddr = (unsigned char *)kmap_atomic(sg_page(sg), KM_USER0);
+                if (!kaddr)
+                        goto out;
+
+                for (j = 0; j < sg->length; j++)
+                        *(kaddr + sg->offset + j) ^= *(buf + offset + j);
+
+                offset += sg->length;
+                kunmap_atomic(kaddr, KM_USER0);
+        }
+        ret = 0;
+out:
+        kfree(buf);
+
+        return ret;
+}
+
 /* When timer goes off this function is called. */
 static void timer_intr_handler(unsigned long indx)
 {
@@ -1989,6 +2050,7 @@ static int scsi_debug_slave_alloc(struct scsi_device * sdp)
         if (SCSI_DEBUG_OPT_NOISE & scsi_debug_opts)
                 printk(KERN_INFO "scsi_debug: slave_alloc <%u %u %u %u>\n",
                        sdp->host->host_no, sdp->channel, sdp->id, sdp->lun);
+        set_bit(QUEUE_FLAG_BIDI, &sdp->request_queue->queue_flags);
         return 0;
 }