Index: uspace/drv/block/ata_bd/ata_bd.c
===================================================================
--- uspace/drv/block/ata_bd/ata_bd.c	(revision 3de67b4cff4994af4a74b190c5b094f45c15b561)
+++ uspace/drv/block/ata_bd/ata_bd.c	(revision f3386d746b4f2e3d741eb9009428dbd331e2b6b1)
@@ -100,6 +100,6 @@
     const void *buf);
 static int disk_init(ata_ctrl_t *ctrl, disk_t *d, int disk_id);
-static int drive_identify(disk_t *disk, void *buf);
-static int identify_pkt_dev(disk_t *disk, void *buf);
+static int ata_identify_dev(disk_t *disk, void *buf);
+static int ata_identify_pkt_dev(disk_t *disk, void *buf);
 static int ata_cmd_packet(disk_t *disk, const void *cpkt, size_t cpkt_size,
     void *obuf, size_t obuf_size);
@@ -353,5 +353,5 @@
 
 	/* Try identify command. */
-	rc = drive_identify(d, &idata);
+	rc = ata_identify_dev(d, &idata);
 	if (rc == EOK) {
 		/* Success. It's a register (non-packet) device. */
@@ -373,5 +373,5 @@
 
 		if (bc == PDEV_SIGNATURE_BC) {
-			rc = identify_pkt_dev(d, &idata);
+			rc = ata_identify_pkt_dev(d, &idata);
 			if (rc == EOK) {
 				/* We have a packet device. */
@@ -570,5 +570,65 @@
 }
 
-/** Issue IDENTIFY command.
+/** PIO data-in command protocol. */
+static int ata_pio_data_in(disk_t *disk, void *obuf, size_t obuf_size,
+    size_t blk_size, size_t nblocks)
+{
+	ata_ctrl_t *ctrl = disk->ctrl;
+	uint16_t data;
+	size_t i;
+	uint8_t status;
+
+	/* XXX Support multiple blocks */
+	assert(nblocks == 1);
+	assert(blk_size % 2 == 0);
+
+	if (wait_status(ctrl, 0, ~SR_BSY, &status, TIMEOUT_BSY) != EOK)
+		return EIO;
+
+	if ((status & SR_DRQ) != 0) {
+		/* Read data from the device buffer. */
+
+		for (i = 0; i < blk_size / 2; i++) {
+			data = pio_read_16(&ctrl->cmd->data_port);
+			((uint16_t *) obuf)[i] = data;
+		}
+	}
+
+	if ((status & SR_ERR) != 0)
+		return EIO;
+
+	return EOK;
+}
+
+/** PIO data-out command protocol. */
+static int ata_pio_data_out(disk_t *disk, const void *buf, size_t buf_size,
+    size_t blk_size, size_t nblocks)
+{
+	ata_ctrl_t *ctrl = disk->ctrl;
+	size_t i;
+	uint8_t status;
+
+	/* XXX Support multiple blocks */
+	assert(nblocks == 1);
+	assert(blk_size % 2 == 0);
+
+	if (wait_status(ctrl, 0, ~SR_BSY, &status, TIMEOUT_BSY) != EOK)
+		return EIO;
+
+	if ((status & SR_DRQ) != 0) {
+		/* Write data to the device buffer. */
+
+		for (i = 0; i < blk_size / 2; i++) {
+			pio_write_16(&ctrl->cmd->data_port, ((uint16_t *) buf)[i]);
+		}
+	}
+
+	if (status & SR_ERR)
+		return EIO;
+
+	return EOK;
+}
+
+/** Issue IDENTIFY DEVICE command.
  *
  * Reads @c identify data into the provided buffer. This is used to detect
@@ -581,11 +641,9 @@
  *			not present). EIO if device responds with error.
  */
-static int drive_identify(disk_t *disk, void *buf)
+static int ata_identify_dev(disk_t *disk, void *buf)
 {
 	ata_ctrl_t *ctrl = disk->ctrl;
-	uint16_t data;
 	uint8_t status;
 	uint8_t drv_head;
-	size_t i;
 
 	drv_head = ((disk_dev_idx(disk) != 0) ? DHR_DRV : 0);
@@ -613,19 +671,16 @@
 	 * the caller to check for one.
 	 */
-	if ((status & SR_ERR) != 0) {
-		return EIO;
-	}
-
+	if ((status & SR_ERR) != 0)
+		return EIO;
+
+	/*
+	 * For probing purposes we need to wait for some status bit to become
+	 * active - otherwise we could be fooled just by receiving all zeroes.
+	 */
 	if (wait_status(ctrl, SR_DRQ, ~SR_BSY, &status, TIMEOUT_PROBE) != EOK)
 		return ETIMEOUT;
 
-	/* Read data from the disk buffer. */
-
-	for (i = 0; i < identify_data_size / 2; i++) {
-		data = pio_read_16(&ctrl->cmd->data_port);
-		((uint16_t *) buf)[i] = data;
-	}
-
-	return EOK;
+	return ata_pio_data_in(disk, buf, identify_data_size,
+	    identify_data_size, 1);
 }
 
@@ -638,11 +693,8 @@
  * @param buf		Pointer to a 512-byte buffer.
  */
-static int identify_pkt_dev(disk_t *disk, void *buf)
+static int ata_identify_pkt_dev(disk_t *disk, void *buf)
 {
 	ata_ctrl_t *ctrl = disk->ctrl;
-	uint16_t data;
-	uint8_t status;
 	uint8_t drv_head;
-	size_t i;
 
 	drv_head = ((disk_dev_idx(disk) != 0) ? DHR_DRV : 0);
@@ -659,20 +711,6 @@
 	pio_write_8(&ctrl->cmd->command, CMD_IDENTIFY_PKT_DEV);
 
-	if (wait_status(ctrl, 0, ~SR_BSY, &status, TIMEOUT_BSY) != EOK)
-		return EIO;
-
-	/* Read data from the device buffer. */
-
-	if ((status & SR_DRQ) != 0) {
-		for (i = 0; i < identify_data_size / 2; i++) {
-			data = pio_read_16(&ctrl->cmd->data_port);
-			((uint16_t *) buf)[i] = data;
-		}
-	}
-
-	if ((status & SR_ERR) != 0)
-		return EIO;
-
-	return EOK;
+	return ata_pio_data_in(disk, buf, identify_data_size,
+	    identify_data_size, 1);
 }
 
@@ -864,5 +902,5 @@
 }
 
-/** Read a physical from the device.
+/** Read a physical block from the device.
  *
  * @param disk		Disk
@@ -877,9 +915,7 @@
 {
 	ata_ctrl_t *ctrl = disk->ctrl;
-	size_t i;
-	uint16_t data;
-	uint8_t status;
 	uint8_t drv_head;
 	block_coord_t bc;
+	int rc;
 
 	/* Silence warning. */
@@ -918,24 +954,10 @@
 	    CMD_READ_SECTORS_EXT : CMD_READ_SECTORS);
 
-	if (wait_status(ctrl, 0, ~SR_BSY, &status, TIMEOUT_BSY) != EOK) {
-		fibril_mutex_unlock(&ctrl->lock);
-		return EIO;
-	}
-
-	if ((status & SR_DRQ) != 0) {
-		/* Read data from the device buffer. */
-
-		for (i = 0; i < disk->block_size / 2; i++) {
-			data = pio_read_16(&ctrl->cmd->data_port);
-			((uint16_t *) buf)[i] = data;
-		}
-	}
+	rc = ata_pio_data_in(disk, buf, blk_cnt * disk->block_size,
+	    disk->block_size, blk_cnt);
 
 	fibril_mutex_unlock(&ctrl->lock);
 
-	if ((status & SR_ERR) != 0)
-		return EIO;
-
-	return EOK;
+	return rc;
 }
 
@@ -953,8 +975,7 @@
 {
 	ata_ctrl_t *ctrl = disk->ctrl;
-	size_t i;
-	uint8_t status;
 	uint8_t drv_head;
 	block_coord_t bc;
+	int rc;
 
 	/* Silence warning. */
@@ -993,23 +1014,9 @@
 	    CMD_WRITE_SECTORS_EXT : CMD_WRITE_SECTORS);
 
-	if (wait_status(ctrl, 0, ~SR_BSY, &status, TIMEOUT_BSY) != EOK) {
-		fibril_mutex_unlock(&ctrl->lock);
-		return EIO;
-	}
-
-	if ((status & SR_DRQ) != 0) {
-		/* Write data to the device buffer. */
-
-		for (i = 0; i < disk->block_size / 2; i++) {
-			pio_write_16(&ctrl->cmd->data_port, ((uint16_t *) buf)[i]);
-		}
-	}
+	rc = ata_pio_data_out(disk, buf, cnt * disk->block_size,
+	    disk->block_size, cnt);
 
 	fibril_mutex_unlock(&ctrl->lock);
-
-	if (status & SR_ERR)
-		return EIO;
-
-	return EOK;
+	return rc;
 }
 
