Add qc_prep to ahci_elphel

src/drivers/ata/ahci_elphel.c

@@ -43,7 +43,6 @@ struct elphel_ahci_priv {
 	u32 base_addr;
 };
 
-
 // What about port_stop and freeing/unmapping ?
 // Or at least check if it is re-started and memory is already allocated/mapped
 static int elphel_port_start(struct ata_port *ap)
@@ -55,10 +54,6 @@ static int elphel_port_start(struct ata_port *ap)
 	struct ahci_host_priv *hpriv = ap->host->private_data;
 	const struct elphel_ahci_priv *dpriv = hpriv->plat_data;
 
-//	const ssize_t align_cdt = 128;
-	const ssize_t align_cdt = 4096; // just trying - page align
-    u32 * dbg_p;
-    int   dbg_i;
 	libahci_debug_init(ap->host);
 
 	dev_dbg(dev, "starting port %d", ap->port_no);
@@ -69,57 +64,22 @@ static int elphel_port_start(struct ata_port *ap)
 	if (!pp)
 		return -ENOMEM;
 
-	// Seems that dmam_alloc_coherent() in Zynq does not really make it "coherent" (write buffers), but stream functions work
-	/*
-Command Table Descriptor Base Address (CTBA): Indicates the 32-bit physical address of
-the command table, which contains the command FIS, ATAPI Command, and PRD table. This
-address must be aligned to a 128-byte cache line, indicated by bits 06:00 being reserved.
-
-	mem = dmam_alloc_coherent(dev, AHCI_CMD_TBL_AR_SZ, &mem_dma, GFP_KERNEL);
+	mem = devm_kmalloc(dev, 0x100000, GFP_KERNEL); // AHCI_CMD_TBL_AR_SZ = 0x16000
 	if (!mem)
 		return -ENOMEM;
-	memset(mem, 0, AHCI_CMD_TBL_AR_SZ); // dmam_alloc_coherent() does this
-	void *dma_alloc_coherent(struct device *dev, size_t size, dma_addr_t *dma_handle, int flag);
-    dma_addr_t dma_map_single(struct device *dev, void *buffer, size_t size, enum dma_data_direction direction);
-    void dma_unmap_single(struct device *dev, dma_addr_t dma_addr, size_t size, enum dma_data_direction direction);
-	*/
-//	mem = devm_kzalloc(dev, AHCI_CMD_TBL_AR_SZ + align_cdt - 1, GFP_KERNEL);
-//	mem = devm_kmalloc(dev, AHCI_CMD_TBL_AR_SZ + align_cdt - 1, GFP_KERNEL); // let some junk be there
-	mem = devm_kmalloc(dev, 0x100000, GFP_KERNEL); // AHCI_CMD_TBL_AR_SZ = 0x16000
-	dbg_p = (u32*) mem;
-	for (dbg_i=0; dbg_i < ((AHCI_CMD_TBL_AR_SZ + align_cdt)>>2); dbg_i++) {
-		dbg_p[dbg_i] = dbg_i;
-	}
-	dbg_i = 0;
-	/*
-	if (((u32) mem) & (align_cdt - 1)) {
-//		mem += align_cdt - (((u32) mem) & (align_cdt - 1));
-		dbg_i = align_cdt - (((u32) mem) & (align_cdt - 1));
-	}
-*/
 	mem_dma = dma_map_single(dev, mem, AHCI_CMD_TBL_AR_SZ, DMA_TO_DEVICE); // maybe DMA_BIDIRECTIONAL, but currently we do not use DMA for received FISes
 
-	dev_dbg(dev, "ahci_elphel.c: dbg_i= 0x%08x, mem= 0x%08x, mem_dma= 0x%08x", dbg_i, (u32) mem, (u32) mem_dma);
-	pp->cmd_tbl = mem + dbg_i;
-	pp->cmd_tbl_dma = mem_dma + dbg_i;
-	dev_dbg(dev, "ahci_elphel.c: dbg_i= 0x%08x, pp->cmd_tbl= 0x%08x, pp->cmd_tbl_dma= 0x%08x", dbg_i, (u32) pp->cmd_tbl, (u32) pp->cmd_tbl_dma);
+	pp->cmd_tbl = mem;
+	pp->cmd_tbl_dma = mem_dma;
 
 	/*
 	 * Set predefined addresses
 	 */
 	pp->cmd_slot = hpriv->mmio + dpriv->clb_offs;
-	//pp->cmd_slot_dma = virt_to_phys(pp->cmd_slot);
-	pp->cmd_slot_dma = 0x80000000 + dpriv->clb_offs;
+	pp->cmd_slot_dma = dpriv->base_addr + dpriv->clb_offs;
 
 	pp->rx_fis = hpriv->mmio + dpriv->fb_offs;
-	//pp->rx_fis_dma = virt_to_phys(pp->rx_fis);
-	pp->rx_fis_dma = 0x80000000 + dpriv->fb_offs;
-
-	/*dev_info(dev, "cmd_slot: 0x%p", pp->cmd_slot);
-	dev_info(dev, "cmd_slot_dma: 0x%08u", pp->cmd_slot_dma);
-	dev_info(dev, "rx_fis: 0x%p", pp->rx_fis);
-	dev_info(dev, "rx_fis_dma: 0x%08u", pp->rx_fis_dma);
-	dev_info(dev, "base_addr: 0x%08u", dpriv->base_addr);*/
+	pp->rx_fis_dma = dpriv->base_addr + dpriv->fb_offs;
 
 	/*
 	 * Save off initial list of interrupts to be enabled.
@@ -129,13 +89,7 @@ address must be aligned to a 128-byte cache line, indicated by bits 06:00 being
 
 	ap->private_data = pp;
 
-//	libahci_debug_state_dump(ap);
-//	libahci_debug_state_dump(ap);
-
-	//libahci_debug_saxigp1_save(ap, 0x3000);
-	//libahci_debug_saxigp1_save(ap, 0x3000);
-
-	dev_dbg(dev, "flags (ATA_FLAG_xxx): %u", ap->flags);
+	dev_dbg(dev, "flags (ATA_FLAG_xxx): %lu", ap->flags);
 	dev_dbg(dev, "pflags (ATA_PFLAG_xxx): %u", ap->pflags);
 
 	dev_dbg(dev, "ahci_elphel.c: Calling  ahci_port_resume()");
@@ -146,28 +100,35 @@ static int elphel_parse_prop(const struct device_node *devn,
 		struct device *dev,
 		struct elphel_ahci_priv *dpriv)
 {
-	u64 size;
-	unsigned int flags;
+	int rc = 0;
 	const __be32 *val;
 	struct resource res;
 
 	if (!devn) {
-		dev_err(dev, "device tree node is not found");
+		dev_err(dev, "elphel-ahci device tree node is not found");
 		return -EINVAL;
 	}
 
 	val = of_get_property(devn, PROP_NAME_CLB_OFFS, NULL);
+	if (!val) {
+		dev_err(dev, "can not find clb_offs in device tree");
+		return -EINVAL;
+	}
 	dpriv->clb_offs = be32_to_cpup(val);
+
 	val = of_get_property(devn, PROP_NAME_FB_OFFS, NULL);
+	if (!val) {
+		dev_err(dev, "can not find fb_offs in device tree");
+		return -EINVAL;
+	}
 	dpriv->fb_offs = be32_to_cpup(val);
-	val = of_get_address(devn, 0, NULL, NULL);
-	if (val != NULL) {
-		dpriv->base_addr = be32_to_cpu(val);
-		dev_dbg(dev, "base_addr: 0x%08u", dpriv->base_addr);
-	} else {
-		dev_err(dev, "can not get register address");
+
+	rc = of_address_to_resource((struct device_node *)devn, 0, &res);
+	if (rc < 0) {
+		dev_err(dev, "can not find address in device tree");
+		return -EINVAL;
 	}
-	//of_address_to_resource(devn, 0, &res);
+	dpriv->base_addr = (u32)res.start;
 
 	return 0;
 }
@@ -176,24 +137,22 @@ static int elphel_drv_probe(struct platform_device *pdev)
 {
 	int ret;
 	struct ahci_host_priv *hpriv;
-	struct elphel_ahci_priv *drv_priv;
+	struct elphel_ahci_priv *dpriv;
 	struct device *dev = &pdev->dev;
 	const struct of_device_id *match;
-
-	struct resource *res;
 	unsigned int reg_val;
 
 	dev_info(&pdev->dev, "probing Elphel AHCI driver");
 
-	drv_priv = devm_kzalloc(dev, sizeof(struct elphel_ahci_priv), GFP_KERNEL);
-	if (!drv_priv)
+	dpriv = devm_kzalloc(dev, sizeof(struct elphel_ahci_priv), GFP_KERNEL);
+	if (!dpriv)
 		return -ENOMEM;
 
 	match = of_match_device(ahci_elphel_of_match, &pdev->dev);
 	if (!match)
 		return -EINVAL;
 
-	ret = elphel_parse_prop(dev->of_node, dev, drv_priv);
+	ret = elphel_parse_prop(dev->of_node, dev, dpriv);
 	if (ret != 0)
 		return ret;
 
@@ -201,7 +160,7 @@ static int elphel_drv_probe(struct platform_device *pdev)
 	if (IS_ERR(hpriv))
 		return PTR_ERR(hpriv);
 
-	hpriv->plat_data = drv_priv;
+	hpriv->plat_data = dpriv;
 
 	reg_val = readl(hpriv->mmio + HOST_CAP);
 	dev_dbg(dev, "HOST CAP register: 0x%08x", reg_val);
@@ -214,18 +173,6 @@ static int elphel_drv_probe(struct platform_device *pdev)
 	reg_val = readl(hpriv->mmio + HOST_VERSION);
 	dev_dbg(dev, "HOST VS register: 0x%08x", reg_val);
 
-	phys_addr_t paddr = virt_to_phys(hpriv->mmio);
-	void *vaddr = phys_to_virt(paddr);
-	dev_err(dev, "current mmio virt addr: 0x%p\n", hpriv->mmio);
-	dev_err(dev, "current mmio virt addr as uint: 0x%08x\n", hpriv->mmio);
-	dev_err(dev, "mmio phys addr: 0x%08x\n", paddr);
-	dev_err(dev, "mmio phys addr as tr: 0x%p\n", paddr);
-	dev_err(dev, "back converted mmio virt addr: 0x%p\n", vaddr);
-	//printk(KERN_DEBUG, "current mmio virt addr: %p\n", hpriv->mmio);
-	//printk(KERN_DEBUG, "mmio phys addr: %u\n", paddr);
-	//printk(KERN_DEBUG, "back converted mmio virt addr: %p\n", vaddr);
-	printk(KERN_DEBUG "======");
-
 	ret = ahci_platform_init_host(pdev, hpriv, &ahci_elphel_port_info,
 			&ahci_platform_sht);
 	if (ret) {
@@ -233,7 +180,6 @@ static int elphel_drv_probe(struct platform_device *pdev)
 		ahci_platform_disable_resources(hpriv);
 		return ret;
 	}
-	dev_dbg(dev, "ahci platform host initialized");
 
 	return 0;
 }
@@ -247,40 +193,73 @@ static int elphel_drv_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static unsigned int elphel_read_id(struct ata_device *dev, struct ata_taskfile *tf, u16 *id)
+static void elphel_qc_prep(struct ata_queued_cmd *qc)
 {
-	u32 err_mask;
-	struct device *d = &dev->tdev;
-	int i, len;
-	char *msg_str;
-
-	err_mask = ata_do_dev_read_id(dev, tf, id);
-	if (err_mask)
-		return err_mask;
-
-	dev_dbg(d, "elphel_read_id(): issue identify command finished\n");
-	/*dev_info(d, "dump IDENTIFY:\n");
-	msg_str = kzalloc(PAGE_SIZE, GFP_KERNEL);
-	if (!msg_str)
-		return 0;
-	len = 0;
-	for (i = 0; i < ATA_ID_WORDS; i++) {
-		if ((i % 16) == 0 && i != 0) {
-			dev_info(d, "%s\n", msg_str);
-			len = 0;
+	struct ata_port *ap = qc->ap;
+	struct ahci_port_priv *pp = ap->private_data;
+	int is_atapi = ata_is_atapi(qc->tf.protocol);
+	void *cmd_tbl;
+	u32 opts;
+	const u32 cmd_fis_len = 5; /* five dwords */
+	unsigned int n_elem;
+	struct scatterlist *sg;
+	struct ahci_sg *ahci_sg;
+
+	/*
+	 * Fill in command table information.  First, the header,
+	 * a SATA Register - Host to Device command FIS.
+	 */
+	dma_sync_single_for_cpu(&qc->dev->tdev, pp->cmd_tbl_dma,
+			AHCI_CMD_TBL_AR_SZ, DMA_TO_DEVICE);
+	cmd_tbl = pp->cmd_tbl + qc->tag * AHCI_CMD_TBL_SZ;
+
+	ata_tf_to_fis(&qc->tf, qc->dev->link->pmp, 1, cmd_tbl);
+
+	if (is_atapi) {
+		memset(cmd_tbl + AHCI_CMD_TBL_CDB, 0, 32);
+		memcpy(cmd_tbl + AHCI_CMD_TBL_CDB, qc->cdb, qc->dev->cdb_len);
 	}
-		len += snprintf(msg_str + len, PAGE_SIZE - len, "0x%04x ", id[i]);
+
+	/*
+	 * Next, the S/G list.
+	 */
+	n_elem = 0;
+	ahci_sg = cmd_tbl + AHCI_CMD_TBL_HDR_SZ;
+	if (qc->flags & ATA_QCFLAG_DMAMAP) {
+		for_each_sg(qc->sg, sg, qc->n_elem, n_elem) {
+			dma_addr_t addr = sg_dma_address(sg);
+			u32 sg_len = sg_dma_len(sg);
+
+			ahci_sg[n_elem].addr = cpu_to_le32(addr & 0xffffffff);
+			ahci_sg[n_elem].addr_hi = cpu_to_le32((addr >> 16) >> 16);
+			ahci_sg[n_elem].flags_size = cpu_to_le32(sg_len - 1);
 		}
-	// print last string
-	dev_info(d, "%s\n", msg_str);*/
 
-	return 0;
+		if (qc->dma_dir == DMA_TO_DEVICE) {
+			dma_sync_sg_for_device(&qc->dev->tdev, qc->sg, qc->n_elem, qc->dma_dir);
+		} else if (qc->dma_dir == DMA_FROM_DEVICE) {
+			dma_sync_sg_for_cpu(&qc->dev->tdev, qc->sg, qc->n_elem, qc->dma_dir);
+		}
+	}
+
+	/*
+	 * Fill in command slot information.
+	 */
+	opts = cmd_fis_len | n_elem << 16 | (qc->dev->link->pmp << 12);
+	if (qc->tf.flags & ATA_TFLAG_WRITE)
+		opts |= AHCI_CMD_WRITE;
+	if (is_atapi)
+		opts |= AHCI_CMD_ATAPI | AHCI_CMD_PREFETCH;
+
+	ahci_fill_cmd_slot(pp, qc->tag, opts);
+	dma_sync_single_for_device(&qc->dev->tdev, pp->cmd_tbl_dma,
+			AHCI_CMD_TBL_AR_SZ, DMA_TO_DEVICE);
 }
 
 static struct ata_port_operations ahci_elphel_ops = {
 		.inherits		= &ahci_ops,
 		.port_start		= elphel_port_start,
-		.read_id		= elphel_read_id,
+		.qc_prep		= elphel_qc_prep,
 };
 
 static const struct ata_port_info ahci_elphel_port_info = {
@@ -308,7 +287,6 @@ MODULE_DEVICE_TABLE(of, ahci_elphel_of_match);
 
 static struct platform_driver ahci_elphel_driver = {
 		.probe			= elphel_drv_probe,
-		/*.remove			= ata_platform_remove_one,*/
 		.remove			= elphel_drv_remove,
 		.driver	= {
 				.name	= DRV_NAME,

src/drivers/ata/libata-eh.c

@@ -2556,10 +2556,6 @@ static void ata_eh_link_report(struct ata_link *link)
 			res->device, qc->err_mask, ata_err_string(qc->err_mask),
 			qc->err_mask & AC_ERR_NCQ ? " <F>" : "");
 
-		// elphel test: crash driver here to stop its execution and preserve memory state
-		u32 *tmp_str = NULL;
-		ata_dev_err(qc->dev, "test: %u", *tmp_str);
-
 #ifdef CONFIG_ATA_VERBOSE_ERROR
 		if (res->command & (ATA_BUSY | ATA_DRDY | ATA_DF | ATA_DRQ |
 				    ATA_ERR)) {