Read multiple sectors to a buffer pointer by S/G list (bbfc3d59) · Commits · Elphel / linux-elphel

src/drivers/ata/ahci_elphel.c

+61 −49

Original line number	Original line	Diff line number	Diff line
	@@ -386,23 +386,24 @@ module_platform_driver(ahci_elphel_driver);
	#define TEST_BUFF_SZ 512		#define TEST_BUFF_SZ 512
	//#define SDA2_LBA_ADDR 124963848		//#define SDA2_LBA_ADDR 124963848
	#define SDA2_LBA_ADDR 1		#define SDA2_LBA_ADDR 1
			#define SG_TBL_SZ 3
	static ssize_t elphel_test_write(struct device dev, struct device_attribute attr,		static ssize_t elphel_test_write(struct device dev, struct device_attribute attr,
	const char *buff, size_t buff_sz)		const char *buff, size_t buff_sz)
	{		{
	int i, n_elem;		int i, n_elem;
	int sg_elems;		int sg_elems = 0;
	struct ata_host *host;		struct ata_host *host;
	struct ata_port *port;		struct ata_port *port;
	struct ahci_host_priv *hpriv;		struct ahci_host_priv *hpriv;
	struct elphel_ahci_priv *dpriv;		struct elphel_ahci_priv *dpriv;
	struct scatterlist sg;		struct scatterlist *sgl;
	struct scatterlist *sg_ptr;		struct scatterlist *sg_ptr;
	// u8 *test_buff = pElphel_buf->d2h_vaddr;		// u8 *test_buff = pElphel_buf->d2h_vaddr;
	u8 *test_buff;		u8 *test_buff;
	unsigned int lba_addr;		unsigned int lba_addr;

	if (sscanf(buff, "%u", &lba_addr) == 1) {		if (sscanf(buff, "%u", &lba_addr) == 1) {
	printk(KERN_DEBUG ">>> got LBA address: %d", lba_addr);		printk(KERN_DEBUG ">>> got LBA address: %d\n", lba_addr);
	} else {		} else {
	lba_addr = SDA2_LBA_ADDR;		lba_addr = SDA2_LBA_ADDR;
	}		}
	@@ -413,30 +414,27 @@ static ssize_t elphel_test_write(struct device dev, struct device_attribute at
	dpriv = hpriv->plat_data;		dpriv = hpriv->plat_data;

	/* prepare buffer and fill it with markers */		/* prepare buffer and fill it with markers */
			sgl = kmalloc(sizeof(struct scatterlist) * SG_TBL_SZ, GFP_KERNEL);
			if (!sgl)
			return ENOMEM;
			sg_init_table(sgl, SG_TBL_SZ);
			for_each_sg(sgl, sg_ptr, SG_TBL_SZ, n_elem) {
	test_buff = kmalloc(TEST_BUFF_SZ, GFP_KERNEL);		test_buff = kmalloc(TEST_BUFF_SZ, GFP_KERNEL);
	if (!test_buff)		if (!test_buff)
	return ENOMEM;		return ENOMEM;
	for (i = 0; i < TEST_BUFF_SZ; i++) {		memset(test_buff, 0xa5, TEST_BUFF_SZ);
	test_buff[i] = 0xa5;		sg_set_buf(sg_ptr, (void *)test_buff, TEST_BUFF_SZ);
			sg_elems++;
	}		}

	/* read test */		printk(KERN_DEBUG ">>> mapped %d SG elemets\n", sg_elems);
	sg_init_one(&sg, test_buff, TEST_BUFF_SZ);
	sg_elems = 1;

	printk(KERN_DEBUG ">>> dump the content of SG list:\n");
	n_elem = 0;
	for_each_sg(&sg, sg_ptr, sg_elems, 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 ">>>\n");

	dma_map_sg(dev, &sg, sg_elems, DMA_FROM_DEVICE);		/* read test */
			dma_map_sg(dev, sgl, sg_elems, DMA_FROM_DEVICE);

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

	while (dpriv->flags & IRQ_SIMPLE) {		while (dpriv->flags & IRQ_SIMPLE) {
	@@ -451,8 +449,13 @@ static ssize_t elphel_test_write(struct device dev, struct device_attribute at
	// printk(KERN_DEBUG ">>> buffer has been mapped for device\n");		// printk(KERN_DEBUG ">>> buffer has been mapped for device\n");

	printk(KERN_DEBUG ">>> dump test buffer after reading: %d bytes\n", TEST_BUFF_SZ);		printk(KERN_DEBUG ">>> dump test buffer after reading: %d bytes\n", TEST_BUFF_SZ);
	dma_unmap_sg(dev, &sg, sg_elems, DMA_FROM_DEVICE);		dma_unmap_sg(dev, sgl, sg_elems, DMA_FROM_DEVICE);
	print_hex_dump_bytes("", DUMP_PREFIX_OFFSET, test_buff, TEST_BUFF_SZ);		for (i = 0; i < sg_elems; i++) {
			dev_dbg(dev, ">>> sector %i\n", i);
			u8 buff[TEST_BUFF_SZ];
			sg_copy_to_buffer(&sgl[i], 1, buff, TEST_BUFF_SZ);
			print_hex_dump_bytes("", DUMP_PREFIX_OFFSET, buff, TEST_BUFF_SZ);
			}

	/* end of read test */		/* end of read test */

	@@ -549,6 +552,26 @@ inline void prep_cfis(u8 *cmd_tbl, ///< pointer to the beginning of co
	cmd_tbl[15] = ctrl; // control		cmd_tbl[15] = ctrl; // control
	}		}

			/** Map S/G list to physical region descriptor table in AHCI controller command table */
			inline void prep_prdt(struct scatterlist *sgl, ///< pointer to S/G list which should be mapped to physical
			///< region description table
			unsigned int n_elem, ///< the number of elements in @e sgl
			struct ahci_sg *ahci_sgl) ///< pointer to physical region description table
			///< @return None
			{
			unsigned int num = 0;
			struct scatterlist *sg_ptr;

			for_each_sg(sgl, sg_ptr, n_elem, num) {
			dma_addr_t addr = sg_dma_address(sg_ptr);
			u32 sg_len = sg_dma_len(sg_ptr);

			ahci_sgl[num].addr = cpu_to_le32(addr & 0xffffffff);
			ahci_sgl[num].addr_hi = cpu_to_le32((addr >> 16) >> 16);
			ahci_sgl[num].flags_size = cpu_to_le32(sg_len - 1);
			}
			}

	static int elphel_write_dma(struct ata_port ap, u64 start, u16 count, struct scatterlist sg, unsigned int elem)		static int elphel_write_dma(struct ata_port ap, u64 start, u16 count, struct scatterlist sg, unsigned int elem)
	{		{
	u32 opts;		u32 opts;
	@@ -589,14 +612,15 @@ static int elphel_write_dma(struct ata_port *ap, u64 start, u16 count, struct sc
	/* prepare physical region descriptor table */		/* prepare physical region descriptor table */
	n_elem = 0;		n_elem = 0;
	ahci_sg = pp->cmd_tbl + slot_num * AHCI_CMD_TBL_SZ + AHCI_CMD_TBL_HDR_SZ;		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) {		prep_prdt(sg, elem, ahci_sg);
	dma_addr_t addr = sg_dma_address(sg_ptr);		// for_each_sg(sg, sg_ptr, elem, n_elem) {
	u32 sg_len = sg_dma_len(sg_ptr);		// 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].addr = cpu_to_le32(addr & 0xffffffff);
	ahci_sg[n_elem].flags_size = cpu_to_le32(sg_len - 1);		// 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 */		/* prepare command header */
	opts = cmd_fis_len \| (n_elem << 16) \| AHCI_CMD_WRITE \| AHCI_CMD_PREFETCH \| AHCI_CMD_CLR_BUSY;		opts = cmd_fis_len \| (n_elem << 16) \| AHCI_CMD_WRITE \| AHCI_CMD_PREFETCH \| AHCI_CMD_CLR_BUSY;
	@@ -612,15 +636,12 @@ static int elphel_write_dma(struct ata_port *ap, u64 start, u16 count, struct sc
	static int elphel_read_dma(struct ata_port ap, u64 start, u16 count, struct scatterlist sgl, unsigned int elem)		static int elphel_read_dma(struct ata_port ap, u64 start, u16 count, struct scatterlist sgl, unsigned int elem)
	{		{
	u32 opts;		u32 opts;
	const u32 cmd_fis_len = 5;
	unsigned int n_elem;
	u8 *cmd_tbl;		u8 *cmd_tbl;
	u8 cmd;		u8 cmd;
	unsigned int slot_num = 0;		unsigned int slot_num = 0;
	struct ahci_port_priv *pp = ap->private_data;		struct ahci_port_priv *pp = ap->private_data;
	struct ahci_host_priv *hpriv = ap->host->private_data;		struct ahci_host_priv *hpriv = ap->host->private_data;
	struct elphel_ahci_priv *dpriv = hpriv->plat_data;		struct elphel_ahci_priv *dpriv = hpriv->plat_data;
	struct scatterlist *sg_ptr;
	struct ahci_sg *ahci_sg;		struct ahci_sg *ahci_sg;
	void __iomem *port_mmio = ahci_port_base(ap);		void __iomem *port_mmio = ahci_port_base(ap);

	@@ -636,20 +657,11 @@ static int elphel_read_dma(struct ata_port *ap, u64 start, u16 count, struct sca
	prep_cfis(cmd_tbl, cmd, start, count);		prep_cfis(cmd_tbl, cmd, start, count);

	/* prepare physical region descriptor table */		/* prepare physical region descriptor table */
	n_elem = 0;
	ahci_sg = pp->cmd_tbl + slot_num * AHCI_CMD_TBL_SZ + AHCI_CMD_TBL_HDR_SZ;		ahci_sg = pp->cmd_tbl + slot_num * AHCI_CMD_TBL_SZ + AHCI_CMD_TBL_HDR_SZ;
	for_each_sg(sgl, sg_ptr, elem, n_elem) {		prep_prdt(sgl, elem, ahci_sg);
	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 */		/* prepare command header */
	opts = CMD_FIS_LEN \| (n_elem << 16) \| AHCI_CMD_PREFETCH \| AHCI_CMD_CLR_BUSY;		opts = CMD_FIS_LEN \| (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);		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);		printk(KERN_DEBUG ">>> dump command table content, first %d bytes, phys addr = 0x%x:\n", TEST_BUFF_SZ, pp->cmd_tbl_dma);

Admin message