1 files changed, 150 insertions, 123 deletions

diff --git a/drivers/block/ub.c b/drivers/block/ub.c
index a026567f5d18..aa0bf7ee008d 100644
--- a/drivers/block/ub.c
+++ b/drivers/block/ub.c
@@ -16,9 +16,10 @@
  *  -- verify the 13 conditions and do bulk resets
  *  -- kill last_pipe and simply do two-state clearing on both pipes
  *  -- verify protocol (bulk) from USB descriptors (maybe...)
- *  -- highmem and sg
+ *  -- highmem
  *  -- move top_sense and work_bcs into separate allocations (if they survive)
  *     for cache purists and esoteric architectures.
+ *  -- Allocate structure for LUN 0 before the first ub_sync_tur, avoid NULL. ?
  *  -- prune comments, they are too volumnous
  *  -- Exterminate P3 printks
  *  -- Resove XXX's
@@ -171,7 +172,7 @@ struct bulk_cs_wrap {
  */
 struct ub_dev;
 
-#define UB_MAX_REQ_SG   1
+#define UB_MAX_REQ_SG   4
 #define UB_MAX_SECTORS 64
 
 /*
@@ -234,13 +235,10 @@ struct ub_scsi_cmd {
 
         int stat_count;                 /* Retries getting status. */
 
-        /*
-         * We do not support transfers from highmem pages
-         * because the underlying USB framework does not do what we need.
-         */
-        char *data;                     /* Requested buffer */
         unsigned int len;               /* Requested length */
-        // struct scatterlist sgv[UB_MAX_REQ_SG];
+        unsigned int current_sg;
+        unsigned int nsg;               /* sgv[nsg] */
+        struct scatterlist sgv[UB_MAX_REQ_SG];
 
         struct ub_lun *lun;
         void (*done)(struct ub_dev *, struct ub_scsi_cmd *);
@@ -389,17 +387,18 @@ struct ub_dev {
         struct bulk_cs_wrap work_bcs;
         struct usb_ctrlrequest work_cr;
 
+        int sg_stat[UB_MAX_REQ_SG+1];
         struct ub_scsi_trace tr;
 };
 
 /*
  */
 static void ub_cleanup(struct ub_dev *sc);
-static int ub_bd_rq_fn_1(struct ub_lun *lun, struct request *rq);
+static int ub_request_fn_1(struct ub_lun *lun, struct request *rq);
 static int ub_cmd_build_block(struct ub_dev *sc, struct ub_lun *lun,
     struct ub_scsi_cmd *cmd, struct request *rq);
-static int ub_cmd_build_packet(struct ub_dev *sc, struct ub_scsi_cmd *cmd,
-    struct request *rq);
+static int ub_cmd_build_packet(struct ub_dev *sc, struct ub_lun *lun,
+    struct ub_scsi_cmd *cmd, struct request *rq);
 static void ub_rw_cmd_done(struct ub_dev *sc, struct ub_scsi_cmd *cmd);
 static void ub_end_rq(struct request *rq, int uptodate);
 static int ub_submit_scsi(struct ub_dev *sc, struct ub_scsi_cmd *cmd);
@@ -407,6 +406,7 @@ static void ub_urb_complete(struct urb *urb, struct pt_regs *pt);
 static void ub_scsi_action(unsigned long _dev);
 static void ub_scsi_dispatch(struct ub_dev *sc);
 static void ub_scsi_urb_compl(struct ub_dev *sc, struct ub_scsi_cmd *cmd);
+static void ub_data_start(struct ub_dev *sc, struct ub_scsi_cmd *cmd);
 static void ub_state_done(struct ub_dev *sc, struct ub_scsi_cmd *cmd, int rc);
 static int __ub_state_stat(struct ub_dev *sc, struct ub_scsi_cmd *cmd);
 static void ub_state_stat(struct ub_dev *sc, struct ub_scsi_cmd *cmd);
@@ -500,7 +500,8 @@ static void ub_cmdtr_sense(struct ub_dev *sc, struct ub_scsi_cmd *cmd,
         }
 }
 
-static ssize_t ub_diag_show(struct device *dev, struct device_attribute *attr, char *page)
+static ssize_t ub_diag_show(struct device *dev, struct device_attribute *attr,
+    char *page)
 {
         struct usb_interface *intf;
         struct ub_dev *sc;
@@ -523,6 +524,13 @@ static ssize_t ub_diag_show(struct device *dev, struct device_attribute *attr, c
         cnt += sprintf(page + cnt,
             "qlen %d qmax %d\n",
             sc->cmd_queue.qlen, sc->cmd_queue.qmax);
+        cnt += sprintf(page + cnt,
+            "sg %d %d %d %d %d\n",
+            sc->sg_stat[0],
+            sc->sg_stat[1],
+            sc->sg_stat[2],
+            sc->sg_stat[3],
+            sc->sg_stat[4]);
 
         list_for_each (p, &sc->luns) {
                 lun = list_entry(p, struct ub_lun, link);
@@ -744,20 +752,20 @@ static struct ub_scsi_cmd *ub_cmdq_pop(struct ub_dev *sc)
  * The request function is our main entry point
  */
 
-static void ub_bd_rq_fn(request_queue_t *q)
+static void ub_request_fn(request_queue_t *q)
 {
         struct ub_lun *lun = q->queuedata;
         struct request *rq;
 
         while ((rq = elv_next_request(q)) != NULL) {
-                if (ub_bd_rq_fn_1(lun, rq) != 0) {
+                if (ub_request_fn_1(lun, rq) != 0) {
                         blk_stop_queue(q);
                         break;
                 }
         }
 }
 
-static int ub_bd_rq_fn_1(struct ub_lun *lun, struct request *rq)
+static int ub_request_fn_1(struct ub_lun *lun, struct request *rq)
 {
         struct ub_dev *sc = lun->udev;
         struct ub_scsi_cmd *cmd;
@@ -774,9 +782,8 @@ static int ub_bd_rq_fn_1(struct ub_lun *lun, struct request *rq)
         memset(cmd, 0, sizeof(struct ub_scsi_cmd));
 
         blkdev_dequeue_request(rq);
-
         if (blk_pc_request(rq)) {
-                rc = ub_cmd_build_packet(sc, cmd, rq);
+                rc = ub_cmd_build_packet(sc, lun, cmd, rq);
         } else {
                 rc = ub_cmd_build_block(sc, lun, cmd, rq);
         }
@@ -791,7 +798,7 @@ static int ub_bd_rq_fn_1(struct ub_lun *lun, struct request *rq)
         cmd->back = rq;
 
         cmd->tag = sc->tagcnt++;
-        if ((rc = ub_submit_scsi(sc, cmd)) != 0) {
+        if (ub_submit_scsi(sc, cmd) != 0) {
                 ub_put_cmd(lun, cmd);
                 ub_end_rq(rq, 0);
                 return 0;
@@ -804,58 +811,31 @@ static int ub_cmd_build_block(struct ub_dev *sc, struct ub_lun *lun,
     struct ub_scsi_cmd *cmd, struct request *rq)
 {
         int ub_dir;
-#if 0 /* We use rq->buffer for now */
-        struct scatterlist *sg;
         int n_elem;
-#endif
         unsigned int block, nblks;
 
         if (rq_data_dir(rq) == WRITE)
                 ub_dir = UB_DIR_WRITE;
         else
                 ub_dir = UB_DIR_READ;
+        cmd->dir = ub_dir;
 
         /*
          * get scatterlist from block layer
          */
-#if 0 /* We use rq->buffer for now */
-        sg = &cmd->sgv[0];
-        n_elem = blk_rq_map_sg(q, rq, sg);
+        n_elem = blk_rq_map_sg(lun->disk->queue, rq, &cmd->sgv[0]);
         if (n_elem <= 0) {
-                ub_put_cmd(lun, cmd);
-                ub_end_rq(rq, 0);
-                blk_start_queue(q);
-                return 0;               /* request with no s/g entries? */
+                printk(KERN_INFO "%s: failed request map (%d)\n",
+                    sc->name, n_elem); /* P3 */
+                return -1;              /* request with no s/g entries? */
         }
-
-        if (n_elem != 1) {              /* Paranoia */
+        if (n_elem > UB_MAX_REQ_SG) {   /* Paranoia */
                 printk(KERN_WARNING "%s: request with %d segments\n",
                     sc->name, n_elem);
-                ub_put_cmd(lun, cmd);
-                ub_end_rq(rq, 0);
-                blk_start_queue(q);
-                return 0;
-        }
-#endif
-
-        /*
-         * XXX Unfortunately, this check does not work. It is quite possible
-         * to get bogus non-null rq->buffer if you allow sg by mistake.
-         */
-        if (rq->buffer == NULL) {
-                /*
-                 * This must not happen if we set the queue right.
-                 * The block level must create bounce buffers for us.
-                 */
-                static int do_print = 1;
-                if (do_print) {
-                        printk(KERN_WARNING "%s: unmapped block request"
-                            " flags 0x%lx sectors %lu\n",
-                            sc->name, rq->flags, rq->nr_sectors);
-                        do_print = 0;
-                }
                 return -1;
         }
+        cmd->nsg = n_elem;
+        sc->sg_stat[n_elem]++;
 
         /*
          * build the command
@@ -876,30 +856,15 @@ static int ub_cmd_build_block(struct ub_dev *sc, struct ub_lun *lun,
         cmd->cdb[8] = nblks;
         cmd->cdb_len = 10;
 
-        cmd->dir = ub_dir;
-        cmd->data = rq->buffer;
         cmd->len = rq->nr_sectors * 512;
 
         return 0;
 }
 
-static int ub_cmd_build_packet(struct ub_dev *sc, struct ub_scsi_cmd *cmd,
-    struct request *rq)
+static int ub_cmd_build_packet(struct ub_dev *sc, struct ub_lun *lun,
+    struct ub_scsi_cmd *cmd, struct request *rq)
 {
-
-        if (rq->data_len != 0 && rq->data == NULL) {
-                static int do_print = 1;
-                if (do_print) {
-                        printk(KERN_WARNING "%s: unmapped packet request"
-                            " flags 0x%lx length %d\n",
-                            sc->name, rq->flags, rq->data_len);
-                        do_print = 0;
-                }
-                return -1;
-        }
-
-        memcpy(&cmd->cdb, rq->cmd, rq->cmd_len);
-        cmd->cdb_len = rq->cmd_len;
+        int n_elem;
 
         if (rq->data_len == 0) {
                 cmd->dir = UB_DIR_NONE;
@@ -908,8 +873,29 @@ static int ub_cmd_build_packet(struct ub_dev *sc, struct ub_scsi_cmd *cmd,
                         cmd->dir = UB_DIR_WRITE;
                 else
                         cmd->dir = UB_DIR_READ;
+
+        }
+
+        /*
+         * get scatterlist from block layer
+         */
+        n_elem = blk_rq_map_sg(lun->disk->queue, rq, &cmd->sgv[0]);
+        if (n_elem < 0) {
+                printk(KERN_INFO "%s: failed request map (%d)\n",
+                    sc->name, n_elem); /* P3 */
+                return -1;
         }
-        cmd->data = rq->data;
+        if (n_elem > UB_MAX_REQ_SG) {   /* Paranoia */
+                printk(KERN_WARNING "%s: request with %d segments\n",
+                    sc->name, n_elem);
+                return -1;
+        }
+        cmd->nsg = n_elem;
+        sc->sg_stat[n_elem]++;
+
+        memcpy(&cmd->cdb, rq->cmd, rq->cmd_len);
+        cmd->cdb_len = rq->cmd_len;
+
         cmd->len = rq->data_len;
 
         return 0;
@@ -919,24 +905,34 @@ static void ub_rw_cmd_done(struct ub_dev *sc, struct ub_scsi_cmd *cmd)
 {
         struct request *rq = cmd->back;
         struct ub_lun *lun = cmd->lun;
-        struct gendisk *disk = lun->disk;
-        request_queue_t *q = disk->queue;
         int uptodate;
 
-        if (blk_pc_request(rq)) {
-                /* UB_SENSE_SIZE is smaller than SCSI_SENSE_BUFFERSIZE */
-                memcpy(rq->sense, sc->top_sense, UB_SENSE_SIZE);
-                rq->sense_len = UB_SENSE_SIZE;
-        }
-
-        if (cmd->error == 0)
+        if (cmd->error == 0) {
                 uptodate = 1;
-        else
+
+                if (blk_pc_request(rq)) {
+                        if (cmd->act_len >= rq->data_len)
+                                rq->data_len = 0;
+                        else
+                                rq->data_len -= cmd->act_len;
+                }
+        } else {
                 uptodate = 0;
 
+                if (blk_pc_request(rq)) {
+                        /* UB_SENSE_SIZE is smaller than SCSI_SENSE_BUFFERSIZE */
+                        memcpy(rq->sense, sc->top_sense, UB_SENSE_SIZE);
+                        rq->sense_len = UB_SENSE_SIZE;
+                        if (sc->top_sense[0] != 0)
+                                rq->errors = SAM_STAT_CHECK_CONDITION;
+                        else
+                                rq->errors = DID_ERROR << 16;
+                }
+        }
+
         ub_put_cmd(lun, cmd);
         ub_end_rq(rq, uptodate);
-        blk_start_queue(q);
+        blk_start_queue(lun->disk->queue);
 }
 
 static void ub_end_rq(struct request *rq, int uptodate)
@@ -1014,7 +1010,7 @@ static int ub_scsi_cmd_start(struct ub_dev *sc, struct ub_scsi_cmd *cmd)
         sc->last_pipe = sc->send_bulk_pipe;
         usb_fill_bulk_urb(&sc->work_urb, sc->dev, sc->send_bulk_pipe,
             bcb, US_BULK_CB_WRAP_LEN, ub_urb_complete, sc);
-        sc->work_urb.transfer_flags = URB_ASYNC_UNLINK;
+        sc->work_urb.transfer_flags = 0;
 
         /* Fill what we shouldn't be filling, because usb-storage did so. */
         sc->work_urb.actual_length = 0;
@@ -1103,7 +1099,6 @@ static void ub_scsi_urb_compl(struct ub_dev *sc, struct ub_scsi_cmd *cmd)
 {
         struct urb *urb = &sc->work_urb;
         struct bulk_cs_wrap *bcs;
-        int pipe;
         int rc;
 
         if (atomic_read(&sc->poison)) {
@@ -1204,38 +1199,13 @@ static void ub_scsi_urb_compl(struct ub_dev *sc, struct ub_scsi_cmd *cmd)
                         goto Bad_End;
                 }
 
-                if (cmd->dir == UB_DIR_NONE) {
+                if (cmd->dir == UB_DIR_NONE || cmd->nsg < 1) {
                         ub_state_stat(sc, cmd);
                         return;
                 }
 
-                UB_INIT_COMPLETION(sc->work_done);
-
-                if (cmd->dir == UB_DIR_READ)
-                        pipe = sc->recv_bulk_pipe;
-                else
-                        pipe = sc->send_bulk_pipe;
-                sc->last_pipe = pipe;
-                usb_fill_bulk_urb(&sc->work_urb, sc->dev, pipe,
-                    cmd->data, cmd->len, ub_urb_complete, sc);
-                sc->work_urb.transfer_flags = URB_ASYNC_UNLINK;
-                sc->work_urb.actual_length = 0;
-                sc->work_urb.error_count = 0;
-                sc->work_urb.status = 0;
-
-                if ((rc = usb_submit_urb(&sc->work_urb, GFP_ATOMIC)) != 0) {
-                        /* XXX Clear stalls */
-                        printk("ub: data #%d submit failed (%d)\n", cmd->tag, rc); /* P3 */
-                        ub_complete(&sc->work_done);
-                        ub_state_done(sc, cmd, rc);
-                        return;
-                }
-
-                sc->work_timer.expires = jiffies + UB_DATA_TIMEOUT;
-                add_timer(&sc->work_timer);
-
-                cmd->state = UB_CMDST_DATA;
-                ub_cmdtr_state(sc, cmd);
+                // udelay(125);         // usb-storage has this
+                ub_data_start(sc, cmd);
 
         } else if (cmd->state == UB_CMDST_DATA) {
                 if (urb->status == -EPIPE) {
@@ -1257,16 +1227,22 @@ static void ub_scsi_urb_compl(struct ub_dev *sc, struct ub_scsi_cmd *cmd)
                 if (urb->status == -EOVERFLOW) {
                         /*
                          * A babble? Failure, but we must transfer CSW now.
+                         * XXX This is going to end in perpetual babble. Reset.
                          */
                         cmd->error = -EOVERFLOW;        /* A cheap trick... */
-                } else {
-                        if (urb->status != 0)
-                                goto Bad_End;
+                        ub_state_stat(sc, cmd);
+                        return;
                 }
+                if (urb->status != 0)
+                        goto Bad_End;
 
-                cmd->act_len = urb->actual_length;
+                cmd->act_len += urb->actual_length;
                 ub_cmdtr_act_len(sc, cmd);
 
+                if (++cmd->current_sg < cmd->nsg) {
+                        ub_data_start(sc, cmd);
+                        return;
+                }
                 ub_state_stat(sc, cmd);
 
         } else if (cmd->state == UB_CMDST_STAT) {
@@ -1401,6 +1377,46 @@ Bad_End: /* Little Excel is dead */
 
 /*
  * Factorization helper for the command state machine:
+ * Initiate a data segment transfer.
+ */
+static void ub_data_start(struct ub_dev *sc, struct ub_scsi_cmd *cmd)
+{
+        struct scatterlist *sg = &cmd->sgv[cmd->current_sg];
+        int pipe;
+        int rc;
+
+        UB_INIT_COMPLETION(sc->work_done);
+
+        if (cmd->dir == UB_DIR_READ)
+                pipe = sc->recv_bulk_pipe;
+        else
+                pipe = sc->send_bulk_pipe;
+        sc->last_pipe = pipe;
+        usb_fill_bulk_urb(&sc->work_urb, sc->dev, pipe,
+            page_address(sg->page) + sg->offset, sg->length,
+            ub_urb_complete, sc);
+        sc->work_urb.transfer_flags = 0;
+        sc->work_urb.actual_length = 0;
+        sc->work_urb.error_count = 0;
+        sc->work_urb.status = 0;
+
+        if ((rc = usb_submit_urb(&sc->work_urb, GFP_ATOMIC)) != 0) {
+                /* XXX Clear stalls */
+                printk("ub: data #%d submit failed (%d)\n", cmd->tag, rc); /* P3 */
+                ub_complete(&sc->work_done);
+                ub_state_done(sc, cmd, rc);
+                return;
+        }
+
+        sc->work_timer.expires = jiffies + UB_DATA_TIMEOUT;
+        add_timer(&sc->work_timer);
+
+        cmd->state = UB_CMDST_DATA;
+        ub_cmdtr_state(sc, cmd);
+}
+
+/*
+ * Factorization helper for the command state machine:
  * Finish the command.
  */
 static void ub_state_done(struct ub_dev *sc, struct ub_scsi_cmd *cmd, int rc)
@@ -1426,7 +1442,7 @@ static int __ub_state_stat(struct ub_dev *sc, struct ub_scsi_cmd *cmd)
         sc->last_pipe = sc->recv_bulk_pipe;
         usb_fill_bulk_urb(&sc->work_urb, sc->dev, sc->recv_bulk_pipe,
             &sc->work_bcs, US_BULK_CS_WRAP_LEN, ub_urb_complete, sc);
-        sc->work_urb.transfer_flags = URB_ASYNC_UNLINK;
+        sc->work_urb.transfer_flags = 0;
         sc->work_urb.actual_length = 0;
         sc->work_urb.error_count = 0;
         sc->work_urb.status = 0;
@@ -1484,6 +1500,7 @@ static void ub_state_stat_counted(struct ub_dev *sc, struct ub_scsi_cmd *cmd)
 static void ub_state_sense(struct ub_dev *sc, struct ub_scsi_cmd *cmd)
 {
         struct ub_scsi_cmd *scmd;
+        struct scatterlist *sg;
         int rc;
 
         if (cmd->cdb[0] == REQUEST_SENSE) {
@@ -1492,12 +1509,17 @@ static void ub_state_sense(struct ub_dev *sc, struct ub_scsi_cmd *cmd)
         }
 
         scmd = &sc->top_rqs_cmd;
+        memset(scmd, 0, sizeof(struct ub_scsi_cmd));
         scmd->cdb[0] = REQUEST_SENSE;
         scmd->cdb[4] = UB_SENSE_SIZE;
         scmd->cdb_len = 6;
         scmd->dir = UB_DIR_READ;
         scmd->state = UB_CMDST_INIT;
-        scmd->data = sc->top_sense;
+        scmd->nsg = 1;
+        sg = &scmd->sgv[0];
+        sg->page = virt_to_page(sc->top_sense);
+        sg->offset = (unsigned int)sc->top_sense & (PAGE_SIZE-1);
+        sg->length = UB_SENSE_SIZE;
         scmd->len = UB_SENSE_SIZE;
         scmd->lun = cmd->lun;
         scmd->done = ub_top_sense_done;
@@ -1541,7 +1563,7 @@ static int ub_submit_clear_stall(struct ub_dev *sc, struct ub_scsi_cmd *cmd,
 
         usb_fill_control_urb(&sc->work_urb, sc->dev, sc->send_ctrl_pipe,
             (unsigned char*) cr, NULL, 0, ub_urb_complete, sc);
-        sc->work_urb.transfer_flags = URB_ASYNC_UNLINK;
+        sc->work_urb.transfer_flags = 0;
         sc->work_urb.actual_length = 0;
         sc->work_urb.error_count = 0;
         sc->work_urb.status = 0;
@@ -1560,7 +1582,7 @@ static int ub_submit_clear_stall(struct ub_dev *sc, struct ub_scsi_cmd *cmd,
  */
 static void ub_top_sense_done(struct ub_dev *sc, struct ub_scsi_cmd *scmd)
 {
-        unsigned char *sense = scmd->data;
+        unsigned char *sense = sc->top_sense;
         struct ub_scsi_cmd *cmd;
 
         /*
@@ -1852,6 +1874,7 @@ static int ub_sync_read_cap(struct ub_dev *sc, struct ub_lun *lun,
     struct ub_capacity *ret)
 {
         struct ub_scsi_cmd *cmd;
+        struct scatterlist *sg;
         char *p;
         enum { ALLOC_SIZE = sizeof(struct ub_scsi_cmd) + 8 };
         unsigned long flags;
@@ -1872,7 +1895,11 @@ static int ub_sync_read_cap(struct ub_dev *sc, struct ub_lun *lun,
         cmd->cdb_len = 10;
         cmd->dir = UB_DIR_READ;
         cmd->state = UB_CMDST_INIT;
-        cmd->data = p;
+        cmd->nsg = 1;
+        sg = &cmd->sgv[0];
+        sg->page = virt_to_page(p);
+        sg->offset = (unsigned int)p & (PAGE_SIZE-1);
+        sg->length = 8;
         cmd->len = 8;
         cmd->lun = lun;
         cmd->done = ub_probe_done;
@@ -2289,7 +2316,7 @@ static int ub_probe_lun(struct ub_dev *sc, int lnum)
         disk->driverfs_dev = &sc->intf->dev;    /* XXX Many to one ok? */
 
         rc = -ENOMEM;
-        if ((q = blk_init_queue(ub_bd_rq_fn, &sc->lock)) == NULL)
+        if ((q = blk_init_queue(ub_request_fn, &sc->lock)) == NULL)
                 goto err_blkqinit;
 
         disk->queue = q;