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;
 };
 
 /**