Donate to e Foundation | Murena handsets with /e/OS | Own a part of Murena! Learn more

Commit f140f0f1 authored by Kuan Luo's avatar Kuan Luo Committed by Jeff Garzik
Browse files

[libata] sata_nv: add SW NCQ support for MCP51/MCP55/MCP61 



Add the Software NCQ support to sata_nv.c for MCP51/MCP55/MCP61 SATA
controller.  NCQ function is disable by default, you can enable it
with 'swncq=1'.  NCQ will be turned off if the drive is Maxtor on
MCP51 or MCP55 rev 0xa2 platform.

[akpm@linux-foundation.org: build fix]
Signed-off-by: default avatarKuan Luo <kluo@nvidia.com>
Signed-off-by: default avatarPeer Chen <pchen@nvidia.com>
Cc: Zoltan Boszormenyi <zboszor@dunaweb.hu>
Signed-off-by: default avatarAndrew Morton <akpm@linux-foundation.org>
Signed-off-by: default avatarJeff Garzik <jgarzik@redhat.com>
parent 2b9e68f7

drivers/ata/sata_nv.c

+842 −9
Original line number Diff line number Diff line
@@ -169,6 +169,35 @@ enum { 
	NV_ADMA_PORT_REGISTER_MODE	= (1 << 0),

	NV_ADMA_ATAPI_SETUP_COMPLETE	= (1 << 1),



	/* MCP55 reg offset */

	NV_CTL_MCP55			= 0x400,

	NV_INT_STATUS_MCP55		= 0x440,

	NV_INT_ENABLE_MCP55		= 0x444,

	NV_NCQ_REG_MCP55		= 0x448,



	/* MCP55 */

	NV_INT_ALL_MCP55		= 0xffff,

	NV_INT_PORT_SHIFT_MCP55		= 16,	/* each port occupies 16 bits */

	NV_INT_MASK_MCP55		= NV_INT_ALL_MCP55 & 0xfffd,



	/* SWNCQ ENABLE BITS*/

	NV_CTL_PRI_SWNCQ		= 0x02,

	NV_CTL_SEC_SWNCQ		= 0x04,



	/* SW NCQ status bits*/

	NV_SWNCQ_IRQ_DEV		= (1 << 0),

	NV_SWNCQ_IRQ_PM			= (1 << 1),

	NV_SWNCQ_IRQ_ADDED		= (1 << 2),

	NV_SWNCQ_IRQ_REMOVED		= (1 << 3),



	NV_SWNCQ_IRQ_BACKOUT		= (1 << 4),

	NV_SWNCQ_IRQ_SDBFIS		= (1 << 5),

	NV_SWNCQ_IRQ_DHREGFIS		= (1 << 6),

	NV_SWNCQ_IRQ_DMASETUP		= (1 << 7),



	NV_SWNCQ_IRQ_HOTPLUG		= NV_SWNCQ_IRQ_ADDED |

					  NV_SWNCQ_IRQ_REMOVED,



};



/* ADMA Physical Region Descriptor - one SG segment */
@@ -226,6 +255,42 @@ struct nv_host_priv { 
	unsigned long		type;

};



struct defer_queue {

	u32		defer_bits;

	unsigned int	head;

	unsigned int	tail;

	unsigned int	tag[ATA_MAX_QUEUE];

};



enum ncq_saw_flag_list {

	ncq_saw_d2h	= (1U << 0),

	ncq_saw_dmas	= (1U << 1),

	ncq_saw_sdb	= (1U << 2),

	ncq_saw_backout	= (1U << 3),

};



struct nv_swncq_port_priv {

	struct ata_prd	*prd;	 /* our SG list */

	dma_addr_t	prd_dma; /* and its DMA mapping */

	void __iomem	*sactive_block;

	void __iomem	*irq_block;

	void __iomem	*tag_block;

	u32		qc_active;



	unsigned int	last_issue_tag;



	/* fifo circular queue to store deferral command */

	struct defer_queue defer_queue;



	/* for NCQ interrupt analysis */

	u32		dhfis_bits;

	u32		dmafis_bits;

	u32		sdbfis_bits;



	unsigned int	ncq_flags;

};





#define NV_ADMA_CHECK_INTR(GCTL, PORT) ((GCTL) & ( 1 << (19 + (12 * (PORT)))))



static int nv_init_one (struct pci_dev *pdev, const struct pci_device_id *ent);
@@ -263,13 +328,29 @@ static void nv_adma_host_stop(struct ata_host *host); 
static void nv_adma_post_internal_cmd(struct ata_queued_cmd *qc);

static void nv_adma_tf_read(struct ata_port *ap, struct ata_taskfile *tf);



static void nv_mcp55_thaw(struct ata_port *ap);

static void nv_mcp55_freeze(struct ata_port *ap);

static void nv_swncq_error_handler(struct ata_port *ap);

static int nv_swncq_slave_config(struct scsi_device *sdev);

static int nv_swncq_port_start(struct ata_port *ap);

static void nv_swncq_qc_prep(struct ata_queued_cmd *qc);

static void nv_swncq_fill_sg(struct ata_queued_cmd *qc);

static unsigned int nv_swncq_qc_issue(struct ata_queued_cmd *qc);

static void nv_swncq_irq_clear(struct ata_port *ap, u16 fis);

static irqreturn_t nv_swncq_interrupt(int irq, void *dev_instance);

#ifdef CONFIG_PM

static int nv_swncq_port_suspend(struct ata_port *ap, pm_message_t mesg);

static int nv_swncq_port_resume(struct ata_port *ap);

#endif



enum nv_host_type

{

	GENERIC,

	NFORCE2,

	NFORCE3 = NFORCE2,	/* NF2 == NF3 as far as sata_nv is concerned */

	CK804,

	ADMA

	ADMA,

	SWNCQ,

};



static const struct pci_device_id nv_pci_tbl[] = {
@@ -280,13 +361,13 @@ static const struct pci_device_id nv_pci_tbl[] = { 
	{ PCI_VDEVICE(NVIDIA, PCI_DEVICE_ID_NVIDIA_NFORCE_CK804_SATA2), CK804 },

	{ PCI_VDEVICE(NVIDIA, PCI_DEVICE_ID_NVIDIA_NFORCE_MCP04_SATA), CK804 },

	{ PCI_VDEVICE(NVIDIA, PCI_DEVICE_ID_NVIDIA_NFORCE_MCP04_SATA2), CK804 },

	{ PCI_VDEVICE(NVIDIA, PCI_DEVICE_ID_NVIDIA_NFORCE_MCP51_SATA), GENERIC },

	{ PCI_VDEVICE(NVIDIA, PCI_DEVICE_ID_NVIDIA_NFORCE_MCP51_SATA2), GENERIC },

	{ PCI_VDEVICE(NVIDIA, PCI_DEVICE_ID_NVIDIA_NFORCE_MCP55_SATA), GENERIC },

	{ PCI_VDEVICE(NVIDIA, PCI_DEVICE_ID_NVIDIA_NFORCE_MCP55_SATA2), GENERIC },

	{ PCI_VDEVICE(NVIDIA, PCI_DEVICE_ID_NVIDIA_NFORCE_MCP61_SATA), GENERIC },

	{ PCI_VDEVICE(NVIDIA, PCI_DEVICE_ID_NVIDIA_NFORCE_MCP61_SATA2), GENERIC },

	{ PCI_VDEVICE(NVIDIA, PCI_DEVICE_ID_NVIDIA_NFORCE_MCP61_SATA3), GENERIC },

	{ PCI_VDEVICE(NVIDIA, PCI_DEVICE_ID_NVIDIA_NFORCE_MCP51_SATA), SWNCQ },

	{ PCI_VDEVICE(NVIDIA, PCI_DEVICE_ID_NVIDIA_NFORCE_MCP51_SATA2), SWNCQ },

	{ PCI_VDEVICE(NVIDIA, PCI_DEVICE_ID_NVIDIA_NFORCE_MCP55_SATA), SWNCQ },

	{ PCI_VDEVICE(NVIDIA, PCI_DEVICE_ID_NVIDIA_NFORCE_MCP55_SATA2), SWNCQ },

	{ PCI_VDEVICE(NVIDIA, PCI_DEVICE_ID_NVIDIA_NFORCE_MCP61_SATA), SWNCQ },

	{ PCI_VDEVICE(NVIDIA, PCI_DEVICE_ID_NVIDIA_NFORCE_MCP61_SATA2), SWNCQ },

	{ PCI_VDEVICE(NVIDIA, PCI_DEVICE_ID_NVIDIA_NFORCE_MCP61_SATA3), SWNCQ },



	{ } /* terminate list */

};
@@ -339,6 +420,25 @@ static struct scsi_host_template nv_adma_sht = { 
	.bios_param		= ata_std_bios_param,

};



static struct scsi_host_template nv_swncq_sht = {

	.module			= THIS_MODULE,

	.name			= DRV_NAME,

	.ioctl			= ata_scsi_ioctl,

	.queuecommand		= ata_scsi_queuecmd,

	.change_queue_depth	= ata_scsi_change_queue_depth,

	.can_queue		= ATA_MAX_QUEUE,

	.this_id		= ATA_SHT_THIS_ID,

	.sg_tablesize		= LIBATA_MAX_PRD,

	.cmd_per_lun		= ATA_SHT_CMD_PER_LUN,

	.emulated		= ATA_SHT_EMULATED,

	.use_clustering		= ATA_SHT_USE_CLUSTERING,

	.proc_name		= DRV_NAME,

	.dma_boundary		= ATA_DMA_BOUNDARY,

	.slave_configure	= nv_swncq_slave_config,

	.slave_destroy		= ata_scsi_slave_destroy,

	.bios_param		= ata_std_bios_param,

};



static const struct ata_port_operations nv_generic_ops = {

	.tf_load		= ata_tf_load,

	.tf_read		= ata_tf_read,
@@ -444,6 +544,35 @@ static const struct ata_port_operations nv_adma_ops = { 
	.host_stop		= nv_adma_host_stop,

};



static const struct ata_port_operations nv_swncq_ops = {

	.tf_load		= ata_tf_load,

	.tf_read		= ata_tf_read,

	.exec_command		= ata_exec_command,

	.check_status		= ata_check_status,

	.dev_select		= ata_std_dev_select,

	.bmdma_setup		= ata_bmdma_setup,

	.bmdma_start		= ata_bmdma_start,

	.bmdma_stop		= ata_bmdma_stop,

	.bmdma_status		= ata_bmdma_status,

	.qc_defer		= ata_std_qc_defer,

	.qc_prep		= nv_swncq_qc_prep,

	.qc_issue		= nv_swncq_qc_issue,

	.freeze			= nv_mcp55_freeze,

	.thaw			= nv_mcp55_thaw,

	.error_handler		= nv_swncq_error_handler,

	.post_internal_cmd	= ata_bmdma_post_internal_cmd,

	.data_xfer		= ata_data_xfer,

	.irq_clear		= ata_bmdma_irq_clear,

	.irq_on			= ata_irq_on,

	.scr_read		= nv_scr_read,

	.scr_write		= nv_scr_write,

#ifdef CONFIG_PM

	.port_suspend		= nv_swncq_port_suspend,

	.port_resume		= nv_swncq_port_resume,

#endif

	.port_start		= nv_swncq_port_start,

};



static const struct ata_port_info nv_port_info[] = {

	/* generic */

	{
@@ -490,6 +619,18 @@ static const struct ata_port_info nv_port_info[] = { 
		.port_ops	= &nv_adma_ops,

		.irq_handler	= nv_adma_interrupt,

	},

	/* SWNCQ */

	{

		.sht		= &nv_swncq_sht,

		.flags	        = ATA_FLAG_SATA | ATA_FLAG_NO_LEGACY |

				  ATA_FLAG_NCQ,

		.link_flags	= ATA_LFLAG_HRST_TO_RESUME,

		.pio_mask	= NV_PIO_MASK,

		.mwdma_mask	= NV_MWDMA_MASK,

		.udma_mask	= NV_UDMA_MASK,

		.port_ops	= &nv_swncq_ops,

		.irq_handler	= nv_swncq_interrupt,

	},

};



MODULE_AUTHOR("NVIDIA");
@@ -499,6 +640,7 @@ MODULE_DEVICE_TABLE(pci, nv_pci_tbl); 
MODULE_VERSION(DRV_VERSION);



static int adma_enabled = 1;

static int swncq_enabled;



static void nv_adma_register_mode(struct ata_port *ap)

{
@@ -1452,6 +1594,34 @@ static void nv_ck804_thaw(struct ata_port *ap) 
	writeb(mask, mmio_base + NV_INT_ENABLE_CK804);

}



static void nv_mcp55_freeze(struct ata_port *ap)

{

	void __iomem *mmio_base = ap->host->iomap[NV_MMIO_BAR];

	int shift = ap->port_no * NV_INT_PORT_SHIFT_MCP55;

	u32 mask;



	writel(NV_INT_ALL_MCP55 << shift, mmio_base + NV_INT_STATUS_MCP55);



	mask = readl(mmio_base + NV_INT_ENABLE_MCP55);

	mask &= ~(NV_INT_ALL_MCP55 << shift);

	writel(mask, mmio_base + NV_INT_ENABLE_MCP55);

	ata_bmdma_freeze(ap);

}



static void nv_mcp55_thaw(struct ata_port *ap)

{

	void __iomem *mmio_base = ap->host->iomap[NV_MMIO_BAR];

	int shift = ap->port_no * NV_INT_PORT_SHIFT_MCP55;

	u32 mask;



	writel(NV_INT_ALL_MCP55 << shift, mmio_base + NV_INT_STATUS_MCP55);



	mask = readl(mmio_base + NV_INT_ENABLE_MCP55);

	mask |= (NV_INT_MASK_MCP55 << shift);

	writel(mask, mmio_base + NV_INT_ENABLE_MCP55);

	ata_bmdma_thaw(ap);

}



static int nv_hardreset(struct ata_link *link, unsigned int *class,

			unsigned long deadline)

{
@@ -1525,6 +1695,663 @@ static void nv_adma_error_handler(struct ata_port *ap) 
			   nv_hardreset, ata_std_postreset);

}



static void nv_swncq_qc_to_dq(struct ata_port *ap, struct ata_queued_cmd *qc)

{

	struct nv_swncq_port_priv *pp = ap->private_data;

	struct defer_queue *dq = &pp->defer_queue;



	/* queue is full */

	WARN_ON(dq->tail - dq->head == ATA_MAX_QUEUE);

	dq->defer_bits |= (1 << qc->tag);

	dq->tag[dq->tail++ & (ATA_MAX_QUEUE - 1)] = qc->tag;

}



static struct ata_queued_cmd *nv_swncq_qc_from_dq(struct ata_port *ap)

{

	struct nv_swncq_port_priv *pp = ap->private_data;

	struct defer_queue *dq = &pp->defer_queue;

	unsigned int tag;



	if (dq->head == dq->tail)	/* null queue */

		return NULL;



	tag = dq->tag[dq->head & (ATA_MAX_QUEUE - 1)];

	dq->tag[dq->head++ & (ATA_MAX_QUEUE - 1)] = ATA_TAG_POISON;

	WARN_ON(!(dq->defer_bits & (1 << tag)));

	dq->defer_bits &= ~(1 << tag);



	return ata_qc_from_tag(ap, tag);

}



static void nv_swncq_fis_reinit(struct ata_port *ap)

{

	struct nv_swncq_port_priv *pp = ap->private_data;



	pp->dhfis_bits = 0;

	pp->dmafis_bits = 0;

	pp->sdbfis_bits = 0;

	pp->ncq_flags = 0;

}



static void nv_swncq_pp_reinit(struct ata_port *ap)

{

	struct nv_swncq_port_priv *pp = ap->private_data;

	struct defer_queue *dq = &pp->defer_queue;



	dq->head = 0;

	dq->tail = 0;

	dq->defer_bits = 0;

	pp->qc_active = 0;

	pp->last_issue_tag = ATA_TAG_POISON;

	nv_swncq_fis_reinit(ap);

}



static void nv_swncq_irq_clear(struct ata_port *ap, u16 fis)

{

	struct nv_swncq_port_priv *pp = ap->private_data;



	writew(fis, pp->irq_block);

}



static void __ata_bmdma_stop(struct ata_port *ap)

{

	struct ata_queued_cmd qc;



	qc.ap = ap;

	ata_bmdma_stop(&qc);

}



static void nv_swncq_ncq_stop(struct ata_port *ap)

{

	struct nv_swncq_port_priv *pp = ap->private_data;

	unsigned int i;

	u32 sactive;

	u32 done_mask;



	ata_port_printk(ap, KERN_ERR,

			"EH in SWNCQ mode,QC:qc_active 0x%X sactive 0x%X\n",

			ap->qc_active, ap->link.sactive);

	ata_port_printk(ap, KERN_ERR,

		"SWNCQ:qc_active 0x%X defer_bits 0x%X last_issue_tag 0x%x\n  "

		"dhfis 0x%X dmafis 0x%X sdbfis 0x%X\n",

		pp->qc_active, pp->defer_queue.defer_bits, pp->last_issue_tag,

		pp->dhfis_bits, pp->dmafis_bits, pp->sdbfis_bits);



	ata_port_printk(ap, KERN_ERR, "ATA_REG 0x%X ERR_REG 0x%X\n",

			ap->ops->check_status(ap),

			ioread8(ap->ioaddr.error_addr));



	sactive = readl(pp->sactive_block);

	done_mask = pp->qc_active ^ sactive;



	ata_port_printk(ap, KERN_ERR, "tag : dhfis dmafis sdbfis sacitve\n");

	for (i = 0; i < ATA_MAX_QUEUE; i++) {

		u8 err = 0;

		if (pp->qc_active & (1 << i))

			err = 0;

		else if (done_mask & (1 << i))

			err = 1;

		else

			continue;



		ata_port_printk(ap, KERN_ERR,

				"tag 0x%x: %01x %01x %01x %01x %s\n", i,

				(pp->dhfis_bits >> i) & 0x1,

				(pp->dmafis_bits >> i) & 0x1,

				(pp->sdbfis_bits >> i) & 0x1,

				(sactive >> i) & 0x1,

				(err ? "error! tag doesn't exit" : " "));

	}



	nv_swncq_pp_reinit(ap);

	ap->ops->irq_clear(ap);

	__ata_bmdma_stop(ap);

	nv_swncq_irq_clear(ap, 0xffff);

}



static void nv_swncq_error_handler(struct ata_port *ap)

{

	struct ata_eh_context *ehc = &ap->link.eh_context;



	if (ap->link.sactive) {

		nv_swncq_ncq_stop(ap);

		ehc->i.action |= ATA_EH_HARDRESET;

	}



	ata_bmdma_drive_eh(ap, ata_std_prereset, ata_std_softreset,

			   nv_hardreset, ata_std_postreset);

}



#ifdef CONFIG_PM

static int nv_swncq_port_suspend(struct ata_port *ap, pm_message_t mesg)

{

	void __iomem *mmio = ap->host->iomap[NV_MMIO_BAR];

	u32 tmp;



	/* clear irq */

	writel(~0, mmio + NV_INT_STATUS_MCP55);



	/* disable irq */

	writel(0, mmio + NV_INT_ENABLE_MCP55);



	/* disable swncq */

	tmp = readl(mmio + NV_CTL_MCP55);

	tmp &= ~(NV_CTL_PRI_SWNCQ | NV_CTL_SEC_SWNCQ);

	writel(tmp, mmio + NV_CTL_MCP55);



	return 0;

}



static int nv_swncq_port_resume(struct ata_port *ap)

{

	void __iomem *mmio = ap->host->iomap[NV_MMIO_BAR];

	u32 tmp;



	/* clear irq */

	writel(~0, mmio + NV_INT_STATUS_MCP55);



	/* enable irq */

	writel(0x00fd00fd, mmio + NV_INT_ENABLE_MCP55);



	/* enable swncq */

	tmp = readl(mmio + NV_CTL_MCP55);

	writel(tmp | NV_CTL_PRI_SWNCQ | NV_CTL_SEC_SWNCQ, mmio + NV_CTL_MCP55);



	return 0;

}

#endif



static void nv_swncq_host_init(struct ata_host *host)

{

	u32 tmp;

	void __iomem *mmio = host->iomap[NV_MMIO_BAR];

	struct pci_dev *pdev = to_pci_dev(host->dev);

	u8 regval;



	/* disable  ECO 398 */

	pci_read_config_byte(pdev, 0x7f, &regval);

	regval &= ~(1 << 7);

	pci_write_config_byte(pdev, 0x7f, regval);



	/* enable swncq */

	tmp = readl(mmio + NV_CTL_MCP55);

	VPRINTK("HOST_CTL:0x%X\n", tmp);

	writel(tmp | NV_CTL_PRI_SWNCQ | NV_CTL_SEC_SWNCQ, mmio + NV_CTL_MCP55);



	/* enable irq intr */

	tmp = readl(mmio + NV_INT_ENABLE_MCP55);

	VPRINTK("HOST_ENABLE:0x%X\n", tmp);

	writel(tmp | 0x00fd00fd, mmio + NV_INT_ENABLE_MCP55);



	/*  clear port irq */

	writel(~0x0, mmio + NV_INT_STATUS_MCP55);

}



static int nv_swncq_slave_config(struct scsi_device *sdev)

{

	struct ata_port *ap = ata_shost_to_port(sdev->host);

	struct pci_dev *pdev = to_pci_dev(ap->host->dev);

	struct ata_device *dev;

	int rc;

	u8 rev;

	u8 check_maxtor = 0;

	unsigned char model_num[ATA_ID_PROD_LEN + 1];



	rc = ata_scsi_slave_config(sdev);

	if (sdev->id >= ATA_MAX_DEVICES || sdev->channel || sdev->lun)

		/* Not a proper libata device, ignore */

		return rc;



	dev = &ap->link.device[sdev->id];

	if (!(ap->flags & ATA_FLAG_NCQ) || dev->class == ATA_DEV_ATAPI)

		return rc;



	/* if MCP51 and Maxtor, then disable ncq */

	if (pdev->device == PCI_DEVICE_ID_NVIDIA_NFORCE_MCP51_SATA ||

		pdev->device == PCI_DEVICE_ID_NVIDIA_NFORCE_MCP51_SATA2)

		check_maxtor = 1;



	/* if MCP55 and rev <= a2 and Maxtor, then disable ncq */

	if (pdev->device == PCI_DEVICE_ID_NVIDIA_NFORCE_MCP55_SATA ||

		pdev->device == PCI_DEVICE_ID_NVIDIA_NFORCE_MCP55_SATA2) {

		pci_read_config_byte(pdev, 0x8, &rev);

		if (rev <= 0xa2)

			check_maxtor = 1;

	}



	if (!check_maxtor)

		return rc;



	ata_id_c_string(dev->id, model_num, ATA_ID_PROD, sizeof(model_num));



	if (strncmp(model_num, "Maxtor", 6) == 0) {

		ata_scsi_change_queue_depth(sdev, 1);

		ata_dev_printk(dev, KERN_NOTICE,

			"Disabling SWNCQ mode (depth %x)\n", sdev->queue_depth);

	}



	return rc;

}



static int nv_swncq_port_start(struct ata_port *ap)

{

	struct device *dev = ap->host->dev;

	void __iomem *mmio = ap->host->iomap[NV_MMIO_BAR];

	struct nv_swncq_port_priv *pp;

	int rc;



	rc = ata_port_start(ap);

	if (rc)

		return rc;



	pp = devm_kzalloc(dev, sizeof(*pp), GFP_KERNEL);

	if (!pp)

		return -ENOMEM;



	pp->prd = dmam_alloc_coherent(dev, ATA_PRD_TBL_SZ * ATA_MAX_QUEUE,

				      &pp->prd_dma, GFP_KERNEL);

	if (!pp->prd)

		return -ENOMEM;

	memset(pp->prd, 0, ATA_PRD_TBL_SZ * ATA_MAX_QUEUE);



	ap->private_data = pp;

	pp->sactive_block = ap->ioaddr.scr_addr + 4 * SCR_ACTIVE;

	pp->irq_block = mmio + NV_INT_STATUS_MCP55 + ap->port_no * 2;

	pp->tag_block = mmio + NV_NCQ_REG_MCP55 + ap->port_no * 2;



	return 0;

}



static void nv_swncq_qc_prep(struct ata_queued_cmd *qc)

{

	if (qc->tf.protocol != ATA_PROT_NCQ) {

		ata_qc_prep(qc);

		return;

	}



	if (!(qc->flags & ATA_QCFLAG_DMAMAP))

		return;



	nv_swncq_fill_sg(qc);

}



static void nv_swncq_fill_sg(struct ata_queued_cmd *qc)

{

	struct ata_port *ap = qc->ap;

	struct scatterlist *sg;

	unsigned int idx;

	struct nv_swncq_port_priv *pp = ap->private_data;

	struct ata_prd *prd;



	WARN_ON(qc->__sg == NULL);

	WARN_ON(qc->n_elem == 0 && qc->pad_len == 0);



	prd = pp->prd + ATA_MAX_PRD * qc->tag;



	idx = 0;

	ata_for_each_sg(sg, qc) {

		u32 addr, offset;

		u32 sg_len, len;



		addr = (u32)sg_dma_address(sg);

		sg_len = sg_dma_len(sg);



		while (sg_len) {

			offset = addr & 0xffff;

			len = sg_len;

			if ((offset + sg_len) > 0x10000)

				len = 0x10000 - offset;



			prd[idx].addr = cpu_to_le32(addr);

			prd[idx].flags_len = cpu_to_le32(len & 0xffff);



			idx++;

			sg_len -= len;

			addr += len;

		}

	}



	if (idx)

		prd[idx - 1].flags_len |= cpu_to_le32(ATA_PRD_EOT);

}



static unsigned int nv_swncq_issue_atacmd(struct ata_port *ap,

					  struct ata_queued_cmd *qc)

{

	struct nv_swncq_port_priv *pp = ap->private_data;



	if (qc == NULL)

		return 0;



	DPRINTK("Enter\n");



	writel((1 << qc->tag), pp->sactive_block);

	pp->last_issue_tag = qc->tag;

	pp->dhfis_bits &= ~(1 << qc->tag);

	pp->dmafis_bits &= ~(1 << qc->tag);

	pp->qc_active |= (0x1 << qc->tag);



	ap->ops->tf_load(ap, &qc->tf);	 /* load tf registers */

	ap->ops->exec_command(ap, &qc->tf);



	DPRINTK("Issued tag %u\n", qc->tag);



	return 0;

}



static unsigned int nv_swncq_qc_issue(struct ata_queued_cmd *qc)

{

	struct ata_port *ap = qc->ap;

	struct nv_swncq_port_priv *pp = ap->private_data;



	if (qc->tf.protocol != ATA_PROT_NCQ)

		return ata_qc_issue_prot(qc);



	DPRINTK("Enter\n");



	if (!pp->qc_active)

		nv_swncq_issue_atacmd(ap, qc);

	else

		nv_swncq_qc_to_dq(ap, qc);	/* add qc to defer queue */



	return 0;

}



static void nv_swncq_hotplug(struct ata_port *ap, u32 fis)

{

	u32 serror;

	struct ata_eh_info *ehi = &ap->link.eh_info;



	ata_ehi_clear_desc(ehi);



	/* AHCI needs SError cleared; otherwise, it might lock up */

	sata_scr_read(&ap->link, SCR_ERROR, &serror);

	sata_scr_write(&ap->link, SCR_ERROR, serror);



	/* analyze @irq_stat */

	if (fis & NV_SWNCQ_IRQ_ADDED)

		ata_ehi_push_desc(ehi, "hot plug");

	else if (fis & NV_SWNCQ_IRQ_REMOVED)

		ata_ehi_push_desc(ehi, "hot unplug");



	ata_ehi_hotplugged(ehi);



	/* okay, let's hand over to EH */

	ehi->serror |= serror;



	ata_port_freeze(ap);

}



static int nv_swncq_sdbfis(struct ata_port *ap)

{

	struct ata_queued_cmd *qc;

	struct nv_swncq_port_priv *pp = ap->private_data;

	struct ata_eh_info *ehi = &ap->link.eh_info;

	u32 sactive;

	int nr_done = 0;

	u32 done_mask;

	int i;

	u8 host_stat;

	u8 lack_dhfis = 0;



	host_stat = ap->ops->bmdma_status(ap);

	if (unlikely(host_stat & ATA_DMA_ERR)) {

		/* error when transfering data to/from memory */

		ata_ehi_clear_desc(ehi);

		ata_ehi_push_desc(ehi, "BMDMA stat 0x%x", host_stat);

		ehi->err_mask |= AC_ERR_HOST_BUS;

		ehi->action |= ATA_EH_SOFTRESET;

		return -EINVAL;

	}



	ap->ops->irq_clear(ap);

	__ata_bmdma_stop(ap);



	sactive = readl(pp->sactive_block);

	done_mask = pp->qc_active ^ sactive;



	if (unlikely(done_mask & sactive)) {

		ata_ehi_clear_desc(ehi);

		ata_ehi_push_desc(ehi, "illegal SWNCQ:qc_active transition"

				  "(%08x->%08x)", pp->qc_active, sactive);

		ehi->err_mask |= AC_ERR_HSM;

		ehi->action |= ATA_EH_HARDRESET;

		return -EINVAL;

	}

	for (i = 0; i < ATA_MAX_QUEUE; i++) {

		if (!(done_mask & (1 << i)))

			continue;



		qc = ata_qc_from_tag(ap, i);

		if (qc) {

			ata_qc_complete(qc);

			pp->qc_active &= ~(1 << i);

			pp->dhfis_bits &= ~(1 << i);

			pp->dmafis_bits &= ~(1 << i);

			pp->sdbfis_bits |= (1 << i);

			nr_done++;

		}

	}



	if (!ap->qc_active) {

		DPRINTK("over\n");

		nv_swncq_pp_reinit(ap);

		return nr_done;

	}



	if (pp->qc_active & pp->dhfis_bits)

		return nr_done;



	if ((pp->ncq_flags & ncq_saw_backout) ||

	    (pp->qc_active ^ pp->dhfis_bits))

		/* if the controller cann't get a device to host register FIS,

		 * The driver needs to reissue the new command.

		 */

		lack_dhfis = 1;



	DPRINTK("id 0x%x QC: qc_active 0x%x,"

		"SWNCQ:qc_active 0x%X defer_bits %X "

		"dhfis 0x%X dmafis 0x%X last_issue_tag %x\n",

		ap->print_id, ap->qc_active, pp->qc_active,

		pp->defer_queue.defer_bits, pp->dhfis_bits,

		pp->dmafis_bits, pp->last_issue_tag);



	nv_swncq_fis_reinit(ap);



	if (lack_dhfis) {

		qc = ata_qc_from_tag(ap, pp->last_issue_tag);

		nv_swncq_issue_atacmd(ap, qc);

		return nr_done;

	}



	if (pp->defer_queue.defer_bits) {

		/* send deferral queue command */

		qc = nv_swncq_qc_from_dq(ap);

		WARN_ON(qc == NULL);

		nv_swncq_issue_atacmd(ap, qc);

	}



	return nr_done;

}



static inline u32 nv_swncq_tag(struct ata_port *ap)

{

	struct nv_swncq_port_priv *pp = ap->private_data;

	u32 tag;



	tag = readb(pp->tag_block) >> 2;

	return (tag & 0x1f);

}



static int nv_swncq_dmafis(struct ata_port *ap)

{

	struct ata_queued_cmd *qc;

	unsigned int rw;

	u8 dmactl;

	u32 tag;

	struct nv_swncq_port_priv *pp = ap->private_data;



	__ata_bmdma_stop(ap);

	tag = nv_swncq_tag(ap);



	DPRINTK("dma setup tag 0x%x\n", tag);

	qc = ata_qc_from_tag(ap, tag);



	if (unlikely(!qc))

		return 0;



	rw = qc->tf.flags & ATA_TFLAG_WRITE;



	/* load PRD table addr. */

	iowrite32(pp->prd_dma + ATA_PRD_TBL_SZ * qc->tag,

		  ap->ioaddr.bmdma_addr + ATA_DMA_TABLE_OFS);



	/* specify data direction, triple-check start bit is clear */

	dmactl = ioread8(ap->ioaddr.bmdma_addr + ATA_DMA_CMD);

	dmactl &= ~ATA_DMA_WR;

	if (!rw)

		dmactl |= ATA_DMA_WR;



	iowrite8(dmactl | ATA_DMA_START, ap->ioaddr.bmdma_addr + ATA_DMA_CMD);



	return 1;

}



static void nv_swncq_host_interrupt(struct ata_port *ap, u16 fis)

{

	struct nv_swncq_port_priv *pp = ap->private_data;

	struct ata_queued_cmd *qc;

	struct ata_eh_info *ehi = &ap->link.eh_info;

	u32 serror;

	u8 ata_stat;

	int rc = 0;



	ata_stat = ap->ops->check_status(ap);

	nv_swncq_irq_clear(ap, fis);

	if (!fis)

		return;



	if (ap->pflags & ATA_PFLAG_FROZEN)

		return;



	if (fis & NV_SWNCQ_IRQ_HOTPLUG) {

		nv_swncq_hotplug(ap, fis);

		return;

	}



	if (!pp->qc_active)

		return;



	if (ap->ops->scr_read(ap, SCR_ERROR, &serror))

		return;

	ap->ops->scr_write(ap, SCR_ERROR, serror);



	if (ata_stat & ATA_ERR) {

		ata_ehi_clear_desc(ehi);

		ata_ehi_push_desc(ehi, "Ata error. fis:0x%X", fis);

		ehi->err_mask |= AC_ERR_DEV;

		ehi->serror |= serror;

		ehi->action |= ATA_EH_SOFTRESET;

		ata_port_freeze(ap);

		return;

	}



	if (fis & NV_SWNCQ_IRQ_BACKOUT) {

		/* If the IRQ is backout, driver must issue

		 * the new command again some time later.

		 */

		pp->ncq_flags |= ncq_saw_backout;

	}



	if (fis & NV_SWNCQ_IRQ_SDBFIS) {

		pp->ncq_flags |= ncq_saw_sdb;

		DPRINTK("id 0x%x SWNCQ: qc_active 0x%X "

			"dhfis 0x%X dmafis 0x%X sactive 0x%X\n",

			ap->print_id, pp->qc_active, pp->dhfis_bits,

			pp->dmafis_bits, readl(pp->sactive_block));

		rc = nv_swncq_sdbfis(ap);

		if (rc < 0)

			goto irq_error;

	}



	if (fis & NV_SWNCQ_IRQ_DHREGFIS) {

		/* The interrupt indicates the new command

		 * was transmitted correctly to the drive.

		 */

		pp->dhfis_bits |= (0x1 << pp->last_issue_tag);

		pp->ncq_flags |= ncq_saw_d2h;

		if (pp->ncq_flags & (ncq_saw_sdb | ncq_saw_backout)) {

			ata_ehi_push_desc(ehi, "illegal fis transaction");

			ehi->err_mask |= AC_ERR_HSM;

			ehi->action |= ATA_EH_HARDRESET;

			goto irq_error;

		}



		if (!(fis & NV_SWNCQ_IRQ_DMASETUP) &&

		    !(pp->ncq_flags & ncq_saw_dmas)) {

			ata_stat = ap->ops->check_status(ap);

			if (ata_stat & ATA_BUSY)

				goto irq_exit;



			if (pp->defer_queue.defer_bits) {

				DPRINTK("send next command\n");

				qc = nv_swncq_qc_from_dq(ap);

				nv_swncq_issue_atacmd(ap, qc);

			}

		}

	}



	if (fis & NV_SWNCQ_IRQ_DMASETUP) {

		/* program the dma controller with appropriate PRD buffers

		 * and start the DMA transfer for requested command.

		 */

		pp->dmafis_bits |= (0x1 << nv_swncq_tag(ap));

		pp->ncq_flags |= ncq_saw_dmas;

		rc = nv_swncq_dmafis(ap);

	}



irq_exit:

	return;

irq_error:

	ata_ehi_push_desc(ehi, "fis:0x%x", fis);

	ata_port_freeze(ap);

	return;

}



static irqreturn_t nv_swncq_interrupt(int irq, void *dev_instance)

{

	struct ata_host *host = dev_instance;

	unsigned int i;

	unsigned int handled = 0;

	unsigned long flags;

	u32 irq_stat;



	spin_lock_irqsave(&host->lock, flags);



	irq_stat = readl(host->iomap[NV_MMIO_BAR] + NV_INT_STATUS_MCP55);



	for (i = 0; i < host->n_ports; i++) {

		struct ata_port *ap = host->ports[i];



		if (ap && !(ap->flags & ATA_FLAG_DISABLED)) {

			if (ap->link.sactive) {

				nv_swncq_host_interrupt(ap, (u16)irq_stat);

				handled = 1;

			} else {

				if (irq_stat)	/* reserve Hotplug */

					nv_swncq_irq_clear(ap, 0xfff0);



				handled += nv_host_intr(ap, (u8)irq_stat);

			}

		}

		irq_stat >>= NV_INT_PORT_SHIFT_MCP55;

	}



	spin_unlock_irqrestore(&host->lock, flags);



	return IRQ_RETVAL(handled);

}



static int nv_init_one (struct pci_dev *pdev, const struct pci_device_id *ent)

{

	static int printed_version = 0;
@@ -1551,7 +2378,7 @@ static int nv_init_one (struct pci_dev *pdev, const struct pci_device_id *ent) 
		return rc;



	/* determine type and allocate host */

	if (type >= CK804 && adma_enabled) {

	if (type == CK804 && adma_enabled) {

		dev_printk(KERN_NOTICE, &pdev->dev, "Using ADMA mode\n");

		type = ADMA;

	}
@@ -1597,6 +2424,9 @@ static int nv_init_one (struct pci_dev *pdev, const struct pci_device_id *ent) 
		rc = nv_adma_host_init(host);

		if (rc)

			return rc;

	} else if (type == SWNCQ && swncq_enabled) {

		dev_printk(KERN_NOTICE, &pdev->dev, "Using SWNCQ mode\n");

		nv_swncq_host_init(host);

	}



	pci_set_master(pdev);
@@ -1696,3 +2526,6 @@ module_init(nv_init); 
module_exit(nv_exit);

module_param_named(adma, adma_enabled, bool, 0444);

MODULE_PARM_DESC(adma, "Enable use of ADMA (Default: true)");

module_param_named(swncq, swncq_enabled, bool, 0444);

MODULE_PARM_DESC(swncq, "Enable use of SWNCQ (Default: false)");