1 files changed, 247 insertions, 258 deletions

diff --git a/drivers/message/i2o/i2o_scsi.c b/drivers/message/i2o/i2o_scsi.c
index 43f5875e0be5..9f1744c3933b 100644
--- a/drivers/message/i2o/i2o_scsi.c
+++ b/drivers/message/i2o/i2o_scsi.c
@@ -54,6 +54,7 @@
 #include <linux/pci.h>
 #include <linux/blkdev.h>
 #include <linux/i2o.h>
+#include <linux/scatterlist.h>
 
 #include <asm/dma.h>
 #include <asm/system.h>
@@ -64,19 +65,23 @@
 #include <scsi/scsi_host.h>
 #include <scsi/scsi_device.h>
 #include <scsi/scsi_cmnd.h>
+#include <scsi/scsi_request.h>
+#include <scsi/sg.h>
+#include <scsi/sg_request.h>
 
 #define OSM_NAME        "scsi-osm"
-#define OSM_VERSION     "$Rev$"
+#define OSM_VERSION     "1.282"
 #define OSM_DESCRIPTION "I2O SCSI Peripheral OSM"
 
 static struct i2o_driver i2o_scsi_driver;
 
-static int i2o_scsi_max_id = 16;
-static int i2o_scsi_max_lun = 8;
+static unsigned int i2o_scsi_max_id = 16;
+static unsigned int i2o_scsi_max_lun = 255;
 
 struct i2o_scsi_host {
         struct Scsi_Host *scsi_host;    /* pointer to the SCSI host */
         struct i2o_controller *iop;     /* pointer to the I2O controller */
+        unsigned int lun;       /* lun's used for block devices */
         struct i2o_device *channel[0];  /* channel->i2o_dev mapping table */
 };
 
@@ -99,11 +104,17 @@ static struct i2o_scsi_host *i2o_scsi_host_alloc(struct i2o_controller *c)
         u8 type;
         int i;
         size_t size;
-        i2o_status_block *sb;
+        u16 body_size = 6;
+
+#ifdef CONFIG_I2O_EXT_ADAPTEC
+        if (c->adaptec)
+                body_size = 8;
+#endif
 
         list_for_each_entry(i2o_dev, &c->devices, list)
-            if (i2o_dev->lct_data.class_id == I2O_CLASS_BUS_ADAPTER_PORT) {
-                if (i2o_parm_field_get(i2o_dev, 0x0000, 0, &type, 1) || (type == 1))    /* SCSI bus */
+            if (i2o_dev->lct_data.class_id == I2O_CLASS_BUS_ADAPTER) {
+                if (i2o_parm_field_get(i2o_dev, 0x0000, 0, &type, 1)
+                    && (type == 0x01))  /* SCSI bus */
                         max_channel++;
         }
 
@@ -125,20 +136,18 @@ static struct i2o_scsi_host *i2o_scsi_host_alloc(struct i2o_controller *c)
         scsi_host->max_id = i2o_scsi_max_id;
         scsi_host->max_lun = i2o_scsi_max_lun;
         scsi_host->this_id = c->unit;
-
-        sb = c->status_block.virt;
-
-        scsi_host->sg_tablesize = (sb->inbound_frame_size -
-                                   sizeof(struct i2o_message) / 4 - 6) / 2;
+        scsi_host->sg_tablesize = i2o_sg_tablesize(c, body_size);
 
         i2o_shost = (struct i2o_scsi_host *)scsi_host->hostdata;
         i2o_shost->scsi_host = scsi_host;
         i2o_shost->iop = c;
+        i2o_shost->lun = 1;
 
         i = 0;
         list_for_each_entry(i2o_dev, &c->devices, list)
-            if (i2o_dev->lct_data.class_id == I2O_CLASS_BUS_ADAPTER_PORT) {
-                if (i2o_parm_field_get(i2o_dev, 0x0000, 0, &type, 1) || (type == 1))    /* only SCSI bus */
+            if (i2o_dev->lct_data.class_id == I2O_CLASS_BUS_ADAPTER) {
+                if (i2o_parm_field_get(i2o_dev, 0x0000, 0, &type, 1)
+                    && (type == 0x01))  /* only SCSI bus */
                         i2o_shost->channel[i++] = i2o_dev;
 
                 if (i >= max_channel)
@@ -178,10 +187,13 @@ static int i2o_scsi_remove(struct device *dev)
         struct i2o_scsi_host *i2o_shost;
         struct scsi_device *scsi_dev;
 
+        osm_info("device removed (TID: %03x)\n", i2o_dev->lct_data.tid);
+
         i2o_shost = i2o_scsi_get_host(c);
 
         shost_for_each_device(scsi_dev, i2o_shost->scsi_host)
             if (scsi_dev->hostdata == i2o_dev) {
+                sysfs_remove_link(&i2o_dev->device.kobj, "scsi");
                 scsi_remove_device(scsi_dev);
                 scsi_device_put(scsi_dev);
                 break;
@@ -207,8 +219,8 @@ static int i2o_scsi_probe(struct device *dev)
         struct Scsi_Host *scsi_host;
         struct i2o_device *parent;
         struct scsi_device *scsi_dev;
-        u32 id;
-        u64 lun;
+        u32 id = -1;
+        u64 lun = -1;
         int channel = -1;
         int i;
 
@@ -218,8 +230,56 @@ static int i2o_scsi_probe(struct device *dev)
 
         scsi_host = i2o_shost->scsi_host;
 
-        if (i2o_parm_field_get(i2o_dev, 0, 3, &id, 4) < 0)
+        switch (i2o_dev->lct_data.class_id) {
+        case I2O_CLASS_RANDOM_BLOCK_STORAGE:
+        case I2O_CLASS_EXECUTIVE:
+#ifdef CONFIG_I2O_EXT_ADAPTEC
+                if (c->adaptec) {
+                        u8 type;
+                        struct i2o_device *d = i2o_shost->channel[0];
+
+                        if (i2o_parm_field_get(d, 0x0000, 0, &type, 1)
+                            && (type == 0x01))  /* SCSI bus */
+                                if (i2o_parm_field_get(d, 0x0200, 4, &id, 4)) {
+                                        channel = 0;
+                                        if (i2o_dev->lct_data.class_id ==
+                                            I2O_CLASS_RANDOM_BLOCK_STORAGE)
+                                                lun = i2o_shost->lun++;
+                                        else
+                                                lun = 0;
+                                }
+                }
+#endif
+                break;
+
+        case I2O_CLASS_SCSI_PERIPHERAL:
+                if (i2o_parm_field_get(i2o_dev, 0x0000, 3, &id, 4) < 0)
+                        return -EFAULT;
+
+                if (i2o_parm_field_get(i2o_dev, 0x0000, 4, &lun, 8) < 0)
+                        return -EFAULT;
+
+                parent = i2o_iop_find_device(c, i2o_dev->lct_data.parent_tid);
+                if (!parent) {
+                        osm_warn("can not find parent of device %03x\n",
+                                 i2o_dev->lct_data.tid);
+                        return -EFAULT;
+                }
+
+                for (i = 0; i <= i2o_shost->scsi_host->max_channel; i++)
+                        if (i2o_shost->channel[i] == parent)
+                                channel = i;
+                break;
+
+        default:
                 return -EFAULT;
+        }
+
+        if (channel == -1) {
+                osm_warn("can not find channel of device %03x\n",
+                         i2o_dev->lct_data.tid);
+                return -EFAULT;
+        }
 
         if (id >= scsi_host->max_id) {
                 osm_warn("SCSI device id (%d) >= max_id of I2O host (%d)", id,
@@ -227,42 +287,26 @@ static int i2o_scsi_probe(struct device *dev)
                 return -EFAULT;
         }
 
-        if (i2o_parm_field_get(i2o_dev, 0, 4, &lun, 8) < 0)
-                return -EFAULT;
         if (lun >= scsi_host->max_lun) {
                 osm_warn("SCSI device id (%d) >= max_lun of I2O host (%d)",
                          (unsigned int)lun, scsi_host->max_lun);
                 return -EFAULT;
         }
 
-        parent = i2o_iop_find_device(c, i2o_dev->lct_data.parent_tid);
-        if (!parent) {
-                osm_warn("can not find parent of device %03x\n",
-                         i2o_dev->lct_data.tid);
-                return -EFAULT;
-        }
-
-        for (i = 0; i <= i2o_shost->scsi_host->max_channel; i++)
-                if (i2o_shost->channel[i] == parent)
-                        channel = i;
-
-        if (channel == -1) {
-                osm_warn("can not find channel of device %03x\n",
-                         i2o_dev->lct_data.tid);
-                return -EFAULT;
-        }
-
         scsi_dev =
             __scsi_add_device(i2o_shost->scsi_host, channel, id, lun, i2o_dev);
 
-        if (!scsi_dev) {
+        if (IS_ERR(scsi_dev)) {
                 osm_warn("can not add SCSI device %03x\n",
                          i2o_dev->lct_data.tid);
-                return -EFAULT;
+                return PTR_ERR(scsi_dev);
         }
 
-        osm_debug("added new SCSI device %03x (cannel: %d, id: %d, lun: %d)\n",
-                  i2o_dev->lct_data.tid, channel, id, (unsigned int)lun);
+        sysfs_create_link(&i2o_dev->device.kobj, &scsi_dev->sdev_gendev.kobj,
+                          "scsi");
+
+        osm_info("device added (TID: %03x) channel: %d, id: %d, lun: %d\n",
+                 i2o_dev->lct_data.tid, channel, id, (unsigned int)lun);
 
         return 0;
 };
@@ -293,162 +337,89 @@ static int i2o_scsi_reply(struct i2o_controller *c, u32 m,
                           struct i2o_message *msg)
 {
         struct scsi_cmnd *cmd;
+        u32 error;
         struct device *dev;
-        u8 as, ds, st;
 
         cmd = i2o_cntxt_list_get(c, le32_to_cpu(msg->u.s.tcntxt));
-
-        if (msg->u.head[0] & (1 << 13)) {
-                struct i2o_message __iomem *pmsg;       /* preserved message */
-                u32 pm;
-                int err = DID_ERROR;
-
-                pm = le32_to_cpu(msg->body[3]);
-
-                pmsg = i2o_msg_in_to_virt(c, pm);
-
-                osm_err("IOP fail.\n");
-                osm_err("From %d To %d Cmd %d.\n",
-                        (msg->u.head[1] >> 12) & 0xFFF,
-                        msg->u.head[1] & 0xFFF, msg->u.head[1] >> 24);
-                osm_err("Failure Code %d.\n", msg->body[0] >> 24);
-                if (msg->body[0] & (1 << 16))
-                        osm_err("Format error.\n");
-                if (msg->body[0] & (1 << 17))
-                        osm_err("Path error.\n");
-                if (msg->body[0] & (1 << 18))
-                        osm_err("Path State.\n");
-                if (msg->body[0] & (1 << 18))
-                {
-                        osm_err("Congestion.\n");
-                        err = DID_BUS_BUSY;
-                }
-
-                osm_debug("Failing message is %p.\n", pmsg);
-
-                cmd = i2o_cntxt_list_get(c, readl(&pmsg->u.s.tcntxt));
-                if (!cmd)
-                        return 1;
-
-                cmd->result = err << 16;
-                cmd->scsi_done(cmd);
-
-                /* Now flush the message by making it a NOP */
-                i2o_msg_nop(c, pm);
-
-                return 1;
+        if (unlikely(!cmd)) {
+                osm_err("NULL reply received!\n");
+                return -1;
         }
 
         /*
          *      Low byte is device status, next is adapter status,
          *      (then one byte reserved), then request status.
          */
-        ds = (u8) le32_to_cpu(msg->body[0]);
-        as = (u8) (le32_to_cpu(msg->body[0]) >> 8);
-        st = (u8) (le32_to_cpu(msg->body[0]) >> 24);
+        error = le32_to_cpu(msg->body[0]);
+
+        osm_debug("Completed %ld\n", cmd->serial_number);
 
+        cmd->result = error & 0xff;
         /*
-         *      Is this a control request coming back - eg an abort ?
+         * if DeviceStatus is not SCSI_SUCCESS copy over the sense data and let
+         * the SCSI layer handle the error
          */
+        if (cmd->result)
+                memcpy(cmd->sense_buffer, &msg->body[3],
+                       min(sizeof(cmd->sense_buffer), (size_t) 40));
 
-        if (!cmd) {
-                if (st)
-                        osm_warn("SCSI abort: %08X", le32_to_cpu(msg->body[0]));
-                osm_info("SCSI abort completed.\n");
-                return -EFAULT;
-        }
+        /* only output error code if AdapterStatus is not HBA_SUCCESS */
+        if ((error >> 8) & 0xff)
+                osm_err("SCSI error %08x\n", error);
 
-        osm_debug("Completed %ld\n", cmd->serial_number);
+        dev = &c->pdev->dev;
+        if (cmd->use_sg)
+                dma_unmap_sg(dev, cmd->request_buffer, cmd->use_sg,
+                             cmd->sc_data_direction);
+        else if (cmd->SCp.dma_handle)
+                dma_unmap_single(dev, cmd->SCp.dma_handle, cmd->request_bufflen,
+                                 cmd->sc_data_direction);
 
-        if (st) {
-                u32 count, error;
-                /* An error has occurred */
-
-                switch (st) {
-                case 0x06:
-                        count = le32_to_cpu(msg->body[1]);
-                        if (count < cmd->underflow) {
-                                int i;
-
-                                osm_err("SCSI underflow 0x%08X 0x%08X\n", count,
-                                        cmd->underflow);
-                                osm_debug("Cmd: ");
-                                for (i = 0; i < 15; i++)
-                                        pr_debug("%02X ", cmd->cmnd[i]);
-                                pr_debug(".\n");
-                                cmd->result = (DID_ERROR << 16);
-                        }
-                        break;
+        cmd->scsi_done(cmd);
 
-                default:
-                        error = le32_to_cpu(msg->body[0]);
-
-                        osm_err("SCSI error %08x\n", error);
-
-                        if ((error & 0xff) == 0x02 /*CHECK_CONDITION */ ) {
-                                int i;
-                                u32 len = sizeof(cmd->sense_buffer);
-                                len = (len > 40) ? 40 : len;
-                                // Copy over the sense data
-                                memcpy(cmd->sense_buffer, (void *)&msg->body[3],
-                                       len);
-                                for (i = 0; i <= len; i++)
-                                        osm_info("%02x\n",
-                                                 cmd->sense_buffer[i]);
-                                if (cmd->sense_buffer[0] == 0x70
-                                    && cmd->sense_buffer[2] == DATA_PROTECT) {
-                                        /* This is to handle an array failed */
-                                        cmd->result = (DID_TIME_OUT << 16);
-                                        printk(KERN_WARNING "%s: SCSI Data "
-                                               "Protect-Device (%d,%d,%d) "
-                                               "hba_status=0x%x, dev_status="
-                                               "0x%x, cmd=0x%x\n", c->name,
-                                               (u32) cmd->device->channel,
-                                               (u32) cmd->device->id,
-                                               (u32) cmd->device->lun,
-                                               (error >> 8) & 0xff,
-                                               error & 0xff, cmd->cmnd[0]);
-                                } else
-                                        cmd->result = (DID_ERROR << 16);
-
-                                break;
-                        }
-
-                        switch (as) {
-                        case 0x0E:
-                                /* SCSI Reset */
-                                cmd->result = DID_RESET << 16;
-                                break;
-
-                        case 0x0F:
-                                cmd->result = DID_PARITY << 16;
-                                break;
-
-                        default:
-                                cmd->result = DID_ERROR << 16;
-                                break;
-                        }
+        return 1;
+};
 
-                        break;
-                }
+/**
+ *      i2o_scsi_notify_device_add - Retrieve notifications of added devices
+ *      @i2o_dev: the I2O device which was added
+ *
+ *      If a I2O device is added we catch the notification, because I2O classes
+ *      other then SCSI peripheral will not be received through
+ *      i2o_scsi_probe().
+ */
+static void i2o_scsi_notify_device_add(struct i2o_device *i2o_dev)
+{
+        switch (i2o_dev->lct_data.class_id) {
+        case I2O_CLASS_EXECUTIVE:
+        case I2O_CLASS_RANDOM_BLOCK_STORAGE:
+                i2o_scsi_probe(&i2o_dev->device);
+                break;
 
-                cmd->scsi_done(cmd);
-                return 1;
+        default:
+                break;
         }
+};
 
-        cmd->result = DID_OK << 16 | ds;
-
-        cmd->scsi_done(cmd);
-
-        dev = &c->pdev->dev;
-        if (cmd->use_sg)
-                dma_unmap_sg(dev, (struct scatterlist *)cmd->buffer,
-                             cmd->use_sg, cmd->sc_data_direction);
-        else if (cmd->request_bufflen)
-                dma_unmap_single(dev, (dma_addr_t) ((long)cmd->SCp.ptr),
-                                 cmd->request_bufflen, cmd->sc_data_direction);
+/**
+ *      i2o_scsi_notify_device_remove - Retrieve notifications of removed
+ *                                      devices
+ *      @i2o_dev: the I2O device which was removed
+ *
+ *      If a I2O device is removed, we catch the notification to remove the
+ *      corresponding SCSI device.
+ */
+static void i2o_scsi_notify_device_remove(struct i2o_device *i2o_dev)
+{
+        switch (i2o_dev->lct_data.class_id) {
+        case I2O_CLASS_EXECUTIVE:
+        case I2O_CLASS_RANDOM_BLOCK_STORAGE:
+                i2o_scsi_remove(&i2o_dev->device);
+                break;
 
-        return 1;
+        default:
+                break;
+        }
 };
 
 /**
@@ -501,7 +472,7 @@ static void i2o_scsi_notify_controller_remove(struct i2o_controller *c)
 
         scsi_remove_host(i2o_shost->scsi_host);
         scsi_host_put(i2o_shost->scsi_host);
-        pr_info("I2O SCSI host removed\n");
+        osm_debug("I2O SCSI host removed\n");
 };
 
 /* SCSI OSM driver struct */
@@ -509,6 +480,8 @@ static struct i2o_driver i2o_scsi_driver = {
         .name = OSM_NAME,
         .reply = i2o_scsi_reply,
         .classes = i2o_scsi_class_id,
+        .notify_device_add = i2o_scsi_notify_device_add,
+        .notify_device_remove = i2o_scsi_notify_device_remove,
         .notify_controller_add = i2o_scsi_notify_controller_add,
         .notify_controller_remove = i2o_scsi_notify_controller_remove,
         .driver = {
@@ -535,26 +508,26 @@ static int i2o_scsi_queuecommand(struct scsi_cmnd *SCpnt,
                                  void (*done) (struct scsi_cmnd *))
 {
         struct i2o_controller *c;
-        struct Scsi_Host *host;
         struct i2o_device *i2o_dev;
-        struct device *dev;
         int tid;
         struct i2o_message __iomem *msg;
         u32 m;
-        u32 scsi_flags, sg_flags;
+        /*
+         * ENABLE_DISCONNECT
+         * SIMPLE_TAG
+         * RETURN_SENSE_DATA_IN_REPLY_MESSAGE_FRAME
+         */
+        u32 scsi_flags = 0x20a00000;
+        u32 sgl_offset;
         u32 __iomem *mptr;
-        u32 __iomem *lenptr;
-        u32 len, reqlen;
-        int i;
+        u32 cmd = I2O_CMD_SCSI_EXEC << 24;
+        int rc = 0;
 
         /*
          *      Do the incoming paperwork
          */
-
         i2o_dev = SCpnt->device->hostdata;
-        host = SCpnt->device->host;
         c = i2o_dev->iop;
-        dev = &c->pdev->dev;
 
         SCpnt->scsi_done = done;
 
@@ -562,7 +535,7 @@ static int i2o_scsi_queuecommand(struct scsi_cmnd *SCpnt,
                 osm_warn("no I2O device in request\n");
                 SCpnt->result = DID_NO_CONNECT << 16;
                 done(SCpnt);
-                return 0;
+                goto exit;
         }
 
         tid = i2o_dev->lct_data.tid;
@@ -571,44 +544,85 @@ static int i2o_scsi_queuecommand(struct scsi_cmnd *SCpnt,
         osm_debug("Real scsi messages.\n");
 
         /*
-         *      Obtain an I2O message. If there are none free then
-         *      throw it back to the scsi layer
-         */
-
-        m = i2o_msg_get_wait(c, &msg, I2O_TIMEOUT_MESSAGE_GET);
-        if (m == I2O_QUEUE_EMPTY)
-                return SCSI_MLQUEUE_HOST_BUSY;
-
-        /*
          *      Put together a scsi execscb message
          */
-
-        len = SCpnt->request_bufflen;
-
         switch (SCpnt->sc_data_direction) {
         case PCI_DMA_NONE:
-                scsi_flags = 0x00000000;        // DATA NO XFER
-                sg_flags = 0x00000000;
+                /* DATA NO XFER */
+                sgl_offset = SGL_OFFSET_0;
                 break;
 
         case PCI_DMA_TODEVICE:
-                scsi_flags = 0x80000000;        // DATA OUT (iop-->dev)
-                sg_flags = 0x14000000;
+                /* DATA OUT (iop-->dev) */
+                scsi_flags |= 0x80000000;
+                sgl_offset = SGL_OFFSET_10;
                 break;
 
         case PCI_DMA_FROMDEVICE:
-                scsi_flags = 0x40000000;        // DATA IN  (iop<--dev)
-                sg_flags = 0x10000000;
+                /* DATA IN  (iop<--dev) */
+                scsi_flags |= 0x40000000;
+                sgl_offset = SGL_OFFSET_10;
                 break;
 
         default:
                 /* Unknown - kill the command */
                 SCpnt->result = DID_NO_CONNECT << 16;
                 done(SCpnt);
-                return 0;
+                goto exit;
         }
 
-        writel(I2O_CMD_SCSI_EXEC << 24 | HOST_TID << 12 | tid, &msg->u.head[1]);
+        /*
+         *      Obtain an I2O message. If there are none free then
+         *      throw it back to the scsi layer
+         */
+
+        m = i2o_msg_get_wait(c, &msg, I2O_TIMEOUT_MESSAGE_GET);
+        if (m == I2O_QUEUE_EMPTY) {
+                rc = SCSI_MLQUEUE_HOST_BUSY;
+                goto exit;
+        }
+
+        mptr = &msg->body[0];
+
+#ifdef CONFIG_I2O_EXT_ADAPTEC
+        if (c->adaptec) {
+                u32 adpt_flags = 0;
+
+                if (SCpnt->sc_request && SCpnt->sc_request->upper_private_data) {
+                        i2o_sg_io_hdr_t __user *usr_ptr =
+                            ((Sg_request *) (SCpnt->sc_request->
+                                             upper_private_data))->header.
+                            usr_ptr;
+
+                        if (usr_ptr)
+                                get_user(adpt_flags, &usr_ptr->flags);
+                }
+
+                switch (i2o_dev->lct_data.class_id) {
+                case I2O_CLASS_EXECUTIVE:
+                case I2O_CLASS_RANDOM_BLOCK_STORAGE:
+                        /* interpret flag has to be set for executive */
+                        adpt_flags ^= I2O_DPT_SG_FLAG_INTERPRET;
+                        break;
+
+                default:
+                        break;
+                }
+
+                /*
+                 * for Adaptec controllers we use the PRIVATE command, because
+                 * the normal SCSI EXEC doesn't support all SCSI commands on
+                 * all controllers (for example READ CAPACITY).
+                 */
+                if (sgl_offset == SGL_OFFSET_10)
+                        sgl_offset = SGL_OFFSET_12;
+                cmd = I2O_CMD_PRIVATE << 24;
+                writel(I2O_VENDOR_DPT << 16 | I2O_CMD_SCSI_EXEC, mptr++);
+                writel(adpt_flags | tid, mptr++);
+        }
+#endif
+
+        writel(cmd | HOST_TID << 12 | tid, &msg->u.head[1]);
         writel(i2o_scsi_driver.context, &msg->u.s.icntxt);
 
         /* We want the SCSI control block back */
@@ -626,7 +640,7 @@ static int i2o_scsi_queuecommand(struct scsi_cmnd *SCpnt,
          */
 
         /* Attach tags to the devices */
-        /*
+        /* FIXME: implement
            if(SCpnt->device->tagged_supported) {
            if(SCpnt->tag == HEAD_OF_QUEUE_TAG)
            scsi_flags |= 0x01000000;
@@ -635,67 +649,35 @@ static int i2o_scsi_queuecommand(struct scsi_cmnd *SCpnt,
            }
          */
 
-        /* Direction, disconnect ok, tag, CDBLen */
-        writel(scsi_flags | 0x20200000 | SCpnt->cmd_len, &msg->body[0]);
-
-        mptr = &msg->body[1];
+        writel(scsi_flags | SCpnt->cmd_len, mptr++);
 
         /* Write SCSI command into the message - always 16 byte block */
         memcpy_toio(mptr, SCpnt->cmnd, 16);
         mptr += 4;
-        lenptr = mptr++;        /* Remember me - fill in when we know */
-
-        reqlen = 12;            // SINGLE SGE
-
-        /* Now fill in the SGList and command */
-        if (SCpnt->use_sg) {
-                struct scatterlist *sg;
-                int sg_count;
-
-                sg = SCpnt->request_buffer;
-                len = 0;
-
-                sg_count = dma_map_sg(dev, sg, SCpnt->use_sg,
-                                      SCpnt->sc_data_direction);
 
-                if (unlikely(sg_count <= 0))
-                        return -ENOMEM;
-
-                for (i = SCpnt->use_sg; i > 0; i--) {
-                        if (i == 1)
-                                sg_flags |= 0xC0000000;
-                        writel(sg_flags | sg_dma_len(sg), mptr++);
-                        writel(sg_dma_address(sg), mptr++);
-                        len += sg_dma_len(sg);
-                        sg++;
+        if (sgl_offset != SGL_OFFSET_0) {
+                /* write size of data addressed by SGL */
+                writel(SCpnt->request_bufflen, mptr++);
+
+                /* Now fill in the SGList and command */
+                if (SCpnt->use_sg) {
+                        if (!i2o_dma_map_sg(c, SCpnt->request_buffer,
+                                            SCpnt->use_sg,
+                                            SCpnt->sc_data_direction, &mptr))
+                                goto nomem;
+                } else {
+                        SCpnt->SCp.dma_handle =
+                            i2o_dma_map_single(c, SCpnt->request_buffer,
+                                               SCpnt->request_bufflen,
+                                               SCpnt->sc_data_direction, &mptr);
+                        if (dma_mapping_error(SCpnt->SCp.dma_handle))
+                                goto nomem;
                 }
-
-                reqlen = mptr - &msg->u.head[0];
-                writel(len, lenptr);
-        } else {
-                len = SCpnt->request_bufflen;
-
-                writel(len, lenptr);
-
-                if (len > 0) {
-                        dma_addr_t dma_addr;
-
-                        dma_addr = dma_map_single(dev, SCpnt->request_buffer,
-                                                  SCpnt->request_bufflen,
-                                                  SCpnt->sc_data_direction);
-                        if (!dma_addr)
-                                return -ENOMEM;
-
-                        SCpnt->SCp.ptr = (void *)(unsigned long)dma_addr;
-                        sg_flags |= 0xC0000000;
-                        writel(sg_flags | SCpnt->request_bufflen, mptr++);
-                        writel(dma_addr, mptr++);
-                } else
-                        reqlen = 9;
         }
 
         /* Stick the headers on */
-        writel(reqlen << 16 | SGL_OFFSET_10, &msg->u.head[0]);
+        writel(I2O_MESSAGE_SIZE(mptr - &msg->u.head[0]) | sgl_offset,
+               &msg->u.head[0]);
 
         /* Queue the message */
         i2o_msg_post(c, m);
@@ -703,6 +685,13 @@ static int i2o_scsi_queuecommand(struct scsi_cmnd *SCpnt,
         osm_debug("Issued %ld\n", SCpnt->serial_number);
 
         return 0;
+
+      nomem:
+        rc = -ENOMEM;
+        i2o_msg_nop(c, m);
+
+      exit:
+        return rc;
 };
 
 /**