From d01183a54585a04f3f2a77827abba0b7988710ed Mon Sep 17 00:00:00 2001 From: Mikhail Karpenko Date: Sat, 30 Jul 2016 11:08:06 -0600 Subject: [PATCH] Read sector to predefined buffer (not SG list) --- src/drivers/ata/ahci_elphel.c | 346 +++++++++++++++++++++++++++++++++- 1 file changed, 344 insertions(+), 2 deletions(-) diff --git a/src/drivers/ata/ahci_elphel.c b/src/drivers/ata/ahci_elphel.c index 2f39ba3..75e87a5 100644 --- a/src/drivers/ata/ahci_elphel.c +++ b/src/drivers/ata/ahci_elphel.c @@ -26,6 +26,8 @@ #include #include "ahci.h" +#include + #define DRV_NAME "elphel-ahci" /* * 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 */ #define PROP_NAME_CLB_OFFS "clb_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 const struct ata_port_info ahci_elphel_port_info; @@ -50,8 +58,17 @@ struct elphel_ahci_priv { u32 clb_offs; u32 fb_offs; 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, 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 device *dev = &pdev->dev; const struct of_device_id *match; - unsigned int reg_val; if (&dev->kobj) { ret = sysfs_create_group(&dev->kobj, &dev_attr_root_group); if (ret < 0) return ret; } - elphel_defer_load(dev); +// elphel_defer_load(dev); 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); 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; } @@ -231,6 +261,13 @@ static int elphel_drv_remove(struct platform_device *pdev) 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) { 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); + dev_dbg(ap->dev, ">>> CFIS dump, data from libahci:\n"); + print_hex_dump_bytes("", DUMP_PREFIX_OFFSET, cmd_tbl, 20); + 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); @@ -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(test_write, S_IWUSR | S_IWGRP, NULL, elphel_test_write); static struct attribute *root_dev_attrs[] = { &dev_attr_load_module.attr, + &dev_attr_test_write.attr, NULL }; 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); +#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_AUTHOR("Elphel, Inc."); MODULE_DESCRIPTION("Elphel AHCI SATA platform driver for elphel393 camera"); -- 2.18.1