[libata] arbitrary size ATAPI PIO support

Signed-off-by: Bartlomiej Zolnierkiewicz <bzolnier@gmail.com>

drivers/scsi/libata-core.c

@@ -2195,11 +2195,50 @@ static void ata_pio_sector(struct ata_queued_cmd *qc)
 	kunmap(page);
 }
 
-static void atapi_pio_sector(struct ata_queued_cmd *qc)
+static void __atapi_pio_bytes(struct ata_queued_cmd *qc, unsigned int bytes)
+{
+	int do_write = (qc->tf.flags & ATA_TFLAG_WRITE);
+	struct scatterlist *sg = qc->sg;
+	struct ata_port *ap = qc->ap;
+	struct page *page;
+	unsigned char *buf;
+	unsigned int count;
+
+	if (qc->curbytes == qc->nbytes - bytes)
+		ap->pio_task_state = PIO_ST_LAST;
+
+next_sg:
+	sg = &sg[qc->cursg];
+	page = sg->page;
+
+	count = min(sg_dma_len(sg) - qc->cursg_ofs, bytes);
+	buf = kmap(page) + sg->offset + qc->cursg_ofs;
+
+	bytes -= count;
+	qc->curbytes += count;
+	qc->cursg_ofs += count;
+
+	if (qc->cursg_ofs == sg_dma_len(sg)) {
+		qc->cursg++;
+		qc->cursg_ofs = 0;
+	}
+
+	DPRINTK("data %s\n", qc->tf.flags & ATA_TFLAG_WRITE ? "write" : "read");
+
+	/* do the actual data transfer */
+	ata_data_xfer(ap, buf, count, do_write);
+
+	kunmap(page);
+
+	if (bytes)
+		goto next_sg;
+}
+
+static void atapi_pio_bytes(struct ata_queued_cmd *qc)
 {
 	struct ata_port *ap = qc->ap;
 	struct ata_device *dev = qc->dev;
-	unsigned int i, ireason, bc_lo, bc_hi, bytes;
+	unsigned int ireason, bc_lo, bc_hi, bytes;
 	int i_write, do_write = (qc->tf.flags & ATA_TFLAG_WRITE) ? 1 : 0;
 
 	ap->ops->tf_read(ap, &qc->tf);
@@ -2217,16 +2256,7 @@ static void atapi_pio_sector(struct ata_queued_cmd *qc)
 	if (do_write != i_write)
 		goto err_out;
 
-	/* make sure byte count is multiple of sector size; not
-	* required by standard (warning! warning!), but IDE driver
-	* does this to simplify things a bit.  We are lazy, and
-	* follow suit.
-	*/
-	if (bytes & (ATA_SECT_SIZE - 1))
-		goto err_out;
-
-	for (i = 0; i < (bytes >> 9); i++)
-		ata_pio_sector(qc);
+	__atapi_pio_bytes(qc, bytes);
 
 	return;
 
@@ -2277,7 +2307,7 @@ static void ata_pio_block(struct ata_port *ap)
 	assert(qc != NULL);
 
 	if (is_atapi_taskfile(&qc->tf))
-		atapi_pio_sector(qc);
+		atapi_pio_bytes(qc);
 	else
 		ata_pio_sector(qc);
 }
@@ -2556,6 +2586,7 @@ struct ata_queued_cmd *ata_qc_new_init(struct ata_port *ap,
 		qc->dev = dev;
 		qc->cursect = qc->cursg = qc->cursg_ofs = 0;
 		qc->nsect = 0;
+		qc->nbytes = qc->nbytes = 0;
 
 		ata_tf_init(ap, &qc->tf, dev->devno);
 

drivers/scsi/libata-scsi.c

@@ -1328,6 +1328,8 @@ static unsigned int atapi_xlat(struct ata_queued_cmd *qc, u8 *scsicmd)
 #endif
 	}
 
+	qc->nbytes = cmd->bufflen;
+
 	return 0;
 }
 

include/linux/libata.h

@@ -230,6 +230,10 @@ struct ata_queued_cmd {
 
 	unsigned int		nsect;
 	unsigned int		cursect;
+
+	unsigned int		nbytes;
+	unsigned int		curbytes;
+
 	unsigned int		cursg;
 	unsigned int		cursg_ofs;