Index: uspace/srv/bd/ata_bd/ata_bd.c
===================================================================
--- uspace/srv/bd/ata_bd/ata_bd.c	(revision 1c1657c6584e0aa14b9b724272aeaf4af2c43c01)
+++ uspace/srv/bd/ata_bd/ata_bd.c	(revision 3fa31606ca37510dcbcdc539c90e98e3d04ba370)
@@ -95,4 +95,6 @@
 static int drive_identify(int drive_id, void *buf);
 static void disk_print_summary(disk_t *d);
+static int coord_calc(disk_t *d, uint64_t blk_idx, block_coord_t *bc);
+static void coord_sc_program(const block_coord_t *bc, uint16_t scnt);
 static int wait_status(unsigned set, unsigned n_reset, uint8_t *pstatus,
     unsigned timeout);
@@ -476,14 +478,156 @@
 	uint16_t data;
 	uint8_t status;
-	uint64_t c, h, s, c1, s1;
-	uint64_t idx;
 	uint8_t drv_head;
 	disk_t *d;
+	block_coord_t bc;
 
 	d = &disk[disk_id];
+	bc.h = 0;	/* Silence warning. */
+
+	/* Compute block coordinates. */
+	if (coord_calc(d, blk_idx, &bc) != EOK)
+		return EINVAL;
+
+	/* New value for Drive/Head register */
+	drv_head =
+	    ((disk_id != 0) ? DHR_DRV : 0) |
+	    ((d->amode != am_chs) ? DHR_LBA : 0) |
+	    (bc.h & 0x0f);
+
+	fibril_mutex_lock(&d->lock);
+
+	/* Program a Read Sectors operation. */
+
+	if (wait_status(0, ~SR_BSY, NULL, TIMEOUT_BSY) != EOK) {
+		fibril_mutex_unlock(&d->lock);
+		return EIO;
+	}
+
+	pio_write_8(&cmd->drive_head, drv_head);
+
+	if (wait_status(SR_DRDY, ~SR_BSY, NULL, TIMEOUT_DRDY) != EOK) {
+		fibril_mutex_unlock(&d->lock);
+		return EIO;
+	}
+
+	/* Program block coordinates into the device. */
+	coord_sc_program(&bc, 1);
+
+	pio_write_8(&cmd->command, d->amode == am_lba48 ?
+	    CMD_READ_SECTORS_EXT : CMD_READ_SECTORS);
+
+	if (wait_status(0, ~SR_BSY, &status, TIMEOUT_BSY) != EOK) {
+		fibril_mutex_unlock(&d->lock);
+		return EIO;
+	}
+
+	if ((status & SR_DRQ) != 0) {
+		/* Read data from the device buffer. */
+
+		for (i = 0; i < block_size / 2; i++) {
+			data = pio_read_16(&cmd->data_port);
+			((uint16_t *) buf)[i] = data;
+		}
+	}
+
+	if ((status & SR_ERR) != 0)
+		return EIO;
+
+	fibril_mutex_unlock(&d->lock);
+	return EOK;
+}
+
+/** Write a physical block to the device.
+ *
+ * @param disk_id	Device index (0 or 1)
+ * @param blk_idx	Index of the first block.
+ * @param blk_cnt	Number of blocks to transfer.
+ * @param buf		Buffer holding the data to write.
+ *
+ * @return EOK on success, EIO on error.
+ */
+static int ata_bd_write_block(int disk_id, uint64_t blk_idx, size_t blk_cnt,
+    const void *buf)
+{
+	size_t i;
+	uint8_t status;
+	uint8_t drv_head;
+	disk_t *d;
+	block_coord_t bc;
+
+	d = &disk[disk_id];
+	bc.h = 0;	/* Silence warning. */
+
+	/* Compute block coordinates. */
+	if (coord_calc(d, blk_idx, &bc) != EOK)
+		return EINVAL;
+
+	/* New value for Drive/Head register */
+	drv_head =
+	    ((disk_id != 0) ? DHR_DRV : 0) |
+	    ((d->amode != am_chs) ? DHR_LBA : 0) |
+	    (bc.h & 0x0f);
+
+	fibril_mutex_lock(&d->lock);
+
+	/* Program a Write Sectors operation. */
+
+	if (wait_status(0, ~SR_BSY, NULL, TIMEOUT_BSY) != EOK) {
+		fibril_mutex_unlock(&d->lock);
+		return EIO;
+	}
+
+	pio_write_8(&cmd->drive_head, drv_head);
+
+	if (wait_status(SR_DRDY, ~SR_BSY, NULL, TIMEOUT_DRDY) != EOK) {
+		fibril_mutex_unlock(&d->lock);
+		return EIO;
+	}
+
+	/* Program block coordinates into the device. */
+	coord_sc_program(&bc, 1);
+
+	pio_write_8(&cmd->command, d->amode == am_lba48 ?
+	    CMD_WRITE_SECTORS_EXT : CMD_WRITE_SECTORS);
+
+	if (wait_status(0, ~SR_BSY, &status, TIMEOUT_BSY) != EOK) {
+		fibril_mutex_unlock(&d->lock);
+		return EIO;
+	}
+
+	if ((status & SR_DRQ) != 0) {
+		/* Write data to the device buffer. */
+
+		for (i = 0; i < block_size / 2; i++) {
+			pio_write_16(&cmd->data_port, ((uint16_t *) buf)[i]);
+		}
+	}
+
+	fibril_mutex_unlock(&d->lock);
+
+	if (status & SR_ERR)
+		return EIO;
+
+	return EOK;
+}
+
+/** Calculate block coordinates.
+ *
+ * Calculates block coordinates in the best coordinate system supported
+ * by the device. These can be later programmed into the device using
+ * @c coord_sc_program().
+ *
+ * @return EOK on success or EINVAL if block index is past end of device.
+ */
+static int coord_calc(disk_t *d, uint64_t blk_idx, block_coord_t *bc)
+{
+	uint64_t c;
+	uint64_t idx;
 
 	/* Check device bounds. */
 	if (blk_idx >= d->blocks)
 		return EINVAL;
+
+	bc->amode = d->amode;
 
 	switch (d->amode) {
@@ -493,195 +637,52 @@
 		idx = blk_idx % (d->geom.heads * d->geom.sectors);
 
-		h = idx / d->geom.sectors;
-		s = 1 + (idx % d->geom.sectors);
+		bc->cyl_lo = c & 0xff;
+		bc->cyl_hi = (c >> 8) & 0xff;
+		bc->h      = (idx / d->geom.sectors) & 0x0f;
+		bc->sector = (1 + (idx % d->geom.sectors)) & 0xff;
 		break;
 
 	case am_lba28:
 		/* Compute LBA-28 coordinates. */
-		s = blk_idx & 0xff;		/* bits 0-7 */
-		c = (blk_idx >> 8) & 0xffff;	/* bits 8-23 */
-		h = (blk_idx >> 24) & 0x0f;	/* bits 24-27 */
+		bc->c0 = blk_idx & 0xff;		/* bits 0-7 */
+		bc->c1 = (blk_idx >> 8) & 0xff;		/* bits 8-15 */
+		bc->c2 = (blk_idx >> 16) & 0xff;	/* bits 16-23 */
+		bc->h  = (blk_idx >> 24) & 0x0f;	/* bits 24-27 */
 		break;
 
 	case am_lba48:
 		/* Compute LBA-48 coordinates. */
-		s = blk_idx & 0xff;		/* bits 0-7 */
-		c = (blk_idx >> 8) & 0xffff;	/* bits 8-23 */
-		s1 = (blk_idx >> 24) & 0xff;	/* bits 24-31 */
-		c1 = (blk_idx >> 32) & 0xffff;	/* bits 32-47 */
-		h = 0;
+		bc->c0 = blk_idx & 0xff;		/* bits 0-7 */
+		bc->c1 = (blk_idx >> 8) & 0xff;		/* bits 8-15 */
+		bc->c2 = (blk_idx >> 16) & 0xff;	/* bits 16-23 */
+		bc->c3 = (blk_idx >> 24) & 0xff;	/* bits 24-31 */
+		bc->c4 = (blk_idx >> 32) & 0xff;	/* bits 32-39 */
+		bc->c5 = (blk_idx >> 40) & 0xff;	/* bits 40-47 */
+		bc->h  = 0;
 		break;
 	}
 
-	/* New value for Drive/Head register */
-	drv_head =
-	    ((disk_id != 0) ? DHR_DRV : 0) |
-	    ((d->amode != am_chs) ? DHR_LBA : 0) |
-	    (h & 0x0f);
-
-	fibril_mutex_lock(&d->lock);
-
-	/* Program a Read Sectors operation. */
-
-	if (wait_status(0, ~SR_BSY, NULL, TIMEOUT_BSY) != EOK) {
-		fibril_mutex_unlock(&d->lock);
-		return EIO;
-	}
-
-	pio_write_8(&cmd->drive_head, drv_head);
-
-	if (wait_status(SR_DRDY, ~SR_BSY, NULL, TIMEOUT_DRDY) != EOK) {
-		fibril_mutex_unlock(&d->lock);
-		return EIO;
-	}
-
-	if (d->amode == am_lba48) {
+	return EOK;
+}
+
+/** Program block coordinates and sector count into ATA registers.
+ *
+ * Note that bc->h must be programmed separately into the device/head register.
+ */
+static void coord_sc_program(const block_coord_t *bc, uint16_t scnt)
+{
+	if (bc->amode == am_lba48) {
 		/* Write high-order bits. */
-		pio_write_8(&cmd->sector_count, 0);
-		pio_write_8(&cmd->sector_number, s1);
-		pio_write_8(&cmd->cylinder_low, c1 & 0xff);
-		pio_write_8(&cmd->cylinder_high, c1 >> 16);
+		pio_write_8(&cmd->sector_count, scnt >> 8);
+		pio_write_8(&cmd->sector_number, bc->c3);
+		pio_write_8(&cmd->cylinder_low, bc->c4);
+		pio_write_8(&cmd->cylinder_high, bc->c5);
 	}
 
 	/* Write low-order bits. */
-	pio_write_8(&cmd->sector_count, 1);
-	pio_write_8(&cmd->sector_number, s);
-	pio_write_8(&cmd->cylinder_low, c & 0xff);
-	pio_write_8(&cmd->cylinder_high, c >> 16);
-
-	pio_write_8(&cmd->command, d->amode == am_lba48 ?
-	    CMD_READ_SECTORS_EXT : CMD_READ_SECTORS);
-
-	if (wait_status(0, ~SR_BSY, &status, TIMEOUT_BSY) != EOK) {
-		fibril_mutex_unlock(&d->lock);
-		return EIO;
-	}
-
-	if ((status & SR_DRQ) != 0) {
-		/* Read data from the device buffer. */
-
-		for (i = 0; i < block_size / 2; i++) {
-			data = pio_read_16(&cmd->data_port);
-			((uint16_t *) buf)[i] = data;
-		}
-	}
-
-	if ((status & SR_ERR) != 0)
-		return EIO;
-
-	fibril_mutex_unlock(&d->lock);
-	return EOK;
-}
-
-/** Write a physical block to the device.
- *
- * @param disk_id	Device index (0 or 1)
- * @param blk_idx	Index of the first block.
- * @param blk_cnt	Number of blocks to transfer.
- * @param buf		Buffer holding the data to write.
- *
- * @return EOK on success, EIO on error.
- */
-static int ata_bd_write_block(int disk_id, uint64_t blk_idx, size_t blk_cnt,
-    const void *buf)
-{
-	size_t i;
-	uint8_t status;
-	uint64_t c, h, s, c1, s1;
-	uint64_t idx;
-	uint8_t drv_head;
-	disk_t *d;
-
-	d = &disk[disk_id];
-
-	/* Check device bounds. */
-	if (blk_idx >= d->blocks)
-		return EINVAL;
-
-	switch (d->amode) {
-	case am_chs:
-		/* Compute CHS coordinates. */
-		c = blk_idx / (d->geom.heads * d->geom.sectors);
-		idx = blk_idx % (d->geom.heads * d->geom.sectors);
-
-		h = idx / d->geom.sectors;
-		s = 1 + (idx % d->geom.sectors);
-		break;
-
-	case am_lba28:
-		/* Compute LBA-28 coordinates. */
-		s = blk_idx & 0xff;		/* bits 0-7 */
-		c = (blk_idx >> 8) & 0xffff;	/* bits 8-23 */
-		h = (blk_idx >> 24) & 0x0f;	/* bits 24-27 */
-		break;
-
-	case am_lba48:
-		/* Compute LBA-48 coordinates. */
-		s = blk_idx & 0xff;		/* bits 0-7 */
-		c = (blk_idx >> 8) & 0xffff;	/* bits 8-23 */
-		s1 = (blk_idx >> 24) & 0xff;	/* bits 24-31 */
-		c1 = (blk_idx >> 32) & 0xffff;	/* bits 32-47 */
-		h = 0;
-		break;
-	}
-
-	/* New value for Drive/Head register */
-	drv_head =
-	    ((disk_id != 0) ? DHR_DRV : 0) |
-	    ((d->amode != am_chs) ? DHR_LBA : 0) |
-	    (h & 0x0f);
-
-	fibril_mutex_lock(&d->lock);
-
-	/* Program a Write Sectors operation. */
-
-	if (wait_status(0, ~SR_BSY, NULL, TIMEOUT_BSY) != EOK) {
-		fibril_mutex_unlock(&d->lock);
-		return EIO;
-	}
-
-	pio_write_8(&cmd->drive_head, drv_head);
-
-	if (wait_status(SR_DRDY, ~SR_BSY, NULL, TIMEOUT_DRDY) != EOK) {
-		fibril_mutex_unlock(&d->lock);
-		return EIO;
-	}
-
-	if (d->amode == am_lba48) {
-		/* Write high-order bits. */
-		pio_write_8(&cmd->sector_count, 0);
-		pio_write_8(&cmd->sector_number, s1);
-		pio_write_8(&cmd->cylinder_low, c1 & 0xff);
-		pio_write_8(&cmd->cylinder_high, c1 >> 16);
-	}
-
-	/* Write low-order bits. */
-	pio_write_8(&cmd->sector_count, 1);
-	pio_write_8(&cmd->sector_number, s);
-	pio_write_8(&cmd->cylinder_low, c & 0xff);
-	pio_write_8(&cmd->cylinder_high, c >> 16);
-
-	pio_write_8(&cmd->command, d->amode == am_lba48 ?
-	    CMD_WRITE_SECTORS_EXT : CMD_WRITE_SECTORS);
-
-	if (wait_status(0, ~SR_BSY, &status, TIMEOUT_BSY) != EOK) {
-		fibril_mutex_unlock(&d->lock);
-		return EIO;
-	}
-
-	if ((status & SR_DRQ) != 0) {
-		/* Write data to the device buffer. */
-
-		for (i = 0; i < block_size / 2; i++) {
-			pio_write_16(&cmd->data_port, ((uint16_t *) buf)[i]);
-		}
-	}
-
-	fibril_mutex_unlock(&d->lock);
-
-	if (status & SR_ERR)
-		return EIO;
-
-	return EOK;
+	pio_write_8(&cmd->sector_count, scnt & 0x00ff);
+	pio_write_8(&cmd->sector_number, bc->c0);
+	pio_write_8(&cmd->cylinder_low, bc->c1);
+	pio_write_8(&cmd->cylinder_high, bc->c2);
 }
 
Index: uspace/srv/bd/ata_bd/ata_bd.h
===================================================================
--- uspace/srv/bd/ata_bd/ata_bd.h	(revision 1c1657c6584e0aa14b9b724272aeaf4af2c43c01)
+++ uspace/srv/bd/ata_bd/ata_bd.h	(revision 3fa31606ca37510dcbcdc539c90e98e3d04ba370)
@@ -242,4 +242,31 @@
 };
 
+/** Block coordinates */
+typedef struct {
+	/** Addressing mode used */
+	enum addr_mode amode;
+
+	union {
+		/** CHS coordinates */
+		struct {
+			uint8_t sector;
+			uint8_t cyl_lo;
+			uint8_t cyl_hi;
+		};
+		/** LBA coordinates */
+		struct {
+			uint8_t c0;
+			uint8_t c1;
+			uint8_t c2;
+			uint8_t c3;
+			uint8_t c4;
+			uint8_t c5;
+		};
+	};
+
+	/** Lower 4 bits for device/head register */
+	uint8_t h;
+} block_coord_t;
+
 typedef struct {
 	bool present;