dm: blk: Add more driver-model support for block devices

index 2d340ef..6d41508 100644
@@ -17,6 +17,19 @@
 
 DECLARE_GLOBAL_DATA_PTR;
 
+#ifndef CONFIG_BLK
+static struct host_block_dev host_devices[CONFIG_HOST_MAX_DEVICES];
+
+static struct host_block_dev *find_host_device(int dev)
+{
+	if (dev >= 0 && dev < CONFIG_HOST_MAX_DEVICES)
+		return &host_devices[dev];
+
+	return NULL;
+}
+#endif
+
+#ifdef CONFIG_BLK
 static unsigned long host_block_read(struct udevice *dev,
 				     unsigned long start, lbaint_t blkcnt,
 				     void *buffer)

@@ -24,6 +37,18 @@ static unsigned long host_block_read(struct udevice *dev,
 	struct host_block_dev *host_dev = dev_get_priv(dev);
 	struct blk_desc *block_dev = dev_get_uclass_platdata(dev);
 
+#else
+static unsigned long host_block_read(struct blk_desc *block_dev,
+				     unsigned long start, lbaint_t blkcnt,
+				     void *buffer)
+{
+	int dev = block_dev->devnum;
+	struct host_block_dev *host_dev = find_host_device(dev);
+
+	if (!host_dev)
+		return -1;
+#endif
+
 	if (os_lseek(host_dev->fd, start * block_dev->blksz, OS_SEEK_SET) ==
 			-1) {
 		printf("ERROR: Invalid block %lx\n", start);

@@ -35,12 +60,21 @@ static unsigned long host_block_read(struct udevice *dev,
 	return -1;
 }
 
+#ifdef CONFIG_BLK
 static unsigned long host_block_write(struct udevice *dev,
 				      unsigned long start, lbaint_t blkcnt,
 				      const void *buffer)
 {
 	struct host_block_dev *host_dev = dev_get_priv(dev);
 	struct blk_desc *block_dev = dev_get_uclass_platdata(dev);
+#else
+static unsigned long host_block_write(struct blk_desc *block_dev,
+				      unsigned long start, lbaint_t blkcnt,
+				      const void *buffer)
+{
+	int dev = block_dev->devnum;
+	struct host_block_dev *host_dev = find_host_device(dev);
+#endif
 
 	if (os_lseek(host_dev->fd, start * block_dev->blksz, OS_SEEK_SET) ==
 			-1) {

@@ -53,6 +87,7 @@ static unsigned long host_block_write(struct udevice *dev,
 	return -1;
 }
 
+#ifdef CONFIG_BLK
 int host_dev_bind(int devnum, char *filename)
 {
 	struct host_block_dev *host_dev;

@@ -115,9 +150,51 @@ err:
 	free(str);
 	return ret;
 }
+#else
+int host_dev_bind(int dev, char *filename)
+{
+	struct host_block_dev *host_dev = find_host_device(dev);
+
+	if (!host_dev)
+		return -1;
+	if (host_dev->blk_dev.priv) {
+		os_close(host_dev->fd);
+		host_dev->blk_dev.priv = NULL;
+	}
+	if (host_dev->filename)
+		free(host_dev->filename);
+	if (filename && *filename) {
+		host_dev->filename = strdup(filename);
+	} else {
+		host_dev->filename = NULL;
+		return 0;
+	}
+
+	host_dev->fd = os_open(host_dev->filename, OS_O_RDWR);
+	if (host_dev->fd == -1) {
+		printf("Failed to access host backing file '%s'\n",
+		       host_dev->filename);
+		return 1;
+	}
+
+	struct blk_desc *blk_dev = &host_dev->blk_dev;
+	blk_dev->if_type = IF_TYPE_HOST;
+	blk_dev->priv = host_dev;
+	blk_dev->blksz = 512;
+	blk_dev->lba = os_lseek(host_dev->fd, 0, OS_SEEK_END) / blk_dev->blksz;
+	blk_dev->block_read = host_block_read;
+	blk_dev->block_write = host_block_write;
+	blk_dev->devnum = dev;
+	blk_dev->part_type = PART_TYPE_UNKNOWN;
+	part_init(blk_dev);
+
+	return 0;
+}
+#endif
 
 int host_get_dev_err(int devnum, struct blk_desc **blk_devp)
 {
+#ifdef CONFIG_BLK
 	struct udevice *dev;
 	int ret;
 

@@ -125,6 +202,17 @@ int host_get_dev_err(int devnum, struct blk_desc **blk_devp)
 	if (ret)
 		return ret;
 	*blk_devp = dev_get_uclass_platdata(dev);
+#else
+	struct host_block_dev *host_dev = find_host_device(devnum);
+
+	if (!host_dev)
+		return -ENODEV;
+
+	if (!host_dev->blk_dev.priv)
+		return -ENOENT;
+
+	*blk_devp = &host_dev->blk_dev;
+#endif
 
 	return 0;
 }

@@ -139,6 +227,7 @@ struct blk_desc *host_get_dev(int dev)
 	return blk_dev;
 }
 
+#ifdef CONFIG_BLK
 static const struct blk_ops sandbox_host_blk_ops = {
 	.read	= host_block_read,
 	.write	= host_block_write,

@@ -150,3 +239,4 @@ U_BOOT_DRIVER(sandbox_host_blk) = {
 	.ops		= &sandbox_host_blk_ops,
 	.priv_auto_alloc_size	= sizeof(struct host_block_dev),
 };
+#endif

index 4619089..4b73a0f 100644
@@ -175,11 +175,7 @@ pci_dev_t pci_find_devices(struct pci_device_id *ids, int index)
 	int bus;
 
 	for (hose = pci_get_hose_head(); hose; hose = hose->next) {
-#ifdef CONFIG_SYS_SCSI_SCAN_BUS_REVERSE
-		for (bus = hose->last_busno; bus >= hose->first_busno; bus--) {
-#else
 		for (bus = hose->first_busno; bus <= hose->last_busno; bus++) {
-#endif
 			bdf = pci_hose_find_devices(hose, bus, ids, &index);
 			if (bdf != -1)
 				return bdf;

index 1f4ed2a..cd8d91f 100644
@@ -358,8 +358,6 @@ extern unsigned long get_board_sys_clk(unsigned long dummy);
 
 #define CONFIG_PCI_SCAN_SHOW		/* show pci devices on startup */
 
-#undef CONFIG_SYS_SCSI_SCAN_BUS_REVERSE
-
 #define CONFIG_PCI_PNP			/* do pci plug-and-play */
 
 

index b7238fb..654e96e 100644
@@ -305,8 +305,6 @@
 
 #define CONFIG_PCI_SCAN_SHOW            /* show pci devices on startup */
 
-#undef CONFIG_SYS_SCSI_SCAN_BUS_REVERSE
-
 #define CONFIG_PCI_PNP			/* do pci plug-and-play */
 
 #undef CONFIG_EEPRO100

index dfb8486..2e47082 100644
@@ -261,7 +261,7 @@ static const struct udevice_id broadwell_ahci_ids[] = {
 
 U_BOOT_DRIVER(ahci_broadwell_drv) = {
 	.name		= "ahci_broadwell",
-	.id		= UCLASS_DISK,
+	.id		= UCLASS_AHCI,
 	.of_match	= broadwell_ahci_ids,
 	.ofdata_to_platdata	= broadwell_sata_ofdata_to_platdata,
 	.probe		= broadwell_sata_probe,

index 93e4505..0fdef6f 100644
@@ -58,7 +58,7 @@ int cpu_common_init(void)
 		return -ENODEV;
 
 	/* Cause the SATA device to do its early init */
-	uclass_first_device(UCLASS_DISK, &dev);
+	uclass_first_device(UCLASS_AHCI, &dev);
 
 	return 0;
 }

index 4c039ac..5b58d6c 100644
@@ -162,7 +162,7 @@ static int bd82x6x_probe(struct udevice *dev)
 		return 0;
 
 	/* Cause the SATA device to do its init */
-	uclass_first_device(UCLASS_DISK, &dev);
+	uclass_first_device(UCLASS_AHCI, &dev);
 
 	ret = syscon_get_by_driver_data(X86_SYSCON_GMA, &gma_dev);
 	if (ret)

index c3d1057..1ce8195 100644
@@ -233,7 +233,7 @@ static const struct udevice_id bd82x6x_ahci_ids[] = {
 
 U_BOOT_DRIVER(ahci_ivybridge_drv) = {
 	.name		= "ahci_ivybridge",
-	.id		= UCLASS_DISK,
+	.id		= UCLASS_AHCI,
 	.of_match	= bd82x6x_ahci_ids,
 	.probe		= bd82x6x_sata_probe,
 };

similarity index 72%
rename from drivers/block/disk-uclass.c
rename to drivers/block/ahci-uclass.c
index d665b35..7b8c326 100644
@@ -8,7 +8,7 @@
 #include <common.h>
 #include <dm.h>
 
-UCLASS_DRIVER(disk) = {
-	.id		= UCLASS_DISK,
-	.name		= "disk",
+UCLASS_DRIVER(ahci) = {
+	.id		= UCLASS_AHCI,
+	.name		= "ahci",
 };

index cbf9b2c..a5cf6e2 100644
@@ -26,11 +26,11 @@ enum uclass_id {
 
 	/* U-Boot uclasses start here - in alphabetical order */
 	UCLASS_ADC,		/* Analog-to-digital converter */
+	UCLASS_AHCI,		/* SATA disk controller */
 	UCLASS_BLK,		/* Block device */
 	UCLASS_CLK,		/* Clock source, e.g. used by peripherals */
 	UCLASS_CPU,		/* CPU, typically part of an SoC */
 	UCLASS_CROS_EC,		/* Chrome OS EC */
-	UCLASS_DISK,		/* Disk controller, e.g. SATA */
 	UCLASS_DISPLAY,		/* Display (e.g. DisplayPort, HDMI) */
 	UCLASS_DMA,		/* Direct Memory Access */
 	UCLASS_RAM,		/* RAM controller */

index 9bd1f16..a0b59fb 100644
@@ -31,10 +31,11 @@
 #define writew(val, addr)	iotrace_writew(val, (const void *)(addr))
 
 #undef readb
-#define readb(addr)	iotrace_readb((const void *)(addr))
+#define readb(addr)	iotrace_readb((const void *)((uintptr_t)addr))
 
 #undef writeb
-#define writeb(val, addr)	iotrace_writeb(val, (const void *)(addr))
+#define writeb(val, addr) \
+	iotrace_writeb(val, (const void *)((uintptr_t)addr))
 
 #endif
 

index 5b87fde..cdbb6ba 100644
@@ -56,6 +56,28 @@ void outl(unsigned int value, unsigned int addr);
 void outw(unsigned int value, unsigned int addr);
 void outb(unsigned int value, unsigned int addr);
 
+static inline void _insw(volatile u16 *port, void *buf, int ns)
+{
+	u16 *data = (u16 *) buf;
+	while (ns--)
+		*data++ = *port;
+}
+
+static inline void _outsw(volatile u16 *port, const void *buf, int ns)
+{
+	u16 *data = (u16 *) buf;
+	while (ns--) {
+		*port = *data++;
+	}
+}
+
+#define insw(port, buf, ns)		_insw((u16 *)port, buf, ns)
+#define outsw(port, buf, ns)		_outsw((u16 *)port, buf, ns)
+
+/* For systemace.c */
+#define out16(addr, val)
+#define in16(addr)		0
+
 #include <iotrace.h>
 
 #endif

new file mode 100644
index 0000000..d342a7a
@@ -0,0 +1,29 @@
+/*
+ * Copyright (C) 2015 Google, Inc
+ * Written by Simon Glass <[email protected]>
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ *
+ * This file contains dummy implementations of SCSI functions requried so
+ * that CONFIG_SCSI_can be enabled for sandbox.
+ */
+
+#include <common.h>
+#include <scsi.h>
+
+void scsi_bus_reset(void)
+{
+}
+
+void scsi_init(void)
+{
+}
+
+int scsi_exec(ccb *pccb)
+{
+	return 0;
+}
+
+void scsi_print_error(ccb *pccb)
+{
+}

new file mode 100644
index 0000000..bd967d2
@@ -0,0 +1,33 @@
+/*
+ * Copyright (C) 2015 Google, Inc
+ * Written by Simon Glass <[email protected]>
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <common.h>
+
+int init_sata(int dev)
+{
+	return 0;
+}
+
+int reset_sata(int dev)
+{
+	return 0;
+}
+
+int scan_sata(int dev)
+{
+	return 0;
+}
+
+ulong sata_read(int dev, ulong blknr, lbaint_t blkcnt, void *buffer)
+{
+	return 0;
+}
+
+ulong sata_write(int dev, ulong blknr, lbaint_t blkcnt, const void *buffer)
+{
+	return 0;
+}

index 8991125..f875f51 100644
@@ -50,6 +50,7 @@ static int scsi_curr_dev; /* current device */
 
 static struct blk_desc scsi_dev_desc[CONFIG_SYS_SCSI_MAX_DEVICE];
 
+#if 0
 /********************************************************************************
  *  forward declerations of some Setup Routines
  */

@@ -70,110 +71,232 @@ static ulong scsi_read(struct blk_desc *block_dev, lbaint_t blknr,
 		       lbaint_t blkcnt, void *buffer);
 static ulong scsi_write(struct blk_desc *block_dev, lbaint_t blknr,
 			lbaint_t blkcnt, const void *buffer);
+#endif
 
-
-/*********************************************************************************
- * (re)-scan the scsi bus and reports scsi device info
- * to the user if mode = 1
+/****************************************************************************************
+ * scsi_read
  */
-void scsi_scan(int mode)
+
+/* almost the maximum amount of the scsi_ext command.. */
+#define SCSI_MAX_READ_BLK 0xFFFF
+#define SCSI_LBA48_READ	0xFFFFFFF
+
+#ifdef CONFIG_SYS_64BIT_LBA
+void scsi_setup_read16(ccb * pccb, lbaint_t start, unsigned long blocks)
 {
-	unsigned char i,perq,modi,lun;
-	lbaint_t capacity;
-	unsigned long blksz;
-	ccb* pccb=(ccb *)&tempccb;
+	pccb->cmd[0] = SCSI_READ16;
+	pccb->cmd[1] = pccb->lun<<5;
+	pccb->cmd[2] = ((unsigned char) (start >> 56)) & 0xff;
+	pccb->cmd[3] = ((unsigned char) (start >> 48)) & 0xff;
+	pccb->cmd[4] = ((unsigned char) (start >> 40)) & 0xff;
+	pccb->cmd[5] = ((unsigned char) (start >> 32)) & 0xff;
+	pccb->cmd[6] = ((unsigned char) (start >> 24)) & 0xff;
+	pccb->cmd[7] = ((unsigned char) (start >> 16)) & 0xff;
+	pccb->cmd[8] = ((unsigned char) (start >> 8)) & 0xff;
+	pccb->cmd[9] = ((unsigned char) (start)) & 0xff;
+	pccb->cmd[10] = 0;
+	pccb->cmd[11] = ((unsigned char) (blocks >> 24)) & 0xff;
+	pccb->cmd[12] = ((unsigned char) (blocks >> 16)) & 0xff;
+	pccb->cmd[13] = ((unsigned char) (blocks >> 8)) & 0xff;
+	pccb->cmd[14] = (unsigned char) blocks & 0xff;
+	pccb->cmd[15] = 0;
+	pccb->cmdlen = 16;
+	pccb->msgout[0] = SCSI_IDENTIFY; /* NOT USED */
+	debug ("scsi_setup_read16: cmd: %02X %02X "
+	       "startblk %02X%02X%02X%02X%02X%02X%02X%02X "
+	       "blccnt %02X%02X%02X%02X\n",
+		pccb->cmd[0], pccb->cmd[1],
+		pccb->cmd[2], pccb->cmd[3], pccb->cmd[4], pccb->cmd[5],
+		pccb->cmd[6], pccb->cmd[7], pccb->cmd[8], pccb->cmd[9],
+		pccb->cmd[11], pccb->cmd[12], pccb->cmd[13], pccb->cmd[14]);
+}
+#endif
 
-	if(mode==1) {
-		printf("scanning bus for devices...\n");
-	}
-	for(i=0;i<CONFIG_SYS_SCSI_MAX_DEVICE;i++) {
-		scsi_dev_desc[i].target=0xff;
-		scsi_dev_desc[i].lun=0xff;
-		scsi_dev_desc[i].lba=0;
-		scsi_dev_desc[i].blksz=0;
-		scsi_dev_desc[i].log2blksz =
-			LOG2_INVALID(typeof(scsi_dev_desc[i].log2blksz));
-		scsi_dev_desc[i].type=DEV_TYPE_UNKNOWN;
-		scsi_dev_desc[i].vendor[0]=0;
-		scsi_dev_desc[i].product[0]=0;
-		scsi_dev_desc[i].revision[0]=0;
-		scsi_dev_desc[i].removable = false;
-		scsi_dev_desc[i].if_type=IF_TYPE_SCSI;
-		scsi_dev_desc[i].devnum = i;
-		scsi_dev_desc[i].part_type=PART_TYPE_UNKNOWN;
-		scsi_dev_desc[i].block_read=scsi_read;
-		scsi_dev_desc[i].block_write = scsi_write;
-	}
-	scsi_max_devs=0;
-	for(i=0;i<CONFIG_SYS_SCSI_MAX_SCSI_ID;i++) {
-		pccb->target=i;
-		for(lun=0;lun<CONFIG_SYS_SCSI_MAX_LUN;lun++) {
-			pccb->lun=lun;
-			pccb->pdata=(unsigned char *)&tempbuff;
-			pccb->datalen=512;
-			scsi_setup_inquiry(pccb);
-			if (scsi_exec(pccb) != true) {
-				if(pccb->contr_stat==SCSI_SEL_TIME_OUT) {
-					debug ("Selection timeout ID %d\n",pccb->target);
-					continue; /* selection timeout => assuming no device present */
-				}
-				scsi_print_error(pccb);
-				continue;
-			}
-			perq=tempbuff[0];
-			modi=tempbuff[1];
-			if((perq & 0x1f)==0x1f) {
-				continue; /* skip unknown devices */
-			}
-			if((modi&0x80)==0x80) /* drive is removable */
-				scsi_dev_desc[scsi_max_devs].removable=true;
-			/* get info for this device */
-			scsi_ident_cpy((unsigned char *)&scsi_dev_desc[scsi_max_devs].vendor[0],
-				       &tempbuff[8], 8);
-			scsi_ident_cpy((unsigned char *)&scsi_dev_desc[scsi_max_devs].product[0],
-				       &tempbuff[16], 16);
-			scsi_ident_cpy((unsigned char *)&scsi_dev_desc[scsi_max_devs].revision[0],
-				       &tempbuff[32], 4);
-			scsi_dev_desc[scsi_max_devs].target=pccb->target;
-			scsi_dev_desc[scsi_max_devs].lun=pccb->lun;
+void scsi_setup_read_ext(ccb * pccb, lbaint_t start, unsigned short blocks)
+{
+	pccb->cmd[0]=SCSI_READ10;
+	pccb->cmd[1]=pccb->lun<<5;
+	pccb->cmd[2]=((unsigned char) (start>>24))&0xff;
+	pccb->cmd[3]=((unsigned char) (start>>16))&0xff;
+	pccb->cmd[4]=((unsigned char) (start>>8))&0xff;
+	pccb->cmd[5]=((unsigned char) (start))&0xff;
+	pccb->cmd[6]=0;
+	pccb->cmd[7]=((unsigned char) (blocks>>8))&0xff;
+	pccb->cmd[8]=(unsigned char) blocks & 0xff;
+	pccb->cmd[6]=0;
+	pccb->cmdlen=10;
+	pccb->msgout[0]=SCSI_IDENTIFY; /* NOT USED */
+	debug ("scsi_setup_read_ext: cmd: %02X %02X startblk %02X%02X%02X%02X blccnt %02X%02X\n",
+		pccb->cmd[0],pccb->cmd[1],
+		pccb->cmd[2],pccb->cmd[3],pccb->cmd[4],pccb->cmd[5],
+		pccb->cmd[7],pccb->cmd[8]);
+}
 
-			pccb->datalen=0;
-			scsi_setup_test_unit_ready(pccb);
-			if (scsi_exec(pccb) != true) {
-				if (scsi_dev_desc[scsi_max_devs].removable == true) {
-					scsi_dev_desc[scsi_max_devs].type=perq;
-					goto removable;
-				}
-				scsi_print_error(pccb);
-				continue;
-			}
-			if (scsi_read_capacity(pccb, &capacity, &blksz)) {
-				scsi_print_error(pccb);
-				continue;
-			}
-			scsi_dev_desc[scsi_max_devs].lba=capacity;
-			scsi_dev_desc[scsi_max_devs].blksz=blksz;
-			scsi_dev_desc[scsi_max_devs].log2blksz =
-				LOG2(scsi_dev_desc[scsi_max_devs].blksz);
-			scsi_dev_desc[scsi_max_devs].type=perq;
-			part_init(&scsi_dev_desc[scsi_max_devs]);
-removable:
-			if(mode==1) {
-				printf ("  Device %d: ", scsi_max_devs);
-				dev_print(&scsi_dev_desc[scsi_max_devs]);
-			} /* if mode */
-			scsi_max_devs++;
-		} /* next LUN */
-	}
-	if(scsi_max_devs>0)
-		scsi_curr_dev=0;
+void scsi_setup_write_ext(ccb *pccb, lbaint_t start, unsigned short blocks)
+{
+	pccb->cmd[0] = SCSI_WRITE10;
+	pccb->cmd[1] = pccb->lun << 5;
+	pccb->cmd[2] = ((unsigned char) (start>>24)) & 0xff;
+	pccb->cmd[3] = ((unsigned char) (start>>16)) & 0xff;
+	pccb->cmd[4] = ((unsigned char) (start>>8)) & 0xff;
+	pccb->cmd[5] = ((unsigned char) (start)) & 0xff;
+	pccb->cmd[6] = 0;
+	pccb->cmd[7] = ((unsigned char) (blocks>>8)) & 0xff;
+	pccb->cmd[8] = (unsigned char)blocks & 0xff;
+	pccb->cmd[9] = 0;
+	pccb->cmdlen = 10;
+	pccb->msgout[0] = SCSI_IDENTIFY;  /* NOT USED */
+	debug("%s: cmd: %02X %02X startblk %02X%02X%02X%02X blccnt %02X%02X\n",
+	      __func__,
+	      pccb->cmd[0], pccb->cmd[1],
+	      pccb->cmd[2], pccb->cmd[3], pccb->cmd[4], pccb->cmd[5],
+	      pccb->cmd[7], pccb->cmd[8]);
+}
+
+void scsi_setup_read6(ccb * pccb, lbaint_t start, unsigned short blocks)
+{
+	pccb->cmd[0]=SCSI_READ6;
+	pccb->cmd[1]=pccb->lun<<5 | (((unsigned char)(start>>16))&0x1f);
+	pccb->cmd[2]=((unsigned char) (start>>8))&0xff;
+	pccb->cmd[3]=((unsigned char) (start))&0xff;
+	pccb->cmd[4]=(unsigned char) blocks & 0xff;
+	pccb->cmd[5]=0;
+	pccb->cmdlen=6;
+	pccb->msgout[0]=SCSI_IDENTIFY; /* NOT USED */
+	debug ("scsi_setup_read6: cmd: %02X %02X startblk %02X%02X blccnt %02X\n",
+		pccb->cmd[0],pccb->cmd[1],
+		pccb->cmd[2],pccb->cmd[3],pccb->cmd[4]);
+}
+
+
+void scsi_setup_inquiry(ccb * pccb)
+{
+	pccb->cmd[0]=SCSI_INQUIRY;
+	pccb->cmd[1]=pccb->lun<<5;
+	pccb->cmd[2]=0;
+	pccb->cmd[3]=0;
+	if(pccb->datalen>255)
+		pccb->cmd[4]=255;
 	else
-		scsi_curr_dev = -1;
+		pccb->cmd[4]=(unsigned char)pccb->datalen;
+	pccb->cmd[5]=0;
+	pccb->cmdlen=6;
+	pccb->msgout[0]=SCSI_IDENTIFY; /* NOT USED */
+}
 
-	printf("Found %d device(s).\n", scsi_max_devs);
-#ifndef CONFIG_SPL_BUILD
-	setenv_ulong("scsidevs", scsi_max_devs);
+static ulong scsi_read(struct blk_desc *block_dev, lbaint_t blknr,
+		       lbaint_t blkcnt, void *buffer)
+{
+	int device = block_dev->devnum;
+	lbaint_t start, blks;
+	uintptr_t buf_addr;
+	unsigned short smallblks = 0;
+	ccb* pccb=(ccb *)&tempccb;
+	device&=0xff;
+	/* Setup  device
+	 */
+	pccb->target=scsi_dev_desc[device].target;
+	pccb->lun=scsi_dev_desc[device].lun;
+	buf_addr=(unsigned long)buffer;
+	start=blknr;
+	blks=blkcnt;
+	debug("\nscsi_read: dev %d startblk " LBAF
+	      ", blccnt " LBAF " buffer %lx\n",
+	      device, start, blks, (unsigned long)buffer);
+	do {
+		pccb->pdata=(unsigned char *)buf_addr;
+#ifdef CONFIG_SYS_64BIT_LBA
+		if (start > SCSI_LBA48_READ) {
+			unsigned long blocks;
+			blocks = min_t(lbaint_t, blks, SCSI_MAX_READ_BLK);
+			pccb->datalen = scsi_dev_desc[device].blksz * blocks;
+			scsi_setup_read16(pccb, start, blocks);
+			start += blocks;
+			blks -= blocks;
+		} else
 #endif
+		if (blks > SCSI_MAX_READ_BLK) {
+			pccb->datalen=scsi_dev_desc[device].blksz * SCSI_MAX_READ_BLK;
+			smallblks=SCSI_MAX_READ_BLK;
+			scsi_setup_read_ext(pccb,start,smallblks);
+			start+=SCSI_MAX_READ_BLK;
+			blks-=SCSI_MAX_READ_BLK;
+		}
+		else {
+			pccb->datalen=scsi_dev_desc[device].blksz * blks;
+			smallblks=(unsigned short) blks;
+			scsi_setup_read_ext(pccb,start,smallblks);
+			start+=blks;
+			blks=0;
+		}
+		debug("scsi_read_ext: startblk " LBAF
+		      ", blccnt %x buffer %" PRIXPTR "\n",
+		      start, smallblks, buf_addr);
+		if (scsi_exec(pccb) != true) {
+			scsi_print_error(pccb);
+			blkcnt-=blks;
+			break;
+		}
+		buf_addr+=pccb->datalen;
+	} while(blks!=0);
+	debug("scsi_read_ext: end startblk " LBAF
+	      ", blccnt %x buffer %" PRIXPTR "\n", start, smallblks, buf_addr);
+	return(blkcnt);
+}
+
+/*******************************************************************************
+ * scsi_write
+ */
+
+/* Almost the maximum amount of the scsi_ext command.. */
+#define SCSI_MAX_WRITE_BLK 0xFFFF
+
+static ulong scsi_write(struct blk_desc *block_dev, lbaint_t blknr,
+			lbaint_t blkcnt, const void *buffer)
+{
+	int device = block_dev->devnum;
+	lbaint_t start, blks;
+	uintptr_t buf_addr;
+	unsigned short smallblks;
+	ccb* pccb = (ccb *)&tempccb;
+	device &= 0xff;
+	/* Setup  device
+	 */
+	pccb->target = scsi_dev_desc[device].target;
+	pccb->lun = scsi_dev_desc[device].lun;
+	buf_addr = (unsigned long)buffer;
+	start = blknr;
+	blks = blkcnt;
+	debug("\n%s: dev %d startblk " LBAF ", blccnt " LBAF " buffer %lx\n",
+	      __func__, device, start, blks, (unsigned long)buffer);
+	do {
+		pccb->pdata = (unsigned char *)buf_addr;
+		if (blks > SCSI_MAX_WRITE_BLK) {
+			pccb->datalen = (scsi_dev_desc[device].blksz *
+					 SCSI_MAX_WRITE_BLK);
+			smallblks = SCSI_MAX_WRITE_BLK;
+			scsi_setup_write_ext(pccb, start, smallblks);
+			start += SCSI_MAX_WRITE_BLK;
+			blks -= SCSI_MAX_WRITE_BLK;
+		} else {
+			pccb->datalen = scsi_dev_desc[device].blksz * blks;
+			smallblks = (unsigned short)blks;
+			scsi_setup_write_ext(pccb, start, smallblks);
+			start += blks;
+			blks = 0;
+		}
+		debug("%s: startblk " LBAF ", blccnt %x buffer %" PRIXPTR "\n",
+		      __func__, start, smallblks, buf_addr);
+		if (scsi_exec(pccb) != true) {
+			scsi_print_error(pccb);
+			blkcnt -= blks;
+			break;
+		}
+		buf_addr += pccb->datalen;
+	} while (blks != 0);
+	debug("%s: end startblk " LBAF ", blccnt %x buffer %" PRIXPTR "\n",
+	      __func__, start, smallblks, buf_addr);
+	return blkcnt;
 }
 
 int scsi_get_disk_count(void)

@@ -327,192 +450,69 @@ int do_scsi (cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
 				printf("... is now current device\n");
 				return 0;
 			}
-			if (strncmp(argv[1],"part",4) == 0) {
-				int dev = (int)simple_strtoul(argv[2], NULL, 10);
-				if(scsi_dev_desc[dev].type != DEV_TYPE_UNKNOWN) {
-					part_print(&scsi_dev_desc[dev]);
-				}
-				else {
-					printf ("\nSCSI device %d not available\n", dev);
-				}
-				return 1;
-			}
-			return CMD_RET_USAGE;
-    default:
-			/* at least 4 args */
-			if (strcmp(argv[1],"read") == 0) {
-				ulong addr = simple_strtoul(argv[2], NULL, 16);
-				ulong blk  = simple_strtoul(argv[3], NULL, 16);
-				ulong cnt  = simple_strtoul(argv[4], NULL, 16);
-				ulong n;
-				printf ("\nSCSI read: device %d block # %ld, count %ld ... ",
-						scsi_curr_dev, blk, cnt);
-				n = scsi_read(&scsi_dev_desc[scsi_curr_dev],
-					      blk, cnt, (ulong *)addr);
-				printf ("%ld blocks read: %s\n",n,(n==cnt) ? "OK" : "ERROR");
-				return 0;
-			} else if (strcmp(argv[1], "write") == 0) {
-				ulong addr = simple_strtoul(argv[2], NULL, 16);
-				ulong blk = simple_strtoul(argv[3], NULL, 16);
-				ulong cnt = simple_strtoul(argv[4], NULL, 16);
-				ulong n;
-				printf("\nSCSI write: device %d block # %ld, "
-				       "count %ld ... ",
-				       scsi_curr_dev, blk, cnt);
-				n = scsi_write(&scsi_dev_desc[scsi_curr_dev],
-					       blk, cnt, (ulong *)addr);
-				printf("%ld blocks written: %s\n", n,
-				       (n == cnt) ? "OK" : "ERROR");
-				return 0;
-			}
-	} /* switch */
-	return CMD_RET_USAGE;
-}
-
-U_BOOT_CMD(
-	scsi, 5, 1, do_scsi,
-	"SCSI sub-system",
-	"reset - reset SCSI controller\n"
-	"scsi info  - show available SCSI devices\n"
-	"scsi scan  - (re-)scan SCSI bus\n"
-	"scsi device [dev] - show or set current device\n"
-	"scsi part [dev] - print partition table of one or all SCSI devices\n"
-	"scsi read addr blk# cnt - read `cnt' blocks starting at block `blk#'\n"
-	"     to memory address `addr'\n"
-	"scsi write addr blk# cnt - write `cnt' blocks starting at block\n"
-	"     `blk#' from memory address `addr'"
-);
-
-U_BOOT_CMD(
-	scsiboot, 3, 1, do_scsiboot,
-	"boot from SCSI device",
-	"loadAddr dev:part"
-);
-#endif
-
-/****************************************************************************************
- * scsi_read
- */
-
-/* almost the maximum amount of the scsi_ext command.. */
-#define SCSI_MAX_READ_BLK 0xFFFF
-#define SCSI_LBA48_READ	0xFFFFFFF
-
-static ulong scsi_read(struct blk_desc *block_dev, lbaint_t blknr,
-		       lbaint_t blkcnt, void *buffer)
-{
-	int device = block_dev->devnum;
-	lbaint_t start, blks;
-	uintptr_t buf_addr;
-	unsigned short smallblks = 0;
-	ccb* pccb=(ccb *)&tempccb;
-	device&=0xff;
-	/* Setup  device
-	 */
-	pccb->target=scsi_dev_desc[device].target;
-	pccb->lun=scsi_dev_desc[device].lun;
-	buf_addr=(unsigned long)buffer;
-	start=blknr;
-	blks=blkcnt;
-	debug("\nscsi_read: dev %d startblk " LBAF
-	      ", blccnt " LBAF " buffer %lx\n",
-	      device, start, blks, (unsigned long)buffer);
-	do {
-		pccb->pdata=(unsigned char *)buf_addr;
-#ifdef CONFIG_SYS_64BIT_LBA
-		if (start > SCSI_LBA48_READ) {
-			unsigned long blocks;
-			blocks = min_t(lbaint_t, blks, SCSI_MAX_READ_BLK);
-			pccb->datalen = scsi_dev_desc[device].blksz * blocks;
-			scsi_setup_read16(pccb, start, blocks);
-			start += blocks;
-			blks -= blocks;
-		} else
-#endif
-		if (blks > SCSI_MAX_READ_BLK) {
-			pccb->datalen=scsi_dev_desc[device].blksz * SCSI_MAX_READ_BLK;
-			smallblks=SCSI_MAX_READ_BLK;
-			scsi_setup_read_ext(pccb,start,smallblks);
-			start+=SCSI_MAX_READ_BLK;
-			blks-=SCSI_MAX_READ_BLK;
-		}
-		else {
-			pccb->datalen=scsi_dev_desc[device].blksz * blks;
-			smallblks=(unsigned short) blks;
-			scsi_setup_read_ext(pccb,start,smallblks);
-			start+=blks;
-			blks=0;
-		}
-		debug("scsi_read_ext: startblk " LBAF
-		      ", blccnt %x buffer %" PRIXPTR "\n",
-		      start, smallblks, buf_addr);
-		if (scsi_exec(pccb) != true) {
-			scsi_print_error(pccb);
-			blkcnt-=blks;
-			break;
-		}
-		buf_addr+=pccb->datalen;
-	} while(blks!=0);
-	debug("scsi_read_ext: end startblk " LBAF
-	      ", blccnt %x buffer %" PRIXPTR "\n", start, smallblks, buf_addr);
-	return(blkcnt);
-}
-
-/*******************************************************************************
- * scsi_write
- */
-
-/* Almost the maximum amount of the scsi_ext command.. */
-#define SCSI_MAX_WRITE_BLK 0xFFFF
-
-static ulong scsi_write(struct blk_desc *block_dev, lbaint_t blknr,
-			lbaint_t blkcnt, const void *buffer)
-{
-	int device = block_dev->devnum;
-	lbaint_t start, blks;
-	uintptr_t buf_addr;
-	unsigned short smallblks;
-	ccb* pccb = (ccb *)&tempccb;
-	device &= 0xff;
-	/* Setup  device
-	 */
-	pccb->target = scsi_dev_desc[device].target;
-	pccb->lun = scsi_dev_desc[device].lun;
-	buf_addr = (unsigned long)buffer;
-	start = blknr;
-	blks = blkcnt;
-	debug("\n%s: dev %d startblk " LBAF ", blccnt " LBAF " buffer %lx\n",
-	      __func__, device, start, blks, (unsigned long)buffer);
-	do {
-		pccb->pdata = (unsigned char *)buf_addr;
-		if (blks > SCSI_MAX_WRITE_BLK) {
-			pccb->datalen = (scsi_dev_desc[device].blksz *
-					 SCSI_MAX_WRITE_BLK);
-			smallblks = SCSI_MAX_WRITE_BLK;
-			scsi_setup_write_ext(pccb, start, smallblks);
-			start += SCSI_MAX_WRITE_BLK;
-			blks -= SCSI_MAX_WRITE_BLK;
-		} else {
-			pccb->datalen = scsi_dev_desc[device].blksz * blks;
-			smallblks = (unsigned short)blks;
-			scsi_setup_write_ext(pccb, start, smallblks);
-			start += blks;
-			blks = 0;
-		}
-		debug("%s: startblk " LBAF ", blccnt %x buffer %" PRIXPTR "\n",
-		      __func__, start, smallblks, buf_addr);
-		if (scsi_exec(pccb) != true) {
-			scsi_print_error(pccb);
-			blkcnt -= blks;
-			break;
-		}
-		buf_addr += pccb->datalen;
-	} while (blks != 0);
-	debug("%s: end startblk " LBAF ", blccnt %x buffer %" PRIXPTR "\n",
-	      __func__, start, smallblks, buf_addr);
-	return blkcnt;
+			if (strncmp(argv[1],"part",4) == 0) {
+				int dev = (int)simple_strtoul(argv[2], NULL, 10);
+				if(scsi_dev_desc[dev].type != DEV_TYPE_UNKNOWN) {
+					part_print(&scsi_dev_desc[dev]);
+				}
+				else {
+					printf ("\nSCSI device %d not available\n", dev);
+				}
+				return 1;
+			}
+			return CMD_RET_USAGE;
+    default:
+			/* at least 4 args */
+			if (strcmp(argv[1],"read") == 0) {
+				ulong addr = simple_strtoul(argv[2], NULL, 16);
+				ulong blk  = simple_strtoul(argv[3], NULL, 16);
+				ulong cnt  = simple_strtoul(argv[4], NULL, 16);
+				ulong n;
+				printf ("\nSCSI read: device %d block # %ld, count %ld ... ",
+						scsi_curr_dev, blk, cnt);
+				n = scsi_read(&scsi_dev_desc[scsi_curr_dev],
+					      blk, cnt, (ulong *)addr);
+				printf ("%ld blocks read: %s\n",n,(n==cnt) ? "OK" : "ERROR");
+				return 0;
+			} else if (strcmp(argv[1], "write") == 0) {
+				ulong addr = simple_strtoul(argv[2], NULL, 16);
+				ulong blk = simple_strtoul(argv[3], NULL, 16);
+				ulong cnt = simple_strtoul(argv[4], NULL, 16);
+				ulong n;
+				printf("\nSCSI write: device %d block # %ld, "
+				       "count %ld ... ",
+				       scsi_curr_dev, blk, cnt);
+				n = scsi_write(&scsi_dev_desc[scsi_curr_dev],
+					       blk, cnt, (ulong *)addr);
+				printf("%ld blocks written: %s\n", n,
+				       (n == cnt) ? "OK" : "ERROR");
+				return 0;
+			}
+	} /* switch */
+	return CMD_RET_USAGE;
 }
 
+U_BOOT_CMD(
+	scsi, 5, 1, do_scsi,
+	"SCSI sub-system",
+	"reset - reset SCSI controller\n"
+	"scsi info  - show available SCSI devices\n"
+	"scsi scan  - (re-)scan SCSI bus\n"
+	"scsi device [dev] - show or set current device\n"
+	"scsi part [dev] - print partition table of one or all SCSI devices\n"
+	"scsi read addr blk# cnt - read `cnt' blocks starting at block `blk#'\n"
+	"     to memory address `addr'\n"
+	"scsi write addr blk# cnt - write `cnt' blocks starting at block\n"
+	"     `blk#' from memory address `addr'"
+);
+
+U_BOOT_CMD(
+	scsiboot, 3, 1, do_scsiboot,
+	"boot from SCSI device",
+	"loadAddr dev:part"
+);
+#endif
+
 /* copy src to dest, skipping leading and trailing blanks
  * and null terminate the string
  */

@@ -630,105 +630,106 @@ void scsi_setup_test_unit_ready(ccb * pccb)
 	pccb->msgout[0]=SCSI_IDENTIFY; /* NOT USED */
 }
 
-#ifdef CONFIG_SYS_64BIT_LBA
-void scsi_setup_read16(ccb * pccb, lbaint_t start, unsigned long blocks)
-{
-	pccb->cmd[0] = SCSI_READ16;
-	pccb->cmd[1] = pccb->lun<<5;
-	pccb->cmd[2] = ((unsigned char) (start >> 56)) & 0xff;
-	pccb->cmd[3] = ((unsigned char) (start >> 48)) & 0xff;
-	pccb->cmd[4] = ((unsigned char) (start >> 40)) & 0xff;
-	pccb->cmd[5] = ((unsigned char) (start >> 32)) & 0xff;
-	pccb->cmd[6] = ((unsigned char) (start >> 24)) & 0xff;
-	pccb->cmd[7] = ((unsigned char) (start >> 16)) & 0xff;
-	pccb->cmd[8] = ((unsigned char) (start >> 8)) & 0xff;
-	pccb->cmd[9] = ((unsigned char) (start)) & 0xff;
-	pccb->cmd[10] = 0;
-	pccb->cmd[11] = ((unsigned char) (blocks >> 24)) & 0xff;
-	pccb->cmd[12] = ((unsigned char) (blocks >> 16)) & 0xff;
-	pccb->cmd[13] = ((unsigned char) (blocks >> 8)) & 0xff;
-	pccb->cmd[14] = (unsigned char) blocks & 0xff;
-	pccb->cmd[15] = 0;
-	pccb->cmdlen = 16;
-	pccb->msgout[0] = SCSI_IDENTIFY; /* NOT USED */
-	debug ("scsi_setup_read16: cmd: %02X %02X "
-	       "startblk %02X%02X%02X%02X%02X%02X%02X%02X "
-	       "blccnt %02X%02X%02X%02X\n",
-		pccb->cmd[0], pccb->cmd[1],
-		pccb->cmd[2], pccb->cmd[3], pccb->cmd[4], pccb->cmd[5],
-		pccb->cmd[6], pccb->cmd[7], pccb->cmd[8], pccb->cmd[9],
-		pccb->cmd[11], pccb->cmd[12], pccb->cmd[13], pccb->cmd[14]);
-}
-#endif
-
-void scsi_setup_read_ext(ccb * pccb, lbaint_t start, unsigned short blocks)
-{
-	pccb->cmd[0]=SCSI_READ10;
-	pccb->cmd[1]=pccb->lun<<5;
-	pccb->cmd[2]=((unsigned char) (start>>24))&0xff;
-	pccb->cmd[3]=((unsigned char) (start>>16))&0xff;
-	pccb->cmd[4]=((unsigned char) (start>>8))&0xff;
-	pccb->cmd[5]=((unsigned char) (start))&0xff;
-	pccb->cmd[6]=0;
-	pccb->cmd[7]=((unsigned char) (blocks>>8))&0xff;
-	pccb->cmd[8]=(unsigned char) blocks & 0xff;
-	pccb->cmd[6]=0;
-	pccb->cmdlen=10;
-	pccb->msgout[0]=SCSI_IDENTIFY; /* NOT USED */
-	debug ("scsi_setup_read_ext: cmd: %02X %02X startblk %02X%02X%02X%02X blccnt %02X%02X\n",
-		pccb->cmd[0],pccb->cmd[1],
-		pccb->cmd[2],pccb->cmd[3],pccb->cmd[4],pccb->cmd[5],
-		pccb->cmd[7],pccb->cmd[8]);
-}
-
-void scsi_setup_write_ext(ccb *pccb, lbaint_t start, unsigned short blocks)
-{
-	pccb->cmd[0] = SCSI_WRITE10;
-	pccb->cmd[1] = pccb->lun << 5;
-	pccb->cmd[2] = ((unsigned char) (start>>24)) & 0xff;
-	pccb->cmd[3] = ((unsigned char) (start>>16)) & 0xff;
-	pccb->cmd[4] = ((unsigned char) (start>>8)) & 0xff;
-	pccb->cmd[5] = ((unsigned char) (start)) & 0xff;
-	pccb->cmd[6] = 0;
-	pccb->cmd[7] = ((unsigned char) (blocks>>8)) & 0xff;
-	pccb->cmd[8] = (unsigned char)blocks & 0xff;
-	pccb->cmd[9] = 0;
-	pccb->cmdlen = 10;
-	pccb->msgout[0] = SCSI_IDENTIFY;  /* NOT USED */
-	debug("%s: cmd: %02X %02X startblk %02X%02X%02X%02X blccnt %02X%02X\n",
-	      __func__,
-	      pccb->cmd[0], pccb->cmd[1],
-	      pccb->cmd[2], pccb->cmd[3], pccb->cmd[4], pccb->cmd[5],
-	      pccb->cmd[7], pccb->cmd[8]);
-}
-
-void scsi_setup_read6(ccb * pccb, lbaint_t start, unsigned short blocks)
+/*********************************************************************************
+ * (re)-scan the scsi bus and reports scsi device info
+ * to the user if mode = 1
+ */
+void scsi_scan(int mode)
 {
-	pccb->cmd[0]=SCSI_READ6;
-	pccb->cmd[1]=pccb->lun<<5 | (((unsigned char)(start>>16))&0x1f);
-	pccb->cmd[2]=((unsigned char) (start>>8))&0xff;
-	pccb->cmd[3]=((unsigned char) (start))&0xff;
-	pccb->cmd[4]=(unsigned char) blocks & 0xff;
-	pccb->cmd[5]=0;
-	pccb->cmdlen=6;
-	pccb->msgout[0]=SCSI_IDENTIFY; /* NOT USED */
-	debug ("scsi_setup_read6: cmd: %02X %02X startblk %02X%02X blccnt %02X\n",
-		pccb->cmd[0],pccb->cmd[1],
-		pccb->cmd[2],pccb->cmd[3],pccb->cmd[4]);
-}
+	unsigned char i,perq,modi,lun;
+	lbaint_t capacity;
+	unsigned long blksz;
+	ccb* pccb=(ccb *)&tempccb;
 
+	if(mode==1) {
+		printf("scanning bus for devices...\n");
+	}
+	for(i=0;i<CONFIG_SYS_SCSI_MAX_DEVICE;i++) {
+		scsi_dev_desc[i].target=0xff;
+		scsi_dev_desc[i].lun=0xff;
+		scsi_dev_desc[i].lba=0;
+		scsi_dev_desc[i].blksz=0;
+		scsi_dev_desc[i].log2blksz =
+			LOG2_INVALID(typeof(scsi_dev_desc[i].log2blksz));
+		scsi_dev_desc[i].type=DEV_TYPE_UNKNOWN;
+		scsi_dev_desc[i].vendor[0]=0;
+		scsi_dev_desc[i].product[0]=0;
+		scsi_dev_desc[i].revision[0]=0;
+		scsi_dev_desc[i].removable = false;
+		scsi_dev_desc[i].if_type=IF_TYPE_SCSI;
+		scsi_dev_desc[i].devnum = i;
+		scsi_dev_desc[i].part_type=PART_TYPE_UNKNOWN;
+		scsi_dev_desc[i].block_read=scsi_read;
+		scsi_dev_desc[i].block_write = scsi_write;
+	}
+	scsi_max_devs=0;
+	for(i=0;i<CONFIG_SYS_SCSI_MAX_SCSI_ID;i++) {
+		pccb->target=i;
+		for(lun=0;lun<CONFIG_SYS_SCSI_MAX_LUN;lun++) {
+			pccb->lun=lun;
+			pccb->pdata=(unsigned char *)&tempbuff;
+			pccb->datalen=512;
+			scsi_setup_inquiry(pccb);
+			if (scsi_exec(pccb) != true) {
+				if(pccb->contr_stat==SCSI_SEL_TIME_OUT) {
+					debug ("Selection timeout ID %d\n",pccb->target);
+					continue; /* selection timeout => assuming no device present */
+				}
+				scsi_print_error(pccb);
+				continue;
+			}
+			perq=tempbuff[0];
+			modi=tempbuff[1];
+			if((perq & 0x1f)==0x1f) {
+				continue; /* skip unknown devices */
+			}
+			if((modi&0x80)==0x80) /* drive is removable */
+				scsi_dev_desc[scsi_max_devs].removable=true;
+			/* get info for this device */
+			scsi_ident_cpy((unsigned char *)&scsi_dev_desc[scsi_max_devs].vendor[0],
+				       &tempbuff[8], 8);
+			scsi_ident_cpy((unsigned char *)&scsi_dev_desc[scsi_max_devs].product[0],
+				       &tempbuff[16], 16);
+			scsi_ident_cpy((unsigned char *)&scsi_dev_desc[scsi_max_devs].revision[0],
+				       &tempbuff[32], 4);
+			scsi_dev_desc[scsi_max_devs].target=pccb->target;
+			scsi_dev_desc[scsi_max_devs].lun=pccb->lun;
 
-void scsi_setup_inquiry(ccb * pccb)
-{
-	pccb->cmd[0]=SCSI_INQUIRY;
-	pccb->cmd[1]=pccb->lun<<5;
-	pccb->cmd[2]=0;
-	pccb->cmd[3]=0;
-	if(pccb->datalen>255)
-		pccb->cmd[4]=255;
+			pccb->datalen=0;
+			scsi_setup_test_unit_ready(pccb);
+			if (scsi_exec(pccb) != true) {
+				if (scsi_dev_desc[scsi_max_devs].removable == true) {
+					scsi_dev_desc[scsi_max_devs].type=perq;
+					goto removable;
+				}
+				scsi_print_error(pccb);
+				continue;
+			}
+			if (scsi_read_capacity(pccb, &capacity, &blksz)) {
+				scsi_print_error(pccb);
+				continue;
+			}
+			scsi_dev_desc[scsi_max_devs].lba=capacity;
+			scsi_dev_desc[scsi_max_devs].blksz=blksz;
+			scsi_dev_desc[scsi_max_devs].log2blksz =
+				LOG2(scsi_dev_desc[scsi_max_devs].blksz);
+			scsi_dev_desc[scsi_max_devs].type=perq;
+			part_init(&scsi_dev_desc[scsi_max_devs]);
+removable:
+			if(mode==1) {
+				printf ("  Device %d: ", scsi_max_devs);
+				dev_print(&scsi_dev_desc[scsi_max_devs]);
+			} /* if mode */
+			scsi_max_devs++;
+		} /* next LUN */
+	}
+	if(scsi_max_devs>0)
+		scsi_curr_dev=0;
 	else
-		pccb->cmd[4]=(unsigned char)pccb->datalen;
-	pccb->cmd[5]=0;
-	pccb->cmdlen=6;
-	pccb->msgout[0]=SCSI_IDENTIFY; /* NOT USED */
+		scsi_curr_dev = -1;
+
+	printf("Found %d device(s).\n", scsi_max_devs);
+#ifndef CONFIG_SPL_BUILD
+	setenv_ulong("scsidevs", scsi_max_devs);
+#endif
 }

index f875f51..751fc58 100644
@@ -50,82 +50,53 @@ static int scsi_curr_dev; /* current device */
 
 static struct blk_desc scsi_dev_desc[CONFIG_SYS_SCSI_MAX_DEVICE];
 
-#if 0
-/********************************************************************************
- *  forward declerations of some Setup Routines
- */
-void scsi_setup_test_unit_ready(ccb * pccb);
-void scsi_setup_read6(ccb * pccb, lbaint_t start, unsigned short blocks);
-void scsi_setup_read_ext(ccb * pccb, lbaint_t start, unsigned short blocks);
-void scsi_setup_read16(ccb * pccb, lbaint_t start, unsigned long blocks);
-
-static void scsi_setup_write_ext(ccb *pccb, lbaint_t start,
-				unsigned short blocks);
-void scsi_setup_inquiry(ccb * pccb);
-void scsi_ident_cpy (unsigned char *dest, unsigned char *src, unsigned int len);
-
-
-static int scsi_read_capacity(ccb *pccb, lbaint_t *capacity,
-			      unsigned long *blksz);
-static ulong scsi_read(struct blk_desc *block_dev, lbaint_t blknr,
-		       lbaint_t blkcnt, void *buffer);
-static ulong scsi_write(struct blk_desc *block_dev, lbaint_t blknr,
-			lbaint_t blkcnt, const void *buffer);
-#endif
-
-/****************************************************************************************
- * scsi_read
- */
-
 /* almost the maximum amount of the scsi_ext command.. */
 #define SCSI_MAX_READ_BLK 0xFFFF
 #define SCSI_LBA48_READ	0xFFFFFFF
 
 #ifdef CONFIG_SYS_64BIT_LBA
-void scsi_setup_read16(ccb * pccb, lbaint_t start, unsigned long blocks)
+void scsi_setup_read16(ccb *pccb, lbaint_t start, unsigned long blocks)
 {
 	pccb->cmd[0] = SCSI_READ16;
-	pccb->cmd[1] = pccb->lun<<5;
-	pccb->cmd[2] = ((unsigned char) (start >> 56)) & 0xff;
-	pccb->cmd[3] = ((unsigned char) (start >> 48)) & 0xff;
-	pccb->cmd[4] = ((unsigned char) (start >> 40)) & 0xff;
-	pccb->cmd[5] = ((unsigned char) (start >> 32)) & 0xff;
-	pccb->cmd[6] = ((unsigned char) (start >> 24)) & 0xff;
-	pccb->cmd[7] = ((unsigned char) (start >> 16)) & 0xff;
-	pccb->cmd[8] = ((unsigned char) (start >> 8)) & 0xff;
-	pccb->cmd[9] = ((unsigned char) (start)) & 0xff;
+	pccb->cmd[1] = pccb->lun << 5;
+	pccb->cmd[2] = (unsigned char)(start >> 56) & 0xff;
+	pccb->cmd[3] = (unsigned char)(start >> 48) & 0xff;
+	pccb->cmd[4] = (unsigned char)(start >> 40) & 0xff;
+	pccb->cmd[5] = (unsigned char)(start >> 32) & 0xff;
+	pccb->cmd[6] = (unsigned char)(start >> 24) & 0xff;
+	pccb->cmd[7] = (unsigned char)(start >> 16) & 0xff;
+	pccb->cmd[8] = (unsigned char)(start >> 8) & 0xff;
+	pccb->cmd[9] = (unsigned char)start & 0xff;
 	pccb->cmd[10] = 0;
-	pccb->cmd[11] = ((unsigned char) (blocks >> 24)) & 0xff;
-	pccb->cmd[12] = ((unsigned char) (blocks >> 16)) & 0xff;
-	pccb->cmd[13] = ((unsigned char) (blocks >> 8)) & 0xff;
-	pccb->cmd[14] = (unsigned char) blocks & 0xff;
+	pccb->cmd[11] = (unsigned char)(blocks >> 24) & 0xff;
+	pccb->cmd[12] = (unsigned char)(blocks >> 16) & 0xff;
+	pccb->cmd[13] = (unsigned char)(blocks >> 8) & 0xff;
+	pccb->cmd[14] = (unsigned char)blocks & 0xff;
 	pccb->cmd[15] = 0;
 	pccb->cmdlen = 16;
 	pccb->msgout[0] = SCSI_IDENTIFY; /* NOT USED */
-	debug ("scsi_setup_read16: cmd: %02X %02X "
-	       "startblk %02X%02X%02X%02X%02X%02X%02X%02X "
-	       "blccnt %02X%02X%02X%02X\n",
-		pccb->cmd[0], pccb->cmd[1],
-		pccb->cmd[2], pccb->cmd[3], pccb->cmd[4], pccb->cmd[5],
-		pccb->cmd[6], pccb->cmd[7], pccb->cmd[8], pccb->cmd[9],
-		pccb->cmd[11], pccb->cmd[12], pccb->cmd[13], pccb->cmd[14]);
+	debug("scsi_setup_read16: cmd: %02X %02X startblk %02X%02X%02X%02X%02X%02X%02X%02X blccnt %02X%02X%02X%02X\n",
+	      pccb->cmd[0], pccb->cmd[1],
+	      pccb->cmd[2], pccb->cmd[3], pccb->cmd[4], pccb->cmd[5],
+	      pccb->cmd[6], pccb->cmd[7], pccb->cmd[8], pccb->cmd[9],
+	      pccb->cmd[11], pccb->cmd[12], pccb->cmd[13], pccb->cmd[14]);
 }
 #endif
 
 void scsi_setup_read_ext(ccb * pccb, lbaint_t start, unsigned short blocks)
 {
-	pccb->cmd[0]=SCSI_READ10;
-	pccb->cmd[1]=pccb->lun<<5;
-	pccb->cmd[2]=((unsigned char) (start>>24))&0xff;
-	pccb->cmd[3]=((unsigned char) (start>>16))&0xff;
-	pccb->cmd[4]=((unsigned char) (start>>8))&0xff;
-	pccb->cmd[5]=((unsigned char) (start))&0xff;
-	pccb->cmd[6]=0;
-	pccb->cmd[7]=((unsigned char) (blocks>>8))&0xff;
-	pccb->cmd[8]=(unsigned char) blocks & 0xff;
-	pccb->cmd[6]=0;
+	pccb->cmd[0] = SCSI_READ10;
+	pccb->cmd[1] = pccb->lun << 5;
+	pccb->cmd[2] = (unsigned char)(start >> 24) & 0xff;
+	pccb->cmd[3] = (unsigned char)(start >> 16) & 0xff;
+	pccb->cmd[4] = (unsigned char)(start >> 8) & 0xff;
+	pccb->cmd[5] = (unsigned char)start & 0xff;
+	pccb->cmd[6] = 0;
+	pccb->cmd[7] = (unsigned char)(blocks >> 8) & 0xff;
+	pccb->cmd[8] = (unsigned char)blocks & 0xff;
+	pccb->cmd[6] = 0;
 	pccb->cmdlen=10;
-	pccb->msgout[0]=SCSI_IDENTIFY; /* NOT USED */
+	pccb->msgout[0] = SCSI_IDENTIFY; /* NOT USED */
 	debug ("scsi_setup_read_ext: cmd: %02X %02X startblk %02X%02X%02X%02X blccnt %02X%02X\n",
 		pccb->cmd[0],pccb->cmd[1],
 		pccb->cmd[2],pccb->cmd[3],pccb->cmd[4],pccb->cmd[5],

@@ -136,12 +107,12 @@ void scsi_setup_write_ext(ccb *pccb, lbaint_t start, unsigned short blocks)
 {
 	pccb->cmd[0] = SCSI_WRITE10;
 	pccb->cmd[1] = pccb->lun << 5;
-	pccb->cmd[2] = ((unsigned char) (start>>24)) & 0xff;
-	pccb->cmd[3] = ((unsigned char) (start>>16)) & 0xff;
-	pccb->cmd[4] = ((unsigned char) (start>>8)) & 0xff;
-	pccb->cmd[5] = ((unsigned char) (start)) & 0xff;
+	pccb->cmd[2] = (unsigned char)(start >> 24) & 0xff;
+	pccb->cmd[3] = (unsigned char)(start >> 16) & 0xff;
+	pccb->cmd[4] = (unsigned char)(start >> 8) & 0xff;
+	pccb->cmd[5] = (unsigned char)start & 0xff;
 	pccb->cmd[6] = 0;
-	pccb->cmd[7] = ((unsigned char) (blocks>>8)) & 0xff;
+	pccb->cmd[7] = ((unsigned char)(blocks >> 8)) & 0xff;
 	pccb->cmd[8] = (unsigned char)blocks & 0xff;
 	pccb->cmd[9] = 0;
 	pccb->cmdlen = 10;

@@ -155,33 +126,33 @@ void scsi_setup_write_ext(ccb *pccb, lbaint_t start, unsigned short blocks)
 
 void scsi_setup_read6(ccb * pccb, lbaint_t start, unsigned short blocks)
 {
-	pccb->cmd[0]=SCSI_READ6;
-	pccb->cmd[1]=pccb->lun<<5 | (((unsigned char)(start>>16))&0x1f);
-	pccb->cmd[2]=((unsigned char) (start>>8))&0xff;
-	pccb->cmd[3]=((unsigned char) (start))&0xff;
-	pccb->cmd[4]=(unsigned char) blocks & 0xff;
-	pccb->cmd[5]=0;
+	pccb->cmd[0] = SCSI_READ6;
+	pccb->cmd[1] = pccb->lun << 5 | ((unsigned char)(start >> 16) & 0x1f);
+	pccb->cmd[2] = (unsigned char)(start >> 8) & 0xff;
+	pccb->cmd[3] = (unsigned char)start & 0xff;
+	pccb->cmd[4] = (unsigned char)blocks & 0xff;
+	pccb->cmd[5] = 0;
 	pccb->cmdlen=6;
-	pccb->msgout[0]=SCSI_IDENTIFY; /* NOT USED */
-	debug ("scsi_setup_read6: cmd: %02X %02X startblk %02X%02X blccnt %02X\n",
-		pccb->cmd[0],pccb->cmd[1],
-		pccb->cmd[2],pccb->cmd[3],pccb->cmd[4]);
+	pccb->msgout[0] = SCSI_IDENTIFY; /* NOT USED */
+	debug("scsi_setup_read6: cmd: %02X %02X startblk %02X%02X blccnt %02X\n",
+	      pccb->cmd[0], pccb->cmd[1],
+	      pccb->cmd[2], pccb->cmd[3], pccb->cmd[4]);
 }
 
 
 void scsi_setup_inquiry(ccb * pccb)
 {
-	pccb->cmd[0]=SCSI_INQUIRY;
-	pccb->cmd[1]=pccb->lun<<5;
-	pccb->cmd[2]=0;
-	pccb->cmd[3]=0;
-	if(pccb->datalen>255)
-		pccb->cmd[4]=255;
+	pccb->cmd[0] = SCSI_INQUIRY;
+	pccb->cmd[1] = pccb->lun << 5;
+	pccb->cmd[2] = 0;
+	pccb->cmd[3] = 0;
+	if (pccb->datalen > 255)
+		pccb->cmd[4] = 255;
 	else
-		pccb->cmd[4]=(unsigned char)pccb->datalen;
-	pccb->cmd[5]=0;
+		pccb->cmd[4] = (unsigned char)pccb->datalen;
+	pccb->cmd[5] = 0;
 	pccb->cmdlen=6;
-	pccb->msgout[0]=SCSI_IDENTIFY; /* NOT USED */
+	pccb->msgout[0] = SCSI_IDENTIFY; /* NOT USED */
 }
 
 static ulong scsi_read(struct blk_desc *block_dev, lbaint_t blknr,

@@ -193,18 +164,18 @@ static ulong scsi_read(struct blk_desc *block_dev, lbaint_t blknr,
 	unsigned short smallblks = 0;
 	ccb* pccb=(ccb *)&tempccb;
 	device&=0xff;
-	/* Setup  device
-	 */
-	pccb->target=scsi_dev_desc[device].target;
-	pccb->lun=scsi_dev_desc[device].lun;
-	buf_addr=(unsigned long)buffer;
-	start=blknr;
-	blks=blkcnt;
+
+	/* Setup device */
+	pccb->target = scsi_dev_desc[device].target;
+	pccb->lun = scsi_dev_desc[device].lun;
+	buf_addr = (unsigned long)buffer;
+	start = blknr;
+	blks = blkcnt;
 	debug("\nscsi_read: dev %d startblk " LBAF
 	      ", blccnt " LBAF " buffer %lx\n",
 	      device, start, blks, (unsigned long)buffer);
 	do {
-		pccb->pdata=(unsigned char *)buf_addr;
+		pccb->pdata = (unsigned char *)buf_addr;
 #ifdef CONFIG_SYS_64BIT_LBA
 		if (start > SCSI_LBA48_READ) {
 			unsigned long blocks;

@@ -216,17 +187,18 @@ static ulong scsi_read(struct blk_desc *block_dev, lbaint_t blknr,
 		} else
 #endif
 		if (blks > SCSI_MAX_READ_BLK) {
-			pccb->datalen=scsi_dev_desc[device].blksz * SCSI_MAX_READ_BLK;
-			smallblks=SCSI_MAX_READ_BLK;
-			scsi_setup_read_ext(pccb,start,smallblks);
-			start+=SCSI_MAX_READ_BLK;
-			blks-=SCSI_MAX_READ_BLK;
+			pccb->datalen = scsi_dev_desc[device].blksz *
+				SCSI_MAX_READ_BLK;
+			smallblks = SCSI_MAX_READ_BLK;
+			scsi_setup_read_ext(pccb, start, smallblks);
+			start += SCSI_MAX_READ_BLK;
+			blks -= SCSI_MAX_READ_BLK;
 		}
 		else {
-			pccb->datalen=scsi_dev_desc[device].blksz * blks;
-			smallblks=(unsigned short) blks;
-			scsi_setup_read_ext(pccb,start,smallblks);
-			start+=blks;
+			pccb->datalen = scsi_dev_desc[device].blksz * blks;
+			smallblks = (unsigned short)blks;
+			scsi_setup_read_ext(pccb, start, smallblks);
+			start += blks;
 			blks=0;
 		}
 		debug("scsi_read_ext: startblk " LBAF

@@ -234,14 +206,14 @@ static ulong scsi_read(struct blk_desc *block_dev, lbaint_t blknr,
 		      start, smallblks, buf_addr);
 		if (scsi_exec(pccb) != true) {
 			scsi_print_error(pccb);
-			blkcnt-=blks;
+			blkcnt -= blks;
 			break;
 		}
 		buf_addr+=pccb->datalen;
-	} while(blks!=0);
+	} while (blks != 0);
 	debug("scsi_read_ext: end startblk " LBAF
 	      ", blccnt %x buffer %" PRIXPTR "\n", start, smallblks, buf_addr);
-	return(blkcnt);
+	return blkcnt;
 }
 
 /*******************************************************************************

@@ -388,106 +360,106 @@ int do_scsi (cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
 		return CMD_RET_USAGE;
 
 	case 2:
-			if (strncmp(argv[1],"res",3) == 0) {
-				printf("\nReset SCSI\n");
-				scsi_bus_reset();
-				scsi_scan(1);
-				return 0;
-			}
-			if (strncmp(argv[1],"inf",3) == 0) {
-				int i;
-				for (i=0; i<CONFIG_SYS_SCSI_MAX_DEVICE; ++i) {
-					if(scsi_dev_desc[i].type==DEV_TYPE_UNKNOWN)
-						continue; /* list only known devices */
-					printf ("SCSI dev. %d:  ", i);
-					dev_print(&scsi_dev_desc[i]);
-				}
-				return 0;
-			}
-			if (strncmp(argv[1],"dev",3) == 0) {
-				if ((scsi_curr_dev < 0) || (scsi_curr_dev >= CONFIG_SYS_SCSI_MAX_DEVICE)) {
-					printf("\nno SCSI devices available\n");
-					return 1;
-				}
-				printf ("\n    Device %d: ", scsi_curr_dev);
-				dev_print(&scsi_dev_desc[scsi_curr_dev]);
-				return 0;
-			}
-			if (strncmp(argv[1],"scan",4) == 0) {
-				scsi_scan(1);
-				return 0;
+		if (strncmp(argv[1], "res", 3) == 0) {
+			printf("\nReset SCSI\n");
+			scsi_bus_reset();
+			scsi_scan(1);
+			return 0;
+		}
+		if (strncmp(argv[1], "inf", 3) == 0) {
+			int i;
+			for (i = 0; i < CONFIG_SYS_SCSI_MAX_DEVICE; ++i) {
+				if (scsi_dev_desc[i].type == DEV_TYPE_UNKNOWN)
+					continue; /* list only known devices */
+				printf("SCSI dev. %d:  ", i);
+				dev_print(&scsi_dev_desc[i]);
 			}
-			if (strncmp(argv[1],"part",4) == 0) {
-				int dev, ok;
-				for (ok=0, dev=0; dev<CONFIG_SYS_SCSI_MAX_DEVICE; ++dev) {
-					if (scsi_dev_desc[dev].type!=DEV_TYPE_UNKNOWN) {
-						ok++;
-						if (dev)
-							printf("\n");
-						debug ("print_part of %x\n",dev);
-						part_print(&scsi_dev_desc[dev]);
-					}
-				}
-				if (!ok)
-					printf("\nno SCSI devices available\n");
+			return 0;
+		}
+		if (strncmp(argv[1], "dev", 3) == 0) {
+			if (scsi_curr_dev < 0 ||
+			    scsi_curr_dev >= CONFIG_SYS_SCSI_MAX_DEVICE) {
+				printf("\nno SCSI devices available\n");
 				return 1;
 			}
-			return CMD_RET_USAGE;
-	case 3:
-			if (strncmp(argv[1],"dev",3) == 0) {
-				int dev = (int)simple_strtoul(argv[2], NULL, 10);
-				printf ("\nSCSI device %d: ", dev);
-				if (dev >= CONFIG_SYS_SCSI_MAX_DEVICE) {
-					printf("unknown device\n");
-					return 1;
-				}
-				printf ("\n    Device %d: ", dev);
-				dev_print(&scsi_dev_desc[dev]);
-				if(scsi_dev_desc[dev].type == DEV_TYPE_UNKNOWN) {
-					return 1;
-				}
-				scsi_curr_dev = dev;
-				printf("... is now current device\n");
-				return 0;
-			}
-			if (strncmp(argv[1],"part",4) == 0) {
-				int dev = (int)simple_strtoul(argv[2], NULL, 10);
-				if(scsi_dev_desc[dev].type != DEV_TYPE_UNKNOWN) {
+			printf("\n    Device %d: ", scsi_curr_dev);
+			dev_print(&scsi_dev_desc[scsi_curr_dev]);
+			return 0;
+		}
+		if (strncmp(argv[1], "scan", 4) == 0) {
+			scsi_scan(1);
+			return 0;
+		}
+		if (strncmp(argv[1], "part", 4) == 0) {
+			int dev, ok;
+			for (ok = 0, dev = 0;
+			     dev < CONFIG_SYS_SCSI_MAX_DEVICE; ++dev) {
+				if (scsi_dev_desc[dev].type !=
+				    DEV_TYPE_UNKNOWN) {
+					ok++;
+					if (dev)
+						printf("\n");
+					debug("print_part of %x\n", dev);
 					part_print(&scsi_dev_desc[dev]);
 				}
-				else {
-					printf ("\nSCSI device %d not available\n", dev);
-				}
-				return 1;
 			}
-			return CMD_RET_USAGE;
-    default:
-			/*@least 4 args */
-			if (strcmp(argv[1],"read") == 0) {
-				ulong addr = simple_strtoul(argv[2], NULL, 16);
-				ulong blk  = simple_strtoul(argv[3], NULL, 16);
-				ulong cnt  = simple_strtoul(argv[4], NULL, 16);
-				ulong n;
-				printf ("\nSCSI read: device %d block # %ld, count %ld ... ",
-						scsi_curr_dev, blk, cnt);
-				n = scsi_read(&scsi_dev_desc[scsi_curr_dev],
-					      blk, cnt, (ulong *)addr);
-				printf ("%ld blocks read: %s\n",n,(n==cnt) ? "OK" : "ERROR");
-				return 0;
-			} else if (strcmp(argv[1], "write") == 0) {
-				ulong addr = simple_strtoul(argv[2], NULL, 16);
-				ulong blk = simple_strtoul(argv[3], NULL, 16);
-				ulong cnt = simple_strtoul(argv[4], NULL, 16);
-				ulong n;
-				printf("\nSCSI write: device %d block # %ld, "
-				       "count %ld ... ",
-				       scsi_curr_dev, blk, cnt);
-				n = scsi_write(&scsi_dev_desc[scsi_curr_dev],
-					       blk, cnt, (ulong *)addr);
-				printf("%ld blocks written: %s\n", n,
-				       (n == cnt) ? "OK" : "ERROR");
-				return 0;
+			if (!ok)
+				printf("\nno SCSI devices available\n");
+			return 1;
+		}
+		return CMD_RET_USAGE;
+	case 3:
+		if (strncmp(argv[1], "dev", 3) == 0) {
+			int dev = (int)simple_strtoul(argv[2], NULL, 10);
+			printf("\nSCSI device %d: ", dev);
+			if (dev >= CONFIG_SYS_SCSI_MAX_DEVICE) {
+				printf("unknown device\n");
+				return 1;
 			}
+			printf("\n    Device %d: ", dev);
+			dev_print(&scsi_dev_desc[dev]);
+			if (scsi_dev_desc[dev].type == DEV_TYPE_UNKNOWN)
+				return 1;
+			scsi_curr_dev = dev;
+			printf("... is now current device\n");
+			return 0;
+		}
+		if (strncmp(argv[1], "part", 4) == 0) {
+			int dev = (int)simple_strtoul(argv[2], NULL, 10);
+			if (scsi_dev_desc[dev].type != DEV_TYPE_UNKNOWN)
+				part_print(&scsi_dev_desc[dev]);
+			else
+				printf("\nSCSI device %d not available\n", dev);
+			return 1;
+		}
+		return CMD_RET_USAGE;
+	default:
+		/* at least 4 args */
+		if (strcmp(argv[1], "read") == 0) {
+			ulong addr = simple_strtoul(argv[2], NULL, 16);
+			ulong blk  = simple_strtoul(argv[3], NULL, 16);
+			ulong cnt  = simple_strtoul(argv[4], NULL, 16);
+			ulong n;
+			printf("\nSCSI read: device %d block # %ld, count %ld ... ",
+			       scsi_curr_dev, blk, cnt);
+			n = scsi_read(&scsi_dev_desc[scsi_curr_dev],
+				      blk, cnt, (ulong *)addr);
+			printf("%ld blocks read: %s\n", n,
+			       n == cnt ? "OK" : "ERROR");
+			return 0;
+		} else if (strcmp(argv[1], "write") == 0) {
+			ulong addr = simple_strtoul(argv[2], NULL, 16);
+			ulong blk = simple_strtoul(argv[3], NULL, 16);
+			ulong cnt = simple_strtoul(argv[4], NULL, 16);
+			ulong n;
+			printf("\nSCSI write: device %d block # %ld, count %ld ... ",
+			       scsi_curr_dev, blk, cnt);
+			n = scsi_write(&scsi_dev_desc[scsi_curr_dev],
+				       blk, cnt, (ulong *)addr);
+			printf("%ld blocks written: %s\n", n,
+			       n == cnt ? "OK" : "ERROR");
+			return 0;
+		}
 	} /* switch */
 	return CMD_RET_USAGE;
 }

@@ -516,25 +488,24 @@ U_BOOT_CMD(
 /* copy src to dest, skipping leading and trailing blanks
  * and null terminate the string
  */
-void scsi_ident_cpy (unsigned char *dest, unsigned char *src, unsigned int len)
+void scsi_ident_cpy(unsigned char *dest, unsigned char *src, unsigned int len)
 {
 	int start,end;
 
-	start=0;
-	while(start<len) {
-		if(src[start]!=' ')
+	start = 0;
+	while (start < len) {
+		if (src[start] != ' ')
 			break;
 		start++;
 	}
-	end=len-1;
-	while(end>start) {
-		if(src[end]!=' ')
+	end = len-1;
+	while (end > start) {
+		if (src[end] != ' ')
 			break;
 		end--;
 	}
-	for( ; start<=end; start++) {
-		*dest++=src[start];
-	}
+	for (; start <= end; start++)
+		*dest ++= src[start];
 	*dest='\0';
 }
 

@@ -557,7 +528,7 @@ int scsi_read_capacity(ccb *pccb, lbaint_t *capacity, unsigned long *blksz)
 {
 	*capacity = 0;
 
-	memset(pccb->cmd, 0, sizeof(pccb->cmd));
+	memset(pccb->cmd, '\0', sizeof(pccb->cmd));
 	pccb->cmd[0] = SCSI_RD_CAPAC10;
 	pccb->cmd[1] = pccb->lun << 5;
 	pccb->cmdlen = 10;

@@ -582,8 +553,7 @@ int scsi_read_capacity(ccb *pccb, lbaint_t *capacity, unsigned long *blksz)
 	}
 
 	/* Read capacity (10) was insufficient. Use read capacity (16). */
-
-	memset(pccb->cmd, 0, sizeof(pccb->cmd));
+	memset(pccb->cmd, '\0', sizeof(pccb->cmd));
 	pccb->cmd[0] = SCSI_RD_CAPAC16;
 	pccb->cmd[1] = 0x10;
 	pccb->cmdlen = 16;

@@ -620,14 +590,14 @@ int scsi_read_capacity(ccb *pccb, lbaint_t *capacity, unsigned long *blksz)
  */
 void scsi_setup_test_unit_ready(ccb * pccb)
 {
-	pccb->cmd[0]=SCSI_TST_U_RDY;
-	pccb->cmd[1]=pccb->lun<<5;
-	pccb->cmd[2]=0;
-	pccb->cmd[3]=0;
-	pccb->cmd[4]=0;
-	pccb->cmd[5]=0;
-	pccb->cmdlen=6;
-	pccb->msgout[0]=SCSI_IDENTIFY; /* NOT USED */
+	pccb->cmd[0] = SCSI_TST_U_RDY;
+	pccb->cmd[1] = pccb->lun << 5;
+	pccb->cmd[2] = 0;
+	pccb->cmd[3] = 0;
+	pccb->cmd[4] = 0;
+	pccb->cmd[5] = 0;
+	pccb->cmdlen = 6;
+	pccb->msgout[0] = SCSI_IDENTIFY; /* NOT USED */
 }
 
 /*********************************************************************************

@@ -641,50 +611,49 @@ void scsi_scan(int mode)
 	unsigned long blksz;
 	ccb* pccb=(ccb *)&tempccb;
 
-	if(mode==1) {
+	if (mode == 1)
 		printf("scanning bus for devices...\n");
-	}
-	for(i=0;i<CONFIG_SYS_SCSI_MAX_DEVICE;i++) {
-		scsi_dev_desc[i].target=0xff;
-		scsi_dev_desc[i].lun=0xff;
-		scsi_dev_desc[i].lba=0;
-		scsi_dev_desc[i].blksz=0;
-		scsi_dev_desc[i].log2blksz =
+	for (i = 0; i < CONFIG_SYS_SCSI_MAX_DEVICE; i++) {
+		scsi_dev_desc[i].target = 0xff;
+		scsi_dev_desc[i].lun = 0xff;
+		scsi_dev_desc[i].lba = 0;
+		scsi_dev_desc[i].blksz = 0;
+		scsi_dev_desc[i].log2blksz  =
 			LOG2_INVALID(typeof(scsi_dev_desc[i].log2blksz));
-		scsi_dev_desc[i].type=DEV_TYPE_UNKNOWN;
-		scsi_dev_desc[i].vendor[0]=0;
-		scsi_dev_desc[i].product[0]=0;
-		scsi_dev_desc[i].revision[0]=0;
+		scsi_dev_desc[i].type = DEV_TYPE_UNKNOWN;
+		scsi_dev_desc[i].vendor[0] = 0;
+		scsi_dev_desc[i].product[0] = 0;
+		scsi_dev_desc[i].revision[0] = 0;
 		scsi_dev_desc[i].removable = false;
-		scsi_dev_desc[i].if_type=IF_TYPE_SCSI;
+		scsi_dev_desc[i].if_type = IF_TYPE_SCSI;
 		scsi_dev_desc[i].devnum = i;
-		scsi_dev_desc[i].part_type=PART_TYPE_UNKNOWN;
-		scsi_dev_desc[i].block_read=scsi_read;
+		scsi_dev_desc[i].part_type = PART_TYPE_UNKNOWN;
+		scsi_dev_desc[i].block_read = scsi_read;
 		scsi_dev_desc[i].block_write = scsi_write;
 	}
-	scsi_max_devs=0;
-	for(i=0;i<CONFIG_SYS_SCSI_MAX_SCSI_ID;i++) {
-		pccb->target=i;
-		for(lun=0;lun<CONFIG_SYS_SCSI_MAX_LUN;lun++) {
-			pccb->lun=lun;
-			pccb->pdata=(unsigned char *)&tempbuff;
-			pccb->datalen=512;
+	scsi_max_devs = 0;
+	for (i = 0; i < CONFIG_SYS_SCSI_MAX_SCSI_ID; i++) {
+		pccb->target = i;
+		for (lun = 0; lun < CONFIG_SYS_SCSI_MAX_LUN; lun++) {
+			pccb->lun = lun;
+			pccb->pdata = (unsigned char *)&tempbuff;
+			pccb->datalen = 512;
 			scsi_setup_inquiry(pccb);
 			if (scsi_exec(pccb) != true) {
 				if(pccb->contr_stat==SCSI_SEL_TIME_OUT) {
-					debug ("Selection timeout ID %d\n",pccb->target);
+					debug("Selection timeout ID %d\n",
+					      pccb->target);
 					continue; /* selection timeout => assuming no device present */
 				}
 				scsi_print_error(pccb);
 				continue;
 			}
-			perq=tempbuff[0];
-			modi=tempbuff[1];
-			if((perq & 0x1f)==0x1f) {
+			perq = tempbuff[0];
+			modi = tempbuff[1];
+			if ((perq & 0x1f) == 0x1f)
 				continue; /* skip unknown devices */
-			}
-			if((modi&0x80)==0x80) /* drive is removable */
-				scsi_dev_desc[scsi_max_devs].removable=true;
+			if ((modi & 0x80) == 0x80) /* drive is removable */
+				scsi_dev_desc[scsi_max_devs].removable = true;
 			/* get info for this device */
 			scsi_ident_cpy((unsigned char *)&scsi_dev_desc[scsi_max_devs].vendor[0],
 				       &tempbuff[8], 8);

@@ -692,14 +661,15 @@ void scsi_scan(int mode)
 				       &tempbuff[16], 16);
 			scsi_ident_cpy((unsigned char *)&scsi_dev_desc[scsi_max_devs].revision[0],
 				       &tempbuff[32], 4);
-			scsi_dev_desc[scsi_max_devs].target=pccb->target;
-			scsi_dev_desc[scsi_max_devs].lun=pccb->lun;
+			scsi_dev_desc[scsi_max_devs].target = pccb->target;
+			scsi_dev_desc[scsi_max_devs].lun = pccb->lun;
 
-			pccb->datalen=0;
+			pccb->datalen = 0;
 			scsi_setup_test_unit_ready(pccb);
 			if (scsi_exec(pccb) != true) {
-				if (scsi_dev_desc[scsi_max_devs].removable == true) {
-					scsi_dev_desc[scsi_max_devs].type=perq;
+				if (scsi_dev_desc[scsi_max_devs].removable) {
+					scsi_dev_desc[scsi_max_devs].type =
+							perq;
 					goto removable;
 				}
 				scsi_print_error(pccb);

@@ -709,11 +679,11 @@ void scsi_scan(int mode)
 				scsi_print_error(pccb);
 				continue;
 			}
-			scsi_dev_desc[scsi_max_devs].lba=capacity;
-			scsi_dev_desc[scsi_max_devs].blksz=blksz;
+			scsi_dev_desc[scsi_max_devs].lba = capacity;
+			scsi_dev_desc[scsi_max_devs].blksz = blksz;
 			scsi_dev_desc[scsi_max_devs].log2blksz =
 				LOG2(scsi_dev_desc[scsi_max_devs].blksz);
-			scsi_dev_desc[scsi_max_devs].type=perq;
+			scsi_dev_desc[scsi_max_devs].type = perq;
 			part_init(&scsi_dev_desc[scsi_max_devs]);
 removable:
 			if(mode==1) {

@@ -723,8 +693,8 @@ removable:
 			scsi_max_devs++;
 		} /* next LUN */
 	}
-	if(scsi_max_devs>0)
-		scsi_curr_dev=0;
+	if (scsi_max_devs > 0)
+		scsi_curr_dev = 0;
 	else
 		scsi_curr_dev = -1;
 

index c4c08c8..a46d0ac 100644
@@ -58,13 +58,13 @@ struct blk_desc ide_dev_desc[CONFIG_SYS_IDE_MAXDEVICE];
 /* ------------------------------------------------------------------------- */
 
 #ifdef CONFIG_IDE_RESET
-static void  ide_reset (void);
+static void  ide_reset(void);
 #else
 #define ide_reset()	/* dummy */
 #endif
 
 static void ide_ident(struct blk_desc *dev_desc);
-static uchar ide_wait  (int dev, ulong t);
+static uchar ide_wait(int dev, ulong t);
 
 #define IDE_TIME_OUT	2000	/* 2 sec timeout */
 

@@ -72,14 +72,15 @@ static uchar ide_wait  (int dev, ulong t);
 
 #define IDE_SPIN_UP_TIME_OUT 5000 /* 5 sec spin-up timeout */
 
-static void ident_cpy (unsigned char *dest, unsigned char *src, unsigned int len);
+static void ident_cpy(unsigned char *dest, unsigned char *src,
+		      unsigned int len);
 
 #ifndef CONFIG_SYS_ATA_PORT_ADDR
 #define CONFIG_SYS_ATA_PORT_ADDR(port) (port)
 #endif
 
 #ifdef CONFIG_ATAPI
-static void	atapi_inquiry(struct blk_desc *dev_desc);
+static void atapi_inquiry(struct blk_desc *dev_desc);
 static ulong atapi_read(struct blk_desc *block_dev, lbaint_t blknr,
 			lbaint_t blkcnt, void *buffer);
 #endif

@@ -150,7 +151,7 @@ int do_ide(cmd_tbl_t *cmdtp, int flag, int argc, char *const argv[])
 		return CMD_RET_USAGE;
 	case 3:
 		if (strncmp(argv[1], "dev", 3) == 0) {
-			int dev = (int) simple_strtoul(argv[2], NULL, 10);
+			int dev = (int)simple_strtoul(argv[2], NULL, 10);
 
 			printf("\nIDE device %d: ", dev);
 			if (dev >= CONFIG_SYS_IDE_MAXDEVICE) {

@@ -169,7 +170,7 @@ int do_ide(cmd_tbl_t *cmdtp, int flag, int argc, char *const argv[])
 
 			return 0;
 		} else if (strncmp(argv[1], "part", 4) == 0) {
-			int dev = (int) simple_strtoul(argv[2], NULL, 10);
+			int dev = (int)simple_strtoul(argv[2], NULL, 10);
 
 			if (ide_dev_desc[dev].part_type != PART_TYPE_UNKNOWN) {
 				part_print(&ide_dev_desc[dev]);

@@ -194,13 +195,13 @@ int do_ide(cmd_tbl_t *cmdtp, int flag, int argc, char *const argv[])
 #ifdef CONFIG_SYS_64BIT_LBA
 			lbaint_t blk = simple_strtoull(argv[3], NULL, 16);
 
-			printf("\nIDE read: device %d block # %lld, count %ld ... ",
-				curr_device, blk, cnt);
+			printf("\nIDE read: device %d block # %lld, count %ld...",
+			       curr_device, blk, cnt);
 #else
 			lbaint_t blk = simple_strtoul(argv[3], NULL, 16);
 
-			printf("\nIDE read: device %d block # %ld, count %ld ... ",
-				curr_device, blk, cnt);
+			printf("\nIDE read: device %d block # %ld, count %ld...",
+			       curr_device, blk, cnt);
 #endif
 
 			dev_desc = &ide_dev_desc[curr_device];

@@ -223,13 +224,13 @@ int do_ide(cmd_tbl_t *cmdtp, int flag, int argc, char *const argv[])
 #ifdef CONFIG_SYS_64BIT_LBA
 			lbaint_t blk = simple_strtoull(argv[3], NULL, 16);
 
-			printf("\nIDE write: device %d block # %lld, count %ld ... ",
-				curr_device, blk, cnt);
+			printf("\nIDE write: device %d block # %lld, count %ld...",
+			       curr_device, blk, cnt);
 #else
 			lbaint_t blk = simple_strtoul(argv[3], NULL, 16);
 
-			printf("\nIDE write: device %d block # %ld, count %ld ... ",
-				curr_device, blk, cnt);
+			printf("\nIDE write: device %d block # %ld, count %ld...",
+			       curr_device, blk, cnt);
 #endif
 			n = ide_write(&ide_dev_desc[curr_device], blk, cnt,
 				      (ulong *)addr);

@@ -833,7 +834,7 @@ ulong ide_read(struct blk_desc *block_dev, lbaint_t blknr, lbaint_t blkcnt,
 	}
 IDE_READ_E:
 	ide_led(DEVICE_LED(device), 0);	/* LED off      */
-	return (n);
+	return n;
 }
 
 /* ------------------------------------------------------------------------- */

@@ -922,7 +923,7 @@ ulong ide_write(struct blk_desc *block_dev, lbaint_t blknr, lbaint_t blkcnt,
 	}
 WR_OUT:
 	ide_led(DEVICE_LED(device), 0);	/* LED off      */
-	return (n);
+	return n;
 }
 
 /* ------------------------------------------------------------------------- */

@@ -974,7 +975,7 @@ static uchar ide_wait(int dev, ulong t)
 		if (delay-- == 0)
 			break;
 	}
-	return (c);
+	return c;
 }
 
 /* ------------------------------------------------------------------------- */

@@ -1098,7 +1099,7 @@ static uchar atapi_wait_mask(int dev, ulong t, uchar mask, uchar res)
 		if (delay-- == 0)
 			break;
 	}
-	return (c);
+	return c;
 }
 
 /*

@@ -1142,7 +1143,7 @@ unsigned char atapi_issue(int device, unsigned char *ccb, int ccblen,
 
 	if ((c & mask) != res) {	/* DRQ must be 1, BSY 0 */
 		printf("ATAPI_ISSUE: Error (no IRQ) before sending ccb dev %d status 0x%02x\n",
-			device, c);
+		       device, c);
 		err = 0xFF;
 		goto AI_OUT;
 	}

@@ -1166,10 +1167,10 @@ unsigned char atapi_issue(int device, unsigned char *ccb, int ccblen,
 		if (c & ATA_STAT_ERR) {
 			err = (ide_inb(device, ATA_ERROR_REG)) >> 4;
 			debug("atapi_issue 1 returned sense key %X status %02X\n",
-				err, c);
+			      err, c);
 		} else {
 			printf("ATAPI_ISSUE: (no DRQ) after sending ccb (%x)  status 0x%02x\n",
-				ccb[0], c);
+			       ccb[0], c);
 			err = 0xFF;
 		}
 		goto AI_OUT;

@@ -1190,7 +1191,7 @@ unsigned char atapi_issue(int device, unsigned char *ccb, int ccblen,
 	}
 	if (n != buflen) {
 		debug("WARNING, transfer bytes %d not equal with requested %d\n",
-			n, buflen);
+		      n, buflen);
 	}
 	if (n != 0) {		/* data transfer */
 		debug("ATAPI_ISSUE: %d Bytes to transfer\n", n);

@@ -1220,7 +1221,7 @@ unsigned char atapi_issue(int device, unsigned char *ccb, int ccblen,
 	}
 AI_OUT:
 	ide_led(DEVICE_LED(device), 0);	/* LED off      */
-	return (err);
+	return err;
 }
 
 /*

@@ -1297,7 +1298,7 @@ retry:
 	       ascq);
 error:
 	debug("ERROR Sense key %02X ASC %02X ASCQ %02X\n", key, asc, ascq);
-	return (0xFF);
+	return 0xFF;
 }
 
 

@@ -1402,8 +1403,8 @@ ulong atapi_read(struct blk_desc *block_dev, lbaint_t blknr, lbaint_t blkcnt,
 	unsigned char ccb[12];	/* Command descriptor block */
 	ulong cnt;
 
-	debug("atapi_read dev %d start " LBAF " blocks " LBAF " buffer at %lX\n",
-	      device, blknr, blkcnt, (ulong) buffer);
+	debug("atapi_read dev %d start " LBAF " blocks " LBAF
+	      " buffer at %lX\n", device, blknr, blkcnt, (ulong) buffer);
 
 	do {
 		if (blkcnt > ATAPI_READ_MAX_BLOCK)

@@ -1428,14 +1429,14 @@ ulong atapi_read(struct blk_desc *block_dev, lbaint_t blknr, lbaint_t blkcnt,
 					(unsigned char *) buffer,
 					cnt * ATAPI_READ_BLOCK_SIZE)
 		    == 0xFF) {
-			return (n);
+			return n;
 		}
 		n += cnt;
 		blkcnt -= cnt;
 		blknr += cnt;
 		buffer += (cnt * ATAPI_READ_BLOCK_SIZE);
 	} while (blkcnt > 0);
-	return (n);
+	return n;
 }
 
 /* ------------------------------------------------------------------------- */

index a46d0ac..db26f43 100644
@@ -57,491 +57,518 @@ static int ide_bus_ok[CONFIG_SYS_IDE_MAXBUS];
 struct blk_desc ide_dev_desc[CONFIG_SYS_IDE_MAXDEVICE];
 /* ------------------------------------------------------------------------- */
 
-#ifdef CONFIG_IDE_RESET
-static void  ide_reset(void);
-#else
-#define ide_reset()	/* dummy */
-#endif
-
-static void ide_ident(struct blk_desc *dev_desc);
-static uchar ide_wait(int dev, ulong t);
-
 #define IDE_TIME_OUT	2000	/* 2 sec timeout */
 
 #define ATAPI_TIME_OUT	7000	/* 7 sec timeout (5 sec seems to work...) */
 
 #define IDE_SPIN_UP_TIME_OUT 5000 /* 5 sec spin-up timeout */
 
-static void ident_cpy(unsigned char *dest, unsigned char *src,
-		      unsigned int len);
-
 #ifndef CONFIG_SYS_ATA_PORT_ADDR
 #define CONFIG_SYS_ATA_PORT_ADDR(port) (port)
 #endif
 
-#ifdef CONFIG_ATAPI
-static void atapi_inquiry(struct blk_desc *dev_desc);
-static ulong atapi_read(struct blk_desc *block_dev, lbaint_t blknr,
-			lbaint_t blkcnt, void *buffer);
+#ifndef CONFIG_IDE_LED	/* define LED macros, they are not used anyways */
+# define DEVICE_LED(x) 0
+# define LED_IDE1 1
+# define LED_IDE2 2
 #endif
 
+#ifdef CONFIG_IDE_RESET
+extern void ide_set_reset(int idereset);
 
-/* ------------------------------------------------------------------------- */
-
-int do_ide(cmd_tbl_t *cmdtp, int flag, int argc, char *const argv[])
+static void ide_reset(void)
 {
-	int rcode = 0;
-
-	switch (argc) {
-	case 0:
-	case 1:
-		return CMD_RET_USAGE;
-	case 2:
-		if (strncmp(argv[1], "res", 3) == 0) {
-			puts("\nReset IDE"
-#ifdef CONFIG_IDE_8xx_DIRECT
-			     " on PCMCIA " PCMCIA_SLOT_MSG
-#endif
-			     ": ");
-
-			ide_init();
-			return 0;
-		} else if (strncmp(argv[1], "inf", 3) == 0) {
-			int i;
+	int i;
 
-			putc('\n');
+	curr_device = -1;
+	for (i = 0; i < CONFIG_SYS_IDE_MAXBUS; ++i)
+		ide_bus_ok[i] = 0;
+	for (i = 0; i < CONFIG_SYS_IDE_MAXDEVICE; ++i)
+		ide_dev_desc[i].type = DEV_TYPE_UNKNOWN;
 
-			for (i = 0; i < CONFIG_SYS_IDE_MAXDEVICE; ++i) {
-				if (ide_dev_desc[i].type == DEV_TYPE_UNKNOWN)
-					continue;  /* list only known devices */
-				printf("IDE device %d: ", i);
-				dev_print(&ide_dev_desc[i]);
-			}
-			return 0;
+	ide_set_reset(1);	/* assert reset */
 
-		} else if (strncmp(argv[1], "dev", 3) == 0) {
-			if ((curr_device < 0)
-			    || (curr_device >= CONFIG_SYS_IDE_MAXDEVICE)) {
-				puts("\nno IDE devices available\n");
-				return 1;
-			}
-			printf("\nIDE device %d: ", curr_device);
-			dev_print(&ide_dev_desc[curr_device]);
-			return 0;
-		} else if (strncmp(argv[1], "part", 4) == 0) {
-			int dev, ok;
+	/* the reset signal shall be asserted for et least 25 us */
+	udelay(25);
 
-			for (ok = 0, dev = 0;
-			     dev < CONFIG_SYS_IDE_MAXDEVICE;
-			     ++dev) {
-				if (ide_dev_desc[dev].part_type !=
-				    PART_TYPE_UNKNOWN) {
-					++ok;
-					if (dev)
-						putc('\n');
-					part_print(&ide_dev_desc[dev]);
-				}
-			}
-			if (!ok) {
-				puts("\nno IDE devices available\n");
-				rcode++;
-			}
-			return rcode;
-		}
-		return CMD_RET_USAGE;
-	case 3:
-		if (strncmp(argv[1], "dev", 3) == 0) {
-			int dev = (int)simple_strtoul(argv[2], NULL, 10);
+	WATCHDOG_RESET();
 
-			printf("\nIDE device %d: ", dev);
-			if (dev >= CONFIG_SYS_IDE_MAXDEVICE) {
-				puts("unknown device\n");
-				return 1;
-			}
-			dev_print(&ide_dev_desc[dev]);
-			/*ide_print (dev); */
+	/* de-assert RESET signal */
+	ide_set_reset(0);
 
-			if (ide_dev_desc[dev].type == DEV_TYPE_UNKNOWN)
-				return 1;
+	/* wait 250 ms */
+	for (i = 0; i < 250; ++i)
+		udelay(1000);
+}
+#else
+#define ide_reset()	/* dummy */
+#endif /* CONFIG_IDE_RESET */
 
-			curr_device = dev;
+/*
+ * Wait until Busy bit is off, or timeout (in ms)
+ * Return last status
+ */
+static uchar ide_wait(int dev, ulong t)
+{
+	ulong delay = 10 * t;	/* poll every 100 us */
+	uchar c;
 
-			puts("... is now current device\n");
+	while ((c = ide_inb(dev, ATA_STATUS)) & ATA_STAT_BUSY) {
+		udelay(100);
+		if (delay-- == 0)
+			break;
+	}
+	return c;
+}
 
-			return 0;
-		} else if (strncmp(argv[1], "part", 4) == 0) {
-			int dev = (int)simple_strtoul(argv[2], NULL, 10);
+/*
+ * copy src to dest, skipping leading and trailing blanks and null
+ * terminate the string
+ * "len" is the size of available memory including the terminating '\0'
+ */
+static void ident_cpy(unsigned char *dst, unsigned char *src,
+		      unsigned int len)
+{
+	unsigned char *end, *last;
 
-			if (ide_dev_desc[dev].part_type != PART_TYPE_UNKNOWN) {
-				part_print(&ide_dev_desc[dev]);
-			} else {
-				printf("\nIDE device %d not available\n",
-				       dev);
-				rcode = 1;
-			}
-			return rcode;
-		}
+	last = dst;
+	end = src + len - 1;
 
-		return CMD_RET_USAGE;
-	default:
-		/*@least 4 args */
+	/* reserve space for '\0' */
+	if (len < 2)
+		goto OUT;
 
-		if (strcmp(argv[1], "read") == 0) {
-			ulong addr = simple_strtoul(argv[2], NULL, 16);
-			ulong cnt = simple_strtoul(argv[4], NULL, 16);
-			struct blk_desc *dev_desc;
-			ulong n;
+	/* skip leading white space */
+	while ((*src) && (src < end) && (*src == ' '))
+		++src;
 
-#ifdef CONFIG_SYS_64BIT_LBA
-			lbaint_t blk = simple_strtoull(argv[3], NULL, 16);
+	/* copy string, omitting trailing white space */
+	while ((*src) && (src < end)) {
+		*dst++ = *src;
+		if (*src++ != ' ')
+			last = dst;
+	}
+OUT:
+	*last = '\0';
+}
 
-			printf("\nIDE read: device %d block # %lld, count %ld...",
-			       curr_device, blk, cnt);
-#else
-			lbaint_t blk = simple_strtoul(argv[3], NULL, 16);
+#ifdef CONFIG_ATAPI
+/****************************************************************************
+ * ATAPI Support
+ */
 
-			printf("\nIDE read: device %d block # %ld, count %ld...",
-			       curr_device, blk, cnt);
-#endif
+#if defined(CONFIG_IDE_SWAP_IO)
+/* since ATAPI may use commands with not 4 bytes alligned length
+ * we have our own transfer functions, 2 bytes alligned */
+__weak void ide_output_data_shorts(int dev, ushort *sect_buf, int shorts)
+{
+	ushort *dbuf;
+	volatile ushort *pbuf;
 
-			dev_desc = &ide_dev_desc[curr_device];
-			n = blk_dread(dev_desc, blk, cnt, (ulong *)addr);
-			/* flush cache after read */
-			flush_cache(addr,
-				    cnt * ide_dev_desc[curr_device].blksz);
+	pbuf = (ushort *)(ATA_CURR_BASE(dev) + ATA_DATA_REG);
+	dbuf = (ushort *)sect_buf;
 
-			printf("%ld blocks read: %s\n",
-			       n, (n == cnt) ? "OK" : "ERROR");
-			if (n == cnt)
-				return 0;
-			else
-				return 1;
-		} else if (strcmp(argv[1], "write") == 0) {
-			ulong addr = simple_strtoul(argv[2], NULL, 16);
-			ulong cnt = simple_strtoul(argv[4], NULL, 16);
-			ulong n;
+	debug("in output data shorts base for read is %lx\n",
+	      (unsigned long) pbuf);
 
-#ifdef CONFIG_SYS_64BIT_LBA
-			lbaint_t blk = simple_strtoull(argv[3], NULL, 16);
+	while (shorts--) {
+		EIEIO;
+		*pbuf = *dbuf++;
+	}
+}
 
-			printf("\nIDE write: device %d block # %lld, count %ld...",
-			       curr_device, blk, cnt);
-#else
-			lbaint_t blk = simple_strtoul(argv[3], NULL, 16);
+__weak void ide_input_data_shorts(int dev, ushort *sect_buf, int shorts)
+{
+	ushort *dbuf;
+	volatile ushort *pbuf;
 
-			printf("\nIDE write: device %d block # %ld, count %ld...",
-			       curr_device, blk, cnt);
-#endif
-			n = ide_write(&ide_dev_desc[curr_device], blk, cnt,
-				      (ulong *)addr);
+	pbuf = (ushort *)(ATA_CURR_BASE(dev) + ATA_DATA_REG);
+	dbuf = (ushort *)sect_buf;
 
-			printf("%ld blocks written: %s\n",
-				n, (n == cnt) ? "OK" : "ERROR");
-			if (n == cnt)
-				return 0;
-			else
-				return 1;
-		} else {
-			return CMD_RET_USAGE;
-		}
+	debug("in input data shorts base for read is %lx\n",
+	      (unsigned long) pbuf);
 
-		return rcode;
+	while (shorts--) {
+		EIEIO;
+		*dbuf++ = *pbuf;
 	}
 }
 
-int do_diskboot(cmd_tbl_t *cmdtp, int flag, int argc, char *const argv[])
+#else  /* ! CONFIG_IDE_SWAP_IO */
+__weak void ide_output_data_shorts(int dev, ushort *sect_buf, int shorts)
 {
-	return common_diskboot(cmdtp, "ide", argc, argv);
+	outsw(ATA_CURR_BASE(dev) + ATA_DATA_REG, sect_buf, shorts);
 }
 
-/* ------------------------------------------------------------------------- */
-
-__weak void ide_led(uchar led, uchar status)
+__weak void ide_input_data_shorts(int dev, ushort *sect_buf, int shorts)
 {
-#if defined(CONFIG_IDE_LED) && defined(PER8_BASE) /* required by LED_PORT */
-	static uchar led_buffer;	/* Buffer for current LED status */
-
-	uchar *led_port = LED_PORT;
+	insw(ATA_CURR_BASE(dev) + ATA_DATA_REG, sect_buf, shorts);
+}
 
-	if (status)		/* switch LED on        */
-		led_buffer |= led;
-	else			/* switch LED off       */
-		led_buffer &= ~led;
+#endif /* CONFIG_IDE_SWAP_IO */
 
-	*led_port = led_buffer;
-#endif
-}
-
-#ifndef CONFIG_IDE_LED	/* define LED macros, they are not used anyways */
-# define DEVICE_LED(x) 0
-# define LED_IDE1 1
-# define LED_IDE2 2
-#endif
-
-/* ------------------------------------------------------------------------- */
-
-__weak void ide_outb(int dev, int port, unsigned char val)
+/*
+ * Wait until (Status & mask) == res, or timeout (in ms)
+ * Return last status
+ * This is used since some ATAPI CD ROMs clears their Busy Bit first
+ * and then they set their DRQ Bit
+ */
+static uchar atapi_wait_mask(int dev, ulong t, uchar mask, uchar res)
 {
-	debug("ide_outb (dev= %d, port= 0x%x, val= 0x%02x) : @ 0x%08lx\n",
-	      dev, port, val,
-	      (ATA_CURR_BASE(dev) + CONFIG_SYS_ATA_PORT_ADDR(port)));
+	ulong delay = 10 * t;	/* poll every 100 us */
+	uchar c;
 
-#if defined(CONFIG_IDE_AHB)
-	if (port) {
-		/* write command */
-		ide_write_register(dev, port, val);
-	} else {
-		/* write data */
-		outb(val, (ATA_CURR_BASE(dev)));
+	/* prevents to read the status before valid */
+	c = ide_inb(dev, ATA_DEV_CTL);
+
+	while (((c = ide_inb(dev, ATA_STATUS)) & mask) != res) {
+		/* break if error occurs (doesn't make sense to wait more) */
+		if ((c & ATA_STAT_ERR) == ATA_STAT_ERR)
+			break;
+		udelay(100);
+		if (delay-- == 0)
+			break;
 	}
-#else
-	outb(val, (ATA_CURR_BASE(dev) + CONFIG_SYS_ATA_PORT_ADDR(port)));
-#endif
+	return c;
 }
 
-__weak unsigned char ide_inb(int dev, int port)
+/*
+ * issue an atapi command
+ */
+unsigned char atapi_issue(int device, unsigned char *ccb, int ccblen,
+			  unsigned char *buffer, int buflen)
 {
-	uchar val;
-
-#if defined(CONFIG_IDE_AHB)
-	val = ide_read_register(dev, port);
-#else
-	val = inb((ATA_CURR_BASE(dev) + CONFIG_SYS_ATA_PORT_ADDR(port)));
-#endif
+	unsigned char c, err, mask, res;
+	int n;
 
-	debug("ide_inb (dev= %d, port= 0x%x) : @ 0x%08lx -> 0x%02x\n",
-	      dev, port,
-	      (ATA_CURR_BASE(dev) + CONFIG_SYS_ATA_PORT_ADDR(port)), val);
-	return val;
-}
+	ide_led(DEVICE_LED(device), 1);	/* LED on       */
 
-void ide_init(void)
-{
-	unsigned char c;
-	int i, bus;
+	/* Select device
+	 */
+	mask = ATA_STAT_BUSY | ATA_STAT_DRQ;
+	res = 0;
+	ide_outb(device, ATA_DEV_HD, ATA_LBA | ATA_DEVICE(device));
+	c = atapi_wait_mask(device, ATAPI_TIME_OUT, mask, res);
+	if ((c & mask) != res) {
+		printf("ATAPI_ISSUE: device %d not ready status %X\n", device,
+		       c);
+		err = 0xFF;
+		goto AI_OUT;
+	}
+	/* write taskfile */
+	ide_outb(device, ATA_ERROR_REG, 0);	/* no DMA, no overlaped */
+	ide_outb(device, ATA_SECT_CNT, 0);
+	ide_outb(device, ATA_SECT_NUM, 0);
+	ide_outb(device, ATA_CYL_LOW, (unsigned char) (buflen & 0xFF));
+	ide_outb(device, ATA_CYL_HIGH,
+		 (unsigned char) ((buflen >> 8) & 0xFF));
+	ide_outb(device, ATA_DEV_HD, ATA_LBA | ATA_DEVICE(device));
 
-#ifdef CONFIG_IDE_8xx_PCCARD
-	extern int ide_devices_found;	/* Initialized in check_ide_device() */
-#endif /* CONFIG_IDE_8xx_PCCARD */
+	ide_outb(device, ATA_COMMAND, ATAPI_CMD_PACKET);
+	udelay(50);
 
-#ifdef CONFIG_IDE_PREINIT
-	WATCHDOG_RESET();
+	mask = ATA_STAT_DRQ | ATA_STAT_BUSY | ATA_STAT_ERR;
+	res = ATA_STAT_DRQ;
+	c = atapi_wait_mask(device, ATAPI_TIME_OUT, mask, res);
 
-	if (ide_preinit()) {
-		puts("ide_preinit failed\n");
-		return;
+	if ((c & mask) != res) {	/* DRQ must be 1, BSY 0 */
+		printf("ATAPI_ISSUE: Error (no IRQ) before sending ccb dev %d status 0x%02x\n",
+		       device, c);
+		err = 0xFF;
+		goto AI_OUT;
 	}
-#endif /* CONFIG_IDE_PREINIT */
 
-	WATCHDOG_RESET();
+	/* write command block */
+	ide_output_data_shorts(device, (unsigned short *)ccb, ccblen / 2);
 
+	/* ATAPI Command written wait for completition */
+	udelay(5000);		/* device must set bsy */
+
+	mask = ATA_STAT_DRQ | ATA_STAT_BUSY | ATA_STAT_ERR;
 	/*
-	 * Reset the IDE just to be sure.
-	 * Light LED's to show
+	 * if no data wait for DRQ = 0 BSY = 0
+	 * if data wait for DRQ = 1 BSY = 0
 	 */
-	ide_led((LED_IDE1 | LED_IDE2), 1);	/* LED's on     */
-
-	/* ATAPI Drives seems to need a proper IDE Reset */
-	ide_reset();
-
-#ifdef CONFIG_IDE_INIT_POSTRESET
-	WATCHDOG_RESET();
-
-	if (ide_init_postreset()) {
-		puts("ide_preinit_postreset failed\n");
-		return;
+	res = 0;
+	if (buflen)
+		res = ATA_STAT_DRQ;
+	c = atapi_wait_mask(device, ATAPI_TIME_OUT, mask, res);
+	if ((c & mask) != res) {
+		if (c & ATA_STAT_ERR) {
+			err = (ide_inb(device, ATA_ERROR_REG)) >> 4;
+			debug("atapi_issue 1 returned sense key %X status %02X\n",
+			      err, c);
+		} else {
+			printf("ATAPI_ISSUE: (no DRQ) after sending ccb (%x)  status 0x%02x\n",
+			       ccb[0], c);
+			err = 0xFF;
+		}
+		goto AI_OUT;
 	}
-#endif /* CONFIG_IDE_INIT_POSTRESET */
+	n = ide_inb(device, ATA_CYL_HIGH);
+	n <<= 8;
+	n += ide_inb(device, ATA_CYL_LOW);
+	if (n > buflen) {
+		printf("ERROR, transfer bytes %d requested only %d\n", n,
+		       buflen);
+		err = 0xff;
+		goto AI_OUT;
+	}
+	if ((n == 0) && (buflen < 0)) {
+		printf("ERROR, transfer bytes %d requested %d\n", n, buflen);
+		err = 0xff;
+		goto AI_OUT;
+	}
+	if (n != buflen) {
+		debug("WARNING, transfer bytes %d not equal with requested %d\n",
+		      n, buflen);
+	}
+	if (n != 0) {		/* data transfer */
+		debug("ATAPI_ISSUE: %d Bytes to transfer\n", n);
+		/* we transfer shorts */
+		n >>= 1;
+		/* ok now decide if it is an in or output */
+		if ((ide_inb(device, ATA_SECT_CNT) & 0x02) == 0) {
+			debug("Write to device\n");
+			ide_output_data_shorts(device, (unsigned short *)buffer,
+					       n);
+		} else {
+			debug("Read from device @ %p shorts %d\n", buffer, n);
+			ide_input_data_shorts(device, (unsigned short *)buffer,
+					      n);
+		}
+	}
+	udelay(5000);		/* seems that some CD ROMs need this... */
+	mask = ATA_STAT_BUSY | ATA_STAT_ERR;
+	res = 0;
+	c = atapi_wait_mask(device, ATAPI_TIME_OUT, mask, res);
+	if ((c & ATA_STAT_ERR) == ATA_STAT_ERR) {
+		err = (ide_inb(device, ATA_ERROR_REG) >> 4);
+		debug("atapi_issue 2 returned sense key %X status %X\n", err,
+		      c);
+	} else {
+		err = 0;
+	}
+AI_OUT:
+	ide_led(DEVICE_LED(device), 0);	/* LED off      */
+	return err;
+}
 
-	/*
-	 * Wait for IDE to get ready.
-	 * According to spec, this can take up to 31 seconds!
-	 */
-	for (bus = 0; bus < CONFIG_SYS_IDE_MAXBUS; ++bus) {
-		int dev =
-			bus * (CONFIG_SYS_IDE_MAXDEVICE /
-			       CONFIG_SYS_IDE_MAXBUS);
+/*
+ * sending the command to atapi_issue. If an status other than good
+ * returns, an request_sense will be issued
+ */
 
-#ifdef CONFIG_IDE_8xx_PCCARD
-		/* Skip non-ide devices from probing */
-		if ((ide_devices_found & (1 << bus)) == 0) {
-			ide_led((LED_IDE1 | LED_IDE2), 0);	/* LED's off */
-			continue;
-		}
-#endif
-		printf("Bus %d: ", bus);
+#define ATAPI_DRIVE_NOT_READY	100
+#define ATAPI_UNIT_ATTN		10
 
-		ide_bus_ok[bus] = 0;
+unsigned char atapi_issue_autoreq(int device,
+				  unsigned char *ccb,
+				  int ccblen,
+				  unsigned char *buffer, int buflen)
+{
+	unsigned char sense_data[18], sense_ccb[12];
+	unsigned char res, key, asc, ascq;
+	int notready, unitattn;
 
-		/* Select device
-		 */
-		udelay(100000);	/* 100 ms */
-		ide_outb(dev, ATA_DEV_HD, ATA_LBA | ATA_DEVICE(dev));
-		udelay(100000);	/* 100 ms */
-		i = 0;
-		do {
-			udelay(10000);	/* 10 ms */
+	unitattn = ATAPI_UNIT_ATTN;
+	notready = ATAPI_DRIVE_NOT_READY;
 
-			c = ide_inb(dev, ATA_STATUS);
-			i++;
-			if (i > (ATA_RESET_TIME * 100)) {
-				puts("** Timeout **\n");
-				/* LED's off */
-				ide_led((LED_IDE1 | LED_IDE2), 0);
-				return;
-			}
-			if ((i >= 100) && ((i % 100) == 0))
-				putc('.');
+retry:
+	res = atapi_issue(device, ccb, ccblen, buffer, buflen);
+	if (res == 0)
+		return 0;	/* Ok */
 
-		} while (c & ATA_STAT_BUSY);
+	if (res == 0xFF)
+		return 0xFF;	/* error */
 
-		if (c & (ATA_STAT_BUSY | ATA_STAT_FAULT)) {
-			puts("not available  ");
-			debug("Status = 0x%02X ", c);
-#ifndef CONFIG_ATAPI		/* ATAPI Devices do not set DRDY */
-		} else if ((c & ATA_STAT_READY) == 0) {
-			puts("not available  ");
-			debug("Status = 0x%02X ", c);
-#endif
-		} else {
-			puts("OK ");
-			ide_bus_ok[bus] = 1;
-		}
-		WATCHDOG_RESET();
-	}
+	debug("(auto_req)atapi_issue returned sense key %X\n", res);
 
-	putc('\n');
+	memset(sense_ccb, 0, sizeof(sense_ccb));
+	memset(sense_data, 0, sizeof(sense_data));
+	sense_ccb[0] = ATAPI_CMD_REQ_SENSE;
+	sense_ccb[4] = 18;	/* allocation Length */
 
-	ide_led((LED_IDE1 | LED_IDE2), 0);	/* LED's off    */
+	res = atapi_issue(device, sense_ccb, 12, sense_data, 18);
+	key = (sense_data[2] & 0xF);
+	asc = (sense_data[12]);
+	ascq = (sense_data[13]);
 
-	curr_device = -1;
-	for (i = 0; i < CONFIG_SYS_IDE_MAXDEVICE; ++i) {
-		int led = (IDE_BUS(i) == 0) ? LED_IDE1 : LED_IDE2;
-		ide_dev_desc[i].type = DEV_TYPE_UNKNOWN;
-		ide_dev_desc[i].if_type = IF_TYPE_IDE;
-		ide_dev_desc[i].devnum = i;
-		ide_dev_desc[i].part_type = PART_TYPE_UNKNOWN;
-		ide_dev_desc[i].blksz = 0;
-		ide_dev_desc[i].log2blksz =
-			LOG2_INVALID(typeof(ide_dev_desc[i].log2blksz));
-		ide_dev_desc[i].lba = 0;
-		ide_dev_desc[i].block_read = ide_read;
-		ide_dev_desc[i].block_write = ide_write;
-		if (!ide_bus_ok[IDE_BUS(i)])
-			continue;
-		ide_led(led, 1);	/* LED on       */
-		ide_ident(&ide_dev_desc[i]);
-		ide_led(led, 0);	/* LED off      */
-		dev_print(&ide_dev_desc[i]);
+	debug("ATAPI_CMD_REQ_SENSE returned %x\n", res);
+	debug(" Sense page: %02X key %02X ASC %02X ASCQ %02X\n",
+	      sense_data[0], key, asc, ascq);
 
-		if ((ide_dev_desc[i].lba > 0) && (ide_dev_desc[i].blksz > 0)) {
-			/* initialize partition type */
-			part_init(&ide_dev_desc[i]);
-			if (curr_device < 0)
-				curr_device = i;
+	if ((key == 0))
+		return 0;	/* ok device ready */
+
+	if ((key == 6) || (asc == 0x29) || (asc == 0x28)) { /* Unit Attention */
+		if (unitattn-- > 0) {
+			udelay(200 * 1000);
+			goto retry;
 		}
+		printf("Unit Attention, tried %d\n", ATAPI_UNIT_ATTN);
+		goto error;
 	}
-	WATCHDOG_RESET();
+	if ((asc == 0x4) && (ascq == 0x1)) {
+		/* not ready, but will be ready soon */
+		if (notready-- > 0) {
+			udelay(200 * 1000);
+			goto retry;
+		}
+		printf("Drive not ready, tried %d times\n",
+		       ATAPI_DRIVE_NOT_READY);
+		goto error;
+	}
+	if (asc == 0x3a) {
+		debug("Media not present\n");
+		goto error;
+	}
+
+	printf("ERROR: Unknown Sense key %02X ASC %02X ASCQ %02X\n", key, asc,
+	       ascq);
+error:
+	debug("ERROR Sense key %02X ASC %02X ASCQ %02X\n", key, asc, ascq);
+	return 0xFF;
 }
 
-/* ------------------------------------------------------------------------- */
+/*
+ * atapi_read:
+ * we transfer only one block per command, since the multiple DRQ per
+ * command is not yet implemented
+ */
+#define ATAPI_READ_MAX_BYTES	2048	/* we read max 2kbytes */
+#define ATAPI_READ_BLOCK_SIZE	2048	/* assuming CD part */
+#define ATAPI_READ_MAX_BLOCK	(ATAPI_READ_MAX_BYTES/ATAPI_READ_BLOCK_SIZE)
 
-#ifdef CONFIG_PARTITIONS
-struct blk_desc *ide_get_dev(int dev)
+ulong atapi_read(struct blk_desc *block_dev, lbaint_t blknr, lbaint_t blkcnt,
+		 void *buffer)
 {
-	return (dev < CONFIG_SYS_IDE_MAXDEVICE) ? &ide_dev_desc[dev] : NULL;
-}
-#endif
+	int device = block_dev->devnum;
+	ulong n = 0;
+	unsigned char ccb[12];	/* Command descriptor block */
+	ulong cnt;
 
-/* ------------------------------------------------------------------------- */
+	debug("atapi_read dev %d start " LBAF " blocks " LBAF
+	      " buffer at %lX\n", device, blknr, blkcnt, (ulong) buffer);
 
-/* We only need to swap data if we are running on a big endian cpu. */
-#if defined(__LITTLE_ENDIAN)
-__weak void ide_input_swap_data(int dev, ulong *sect_buf, int words)
-{
-	ide_input_data(dev, sect_buf, words);
+	do {
+		if (blkcnt > ATAPI_READ_MAX_BLOCK)
+			cnt = ATAPI_READ_MAX_BLOCK;
+		else
+			cnt = blkcnt;
+
+		ccb[0] = ATAPI_CMD_READ_12;
+		ccb[1] = 0;	/* reserved */
+		ccb[2] = (unsigned char) (blknr >> 24) & 0xFF;	/* MSB Block */
+		ccb[3] = (unsigned char) (blknr >> 16) & 0xFF;	/*  */
+		ccb[4] = (unsigned char) (blknr >> 8) & 0xFF;
+		ccb[5] = (unsigned char) blknr & 0xFF;	/* LSB Block */
+		ccb[6] = (unsigned char) (cnt >> 24) & 0xFF; /* MSB Block cnt */
+		ccb[7] = (unsigned char) (cnt >> 16) & 0xFF;
+		ccb[8] = (unsigned char) (cnt >> 8) & 0xFF;
+		ccb[9] = (unsigned char) cnt & 0xFF;	/* LSB Block */
+		ccb[10] = 0;	/* reserved */
+		ccb[11] = 0;	/* reserved */
+
+		if (atapi_issue_autoreq(device, ccb, 12,
+					(unsigned char *)buffer,
+					cnt * ATAPI_READ_BLOCK_SIZE)
+		    == 0xFF) {
+			return n;
+		}
+		n += cnt;
+		blkcnt -= cnt;
+		blknr += cnt;
+		buffer += (cnt * ATAPI_READ_BLOCK_SIZE);
+	} while (blkcnt > 0);
+	return n;
 }
-#else
-__weak void ide_input_swap_data(int dev, ulong *sect_buf, int words)
+
+static void atapi_inquiry(struct blk_desc *dev_desc)
 {
-	volatile ushort *pbuf =
-		(ushort *) (ATA_CURR_BASE(dev) + ATA_DATA_REG);
-	ushort *dbuf = (ushort *) sect_buf;
+	unsigned char ccb[12];	/* Command descriptor block */
+	unsigned char iobuf[64];	/* temp buf */
+	unsigned char c;
+	int device;
 
-	debug("in input swap data base for read is %lx\n",
-	      (unsigned long) pbuf);
+	device = dev_desc->devnum;
+	dev_desc->type = DEV_TYPE_UNKNOWN;	/* not yet valid */
+	dev_desc->block_read = atapi_read;
 
-	while (words--) {
-#ifdef __MIPS__
-		*dbuf++ = swab16p((u16 *) pbuf);
-		*dbuf++ = swab16p((u16 *) pbuf);
-#else
-		*dbuf++ = ld_le16(pbuf);
-		*dbuf++ = ld_le16(pbuf);
-#endif /* !MIPS */
-	}
-}
-#endif /* __LITTLE_ENDIAN */
+	memset(ccb, 0, sizeof(ccb));
+	memset(iobuf, 0, sizeof(iobuf));
 
+	ccb[0] = ATAPI_CMD_INQUIRY;
+	ccb[4] = 40;		/* allocation Legnth */
+	c = atapi_issue_autoreq(device, ccb, 12, (unsigned char *)iobuf, 40);
 
-#if defined(CONFIG_IDE_SWAP_IO)
-__weak void ide_output_data(int dev, const ulong *sect_buf, int words)
-{
-	ushort *dbuf;
-	volatile ushort *pbuf;
+	debug("ATAPI_CMD_INQUIRY returned %x\n", c);
+	if (c != 0)
+		return;
 
-	pbuf = (ushort *) (ATA_CURR_BASE(dev) + ATA_DATA_REG);
-	dbuf = (ushort *) sect_buf;
-	while (words--) {
-		EIEIO;
-		*pbuf = *dbuf++;
-		EIEIO;
-		*pbuf = *dbuf++;
-	}
-}
-#else  /* ! CONFIG_IDE_SWAP_IO */
-__weak void ide_output_data(int dev, const ulong *sect_buf, int words)
-{
-#if defined(CONFIG_IDE_AHB)
-	ide_write_data(dev, sect_buf, words);
-#else
-	outsw(ATA_CURR_BASE(dev) + ATA_DATA_REG, sect_buf, words << 1);
-#endif
-}
-#endif /* CONFIG_IDE_SWAP_IO */
+	/* copy device ident strings */
+	ident_cpy((unsigned char *)dev_desc->vendor, &iobuf[8], 8);
+	ident_cpy((unsigned char *)dev_desc->product, &iobuf[16], 16);
+	ident_cpy((unsigned char *)dev_desc->revision, &iobuf[32], 5);
 
-#if defined(CONFIG_IDE_SWAP_IO)
-__weak void ide_input_data(int dev, ulong *sect_buf, int words)
-{
-	ushort *dbuf;
-	volatile ushort *pbuf;
+	dev_desc->lun = 0;
+	dev_desc->lba = 0;
+	dev_desc->blksz = 0;
+	dev_desc->log2blksz = LOG2_INVALID(typeof(dev_desc->log2blksz));
+	dev_desc->type = iobuf[0] & 0x1f;
 
-	pbuf = (ushort *) (ATA_CURR_BASE(dev) + ATA_DATA_REG);
-	dbuf = (ushort *) sect_buf;
+	if ((iobuf[1] & 0x80) == 0x80)
+		dev_desc->removable = 1;
+	else
+		dev_desc->removable = 0;
 
-	debug("in input data base for read is %lx\n", (unsigned long) pbuf);
+	memset(ccb, 0, sizeof(ccb));
+	memset(iobuf, 0, sizeof(iobuf));
+	ccb[0] = ATAPI_CMD_START_STOP;
+	ccb[4] = 0x03;		/* start */
 
-	while (words--) {
-		EIEIO;
-		*dbuf++ = *pbuf;
-		EIEIO;
-		*dbuf++ = *pbuf;
-	}
-}
-#else  /* ! CONFIG_IDE_SWAP_IO */
-__weak void ide_input_data(int dev, ulong *sect_buf, int words)
-{
-#if defined(CONFIG_IDE_AHB)
-	ide_read_data(dev, sect_buf, words);
-#else
-	insw(ATA_CURR_BASE(dev) + ATA_DATA_REG, sect_buf, words << 1);
+	c = atapi_issue_autoreq(device, ccb, 12, (unsigned char *)iobuf, 0);
+
+	debug("ATAPI_CMD_START_STOP returned %x\n", c);
+	if (c != 0)
+		return;
+
+	memset(ccb, 0, sizeof(ccb));
+	memset(iobuf, 0, sizeof(iobuf));
+	c = atapi_issue_autoreq(device, ccb, 12, (unsigned char *)iobuf, 0);
+
+	debug("ATAPI_CMD_UNIT_TEST_READY returned %x\n", c);
+	if (c != 0)
+		return;
+
+	memset(ccb, 0, sizeof(ccb));
+	memset(iobuf, 0, sizeof(iobuf));
+	ccb[0] = ATAPI_CMD_READ_CAP;
+	c = atapi_issue_autoreq(device, ccb, 12, (unsigned char *)iobuf, 8);
+	debug("ATAPI_CMD_READ_CAP returned %x\n", c);
+	if (c != 0)
+		return;
+
+	debug("Read Cap: LBA %02X%02X%02X%02X blksize %02X%02X%02X%02X\n",
+	      iobuf[0], iobuf[1], iobuf[2], iobuf[3],
+	      iobuf[4], iobuf[5], iobuf[6], iobuf[7]);
+
+	dev_desc->lba = ((unsigned long) iobuf[0] << 24) +
+		((unsigned long) iobuf[1] << 16) +
+		((unsigned long) iobuf[2] << 8) + ((unsigned long) iobuf[3]);
+	dev_desc->blksz = ((unsigned long) iobuf[4] << 24) +
+		((unsigned long) iobuf[5] << 16) +
+		((unsigned long) iobuf[6] << 8) + ((unsigned long) iobuf[7]);
+	dev_desc->log2blksz = LOG2(dev_desc->blksz);
+#ifdef CONFIG_LBA48
+	/* ATAPI devices cannot use 48bit addressing (ATA/ATAPI v7) */
+	dev_desc->lba48 = 0;
 #endif
+	return;
 }
 
-#endif /* CONFIG_IDE_SWAP_IO */
+#endif /* CONFIG_ATAPI */
 
-/* -------------------------------------------------------------------------
- */
 static void ide_ident(struct blk_desc *dev_desc)
 {
 	unsigned char c;