Add qc_prep to ahci_elphel (628a0bcc) · Commits · Elphel / linux-elphel

src/drivers/ata/ahci_elphel.c

+91 −113

Original line number	Original line	Diff line number	Diff line
	@@ -43,7 +43,6 @@ struct elphel_ahci_priv {
	u32 base_addr;		u32 base_addr;
	};		};


	// What about port_stop and freeing/unmapping ?		// What about port_stop and freeing/unmapping ?
	// Or at least check if it is re-started and memory is already allocated/mapped		// Or at least check if it is re-started and memory is already allocated/mapped
	static int elphel_port_start(struct ata_port *ap)		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;		struct ahci_host_priv *hpriv = ap->host->private_data;
	const struct elphel_ahci_priv *dpriv = hpriv->plat_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);		libahci_debug_init(ap->host);

	dev_dbg(dev, "starting port %d", ap->port_no);		dev_dbg(dev, "starting port %d", ap->port_no);
	@@ -69,57 +64,22 @@ static int elphel_port_start(struct ata_port *ap)
	if (!pp)		if (!pp)
	return -ENOMEM;		return -ENOMEM;

	// Seems that dmam_alloc_coherent() in Zynq does not really make it "coherent" (write buffers), but stream functions work		mem = devm_kmalloc(dev, 0x100000, GFP_KERNEL); // AHCI_CMD_TBL_AR_SZ = 0x16000
	/*
	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);
	if (!mem)		if (!mem)
	return -ENOMEM;		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		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;
	pp->cmd_tbl = mem + dbg_i;		pp->cmd_tbl_dma = mem_dma;
	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);

	/*		/*
	* Set predefined addresses		* Set predefined addresses
	*/		*/
	pp->cmd_slot = hpriv->mmio + dpriv->clb_offs;		pp->cmd_slot = hpriv->mmio + dpriv->clb_offs;
	//pp->cmd_slot_dma = virt_to_phys(pp->cmd_slot);		pp->cmd_slot_dma = dpriv->base_addr + dpriv->clb_offs;
	pp->cmd_slot_dma = 0x80000000 + dpriv->clb_offs;

	pp->rx_fis = hpriv->mmio + dpriv->fb_offs;		pp->rx_fis = hpriv->mmio + dpriv->fb_offs;
	//pp->rx_fis_dma = virt_to_phys(pp->rx_fis);		pp->rx_fis_dma = dpriv->base_addr + dpriv->fb_offs;
	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);*/

	/*		/*
	* Save off initial list of interrupts to be enabled.		* 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;		ap->private_data = pp;

	// libahci_debug_state_dump(ap);		dev_dbg(dev, "flags (ATA_FLAG_xxx): %lu", ap->flags);
	// 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, "pflags (ATA_PFLAG_xxx): %u", ap->pflags);		dev_dbg(dev, "pflags (ATA_PFLAG_xxx): %u", ap->pflags);

	dev_dbg(dev, "ahci_elphel.c: Calling ahci_port_resume()");		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 device *dev,
	struct elphel_ahci_priv *dpriv)		struct elphel_ahci_priv *dpriv)
	{		{
	u64 size;		int rc = 0;
	unsigned int flags;
	const __be32 *val;		const __be32 *val;
	struct resource res;		struct resource res;

	if (!devn) {		if (!devn) {
	dev_err(dev, "device tree node is not found");		dev_err(dev, "elphel-ahci device tree node is not found");
	return -EINVAL;		return -EINVAL;
	}		}

	val = of_get_property(devn, PROP_NAME_CLB_OFFS, NULL);		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);		dpriv->clb_offs = be32_to_cpup(val);

	val = of_get_property(devn, PROP_NAME_FB_OFFS, NULL);		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);		dpriv->fb_offs = be32_to_cpup(val);
	val = of_get_address(devn, 0, NULL, NULL);
	if (val != NULL) {		rc = of_address_to_resource((struct device_node *)devn, 0, &res);
	dpriv->base_addr = be32_to_cpu(val);		if (rc < 0) {
	dev_dbg(dev, "base_addr: 0x%08u", dpriv->base_addr);		dev_err(dev, "can not find address in device tree");
	} else {		return -EINVAL;
	dev_err(dev, "can not get register address");
	}		}
	//of_address_to_resource(devn, 0, &res);		dpriv->base_addr = (u32)res.start;

	return 0;		return 0;
	}		}
	@@ -176,24 +137,22 @@ static int elphel_drv_probe(struct platform_device *pdev)
	{		{
	int ret;		int ret;
	struct ahci_host_priv *hpriv;		struct ahci_host_priv *hpriv;
	struct elphel_ahci_priv *drv_priv;		struct elphel_ahci_priv *dpriv;
	struct device *dev = &pdev->dev;		struct device *dev = &pdev->dev;
	const struct of_device_id *match;		const struct of_device_id *match;

	struct resource *res;
	unsigned int reg_val;		unsigned int reg_val;

	dev_info(&pdev->dev, "probing Elphel AHCI driver");		dev_info(&pdev->dev, "probing Elphel AHCI driver");

	drv_priv = devm_kzalloc(dev, sizeof(struct elphel_ahci_priv), GFP_KERNEL);		dpriv = devm_kzalloc(dev, sizeof(struct elphel_ahci_priv), GFP_KERNEL);
	if (!drv_priv)		if (!dpriv)
	return -ENOMEM;		return -ENOMEM;

	match = of_match_device(ahci_elphel_of_match, &pdev->dev);		match = of_match_device(ahci_elphel_of_match, &pdev->dev);
	if (!match)		if (!match)
	return -EINVAL;		return -EINVAL;

	ret = elphel_parse_prop(dev->of_node, dev, drv_priv);		ret = elphel_parse_prop(dev->of_node, dev, dpriv);
	if (ret != 0)		if (ret != 0)
	return ret;		return ret;

	@@ -201,7 +160,7 @@ static int elphel_drv_probe(struct platform_device *pdev)
	if (IS_ERR(hpriv))		if (IS_ERR(hpriv))
	return PTR_ERR(hpriv);		return PTR_ERR(hpriv);

	hpriv->plat_data = drv_priv;		hpriv->plat_data = dpriv;

	reg_val = readl(hpriv->mmio + HOST_CAP);		reg_val = readl(hpriv->mmio + HOST_CAP);
	dev_dbg(dev, "HOST CAP register: 0x%08x", reg_val);		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);		reg_val = readl(hpriv->mmio + HOST_VERSION);
	dev_dbg(dev, "HOST VS register: 0x%08x", reg_val);		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,		ret = ahci_platform_init_host(pdev, hpriv, &ahci_elphel_port_info,
	&ahci_platform_sht);		&ahci_platform_sht);
	if (ret) {		if (ret) {
	@@ -233,7 +180,6 @@ static int elphel_drv_probe(struct platform_device *pdev)
	ahci_platform_disable_resources(hpriv);		ahci_platform_disable_resources(hpriv);
	return ret;		return ret;
	}		}
	dev_dbg(dev, "ahci platform host initialized");

	return 0;		return 0;
	}		}
	@@ -247,40 +193,73 @@ static int elphel_drv_remove(struct platform_device *pdev)
	return 0;		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 ata_port *ap = qc->ap;
	struct device *d = &dev->tdev;		struct ahci_port_priv *pp = ap->private_data;
	int i, len;		int is_atapi = ata_is_atapi(qc->tf.protocol);
	char *msg_str;		void *cmd_tbl;
			u32 opts;
	err_mask = ata_do_dev_read_id(dev, tf, id);		const u32 cmd_fis_len = 5; /* five dwords */
	if (err_mask)		unsigned int n_elem;
	return err_mask;		struct scatterlist *sg;
			struct ahci_sg *ahci_sg;
	dev_dbg(d, "elphel_read_id(): issue identify command finished\n");
	/*dev_info(d, "dump IDENTIFY:\n");		/*
	msg_str = kzalloc(PAGE_SIZE, GFP_KERNEL);		* Fill in command table information. First, the header,
	if (!msg_str)		* a SATA Register - Host to Device command FIS.
	return 0;		*/
	len = 0;		dma_sync_single_for_cpu(&qc->dev->tdev, pp->cmd_tbl_dma,
	for (i = 0; i < ATA_ID_WORDS; i++) {		AHCI_CMD_TBL_AR_SZ, DMA_TO_DEVICE);
	if ((i % 16) == 0 && i != 0) {		cmd_tbl = pp->cmd_tbl + qc->tag * AHCI_CMD_TBL_SZ;
	dev_info(d, "%s\n", msg_str);
	len = 0;		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 = {		static struct ata_port_operations ahci_elphel_ops = {
	.inherits = &ahci_ops,		.inherits = &ahci_ops,
	.port_start = elphel_port_start,		.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 = {		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 = {		static struct platform_driver ahci_elphel_driver = {
	.probe = elphel_drv_probe,		.probe = elphel_drv_probe,
	/.remove = ata_platform_remove_one,/
	.remove = elphel_drv_remove,		.remove = elphel_drv_remove,
	.driver = {		.driver = {
	.name = DRV_NAME,		.name = DRV_NAME,

src/drivers/ata/libata-eh.c

+0 −4

Original line number	Original line	Diff line number	Diff line
	@@ -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),		res->device, qc->err_mask, ata_err_string(qc->err_mask),
	qc->err_mask & AC_ERR_NCQ ? " <F>" : "");		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		#ifdef CONFIG_ATA_VERBOSE_ERROR
	if (res->command & (ATA_BUSY \| ATA_DRDY \| ATA_DF \| ATA_DRQ \|		if (res->command & (ATA_BUSY \| ATA_DRDY \| ATA_DF \| ATA_DRQ \|
	ATA_ERR)) {		ATA_ERR)) {

Admin message