libata: implement 32-bit transfers for PIO mode

index 1cea18f..8b4ca45 100644
@@ -448,22 +448,76 @@ int ata_task_ioctl(struct scsi_device *scsidev, void __user *arg)
 	return rc;
 }
 
+/**
+ *	ata_get_io32 - Handler for ATA_IOC_GET_IO32 ioctl
+ *	@sdev: SCSI device to get identify data for
+ *	@arg: User buffer area for return value
+ *
+ *	LOCKING:
+ *	Defined by the SCSI layer.  We don't really care.
+ *
+ *	RETURNS:
+ *	Zero on success, negative errno on error.
+ */
+static int ata_get_io32(struct scsi_device *sdev, void __user *arg)
+{
+	struct ata_port *ap = ata_shost_to_port(sdev->host);
+	struct ata_device *dev = ata_scsi_find_dev(ap, sdev);
+	int val;
+
+	if (!dev)
+		return -ENOMSG;
+
+	val = (dev->flags & ATA_DFLAG_32BIT_PIO) != 0;
+	if (copy_to_user(arg, &val, 1))
+		return -EFAULT;
+
+	return 0;
+}
+
+/**
+ *	ata_set_io32 - Handler for ATA_IOC_SET_IO32 ioctl
+ *	@sdev: SCSI device to get identify data for
+ *	@arg: boolean integer value passed by user
+ *
+ *	LOCKING:
+ *	Defined by the SCSI layer.  We don't really care.
+ *
+ *	RETURNS:
+ *	Zero on success, negative errno on error.
+ */
+static int ata_set_io32(struct scsi_device *sdev, void __user *arg)
+{
+	struct ata_port *ap = ata_shost_to_port(sdev->host);
+	struct ata_device *dev = ata_scsi_find_dev(ap, sdev);
+
+	if (!dev)
+		return -ENOMSG;
+
+	switch ((unsigned long) arg) {
+	case 0:
+		dev->flags &= ~ATA_DFLAG_32BIT_PIO;
+		return 0;
+	case 1:
+		dev->flags |=  ATA_DFLAG_32BIT_PIO;
+		return 0;
+	default:
+		break;
+	}
+
+	return -EFAULT;
+}
+
 int ata_scsi_ioctl(struct scsi_device *scsidev, int cmd, void __user *arg)
 {
 	int val = -EINVAL, rc = -EINVAL;
 
 	switch (cmd) {
 	case ATA_IOC_GET_IO32:
-		val = 0;
-		if (copy_to_user(arg, &val, 1))
-			return -EFAULT;
-		return 0;
+		return ata_get_io32(scsidev, arg);
 
 	case ATA_IOC_SET_IO32:
-		val = (unsigned long) arg;
-		if (val != 0)
-			return -EINVAL;
-		return 0;
+		return ata_set_io32(scsidev, arg);
 
 	case HDIO_GET_IDENTITY:
 		return ata_get_identity(scsidev, arg);

index bc5a8d0..ea9e3fc 100644
@@ -138,6 +138,7 @@ enum {
 	ATA_DFLAG_AN		= (1 << 7), /* AN configured */
 	ATA_DFLAG_HIPM		= (1 << 8), /* device supports HIPM */
 	ATA_DFLAG_DIPM		= (1 << 9), /* device supports DIPM */
+	ATA_DFLAG_32BIT_PIO	= (1 << 10), /* device supports 32-bit in PIO mode */
 	ATA_DFLAG_CFG_MASK	= (1 << 12) - 1,
 
 	ATA_DFLAG_PIO		= (1 << 12), /* device limited to PIO mode */

index beaa3a9..2651130 100644
@@ -4936,7 +4936,7 @@ void swap_buf_le16(u16 *buf, unsigned int buf_words)
 }
 
 /**
- *	ata_data_xfer - Transfer data by PIO
+ *	ata_data_xfer - Transfer data by PIO (16 or 32-bit)
  *	@dev: device to target
  *	@buf: data buffer
  *	@buflen: buffer length

@@ -4955,30 +4955,48 @@ unsigned int ata_data_xfer(struct ata_device *dev, unsigned char *buf,
 {
 	struct ata_port *ap = dev->link->ap;
 	void __iomem *data_addr = ap->ioaddr.data_addr;
-	unsigned int words = buflen >> 1;
+	unsigned int words;
 
-	/* Transfer multiple of 2 bytes */
-	if (rw == READ)
-		ioread16_rep(data_addr, buf, words);
-	else
-		iowrite16_rep(data_addr, buf, words);
+	if (dev->flags & ATA_DFLAG_32BIT_PIO) {
+		/* Transfer in 32-bit words */
+		words = buflen >> 2;
+		if (rw == READ)
+			ioread32_rep(data_addr, buf, words);
+		else
+			iowrite32_rep(data_addr, buf, words);
+		words <<= 2;
+	} else {
+		/* Transfer in 16-bit words */
+		words = buflen >> 1;
+		if (rw == READ)
+			ioread16_rep(data_addr, buf, words);
+		else
+			iowrite16_rep(data_addr, buf, words);
+		words <<= 1;
+	}
 
-	/* Transfer trailing 1 byte, if any. */
-	if (unlikely(buflen & 0x01)) {
+	/* Transfer trailing 1, 2 or 3 bytes, if any. We know how many bytes
+	 * remain because <words> now contains the number of bytes transferred.
+	 */
+	while (buflen > words) {
 		__le16 align_buf[1] = { 0 };
-		unsigned char *trailing_buf = buf + buflen - 1;
+		unsigned char *trailing_buf = buf + words;
+		int len = buflen - words;
+
+		if (len > 2)
+			len = 2;
 
 		if (rw == READ) {
 			align_buf[0] = cpu_to_le16(ioread16(data_addr));
-			memcpy(trailing_buf, align_buf, 1);
+			memcpy(trailing_buf, align_buf, len);
 		} else {
-			memcpy(align_buf, trailing_buf, 1);
+			memcpy(align_buf, trailing_buf, len);
 			iowrite16(le16_to_cpu(align_buf[0]), data_addr);
 		}
-		words++;
+		words += len;
 	}
 
-	return words << 1;
+	return words;
 }
 
 /**

index be95fdb..22c2643 100644
@@ -5036,7 +5036,7 @@ void swap_buf_le16(u16 *buf, unsigned int buf_words)
 }
 
 /**
- *	ata_data_xfer - Transfer data by PIO
+ *	ata_data_xfer - Transfer data by PIO (16 or 32-bit)
  *	@dev: device to target
  *	@buf: data buffer
  *	@buflen: buffer length

@@ -5055,30 +5055,48 @@ unsigned int ata_data_xfer(struct ata_device *dev, unsigned char *buf,
 {
 	struct ata_port *ap = dev->link->ap;
 	void __iomem *data_addr = ap->ioaddr.data_addr;
-	unsigned int words = buflen >> 1;
+	unsigned int words;
 
-	/* Transfer multiple of 2 bytes */
-	if (rw == READ)
-		ioread16_rep(data_addr, buf, words);
-	else
-		iowrite16_rep(data_addr, buf, words);
+	if (dev->flags & ATA_DFLAG_32BIT_PIO) {
+		/* Transfer in 32-bit words */
+		words = buflen >> 2;
+		if (rw == READ)
+			ioread32_rep(data_addr, buf, words);
+		else
+			iowrite32_rep(data_addr, buf, words);
+		words <<= 2;
+	} else {
+		/* Transfer in 16-bit words */
+		words = buflen >> 1;
+		if (rw == READ)
+			ioread16_rep(data_addr, buf, words);
+		else
+			iowrite16_rep(data_addr, buf, words);
+		words <<= 1;
+	}
 
-	/* Transfer trailing 1 byte, if any. */
-	if (unlikely(buflen & 0x01)) {
+	/* Transfer trailing 1, 2 or 3 bytes, if any. We know how many bytes
+	 * remain because <words> now contains the number of bytes transferred.
+	 */
+	while (buflen > words) {
 		__le16 align_buf[1] = { 0 };
-		unsigned char *trailing_buf = buf + buflen - 1;
+		unsigned char *trailing_buf = buf + words;
+		int len = buflen - words;
+
+		if (len > 2)
+			len = 2;
 
 		if (rw == READ) {
 			align_buf[0] = cpu_to_le16(ioread16(data_addr));
-			memcpy(trailing_buf, align_buf, 1);
+			memcpy(trailing_buf, align_buf, len);
 		} else {
-			memcpy(align_buf, trailing_buf, 1);
+			memcpy(align_buf, trailing_buf, len);
 			iowrite16(le16_to_cpu(align_buf[0]), data_addr);
 		}
-		words++;
+		words += len;
 	}
 
-	return words << 1;
+	return words;
 }
 
 /**

97 lines not on this page · raw message

index 37ee881..1f1a9be 100644
@@ -139,6 +139,7 @@ enum {
 	ATA_DFLAG_HIPM		= (1 << 8), /* device supports HIPM */
 	ATA_DFLAG_DIPM		= (1 << 9), /* device supports DIPM */
 	ATA_DFLAG_DMADIR	= (1 << 10), /* device requires DMADIR */
+	ATA_DFLAG_32BIT_PIO	= (1 << 11), /* device supports 32-bit in PIO mode */
 	ATA_DFLAG_CFG_MASK	= (1 << 12) - 1,
 
 	ATA_DFLAG_PIO		= (1 << 12), /* device limited to PIO mode */