diff --git a/drivers/ata/libata-core.c b/drivers/ata/libata-core.c index 13dcef809e5c..3fd7c7932707 100644 --- a/drivers/ata/libata-core.c +++ b/drivers/ata/libata-core.c @@ -239,6 +239,49 @@ int ata_rwcmd_protocol(struct ata_queued_cmd *qc) return -1; } +/** + * ata_tf_read_block - Read block address from ATA taskfile + * @tf: ATA taskfile of interest + * @dev: ATA device @tf belongs to + * + * LOCKING: + * None. + * + * Read block address from @tf. This function can handle all + * three address formats - LBA, LBA48 and CHS. tf->protocol and + * flags select the address format to use. + * + * RETURNS: + * Block address read from @tf. + */ +u64 ata_tf_read_block(struct ata_taskfile *tf, struct ata_device *dev) +{ + u64 block = 0; + + if (tf->flags & ATA_TFLAG_LBA) { + if (tf->flags & ATA_TFLAG_LBA48) { + block |= (u64)tf->hob_lbah << 40; + block |= (u64)tf->hob_lbam << 32; + block |= tf->hob_lbal << 24; + } else + block |= (tf->device & 0xf) << 24; + + block |= tf->lbah << 16; + block |= tf->lbam << 8; + block |= tf->lbal; + } else { + u32 cyl, head, sect; + + cyl = tf->lbam | (tf->lbah << 8); + head = tf->device & 0xf; + sect = tf->lbal; + + block = (cyl * dev->heads + head) * dev->sectors + sect; + } + + return block; +} + /** * ata_pack_xfermask - Pack pio, mwdma and udma masks into xfer_mask * @pio_mask: pio_mask diff --git a/drivers/ata/libata.h b/drivers/ata/libata.h index be2ac39f013b..2d532da8c398 100644 --- a/drivers/ata/libata.h +++ b/drivers/ata/libata.h @@ -52,6 +52,7 @@ extern int atapi_dmadir; extern int libata_fua; extern struct ata_queued_cmd *ata_qc_new_init(struct ata_device *dev); extern int ata_rwcmd_protocol(struct ata_queued_cmd *qc); +extern u64 ata_tf_read_block(struct ata_taskfile *tf, struct ata_device *dev); extern void ata_dev_disable(struct ata_device *dev); extern void ata_port_flush_task(struct ata_port *ap); extern unsigned ata_exec_internal(struct ata_device *dev,