This is the much rumoured ATA mkIII update that I've been working on.

o       ATA is now fully newbus'd and split into modules.
        This means that on a modern system you just load "atapci and ata"
        to get the base support, and then one or more of the device
        subdrivers "atadisk atapicd atapifd atapist ataraid".
        All can be loaded/unloaded anytime, but for obvious reasons you
        dont want to unload atadisk when you have mounted filesystems.

o       The device identify part of the probe has been rewritten to fix
        the problems with odd devices the old had, and to try to remove
        so of the long delays some HW could provoke. Also probing is done
	without the need for interrupts, making earlier probing possible.

o       SATA devices can be hot inserted/removed and devices will be created/
        removed in /dev accordingly.
	NOTE: only supported on controllers that has this feature:
	Promise and Silicon Image for now.
	On other controllers the usual atacontrol detach/attach dance is
	still needed.

o	Support for "atomic" composite ATA requests used for RAID.

o       ATA RAID support has been rewritten and and now supports these
        metadata formats:
                 "Adaptec HostRAID"
                 "Highpoint V2 RocketRAID"
                 "Highpoint V3 RocketRAID"
                 "Intel MatrixRAID"
                 "Integrated Technology Express"
                 "LSILogic V2 MegaRAID"
                 "LSILogic V3 MegaRAID"
                 "Promise FastTrak"
                 "Silicon Image Medley"
		 "FreeBSD PseudoRAID"

o       Update the ioctl API to match new RAID levels etc.

o       Update atacontrol to know about the new RAID levels etc
        NOTE: you need to recompile atacontrol with the new sys/ata.h,
        make world will take care of that.
	NOTE2: that rebuild is done differently from the old system as
	the rebuild is now done piggybacked on read requests to the
	array, so atacontrol simply starts a background "dd" to rebuild
	the array.

o       The reinit code has been worked over to be much more robust.

o       The timeout code has been overhauled for races.

o	Support of new chipsets.

o       Lots of fixes for bugs found while doing the modulerization and
        reviewing the old code.

Missing or changed features from current ATA:

o       atapi-cd no longer has support for ATAPI changers. Todays its
        much cheaper and alot faster to copy those CD images to disk
        and serve them from there. Besides they dont seem to be made
        anymore, maybe for that exact reason.

o       ATA RAID can only read metadata from all the above metadata formats,
	not write all of them (Promise and Highpoint V2 so far). This means
	that arrays can be picked up from the BIOS, but they cannot be
	created from FreeBSD. There is more to it than just the missing
	write metadata support, those formats are not unique to a given
	controller like Promise and Highpoint formats, instead they exist
	for several types, and even worse, some controllers can have
	different formats and its impossible to tell which one.
	The outcome is that we cannot reliably create the metadata of those
	formats and be sure the controller BIOS will understand it.
	However write support is needed to update/fail/rebuild the arrays
	properly so it sits fairly high on the TODO list.

o       So far atapicam is not supported with these changes. When/if this
	will change is up to the maintainer of atapi-cam so go there for
	questions.

HW donated by:  Webveveriet AS
HW donated by:  Frode Nordahl
HW donated by:  Yahoo!
HW donated by:  Sentex
Patience by:	Vife and my boys (and even the cats)

1 files changed, 347 insertions, 186 deletions

diff --git a/sys/dev/ata/ata-lowlevel.c b/sys/dev/ata/ata-lowlevel.c
index aec3b05..bb9d9e0 100644
--- a/sys/dev/ata/ata-lowlevel.c
+++ b/sys/dev/ata/ata-lowlevel.c
@@ -1,5 +1,5 @@
 /*-
- * Copyright (c) 1998 - 2004 Søren Schmidt <sos@FreeBSD.org>
+ * Copyright (c) 1998 - 2005 Søren Schmidt <sos@FreeBSD.org>
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -32,9 +32,11 @@ __FBSDID("$FreeBSD$");
 #include "opt_ata.h"
 #include <sys/param.h>
 #include <sys/systm.h>
-#include <sys/ata.h>
 #include <sys/kernel.h>
+#include <sys/endian.h>
+#include <sys/ata.h>
 #include <sys/conf.h>
+#include <sys/ctype.h>
 #include <sys/bus.h>
 #include <sys/sema.h>
 #include <sys/taskqueue.h>
@@ -42,17 +44,108 @@ __FBSDID("$FreeBSD$");
 #include <machine/bus.h>
 #include <sys/rman.h>
 #include <dev/ata/ata-all.h>
+#include <dev/ata/ata-commands.h>
+#include <dev/ata/ata-pci.h>
+#include <ata_if.h>
 
 /* prototypes */
 static int ata_begin_transaction(struct ata_request *);
 static int ata_end_transaction(struct ata_request *);
 static void ata_generic_reset(struct ata_channel *);
-static int ata_wait(struct ata_device *, u_int8_t);
+static int ata_wait(struct ata_channel *ch, struct ata_device *, u_int8_t);
 static void ata_pio_read(struct ata_request *, int);
 static void ata_pio_write(struct ata_request *, int);
+static void bswap(int8_t *, int);
+static void btrim(int8_t *, int);
+static void bpack(int8_t *, int8_t *, int);
+
+/* get device parameter page from device */
+int
+ata_getparam(device_t parent, struct ata_device *atadev, u_int8_t command)
+{
+    struct ata_channel *ch = device_get_softc(parent);
+    int error = 0, retry = 0;
+
+    do {
+	if (retry++ > 4) {
+	    if (bootverbose)
+		printf("ata%d-%s: %s-identify retries exceeded\n", ch->unit,
+		       atadev->unit == ATA_MASTER ? "master" : "slave",
+		       command == ATA_ATAPI_IDENTIFY ? "ATAPI" : "ATA");
+	    error = ENXIO;      
+	    break;
+	}
+
+	/* select device */
+	ATA_IDX_OUTB(ch, ATA_DRIVE, ATA_D_IBM | ATA_D_LBA | atadev->unit);
+
+	/* disable interrupt */
+	ATA_IDX_OUTB(ch, ATA_ALTSTAT, ATA_A_4BIT | ATA_A_IDS);
+
+	/* ready to issue command ? */
+	if ((error = ata_wait(ch, atadev, 0)) < 0) { 
+	    printf("ata%d-%s: timeout sending %s-identify error=%d\n",
+		   device_get_unit(parent), 
+		   atadev->unit == ATA_MASTER ? "master" : "slave",
+		   command == ATA_ATAPI_IDENTIFY ? "ATAPI" : "ATA", error);
+	    error = ENXIO;      
+	    break;
+	}
+
+	/* select device */
+	ATA_IDX_OUTB(ch, ATA_DRIVE, ATA_D_IBM | ATA_D_LBA | atadev->unit);
+
+	/* issue command */
+	ATA_IDX_OUTB(ch, ATA_CMD, command);
+
+    } while (ata_wait(ch, atadev, ATA_S_DRQ));
+
+    if (!error) {
+	ATA_IDX_INSW_STRM(ch, ATA_DATA, (void *)&atadev->param,
+			  sizeof(struct ata_params)/sizeof(int16_t));
+	ATA_IDX_INB(ch, ATA_STATUS);
+    }
+
+    if (!error && (isprint(atadev->param.model[0]) ||
+		   isprint(atadev->param.model[1]))) {
+	struct ata_params *atacap = &atadev->param;
+#if BYTE_ORDER == BIG_ENDIAN
+	int16_t *ptr;
+
+	for (ptr = (int16_t *)atacap;
+	     ptr < (int16_t *)atacap + sizeof(struct ata_params)/2; ptr++) {
+	    *ptr = bswap16(*ptr);
+	}
+#endif
+	if (!(!strncmp(atacap->model, "FX", 2) ||
+	      !strncmp(atacap->model, "NEC", 3) ||
+	      !strncmp(atacap->model, "Pioneer", 7) ||
+	      !strncmp(atacap->model, "SHARP", 5))) {
+	    bswap(atacap->model, sizeof(atacap->model));
+	    bswap(atacap->revision, sizeof(atacap->revision));
+	    bswap(atacap->serial, sizeof(atacap->serial));
+	}
+	btrim(atacap->model, sizeof(atacap->model));
+	bpack(atacap->model, atacap->model, sizeof(atacap->model));
+	btrim(atacap->revision, sizeof(atacap->revision));
+	bpack(atacap->revision, atacap->revision, sizeof(atacap->revision));
+	btrim(atacap->serial, sizeof(atacap->serial));
+	bpack(atacap->serial, atacap->serial, sizeof(atacap->serial));
+	if (bootverbose)
+	    printf("ata%d-%s: pio=%s wdma=%s udma=%s cable=%s wire\n",
+		   ch->unit, atadev->unit == ATA_MASTER ? "master":"slave",
+		   ata_mode2str(ata_pmode(atacap)),
+		   ata_mode2str(ata_wmode(atacap)),
+		   ata_mode2str(ata_umode(atacap)),
+		   (atacap->hwres & ATA_CABLE_ID) ? "80":"40");
+    }
+    else {
+	if (!error)
+	    error = ENXIO;
+    }
 
-/* local vars */
-static int atadebug = 0;
+    return error;
+}
 
 /*
  * low level ATA functions 
@@ -70,14 +163,8 @@ ata_generic_hw(struct ata_channel *ch)
 static int
 ata_begin_transaction(struct ata_request *request)
 {
-    struct ata_channel *ch = request->device->channel;
-
-    /* safetybelt for HW that went away */
-    if (!request->device->param || request->device->channel->flags&ATA_HWGONE) {
-	request->retries = 0;
-	request->result = ENXIO;
-	return ATA_OP_FINISHED;
-    }
+    struct ata_channel *ch = device_get_softc(device_get_parent(request->dev));
+    struct ata_device *atadev = device_get_softc(request->dev);
 
     ATA_DEBUG_RQ(request, "begin transaction");
 
@@ -96,17 +183,17 @@ ata_begin_transaction(struct ata_request *request)
 	int write = (request->flags & ATA_R_WRITE);
 
 	    /* issue command */
-	    if (ch->hw.command(request->device, request->u.ata.command,
+	    if (ch->hw.command(atadev, request->u.ata.command,
 			       request->u.ata.lba, request->u.ata.count,
 			       request->u.ata.feature)) {
-		ata_prtdev(request->device, "error issueing %s command\n",
+		device_printf(request->dev, "error issueing %s command\n",
 			   ata_cmd2str(request));
 		request->result = EIO;
 		break;
 	    }
 
 	    /* device reset doesn't interrupt */
-	    if (request->u.ata.command == ATA_ATAPI_RESET) {
+	    if (request->u.ata.command == ATA_DEVICE_RESET) {
 		int timeout = 1000000;
 		do {
 		    DELAY(10);
@@ -119,9 +206,9 @@ ata_begin_transaction(struct ata_request *request)
 
 	    /* if write command output the data */
 	    if (write) {
-		if (ata_wait(request->device,
+		if (ata_wait(ch, atadev,
 			     (ATA_S_READY | ATA_S_DSC | ATA_S_DRQ)) < 0) {
-		    ata_prtdev(request->device,"timeout waiting for write DRQ");
+		    device_printf(request->dev,"timeout waiting for write DRQ");
 		    request->result = EIO;
 		    break;
 		}
@@ -133,18 +220,18 @@ ata_begin_transaction(struct ata_request *request)
     /* ATA DMA data transfer commands */
     case ATA_R_DMA:
 	/* check sanity, setup SG list and DMA engine */
-	if (ch->dma->load(request->device, request->data, request->bytecount,
+	if (ch->dma->load(atadev, request->data, request->bytecount,
 			  request->flags & ATA_R_READ)) {
-	    ata_prtdev(request->device, "setting up DMA failed\n");
+	    device_printf(request->dev, "setting up DMA failed\n");
 	    request->result = EIO;
 	    break;
 	}
 
 	/* issue command */
-	if (ch->hw.command(request->device, request->u.ata.command,
+	if (ch->hw.command(atadev, request->u.ata.command,
 			   request->u.ata.lba, request->u.ata.count,
 			   request->u.ata.feature)) {
-	    ata_prtdev(request->device, "error issueing %s command\n",
+	    device_printf(request->dev, "error issueing %s command\n",
 		       ata_cmd2str(request));
 	    request->result = EIO;
 	    break;
@@ -152,7 +239,7 @@ ata_begin_transaction(struct ata_request *request)
 
 	/* start DMA engine */
 	if (ch->dma->start(ch)) {
-	    ata_prtdev(request->device, "error starting DMA\n");
+	    device_printf(request->dev, "error starting DMA\n");
 	    request->result = EIO;
 	    break;
 	}
@@ -162,8 +249,7 @@ ata_begin_transaction(struct ata_request *request)
     case ATA_R_ATAPI:
 	/* is this just a POLL DSC command ? */
 	if (request->u.atapi.ccb[0] == ATAPI_POLL_DSC) {
-	    ATA_IDX_OUTB(ch, ATA_DRIVE,
-			 ATA_D_IBM | request->device->unit);
+	    ATA_IDX_OUTB(ch, ATA_DRIVE, ATA_D_IBM | atadev->unit);
 	    DELAY(10);
 	    if (!(ATA_IDX_INB(ch, ATA_ALTSTAT)&ATA_S_DSC))
 		request->result = EBUSY;
@@ -171,15 +257,15 @@ ata_begin_transaction(struct ata_request *request)
 	}
 
 	/* start ATAPI operation */
-	if (ch->hw.command(request->device, ATA_PACKET_CMD,
+	if (ch->hw.command(atadev, ATA_PACKET_CMD,
 			   request->transfersize << 8, 0, 0)) {
-	    ata_prtdev(request->device, "error issuing ATA PACKET command\n");
+	    device_printf(request->dev, "error issuing ATA PACKET command\n");
 	    request->result = EIO;
 	    break;
 	}
 
 	/* command interrupt device ? just return and wait for interrupt */
-	if ((request->device->param->config & ATA_DRQ_MASK) == ATA_DRQ_INTR)
+	if ((atadev->param.config & ATA_DRQ_MASK) == ATA_DRQ_INTR)
 	    return ATA_OP_CONTINUES;
 
 	/* wait for ready to write ATAPI command block */
@@ -195,8 +281,7 @@ ata_begin_transaction(struct ata_request *request)
 		DELAY(20);
 	    }
 	    if (timeout <= 0) {
-		ata_prtdev(request->device,
-			   "timeout waiting for ATAPI ready\n");
+		device_printf(request->dev,"timeout waiting for ATAPI ready\n");
 		request->result = EIO;
 		break;
 	    }
@@ -208,15 +293,14 @@ ata_begin_transaction(struct ata_request *request)
 	/* output actual command block */
 	ATA_IDX_OUTSW_STRM(ch, ATA_DATA, 
 			   (int16_t *)request->u.atapi.ccb,
-			   (request->device->param->config & ATA_PROTO_MASK) ==
+			   (atadev->param.config & ATA_PROTO_MASK) ==
 			   ATA_PROTO_ATAPI_12 ? 6 : 8);
 	return ATA_OP_CONTINUES;
 
     case ATA_R_ATAPI|ATA_R_DMA:
 	/* is this just a POLL DSC command ? */
 	if (request->u.atapi.ccb[0] == ATAPI_POLL_DSC) {
-	    ATA_IDX_OUTB(ch, ATA_DRIVE,
-			 ATA_D_IBM | request->device->unit);
+	    ATA_IDX_OUTB(ch, ATA_DRIVE, ATA_D_IBM | atadev->unit);
 	    DELAY(10);
 	    if (!(ATA_IDX_INB(ch, ATA_ALTSTAT)&ATA_S_DSC))
 		request->result = EBUSY;
@@ -224,18 +308,16 @@ ata_begin_transaction(struct ata_request *request)
 	}
 
 	/* check sanity, setup SG list and DMA engine */
-	if (ch->dma->load(request->device,
-						request->data,
-						request->bytecount,
-						request->flags & ATA_R_READ)) {
-	    ata_prtdev(request->device, "setting up DMA failed\n");
+	if (ch->dma->load(atadev, request->data, request->bytecount,
+			  request->flags & ATA_R_READ)) {
+	    device_printf(request->dev, "setting up DMA failed\n");
 	    request->result = EIO;
 	    break;
 	}
 
 	/* start ATAPI operation */
-	if (ch->hw.command(request->device, ATA_PACKET_CMD, 0, 0, ATA_F_DMA)) {
-	    ata_prtdev(request->device, "error issuing ATAPI packet command\n");
+	if (ch->hw.command(atadev, ATA_PACKET_CMD, 0, 0, ATA_F_DMA)) {
+	    device_printf(request->dev, "error issuing ATAPI packet command\n");
 	    request->result = EIO;
 	    break;
 	}
@@ -253,7 +335,7 @@ ata_begin_transaction(struct ata_request *request)
 		DELAY(20);
 	    }
 	    if (timeout <= 0) {
-		ata_prtdev(request->device,"timeout waiting for ATAPI ready\n");
+		device_printf(request->dev,"timeout waiting for ATAPI ready\n");
 		request->result = EIO;
 		break;
 	    }
@@ -265,7 +347,7 @@ ata_begin_transaction(struct ata_request *request)
 	/* output actual command block */
 	ATA_IDX_OUTSW_STRM(ch, ATA_DATA, 
 			   (int16_t *)request->u.atapi.ccb,
-			   (request->device->param->config & ATA_PROTO_MASK) ==
+			   (atadev->param.config & ATA_PROTO_MASK) ==
 			   ATA_PROTO_ATAPI_12 ? 6 : 8);
 
 	/* start DMA engine */
@@ -285,7 +367,8 @@ ata_begin_transaction(struct ata_request *request)
 static int
 ata_end_transaction(struct ata_request *request)
 {
-    struct ata_channel *ch = request->device->channel;
+    struct ata_channel *ch = device_get_softc(device_get_parent(request->dev));
+    struct ata_device *atadev = device_get_softc(request->dev);
     int length;
 
     ATA_DEBUG_RQ(request, "end transaction");
@@ -297,9 +380,11 @@ ata_end_transaction(struct ata_request *request)
 
     /* ATA PIO data transfer and control commands */
     default:
-	/* XXX Doesn't handle the non-PIO case. */
+
+	/* on timeouts we have no data or anything so just return */
 	if (request->flags & ATA_R_TIMEOUT)
-	    return ATA_OP_FINISHED;
+	    //return ATA_OP_FINISHED;
+	    break;
 
 	/* on control commands read back registers to the request struct */
 	if (request->flags & ATA_R_CONTROL) {
@@ -307,21 +392,30 @@ ata_end_transaction(struct ata_request *request)
 	    request->u.ata.lba = ATA_IDX_INB(ch, ATA_SECTOR) |
 				 (ATA_IDX_INB(ch, ATA_CYL_LSB) << 8) |
 				 (ATA_IDX_INB(ch, ATA_CYL_MSB) << 16) |
-	    			 ((ATA_IDX_INB(ch, ATA_DRIVE) & 0x0f) << 24);
+				 ((ATA_IDX_INB(ch, ATA_DRIVE) & 0x0f) << 24);
 	}
 
 	/* if we got an error we are done with the HW */
 	if (request->status & ATA_S_ERROR) {
 	    request->error = ATA_IDX_INB(ch, ATA_ERROR);
-	    return ATA_OP_FINISHED;
+	    //return ATA_OP_FINISHED;
+	    break;
 	}
 	
 	/* are we moving data ? */
 	if (request->flags & (ATA_R_READ | ATA_R_WRITE)) {
 
 	    /* if read data get it */
-	    if (request->flags & ATA_R_READ)
+	    if (request->flags & ATA_R_READ) {
+		if (ata_wait(ch, atadev,
+			     (ATA_S_READY | ATA_S_DSC | ATA_S_DRQ)) < 0) {
+		    device_printf(request->dev, "timeout waiting for read DRQ");
+		    request->result = EIO;
+		    //return ATA_OP_FINISHED;
+		    break;
+		}
 		ata_pio_read(request, request->transfersize);
+	    }
 
 	    /* update how far we've gotten */
 	    request->donecount += request->transfersize;
@@ -334,19 +428,17 @@ ata_end_transaction(struct ata_request *request)
 		    min((request->bytecount - request->donecount),
 			request->transfersize);
 
-		/* clear interrupt seen flag as we need to wait again */
-		request->flags &= ~ATA_R_INTR_SEEN;
-
 		/* if data write command, output the data */
 		if (request->flags & ATA_R_WRITE) {
 
 		    /* if we get an error here we are done with the HW */
-		    if (ata_wait(request->device,
+		    if (ata_wait(ch, atadev,
 				 (ATA_S_READY | ATA_S_DSC | ATA_S_DRQ)) < 0) {
-			ata_prtdev(request->device,
-				   "timeout waiting for write DRQ");
+			device_printf(request->dev,
+				      "timeout waiting for write DRQ");
 			request->status = ATA_IDX_INB(ch, ATA_STATUS);
-			return ATA_OP_FINISHED;
+			//return ATA_OP_FINISHED;
+			break;
 		    }
 
 		    /* output data and return waiting for new interrupt */
@@ -360,7 +452,8 @@ ata_end_transaction(struct ata_request *request)
 	    }
 	}
 	/* done with HW */
-	return ATA_OP_FINISHED;
+	//return ATA_OP_FINISHED;
+	break;
 
     /* ATA DMA data transfer commands */
     case ATA_R_DMA:
@@ -374,19 +467,25 @@ ata_end_transaction(struct ata_request *request)
 	    request->error = ATA_IDX_INB(ch, ATA_ERROR);
 	else if (request->dmastat & ATA_BMSTAT_ERROR)
 	    request->status |= ATA_S_ERROR;
-	else
+	else if (!(request->flags & ATA_R_TIMEOUT))
 	    request->donecount = request->bytecount;
 
 	/* release SG list etc */
 	ch->dma->unload(ch);
 
 	/* done with HW */
-	return ATA_OP_FINISHED;
+	//return ATA_OP_FINISHED;
+	break;
 
     /* ATAPI PIO commands */
     case ATA_R_ATAPI:
 	length = ATA_IDX_INB(ch, ATA_CYL_LSB)|(ATA_IDX_INB(ch, ATA_CYL_MSB)<<8);
 
+	/* on timeouts we have no data or anything so just return */
+	if (request->flags & ATA_R_TIMEOUT)
+	    //return ATA_OP_FINISHED;
+	    break;
+
 	switch ((ATA_IDX_INB(ch, ATA_IREASON) & (ATA_I_CMD | ATA_I_IN)) |
 		(request->status & ATA_S_DRQ)) {
 
@@ -395,12 +494,13 @@ ata_end_transaction(struct ata_request *request)
 	    DELAY(10);
 
 	    if (!(request->status & ATA_S_DRQ)) {
-		ata_prtdev(request->device, "command interrupt without DRQ\n");
+		device_printf(request->dev, "command interrupt without DRQ\n");
 		request->status = ATA_S_ERROR;
-		return ATA_OP_FINISHED;
+		//return ATA_OP_FINISHED;
+		break;
 	    }
 	    ATA_IDX_OUTSW_STRM(ch, ATA_DATA, (int16_t *)request->u.atapi.ccb,
-			       (request->device->param->config &
+			       (atadev->param.config &
 				ATA_PROTO_MASK)== ATA_PROTO_ATAPI_12 ? 6 : 8);
 	    /* return wait for interrupt */
 	    return ATA_OP_CONTINUES;
@@ -408,10 +508,11 @@ ata_end_transaction(struct ata_request *request)
 	case ATAPI_P_WRITE:
 	    if (request->flags & ATA_R_READ) {
 		request->status = ATA_S_ERROR;
-		ata_prtdev(request->device,
-			   "%s trying to write on read buffer\n",
+		device_printf(request->dev,
+			      "%s trying to write on read buffer\n",
 			   ata_cmd2str(request));
-		return ATA_OP_FINISHED;
+		//return ATA_OP_FINISHED;
+		break;
 	    }
 	    ata_pio_write(request, length);
 	    request->donecount += length;
@@ -425,10 +526,11 @@ ata_end_transaction(struct ata_request *request)
 	case ATAPI_P_READ:
 	    if (request->flags & ATA_R_WRITE) {
 		request->status = ATA_S_ERROR;
-		ata_prtdev(request->device,
-			   "%s trying to read on write buffer\n",
+		device_printf(request->dev,
+			      "%s trying to read on write buffer\n",
 			   ata_cmd2str(request));
-		return ATA_OP_FINISHED;
+		//return ATA_OP_FINISHED;
+		break;
 	    }
 	    ata_pio_read(request, length);
 	    request->donecount += length;
@@ -440,9 +542,9 @@ ata_end_transaction(struct ata_request *request)
 	    return ATA_OP_CONTINUES;
 
 	case ATAPI_P_DONEDRQ:
-	    ata_prtdev(request->device,
-		       "WARNING - %s DONEDRQ non conformant device\n",
-		       ata_cmd2str(request));
+	    device_printf(request->dev,
+			  "WARNING - %s DONEDRQ non conformant device\n",
+			  ata_cmd2str(request));
 	    if (request->flags & ATA_R_READ) {
 		ata_pio_read(request, length);
 		request->donecount += length;
@@ -459,15 +561,17 @@ ata_end_transaction(struct ata_request *request)
 	case ATAPI_P_DONE:
 	    if (request->status & (ATA_S_ERROR | ATA_S_DWF))
 		request->error = ATA_IDX_INB(ch, ATA_ERROR);
-	    return ATA_OP_FINISHED;
+	    //return ATA_OP_FINISHED;
+	    break;
 
 	default:
-	    ata_prtdev(request->device, "unknown transfer phase\n");
+	    device_printf(request->dev, "unknown transfer phase\n");
 	    request->status = ATA_S_ERROR;
 	}
 
 	/* done with HW */
-	return ATA_OP_FINISHED;
+	//return ATA_OP_FINISHED;
+	break;
 
     /* ATAPI DMA commands */
     case ATA_R_ATAPI|ATA_R_DMA:
@@ -481,39 +585,44 @@ ata_end_transaction(struct ata_request *request)
 	    request->error = ATA_IDX_INB(ch, ATA_ERROR);
 	else if (request->dmastat & ATA_BMSTAT_ERROR)
 	    request->status |= ATA_S_ERROR;
-	else
+	else if (!(request->flags & ATA_R_TIMEOUT))
 	    request->donecount = request->bytecount;
  
 	/* release SG list etc */
 	ch->dma->unload(ch);
 
 	/* done with HW */
-	return ATA_OP_FINISHED;
+	//return ATA_OP_FINISHED;
+	break;
     }
+
+    /* disable interrupt */
+    //ATA_IDX_OUTB(ch, ATA_ALTSTAT, ATA_A_4BIT | ATA_A_IDS);
+
+    return ATA_OP_FINISHED;
 }
 
 /* must be called with ATA channel locked */
 static void
 ata_generic_reset(struct ata_channel *ch)
 {
-    u_int8_t err = 0, lsb = 0, msb = 0, ostat0, ostat1;
-    u_int8_t stat0 = 0, stat1 = 0;
+    u_int8_t ostat0 = 0, stat0 = 0, ostat1 = 0, stat1 = 0;
+    u_int8_t err = 0, lsb = 0, msb = 0;
     int mask = 0, timeout;
 
     /* if DMA functionality present stop it  */
     if (ch->dma) {
-        if (ch->dma->stop)
-            ch->dma->stop(ch);
-        if (ch->dma->flags & ATA_DMA_LOADED)
-            ch->dma->unload(ch);
+	if (ch->dma->stop)
+	    ch->dma->stop(ch);
+	if (ch->dma->flags & ATA_DMA_LOADED)
+	    ch->dma->unload(ch);
     }
 
     /* reset host end of channel (if supported) */
-    if (ch->reset)
-	ch->reset(ch);
+    ATA_RESET(device_get_parent(ch->dev), ch->dev);
 
     /* do we have any signs of ATA/ATAPI HW being present ? */
-    ATA_IDX_OUTB(ch, ATA_DRIVE, ATA_D_IBM | ATA_MASTER);
+    ATA_IDX_OUTB(ch, ATA_DRIVE, ATA_D_IBM | ATA_D_LBA | ATA_MASTER);
     DELAY(10);
     ostat0 = ATA_IDX_INB(ch, ATA_STATUS);
     if ((ostat0 & 0xf8) != 0xf8 && ostat0 != 0xa5) {
@@ -521,12 +630,11 @@ ata_generic_reset(struct ata_channel *ch)
 	mask |= 0x01;
     }
 
-    ATA_IDX_OUTB(ch, ATA_DRIVE, ATA_D_IBM | ATA_SLAVE);
-    DELAY(10);	
-    ostat1 = ATA_IDX_INB(ch, ATA_STATUS);
-
     /* in some setups we dont want to test for a slave */
     if (!(ch->flags & ATA_NO_SLAVE)) {
+	ATA_IDX_OUTB(ch, ATA_DRIVE, ATA_D_IBM | ATA_D_LBA | ATA_SLAVE);
+	DELAY(10);      
+	ostat1 = ATA_IDX_INB(ch, ATA_STATUS);
 	if ((ostat1 & 0xf8) != 0xf8 && ostat1 != 0xa5) {
 	    stat1 = ATA_S_BUSY;
 	    mask |= 0x02;
@@ -534,8 +642,8 @@ ata_generic_reset(struct ata_channel *ch)
     }
 
     if (bootverbose)
-	ata_printf(ch, -1, "reset tp1 mask=%02x ostat0=%02x ostat1=%02x\n",
-		   mask, ostat0, ostat1);
+	device_printf(ch->dev, "reset tp1 mask=%02x ostat0=%02x ostat1=%02x\n",
+		      mask, ostat0, ostat1);
 
     /* if nothing showed up there is no need to get any further */
     /* SOS is that too strong?, we just might loose devices here XXX */
@@ -544,7 +652,7 @@ ata_generic_reset(struct ata_channel *ch)
 	return;
 
     /* reset (both) devices on this channel */
-    ATA_IDX_OUTB(ch, ATA_DRIVE, ATA_D_IBM | ATA_MASTER);
+    ATA_IDX_OUTB(ch, ATA_DRIVE, ATA_D_IBM | ATA_D_LBA | ATA_MASTER);
     DELAY(10);
     ATA_IDX_OUTB(ch, ATA_ALTSTAT, ATA_A_IDS | ATA_A_RESET);
     ata_udelay(10000); 
@@ -554,17 +662,19 @@ ata_generic_reset(struct ata_channel *ch)
 
     /* wait for BUSY to go inactive */
     for (timeout = 0; timeout < 310; timeout++) {
-	if (stat0 & ATA_S_BUSY) {
+	if ((mask & 0x01) && (stat0 & ATA_S_BUSY)) {
 	    ATA_IDX_OUTB(ch, ATA_DRIVE, ATA_D_IBM | ATA_MASTER);
 	    DELAY(10);
-    	    err = ATA_IDX_INB(ch, ATA_ERROR);
+	    err = ATA_IDX_INB(ch, ATA_ERROR);
 	    lsb = ATA_IDX_INB(ch, ATA_CYL_LSB);
 	    msb = ATA_IDX_INB(ch, ATA_CYL_MSB);
 	    stat0 = ATA_IDX_INB(ch, ATA_STATUS);
 	    if (bootverbose)
-		ata_printf(ch, ATA_MASTER,
-			   "stat=0x%02x err=0x%02x lsb=0x%02x msb=0x%02x\n",
-			   stat0, err, lsb, msb);
+		device_printf(ch->dev,
+			      "stat0=0x%02x err=0x%02x lsb=0x%02x msb=0x%02x\n",
+			      stat0, err, lsb, msb);
+	    if (stat0 == err && lsb == err && msb == err && timeout > 10)
+		mask &= ~0x01;
 	    if (!(stat0 & ATA_S_BUSY)) {
 		if ((err & 0x7f) == ATA_E_ILI) {
 		    if (lsb == ATAPI_MAGIC_LSB && msb == ATAPI_MAGIC_MSB) {
@@ -574,22 +684,25 @@ ata_generic_reset(struct ata_channel *ch)
 			ch->devices |= ATA_ATA_MASTER;
 		    }
 		}
-		else if ((stat0 & 0x4f) && err == lsb && err == msb) {
+		else if ((stat0 & 0x0f) && err == lsb && err == msb) {
 		    stat0 |= ATA_S_BUSY;
 		}
 	    }
 	}
-	if (!((mask == 0x03) && (stat0 & ATA_S_BUSY)) && (stat1 & ATA_S_BUSY)) {
+
+	if ((mask & 0x02) && (stat1 & ATA_S_BUSY)) {
 	    ATA_IDX_OUTB(ch, ATA_DRIVE, ATA_D_IBM | ATA_SLAVE);
 	    DELAY(10);
-    	    err = ATA_IDX_INB(ch, ATA_ERROR);
+	    err = ATA_IDX_INB(ch, ATA_ERROR);
 	    lsb = ATA_IDX_INB(ch, ATA_CYL_LSB);
 	    msb = ATA_IDX_INB(ch, ATA_CYL_MSB);
 	    stat1 = ATA_IDX_INB(ch, ATA_STATUS);
 	    if (bootverbose)
-		ata_printf(ch, ATA_SLAVE,
-			   " stat=0x%02x err=0x%02x lsb=0x%02x msb=0x%02x\n",
-			   stat1, err, lsb, msb);
+		device_printf(ch->dev,
+			      "stat1=0x%02x err=0x%02x lsb=0x%02x msb=0x%02x\n",
+			      stat1, err, lsb, msb);
+	    if (stat1 == err && lsb == err && msb == err && timeout > 10)
+		mask &= ~0x02;
 	    if (!(stat1 & ATA_S_BUSY)) {
 		if ((err & 0x7f) == ATA_E_ILI) {
 		    if (lsb == ATAPI_MAGIC_LSB && msb == ATAPI_MAGIC_MSB) {
@@ -599,44 +712,39 @@ ata_generic_reset(struct ata_channel *ch)
 			ch->devices |= ATA_ATA_SLAVE;
 		    }
 		}
-		else if ((stat1 & 0x4f) && err == lsb && err == msb) {
-		    stat1 |= ATA_S_BUSY;
+		else if ((stat0 & 0x0f) && err == lsb && err == msb) {
+		    stat0 |= ATA_S_BUSY;
 		}
 	    }
 	}
-	if (mask == 0x01)	/* wait for master only */
-	    if (!(stat0 & ATA_S_BUSY) || (stat0 == 0xff && timeout > 5) ||
-		(stat0 == err && lsb == err && msb == err && timeout > 5))
+
+	if (mask == 0x00)       /* nothing to wait for */
+	    break;
+	if (mask == 0x01)       /* wait for master only */
+	    if (!(stat0 & ATA_S_BUSY) || (stat0 == 0xff && timeout > 10))
 		break;
-	if (mask == 0x02)	/* wait for slave only */
-	    if (!(stat1 & ATA_S_BUSY) || (stat1 == 0xff && timeout > 5) ||
-		(stat1 == err && lsb == err && msb == err && timeout > 5))
+	if (mask == 0x02)       /* wait for slave only */
+	    if (!(stat1 & ATA_S_BUSY) || (stat1 == 0xff && timeout > 10))
 		break;
-	if (mask == 0x03) {	/* wait for both master & slave */
+	if (mask == 0x03) {     /* wait for both master & slave */
 	    if (!(stat0 & ATA_S_BUSY) && !(stat1 & ATA_S_BUSY))
 		break;
-	    if ((stat0 == 0xff && timeout > 5) ||
-		(stat0 == err && lsb == err && msb == err && timeout > 5))
+	    if ((stat0 == 0xff) && (timeout > 20))
 		mask &= ~0x01;
-	    if ((stat1 == 0xff && timeout > 5) ||
-		(stat1 == err && lsb == err && msb == err && timeout > 5))
+	    if ((stat1 == 0xff) && (timeout > 20))
 		mask &= ~0x02;
 	}
-	if (mask == 0 && !(stat0 & ATA_S_BUSY) && !(stat1 & ATA_S_BUSY))
-	    break;
-
 	ata_udelay(100000);
     }
 
     if (bootverbose)
-	ata_printf(ch, -1,
-		   "reset tp2 stat0=%02x stat1=%02x devices=0x%b\n",
-		   stat0, stat1, ch->devices,
-		   "\20\4ATAPI_SLAVE\3ATAPI_MASTER\2ATA_SLAVE\1ATA_MASTER");
+	device_printf(ch->dev, "reset tp2 stat0=%02x stat1=%02x devices=0x%b\n",
+		      stat0, stat1, ch->devices,
+		      "\20\4ATAPI_SLAVE\3ATAPI_MASTER\2ATA_SLAVE\1ATA_MASTER");
 }
 
 static int
-ata_wait(struct ata_device *atadev, u_int8_t mask)
+ata_wait(struct ata_channel *ch, struct ata_device *atadev, u_int8_t mask)
 {
     u_int8_t status;
     int timeout = 0;
@@ -645,21 +753,19 @@ ata_wait(struct ata_device *atadev, u_int8_t mask)
 
     /* wait 5 seconds for device to get !BUSY */
     while (timeout < 5000000) {
-	status = ATA_IDX_INB(atadev->channel, ATA_STATUS);
+	status = ATA_IDX_INB(ch, ATA_ALTSTAT);
 
-	/* if drive fails status, reselect the drive just to be sure */
+	/* if drive fails status, reselect the drive and try again */
 	if (status == 0xff) {
-	    ata_prtdev(atadev, "WARNING no status, reselecting device\n");
-	    ATA_IDX_OUTB(atadev->channel, ATA_DRIVE, ATA_D_IBM | atadev->unit);
-	    DELAY(10);
-	    status = ATA_IDX_INB(atadev->channel, ATA_STATUS);
-	    if (status == 0xff)
-		return -1;
+	    ATA_IDX_OUTB(ch, ATA_DRIVE, ATA_D_IBM | atadev->unit);
+	    timeout += 1000;
+	    DELAY(1000);
+	    continue;
 	}
 
 	/* are we done ? */
 	if (!(status & ATA_S_BUSY))
-	    break;	      
+	    break;            
 
 	if (timeout > 1000) {
 	    timeout += 1000;
@@ -669,50 +775,46 @@ ata_wait(struct ata_device *atadev, u_int8_t mask)
 	    timeout += 10;
 	    DELAY(10);
 	}
-    }	 
-    if (timeout >= 5000000)	 
-	return -1;	    
-    if (!mask)	   
-	return (status & ATA_S_ERROR);	 
+    }    
+    if (timeout >= 5000000)      
+	return -2;          
+    if (!mask)     
+	return (status & ATA_S_ERROR);   
 
     DELAY(1);
     
-    /* wait 50 msec for bits wanted */	   
+    /* wait 50 msec for bits wanted */     
     timeout = 5000;
-    while (timeout--) {	  
-	status = ATA_IDX_INB(atadev->channel, ATA_STATUS);
+    while (timeout--) {   
+	status = ATA_IDX_INB(ch, ATA_ALTSTAT);
 	if ((status & mask) == mask) 
-	    return (status & ATA_S_ERROR);	      
-	DELAY (10);	   
-    }	  
-    return -1;	    
+	    return (status & ATA_S_ERROR);            
+	DELAY(10);         
+    }     
+    return -3;      
 }   
 
 int
 ata_generic_command(struct ata_device *atadev, u_int8_t command,
-	    	    u_int64_t lba, u_int16_t count, u_int16_t feature)
+		    u_int64_t lba, u_int16_t count, u_int16_t feature)
 {
-    if (atadebug)
-	ata_prtdev(atadev, "ata_command: addr=%04lx, command=%02x, "
-		   "lba=%jd, count=%d, feature=%d\n",
-		   rman_get_start(atadev->channel->r_io[ATA_DATA].res), 
-		   command, (intmax_t)lba, count, feature);
+    struct ata_channel *ch = device_get_softc(device_get_parent(atadev->dev));
 
     /* select device */
-    ATA_IDX_OUTB(atadev->channel, ATA_DRIVE, ATA_D_IBM | atadev->unit);
+    ATA_IDX_OUTB(ch, ATA_DRIVE, ATA_D_IBM | ATA_D_LBA | atadev->unit);
 
     /* ready to issue command ? */
-    if (ata_wait(atadev, 0) < 0) { 
-	ata_prtdev(atadev, "timeout sending command=%02x\n", command);
+    if (ata_wait(ch, atadev, 0) < 0) { 
+	device_printf(atadev->dev, "timeout sending command=%02x\n", command);
 	return -1;
     }
 
     /* enable interrupt */
-    ATA_IDX_OUTB(atadev->channel, ATA_ALTSTAT, ATA_A_4BIT);
+    ATA_IDX_OUTB(ch, ATA_ALTSTAT, ATA_A_4BIT);
 
     /* only use 48bit addressing if needed (avoid bugs and overhead) */
-    if ((lba >= ATA_MAX_28BIT_LBA || count > 256) && atadev->param && 
-	atadev->param->support.command2 & ATA_SUPPORT_ADDRESS48) {
+    if ((lba >= ATA_MAX_28BIT_LBA || count > 256) &&
+	atadev->param.support.command2 & ATA_SUPPORT_ADDRESS48) {
 
 	/* translate command into 48bit version */
 	switch (command) {
@@ -735,39 +837,54 @@ ata_generic_command(struct ata_device *atadev, u_int8_t command,
 	case ATA_FLUSHCACHE:
 	    command = ATA_FLUSHCACHE48; break;
 	default:
-	    ata_prtdev(atadev, "can't translate cmd to 48bit version\n");
+	    device_printf(atadev->dev,"can't translate cmd to 48bit version\n");
 	    return -1;
 	}
-	ATA_IDX_OUTB(atadev->channel, ATA_FEATURE, (feature>>8) & 0xff);
-	ATA_IDX_OUTB(atadev->channel, ATA_FEATURE, feature & 0xff);
-	ATA_IDX_OUTB(atadev->channel, ATA_COUNT, (count>>8) & 0xff);
-	ATA_IDX_OUTB(atadev->channel, ATA_COUNT, count & 0xff);
-	ATA_IDX_OUTB(atadev->channel, ATA_SECTOR, (lba>>24) & 0xff);
-	ATA_IDX_OUTB(atadev->channel, ATA_SECTOR, lba & 0xff);
-	ATA_IDX_OUTB(atadev->channel, ATA_CYL_LSB, (lba>>32) & 0xff);
-	ATA_IDX_OUTB(atadev->channel, ATA_CYL_LSB, (lba>>8) & 0xff);
-	ATA_IDX_OUTB(atadev->channel, ATA_CYL_MSB, (lba>>40) & 0xff);
-	ATA_IDX_OUTB(atadev->channel, ATA_CYL_MSB, (lba>>16) & 0xff);
-	ATA_IDX_OUTB(atadev->channel, ATA_DRIVE, ATA_D_LBA | atadev->unit);
-	atadev->channel->flags |= ATA_48BIT_ACTIVE;
+	ATA_IDX_OUTB(ch, ATA_FEATURE, (feature>>8) & 0xff);
+	ATA_IDX_OUTB(ch, ATA_FEATURE, feature & 0xff);
+	ATA_IDX_OUTB(ch, ATA_COUNT, (count>>8) & 0xff);
+	ATA_IDX_OUTB(ch, ATA_COUNT, count & 0xff);
+	ATA_IDX_OUTB(ch, ATA_SECTOR, (lba>>24) & 0xff);
+	ATA_IDX_OUTB(ch, ATA_SECTOR, lba & 0xff);
+	ATA_IDX_OUTB(ch, ATA_CYL_LSB, (lba>>32) & 0xff);
+	ATA_IDX_OUTB(ch, ATA_CYL_LSB, (lba>>8) & 0xff);
+	ATA_IDX_OUTB(ch, ATA_CYL_MSB, (lba>>40) & 0xff);
+	ATA_IDX_OUTB(ch, ATA_CYL_MSB, (lba>>16) & 0xff);
+	ATA_IDX_OUTB(ch, ATA_DRIVE, ATA_D_LBA | atadev->unit);
+	ch->flags |= ATA_48BIT_ACTIVE;
     }
     else {
-	ATA_IDX_OUTB(atadev->channel, ATA_FEATURE, feature);
-	ATA_IDX_OUTB(atadev->channel, ATA_COUNT, count);
-	ATA_IDX_OUTB(atadev->channel, ATA_SECTOR, lba & 0xff);
-	ATA_IDX_OUTB(atadev->channel, ATA_CYL_LSB, (lba>>8) & 0xff);
-	ATA_IDX_OUTB(atadev->channel, ATA_CYL_MSB, (lba>>16) & 0xff);
-	if (atadev->flags & ATA_D_USE_CHS)
-	    ATA_IDX_OUTB(atadev->channel, ATA_DRIVE,
-			 ATA_D_IBM | atadev->unit | ((lba>>24) & 0xf));
-	else
-	    ATA_IDX_OUTB(atadev->channel, ATA_DRIVE,
-			 ATA_D_IBM | ATA_D_LBA | atadev->unit|((lba>>24)&0xf));
-	atadev->channel->flags &= ~ATA_48BIT_ACTIVE;
+	ATA_IDX_OUTB(ch, ATA_FEATURE, feature);
+	ATA_IDX_OUTB(ch, ATA_COUNT, count);
+	if (atadev->flags & ATA_D_USE_CHS) {
+	    int heads, sectors;
+
+	    if (atadev->param.atavalid & ATA_FLAG_54_58) {
+		heads = atadev->param.current_heads;
+		sectors = atadev->param.current_sectors;
+	    }
+	    else {
+		heads = atadev->param.heads;
+		sectors = atadev->param.sectors;
+	    }
+	    ATA_IDX_OUTB(ch, ATA_SECTOR, (lba % sectors) + 1);
+	    ATA_IDX_OUTB(ch, ATA_CYL_LSB, (lba / (sectors * heads)));
+	    ATA_IDX_OUTB(ch, ATA_CYL_MSB, (lba / (sectors * heads)) >> 8);
+	    ATA_IDX_OUTB(ch, ATA_DRIVE, ATA_D_IBM | atadev->unit | 
+			 (((lba % (sectors * heads)) / sectors) & 0xf));
+	}
+	else {
+	    ATA_IDX_OUTB(ch, ATA_SECTOR, lba & 0xff);
+	    ATA_IDX_OUTB(ch, ATA_CYL_LSB, (lba>>8) & 0xff);
+	    ATA_IDX_OUTB(ch, ATA_CYL_MSB, (lba>>16) & 0xff);
+	    ATA_IDX_OUTB(ch, ATA_DRIVE, ATA_D_IBM | ATA_D_LBA | atadev->unit |
+					((lba>>24) & 0x0f));
+	}
+	ch->flags &= ~ATA_48BIT_ACTIVE;
     }
 
     /* issue command to controller */
-    ATA_IDX_OUTB(atadev->channel, ATA_CMD, command);
+    ATA_IDX_OUTB(ch, ATA_CMD, command);
 
     return 0;
 }
@@ -775,8 +892,8 @@ ata_generic_command(struct ata_device *atadev, u_int8_t command,
 static void
 ata_pio_read(struct ata_request *request, int length)
 {
+    struct ata_channel *ch = device_get_softc(device_get_parent(request->dev));
     int size = min(request->transfersize, length);
-    struct ata_channel *ch = request->device->channel;
     int resid;
 
     if (ch->flags & ATA_USE_16BIT || (size % sizeof(int32_t)))
@@ -789,7 +906,7 @@ ata_pio_read(struct ata_request *request, int length)
 			  size / sizeof(int32_t));
 
     if (request->transfersize < length) {
-	ata_prtdev(request->device, "WARNING - %s read data overrun %d>%d\n",
+	device_printf(request->dev, "WARNING - %s read data overrun %d>%d\n",
 		   ata_cmd2str(request), length, request->transfersize);
 	for (resid = request->transfersize; resid < length;
 	     resid += sizeof(int16_t))
@@ -800,8 +917,8 @@ ata_pio_read(struct ata_request *request, int length)
 static void
 ata_pio_write(struct ata_request *request, int length)
 {
+    struct ata_channel *ch = device_get_softc(device_get_parent(request->dev));
     int size = min(request->transfersize, length);
-    struct ata_channel *ch = request->device->channel;
     int resid;
 
     if (ch->flags & ATA_USE_16BIT || (size % sizeof(int32_t)))
@@ -814,10 +931,54 @@ ata_pio_write(struct ata_request *request, int length)
 			   size / sizeof(int32_t));
 
     if (request->transfersize < length) {
-	ata_prtdev(request->device, "WARNING - %s write data underrun %d>%d\n",
+	device_printf(request->dev, "WARNING - %s write data underrun %d>%d\n",
 		   ata_cmd2str(request), length, request->transfersize);
 	for (resid = request->transfersize; resid < length;
 	     resid += sizeof(int16_t))
 	    ATA_IDX_OUTW(ch, ATA_DATA, 0);
     }
 }
+
+static void
+bswap(int8_t *buf, int len)
+{
+    u_int16_t *ptr = (u_int16_t*)(buf + len);
+
+    while (--ptr >= (u_int16_t*)buf)
+	*ptr = ntohs(*ptr);
+}
+
+static void
+btrim(int8_t *buf, int len)
+{
+    int8_t *ptr;
+
+    for (ptr = buf; ptr < buf+len; ++ptr)
+	if (!*ptr || *ptr == '_')
+	    *ptr = ' ';
+    for (ptr = buf + len - 1; ptr >= buf && *ptr == ' '; --ptr)
+	*ptr = 0;
+}
+
+static void
+bpack(int8_t *src, int8_t *dst, int len)
+{
+    int i, j, blank;
+
+    for (i = j = blank = 0 ; i < len; i++) {
+	if (blank && src[i] == ' ') continue;
+	if (blank && src[i] != ' ') {
+	    dst[j++] = src[i];
+	    blank = 0;
+	    continue;
+	}
+	if (src[i] == ' ') {
+	    blank = 1;
+	    if (i == 0)
+		continue;
+	}
+	dst[j++] = src[i];
+    }
+    if (j < len)
+	dst[j] = 0x00;
+}