Read sector to predefined buffer (not SG list) (d01183a5) · Commits · Elphel / linux-elphel

src/drivers/ata/ahci_elphel.c

+344 −2

Original line number	Original line	Diff line number	Diff line
	@@ -26,6 +26,8 @@
	#include <linux/sysfs.h>		#include <linux/sysfs.h>
	#include "ahci.h"		#include "ahci.h"

			#include <elphel/elphel393-mem.h>

	#define DRV_NAME "elphel-ahci"		#define DRV_NAME "elphel-ahci"
	/*		/*
	* FPGA bitstream control address and bit mask. These are used to check whether		* FPGA bitstream control address and bit mask. These are used to check whether
	@@ -37,6 +39,12 @@
	/* Property names from device tree, these are specific for the controller */		/* Property names from device tree, these are specific for the controller */
	#define PROP_NAME_CLB_OFFS "clb_offs"		#define PROP_NAME_CLB_OFFS "clb_offs"
	#define PROP_NAME_FB_OFFS "fb_offs"		#define PROP_NAME_FB_OFFS "fb_offs"
			/** Flag indicating that IRQ should not be processed in ahci_port_interrupt */
			#define IRQ_SIMPLE (1 << 0)
			/** The length of a command FIS in double words */
			#define CMD_FIS_LEN 5
			/** This is used to get 28-bit address from 64-bit value */
			#define ADDR_MASK_28_BIT ((u64)0xfffffff)

	static struct ata_port_operations ahci_elphel_ops;		static struct ata_port_operations ahci_elphel_ops;
	static const struct ata_port_info ahci_elphel_port_info;		static const struct ata_port_info ahci_elphel_port_info;
	@@ -50,8 +58,17 @@ struct elphel_ahci_priv {
	u32 clb_offs;		u32 clb_offs;
	u32 fb_offs;		u32 fb_offs;
	u32 base_addr;		u32 base_addr;
			u32 flags;
	};		};

			static struct platform_device *g_pdev;
			static ssize_t elphel_test_write(struct device dev, struct device_attribute attr,
			const char *buff, size_t buff_sz);
			static irqreturn_t elphel_irq_handler(int irq, void * dev_instance);
			static int elphel_write_dma(struct ata_port ap, u64 start, u16 count, struct scatterlist sg, unsigned int elem);
			static int elphel_read_dma(struct ata_port ap, u64 start, u16 count, u8 buff, unsigned int elem);
			void prep_cfis(u8 *cmd_tbl, u8 cmd, u64 start_addr, u16 count);

	static ssize_t set_load_flag(struct device dev, struct device_attribute attr,		static ssize_t set_load_flag(struct device dev, struct device_attribute attr,
	const char *buff, size_t buff_sz)		const char *buff, size_t buff_sz)
	{		{
	@@ -182,14 +199,13 @@ static int elphel_drv_probe(struct platform_device *pdev)
	struct elphel_ahci_priv *dpriv;		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;
	unsigned int reg_val;

	if (&dev->kobj) {		if (&dev->kobj) {
	ret = sysfs_create_group(&dev->kobj, &dev_attr_root_group);		ret = sysfs_create_group(&dev->kobj, &dev_attr_root_group);
	if (ret < 0)		if (ret < 0)
	return ret;		return ret;
	}		}
	elphel_defer_load(dev);		// elphel_defer_load(dev);

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

	@@ -218,6 +234,20 @@ static int elphel_drv_probe(struct platform_device *pdev)
	ahci_platform_disable_resources(hpriv);		ahci_platform_disable_resources(hpriv);
	return ret;		return ret;
	}		}
			g_pdev = pdev;
			/* reassign interrupt handler*/
			int rc;
			unsigned int irq_flags = IRQF_SHARED;
			int irq = platform_get_irq(pdev, 0);
			struct ata_host *ahost = platform_get_drvdata(pdev);
			printk(KERN_DEBUG ">>> removing automatically assigned irq handler\n");
			devm_free_irq(dev, irq, ahost);
			rc = devm_request_irq(dev, irq, elphel_irq_handler, irq_flags, dev_name(dev), ahost);
			if (rc) {
			printk(KERN_DEBUG ">>> failed to request irq\n");
			return rc;
			}
			printk(KERN_DEBUG ">>> irq handler reassigned\n");

	return 0;		return 0;
	}		}
	@@ -231,6 +261,13 @@ static int elphel_drv_remove(struct platform_device *pdev)
	return 0;		return 0;
	}		}

			void dump_tf_addr(struct ata_taskfile *tf)
			{
			printk(KERN_DEBUG ">>> taskfile dump: lbal = 0x%x, lbam = 0x%x, lbah = 0x%x, "
			"hob_lbal = 0x%x, hod_lbam = 0x%x, hob_lbah = 0x%x\n",
			tf->lbal, tf->lbam, tf->lbah,
			tf->hob_lbal, tf->hob_lbam, tf->hob_lbah);
			}
	static void elphel_qc_prep(struct ata_queued_cmd *qc)		static void elphel_qc_prep(struct ata_queued_cmd *qc)
	{		{
	struct ata_port *ap = qc->ap;		struct ata_port *ap = qc->ap;
	@@ -256,6 +293,9 @@ static void elphel_qc_prep(struct ata_queued_cmd *qc)

	ata_tf_to_fis(&qc->tf, qc->dev->link->pmp, 1, cmd_tbl);		ata_tf_to_fis(&qc->tf, qc->dev->link->pmp, 1, cmd_tbl);

			dev_dbg(ap->dev, ">>> CFIS dump, data from libahci:\n");
			print_hex_dump_bytes("", DUMP_PREFIX_OFFSET, cmd_tbl, 20);

	if (is_atapi) {		if (is_atapi) {
	memset(cmd_tbl + AHCI_CMD_TBL_CDB, 0, 32);		memset(cmd_tbl + AHCI_CMD_TBL_CDB, 0, 32);
	memcpy(cmd_tbl + AHCI_CMD_TBL_CDB, qc->cdb, qc->dev->cdb_len);		memcpy(cmd_tbl + AHCI_CMD_TBL_CDB, qc->cdb, qc->dev->cdb_len);
	@@ -292,8 +332,10 @@ static void elphel_qc_prep(struct ata_queued_cmd *qc)
	}		}

	static DEVICE_ATTR(load_module, S_IWUSR \| S_IWGRP, NULL, set_load_flag);		static DEVICE_ATTR(load_module, S_IWUSR \| S_IWGRP, NULL, set_load_flag);
			static DEVICE_ATTR(test_write, S_IWUSR \| S_IWGRP, NULL, elphel_test_write);
	static struct attribute *root_dev_attrs[] = {		static struct attribute *root_dev_attrs[] = {
	&dev_attr_load_module.attr,		&dev_attr_load_module.attr,
			&dev_attr_test_write.attr,
	NULL		NULL
	};		};
	static const struct attribute_group dev_attr_root_group = {		static const struct attribute_group dev_attr_root_group = {
	@@ -341,6 +383,306 @@ static struct platform_driver ahci_elphel_driver = {
	};		};
	module_platform_driver(ahci_elphel_driver);		module_platform_driver(ahci_elphel_driver);

			#define TEST_BUFF_SZ 512
			//#define SDA2_LBA_ADDR 124963848
			#define SDA2_LBA_ADDR 1
			static ssize_t elphel_test_write(struct device dev, struct device_attribute attr,
			const char *buff, size_t buff_sz)
			{
			int i, n_elem;
			struct ata_host *host;
			struct ata_port *port;
			struct ahci_host_priv *hpriv;
			struct elphel_ahci_priv *dpriv;
			struct scatterlist sg;
			struct scatterlist *sg_ptr;
			u8 *test_buff = pElphel_buf->d2h_vaddr;
			unsigned int lba_addr;

			if (sscanf(buff, "%u", &lba_addr) == 1) {
			printk(KERN_DEBUG ">>> got LBA address: %d", lba_addr);
			} else {
			lba_addr = SDA2_LBA_ADDR;
			}

			host = platform_get_drvdata(g_pdev);
			port = host->ports[0];
			hpriv = port->host->private_data;
			dpriv = hpriv->plat_data;

			/* read test */
			// printk(KERN_DEBUG ">>> dump test buffer before reading: %d bytes, addr = 0x%p\n", TEST_BUFF_SZ, test_buff);
			// dma_sync_single_for_cpu(dev, pElphel_buf->d2h_paddr, pElphel_buf->d2h_size, DMA_FROM_DEVICE);
			// print_hex_dump_bytes("", DUMP_PREFIX_OFFSET, test_buff, TEST_BUFF_SZ);
			//// sg_init_one(&sg, (const void *)test_buff, TEST_BUFF_SZ);
			dma_sync_single_for_device(dev, pElphel_buf->d2h_paddr, pElphel_buf->d2h_size, DMA_FROM_DEVICE);
			// printk(KERN_DEBUG ">>> buffer has been mapped for device\n");

			// printk(KERN_DEBUG ">>> dump the content of SG list:\n");
			// n_elem = 0;
			// for_each_sg(&sg, sg_ptr, 1, n_elem) {
			// dma_addr_t addr = sg_dma_address(sg_ptr);
			// u32 sg_len = sg_dma_len(sg_ptr);
			// printk(KERN_DEBUG ">>>\t# %d, addr = 0x%x, %d\n", n_elem, addr, sg_len);
			// }
			// printk(KERN_DEBUG ">>>\n");

			printk(KERN_DEBUG ">>> trying to read data to sg list\n");
			elphel_read_dma(port, lba_addr, 1, test_buff, TEST_BUFF_SZ);
			printk(KERN_DEBUG ">>> command has been issued\n");

			while (dpriv->flags & IRQ_SIMPLE) {
			printk_once(KERN_DEBUG ">>> waiting for interrupt\n");
			msleep(1);
			}

			printk(KERN_DEBUG ">>> dump test buffer after reading: %d bytes\n", TEST_BUFF_SZ);
			dma_sync_single_for_cpu(dev, pElphel_buf->d2h_paddr, pElphel_buf->d2h_size, DMA_FROM_DEVICE);
			print_hex_dump_bytes("", DUMP_PREFIX_OFFSET, test_buff, TEST_BUFF_SZ);
			dma_sync_single_for_device(dev, pElphel_buf->d2h_paddr, pElphel_buf->d2h_size, DMA_FROM_DEVICE);
			printk(KERN_DEBUG ">>> buffer has been mapped for device\n");
			/* end of read test */

			// printk(KERN_DEBUG ">>> interrupt flag has been cleared\n");
			// printk(KERN_DEBUG ">>> dump of SG list area\n");
			// dma_sync_single_for_cpu(dev, pElphel_buf->h2d_paddr, pElphel_buf->h2d_size, DMA_TO_DEVICE);
			// print_hex_dump_bytes("", DUMP_PREFIX_NONE, test_buff, TEST_BUFF_SZ);
			// dma_sync_single_for_device(dev, pElphel_buf->h2d_paddr, pElphel_buf->h2d_size, DMA_TO_DEVICE);
			// /* end of read test */
			//
			// /* write test */
			// printk(KERN_DEBUG ">>> filling test buffer: %d bytes\n", TEST_BUFF_SZ);
			// dma_sync_single_for_cpu(dev, pElphel_buf->h2d_paddr, pElphel_buf->h2d_size, DMA_TO_DEVICE);
			// for (i = 0; i < TEST_BUFF_SZ - 1; i += 2) {
			// test_buff[i] = 0xaa;
			// test_buff[i + 1] = 0x55;
			// }
			// print_hex_dump_bytes("", DUMP_PREFIX_NONE, test_buff, TEST_BUFF_SZ);
			// dma_sync_single_for_device(dev, pElphel_buf->h2d_paddr, pElphel_buf->h2d_size, DMA_TO_DEVICE);
			// printk(KERN_DEBUG ">>> buffer has been mapped\n");
			//
			// sg_init_one(&sg, pElphel_buf->h2d_vaddr, TEST_BUFF_SZ);
			//
			// printk(KERN_DEBUG ">>> trying to read data to sg list\n");
			// elphel_read_dma(port, SDA2_LBA_ADDR, 1, &sg, 1);
			// printk(KERN_DEBUG ">>> command has been issued\n");
			// /* end of write test */
			//
			//// while (dpriv->flags & IRQ_SIMPLE) {
			//// msleep(1);
			//// }
			// printk(KERN_DEBUG ">>> interrupt flag has been cleared\n");
			// printk(KERN_DEBUG ">>> dump of SG list area\n");
			// dma_sync_single_for_cpu(dev, pElphel_buf->h2d_paddr, pElphel_buf->h2d_size, DMA_TO_DEVICE);
			// print_hex_dump_bytes("", DUMP_PREFIX_NONE, test_buff, TEST_BUFF_SZ);
			// dma_sync_single_for_device(dev, pElphel_buf->h2d_paddr, pElphel_buf->h2d_size, DMA_TO_DEVICE);

			return buff_sz;
			}

			/** Prepare software constructed command FIS in command table area. The structure of the
			* command FIS is described in Transport Layer chapter of Serial ATA revision 3.1 documentation.
			*/
			inline void prep_cfis(u8 *cmd_tbl, ///< pointer to the beginning of command table
			u8 cmd, ///< ATA command as described in ATA/ATAPI command set
			u64 start_addr, ///< LBA start address
			u16 count) ///< sector count, the number of 512 byte sectors to read or write
			///< @return None
			{
			u8 device, ctrl;

			/* select the content of Device and Control registers based on command, read the description of
			* a command in ATA/ATAPI command set documentation
			*/
			switch (cmd) {
			case ATA_CMD_WRITE_EXT:
			// not verified yet
			device = 0x00;
			ctrl = 0x00;
			break;
			case ATA_CMD_READ:
			device = 0xe0 \| ((start_addr >> 24) & 0x0f);
			ctrl = 0x08;
			/* this is 28-bit command; 4 bits of the address have already been
			* placed to Device register, invalidate the remaining (if any) upper
			* bits of the address and leave only 24 significant bits (just in case)
			*/
			start_addr &= 0xffffff;
			break;
			case ATA_CMD_READ_EXT:
			device = 0xe0;
			ctrl = 0x08;
			break;
			default:
			device = 0x00;
			ctrl = 0x00;
			}

			cmd_tbl[0] = 0x27; // H2D register FIS
			cmd_tbl[1] = 0x80; // set C = 1
			cmd_tbl[2] = cmd; // ATA READ or WRITE DMA command as described in ATA/ATAPI command set
			cmd_tbl[3] = 0; // features(7:0)
			cmd_tbl[4] = start_addr & 0xff; // LBA(7:0)
			cmd_tbl[5] = (start_addr >> 8) & 0xff; // LBA(15:8)
			cmd_tbl[6] = (start_addr >> 16) & 0xff; // LBA(23:16)
			cmd_tbl[7] = device; // device
			cmd_tbl[8] = (start_addr >> 24) & 0xff; // LBA(31:24)
			cmd_tbl[9] = (start_addr >> 32) & 0xff; // LBA(39:32)
			cmd_tbl[10] = (start_addr >> 40) & 0xff; // LBA(47:40)
			cmd_tbl[11] = 0; // features(15:8)
			cmd_tbl[12] = count & 0xff; // count(7:0)
			cmd_tbl[13] = (count >> 8) & 0xff; // count(15:8)
			cmd_tbl[14] = 0; // ICC (isochronous command completion)
			cmd_tbl[15] = ctrl; // control
			}

			static int elphel_write_dma(struct ata_port ap, u64 start, u16 count, struct scatterlist sg, unsigned int elem)
			{
			u32 opts;
			const u32 cmd_fis_len = 5;
			unsigned int n_elem;
			// void *cmd_tbl;
			u8 *cmd_tbl;
			unsigned int slot_num = 0;
			struct ahci_port_priv *pp = ap->private_data;
			struct ahci_host_priv *hpriv = ap->host->private_data;
			struct elphel_ahci_priv *dpriv = hpriv->plat_data;
			struct scatterlist *sg_ptr;
			struct ahci_sg *ahci_sg;
			void __iomem *port_mmio = ahci_port_base(ap);

			dpriv->flags \|= IRQ_SIMPLE;

			/* prepare command FIS */
			dma_sync_single_for_cpu(ap->dev, pp->cmd_tbl_dma, AHCI_CMD_TBL_AR_SZ, DMA_TO_DEVICE);
			cmd_tbl = pp->cmd_tbl + slot_num * AHCI_CMD_TBL_SZ;
			cmd_tbl[0] = 0x27; // H2D register FIS
			cmd_tbl[1] = 0x80; // set C = 1
			cmd_tbl[2] = ATA_CMD_WRITE; // ATA WRITE DMA command as described in ATA/ATAPI command set
			cmd_tbl[3] = 0; // features(7:0)
			cmd_tbl[4] = (start >> 0) & 0xff; // LBA(7:0)
			cmd_tbl[5] = (start >> 8) & 0xff; // LBA(15:8)
			cmd_tbl[6] = (start >> 16) & 0xff; // LBA(23:16)
			cmd_tbl[7] = 0; // device
			cmd_tbl[8] = (start >> 24) & 0xff; // LBA(31:24)
			cmd_tbl[9] = (start >> 32) & 0xff; // LBA(39:32)
			cmd_tbl[10] = (start >> 40) & 0xff; // LBA(47:40)
			cmd_tbl[11] = 0; // features(15:8)
			cmd_tbl[12] = (count >> 0) & 0xff; // count(7:0)
			cmd_tbl[13] = (count >> 8) & 0xff; // count(15:8)
			cmd_tbl[14] = 0; // ICC (isochronous command completion)
			cmd_tbl[15] = 0; // control

			/* prepare physical region descriptor table */
			n_elem = 0;
			ahci_sg = pp->cmd_tbl + slot_num * AHCI_CMD_TBL_SZ + AHCI_CMD_TBL_HDR_SZ;
			for_each_sg(sg, sg_ptr, elem, n_elem) {
			dma_addr_t addr = sg_dma_address(sg_ptr);
			u32 sg_len = sg_dma_len(sg_ptr);

			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);
			}

			/* prepare command header */
			opts = cmd_fis_len \| (n_elem << 16) \| AHCI_CMD_WRITE \| AHCI_CMD_PREFETCH \| AHCI_CMD_CLR_BUSY;
			ahci_fill_cmd_slot(pp, slot_num, opts);
			dma_sync_single_for_device(ap->dev, pp->cmd_tbl_dma, AHCI_CMD_TBL_AR_SZ, DMA_TO_DEVICE);

			/* issue command */
			writel(1 << slot_num, port_mmio + PORT_CMD_ISSUE);

			return 0;
			}

			static int elphel_read_dma(struct ata_port ap, u64 start, u16 count, u8 buff, unsigned int elem)
			{
			u32 opts;
			const u32 cmd_fis_len = 5;
			unsigned int n_elem;
			u8 *cmd_tbl;
			u8 cmd;
			unsigned int slot_num = 0;
			struct ahci_port_priv *pp = ap->private_data;
			struct ahci_host_priv *hpriv = ap->host->private_data;
			struct elphel_ahci_priv *dpriv = hpriv->plat_data;
			struct scatterlist *sg_ptr;
			struct ahci_sg *ahci_sg;
			void __iomem *port_mmio = ahci_port_base(ap);

			dpriv->flags \|= IRQ_SIMPLE;

			/* prepare command FIS */
			dma_sync_single_for_cpu(ap->dev, pp->cmd_tbl_dma, AHCI_CMD_TBL_AR_SZ, DMA_TO_DEVICE);
			cmd_tbl = pp->cmd_tbl + slot_num * AHCI_CMD_TBL_SZ;
			if (start & ~ADDR_MASK_28_BIT)
			cmd = ATA_CMD_READ_EXT;
			else
			cmd = ATA_CMD_READ;
			prep_cfis(cmd_tbl, cmd, start, count);

			/* prepare physical region descriptor table */
			// n_elem = 0;
			// ahci_sg = pp->cmd_tbl + slot_num * AHCI_CMD_TBL_SZ + AHCI_CMD_TBL_HDR_SZ;
			// for_each_sg(sg, sg_ptr, elem, n_elem) {
			// dma_addr_t addr = sg_dma_address(sg_ptr);
			// u32 sg_len = sg_dma_len(sg_ptr);
			//
			// 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);
			// }
			ahci_sg = pp->cmd_tbl + slot_num * AHCI_CMD_TBL_SZ + AHCI_CMD_TBL_HDR_SZ;
			ahci_sg->addr = cpu_to_le32(pElphel_buf->d2h_paddr & 0xffffffff);
			ahci_sg->addr_hi = cpu_to_le32((pElphel_buf->d2h_paddr >> 16) >> 16);
			ahci_sg->flags_size = cpu_to_le32((elem - 1) \| (1 << 31));
			n_elem = 1;

			/* prepare command header */
			opts = CMD_FIS_LEN \| (n_elem << 16) \| AHCI_CMD_PREFETCH \| AHCI_CMD_CLR_BUSY;
			// opts = sizeof(struct ahci_cmd_hdr) \| (n_elem << 16) \| AHCI_CMD_PREFETCH \| AHCI_CMD_CLR_BUSY;
			ahci_fill_cmd_slot(pp, slot_num, opts);

			printk(KERN_DEBUG ">>> dump command table content, first %d bytes, phys addr = 0x%x:\n", TEST_BUFF_SZ, pp->cmd_tbl_dma);
			print_hex_dump_bytes("", DUMP_PREFIX_OFFSET, pp->cmd_tbl, TEST_BUFF_SZ);

			dma_sync_single_for_device(ap->dev, pp->cmd_tbl_dma, AHCI_CMD_TBL_AR_SZ, DMA_TO_DEVICE);

			/* issue command */
			writel(0x11, port_mmio + PORT_CMD);
			writel(1 << slot_num, port_mmio + PORT_CMD_ISSUE);

			return 0;
			}

			static irqreturn_t elphel_irq_handler(int irq, void * dev_instance)
			{
			irqreturn_t handled;
			struct ata_host *host = dev_instance;
			struct ahci_host_priv *hpriv = host->private_data;
			struct elphel_ahci_priv *dpriv = hpriv->plat_data;
			u32 irq_stat, irq_masked;

			if (dpriv->flags & IRQ_SIMPLE) {
			/* handle interrupt */
			printk(KERN_DEBUG ">>> handling interrupt\n");
			dpriv->flags &= ~IRQ_SIMPLE;
			// clear_bit(IRQ_SIMPLE, &dpriv->flags);
			irq_stat = readl(hpriv->mmio + HOST_IRQ_STAT);
			if (!irq_stat)
			return IRQ_NONE;
			// irq_masked = irq_stat & hpriv->port_map;
			writel(irq_stat, hpriv->mmio + HOST_IRQ_STAT);
			handled = IRQ_HANDLED;
			} else {
			/* pass handling to AHCI level*/
			handled = ahci_single_irq_intr(irq, dev_instance);
			}

			return handled;
			}

	MODULE_LICENSE("GPL");		MODULE_LICENSE("GPL");
	MODULE_AUTHOR("Elphel, Inc.");		MODULE_AUTHOR("Elphel, Inc.");
	MODULE_DESCRIPTION("Elphel AHCI SATA platform driver for elphel393 camera");		MODULE_DESCRIPTION("Elphel AHCI SATA platform driver for elphel393 camera");

Admin message