diff --git a/arch/arm/boot/dts/broadcom/rp1.dtsi b/arch/arm/boot/dts/broadcom/rp1.dtsi index 33bcc308607e..f1bd1bcb3fca 100644 --- a/arch/arm/boot/dts/broadcom/rp1.dtsi +++ b/arch/arm/boot/dts/broadcom/rp1.dtsi @@ -999,6 +999,25 @@ status = "disabled"; }; + rp1_csi1_test: csitest@128000 { + compatible = "raspberrypi,rp1-csi-test"; + reg = <0xc0 0x40128000 0x0 0x100>, // CSI2 DMA address + <0xc0 0x4012c000 0x0 0x100>, // PHY/CSI Host address + <0xc0 0x40138000 0x0 0x100>, // MIPI CFG address + <0xc0 0x4013c000 0x0 0x1000>; // PiSP FE address + + // interrupts must match rp1_pisp_fe setup + interrupts = ; + + clocks = <&rp1_clocks RP1_CLK_MIPI1_CFG>; + assigned-clocks = <&rp1_clocks RP1_CLK_MIPI1_CFG>; + assigned-clock-rates = <25000000>; + + #address-cells = <1>; + #size-cells = <0>; + status = "ok"; + }; + rp1_csi1: csi@128000 { compatible = "raspberrypi,rp1-cfe"; reg = <0xc0 0x40128000 0x0 0x100>, // CSI2 DMA address @@ -1100,6 +1119,28 @@ status = "disabled"; }; + rp1_dsi0_test: dsitest@110000 { + compatible = "raspberrypi,rp1-dsi-test"; + status = "ok"; + reg = <0xc0 0x40118000 0x0 0x1000>, // MIPI0 DSI DMA (ArgonDPI) + <0xc0 0x4011c000 0x0 0x1000>, // MIPI0 DSI Host (SNPS) + <0xc0 0x40120000 0x0 0x1000>; // MIPI0 CFG + + interrupts = ; + + clocks = <&rp1_clocks RP1_CLK_MIPI0_CFG>, + <&rp1_clocks RP1_CLK_MIPI0_DPI>, + <&rp1_clocks RP1_CLK_MIPI0_DSI_BYTECLOCK>, + <&clk_xosc>, // hardwired to DSI "refclk" + <&rp1_clocks RP1_PLL_SYS>; // alternate parent for divide + clock-names = "cfgclk", "dpiclk", "byteclk", "refclk", "pllsys"; + + assigned-clocks = <&rp1_clocks RP1_CLK_MIPI0_CFG>, + <&rp1_clocks RP1_CLK_MIPI0_DPI>, + <&rp1_clocks RP1_CLK_MIPI0_DSI_BYTECLOCK>; + assigned-clock-rates = <25000000>; + }; + rp1_dsi0: dsi@110000 { compatible = "raspberrypi,rp1dsi"; status = "disabled"; diff --git a/drivers/gpu/drm/rp1/rp1-dsi/Makefile b/drivers/gpu/drm/rp1/rp1-dsi/Makefile index 1a9672c7bda0..0d6cc44d41cf 100644 --- a/drivers/gpu/drm/rp1/rp1-dsi/Makefile +++ b/drivers/gpu/drm/rp1/rp1-dsi/Makefile @@ -1,5 +1,6 @@ # SPDX-License-Identifier: GPL-2.0-only drm-rp1-dsi-y := rp1_dsi.o rp1_dsi_dma.o rp1_dsi_dsi.o +rp1-dsi-test-y := rp1_dsi_test.o rp1_dsi_dsi.o -obj-$(CONFIG_DRM_RP1_DSI) += drm-rp1-dsi.o +obj-$(CONFIG_DRM_RP1_DSI) += drm-rp1-dsi.o rp1-dsi-test.o diff --git a/drivers/gpu/drm/rp1/rp1-dsi/rp1_dsi_dsi.c b/drivers/gpu/drm/rp1/rp1-dsi/rp1_dsi_dsi.c index fae5d13c74af..d0f21f6fe2b0 100644 --- a/drivers/gpu/drm/rp1/rp1-dsi/rp1_dsi_dsi.c +++ b/drivers/gpu/drm/rp1/rp1-dsi/rp1_dsi_dsi.c @@ -1386,7 +1386,19 @@ void rp1dsi_dsi_setup(struct rp1_dsi *dsi, struct drm_display_mode const *mode) if (dsi->display_flags & MIPI_DSI_MODE_VIDEO_BURST) vid_mode_cfg |= 0x02; DSI_WRITE(DSI_VID_MODE_CFG, vid_mode_cfg); - DSI_WRITE(DSI_CMD_MODE_CFG, 0x10F7F00); + + /* + * Modification for rp1dsitest to allow commands to be sent as HS. + * This is no worse than the current rpi-6.6.y version, but neither + * is strictly correct! A more correct behaviour is to require + * MIPI_DSI_MODE_VIDEO, and ignore MIPI_DSI_MODE_LPM (since + * we currently support only video-mode displays); and instead + * use MIPI_DSI_MSG_USE_LPM to select LP or HS for each command. + */ + if (dsi->display_flags & (MIPI_DSI_MODE_VIDEO | MIPI_DSI_MODE_LPM)) + DSI_WRITE(DSI_CMD_MODE_CFG, 0x10F7F00); /* <- existing behaviour */ + else + DSI_WRITE(DSI_CMD_MODE_CFG, 0); /* <- added for rp1dsitest */ /* Select Command Mode */ DSI_WRITE(DSI_MODE_CFG, 1); diff --git a/drivers/gpu/drm/rp1/rp1-dsi/rp1_dsi_test.c b/drivers/gpu/drm/rp1/rp1-dsi/rp1_dsi_test.c new file mode 100644 index 000000000000..7af1fea4fc48 --- /dev/null +++ b/drivers/gpu/drm/rp1/rp1-dsi/rp1_dsi_test.c @@ -0,0 +1,298 @@ +// SPDX-License-Identifier: GPL-2.0-or-later +/* + * DRM Driver for DSI output on Raspberry Pi RP1 + * + * Copyright (c) 2023 Raspberry Pi Limited. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "rp1_dsi.h" +#undef DRIVER_NAME +#undef MODULE_NAME +#define DRIVER_NAME "rp1-dsi-test" +#define MODULE_NAME "rp1-dsi-test" + +int num_lanes = 4; +module_param_named(num_lanes, num_lanes, int, 0600); +MODULE_PARM_DESC(num_lanes, "Number of lanes to test\n"); + +int mbps = 200; +module_param_named(mbps, mbps, int, 0600); +MODULE_PARM_DESC(pix_clock, "Megabits per second per lane\n"); + +bool lpmode = false; +module_param_named(lpmode, lpmode, bool, 0600); +MODULE_PARM_DESC(pix_clock, "Force LP mode (1 lane, low speed)\n"); + +static void rp1dsitest_setup_dsihost(struct rp1_dsi *dsi) +{ + struct drm_display_mode mode; + bool use24bpp = (num_lanes == 3 || num_lanes * mbps > 3200); + + /* + * "mode" is largely ignored, as we won't be streaming any video, + * but its pixel clock (together with display_format) determines + * the MIPI D-PHY clock and data rate. The MIPI clock should run + * continuously, even when we are using LP commands. + */ + mode.hdisplay = 800; + mode.hsync_start = 832; + mode.hsync_end = 840; + mode.htotal = 900; + mode.vdisplay = 480; + mode.vsync_start = 496; + mode.vsync_end = 500; + mode.vtotal = 525; + mode.clock = (1000 * num_lanes * mbps) / (use24bpp ? 24 : 16); + dsi->lanes = num_lanes; + dsi->display_format = use24bpp ? MIPI_DSI_FMT_RGB888 : MIPI_DSI_FMT_RGB565; + dsi->display_flags = lpmode ? MIPI_DSI_MODE_LPM : 0; + + drm_info(dsi->drm, "Setup lanes=%d mbps=%d bpp=%d (pixclock %d)\n", + num_lanes, mbps, use24bpp ? 24 : 16, mode.clock); + + if (dsi->clocks[RP1DSI_CLOCK_CFG]) + clk_prepare_enable(dsi->clocks[RP1DSI_CLOCK_CFG]); + + rp1dsi_dsi_setup(dsi, &mode); + dsi->dsi_running = true; +} + +static void rp1dsitest_teardown_dsihost(struct rp1_dsi *dsi) +{ + if (dsi) { + if (dsi->dsi_running) { + drm_info(dsi->drm, "Stopping DSI\n"); + rp1dsi_dsi_stop(dsi); + dsi->dsi_running = false; + + if (dsi->clocks[RP1DSI_CLOCK_CFG]) + clk_disable_unprepare(dsi->clocks[RP1DSI_CLOCK_CFG]); + } + } +} + +/* SYSFS interface for running tests */ + +static struct rp1_dsi *the_dsi; +static size_t data_size; +static char data_buf[PAGE_SIZE]; +static DEFINE_MUTEX(sysfs_mutex); + +static ssize_t rp1dsitest_show(struct kobject *kobj, struct kobj_attribute *attr, char *buf) +{ + size_t sz; + + mutex_lock(&sysfs_mutex); + sz = data_size; + printk(KERN_INFO "DSI: show %d\n", (int)sz); + if (sz) + memcpy(buf, data_buf, sz); + + mutex_unlock(&sysfs_mutex); + return sz; +} + +static ssize_t rp1dsitest_store(struct kobject *kobj, struct kobj_attribute *attr, const char *buf, size_t count) +{ + struct rp1_dsi *my_dsi; + + mutex_lock(&sysfs_mutex); + + if (count > PAGE_SIZE) + count = PAGE_SIZE; + + memcpy(data_buf, buf, count); + data_size = count; + + my_dsi = the_dsi; + if (!my_dsi) { + mutex_unlock(&sysfs_mutex); + return -EIO; + } + + printk(KERN_INFO "DSI: store %d\n", (int)data_size); + if (count > 1 || (count == 1 && buf[0])) { + if (!my_dsi->dsi_running) + rp1dsitest_setup_dsihost(my_dsi); + usleep_range(50, 100); /* Allow receiver to see all lanes in LP11 */ + rp1dsi_dsi_send(my_dsi, (count<<8) | 0x29, count, buf); + usleep_range(50, 100); /* Ensure all lanes have returned to LP11 */ + } else { + rp1dsitest_teardown_dsihost(my_dsi); + } + + mutex_unlock(&sysfs_mutex); + + return count; +} + +static struct kobj_attribute kobj_attr = __ATTR(rp1_dsi_test, 0644, rp1dsitest_show, rp1dsitest_store); + +static struct attribute *attrs[] = { + &kobj_attr.attr, + NULL +}; + +static struct attribute_group attr_group = { + .attrs = attrs, +}; + +static struct kobject *rp1dsitest_kobj; + +static struct drm_driver rp1dsitest_driver = { + .driver_features = 0, + .name = "rp1-dsi-test", + .desc = "rp1-dsi-test" +}; + +static int rp1dsitest_platform_probe(struct platform_device *pdev) +{ + struct device *dev = &pdev->dev; + struct drm_device *drm; + struct rp1_dsi *dsi; + int i, ret; + + drm = drm_dev_alloc(&rp1dsitest_driver, dev); + if (IS_ERR(drm)) { + ret = PTR_ERR(drm); + return ret; + } + dsi = drmm_kzalloc(drm, sizeof(*dsi), GFP_KERNEL); + if (!dsi) { + ret = -ENOMEM; + goto err_free_drm; + } + + init_completion(&dsi->finished); + dsi->drm = drm; + dsi->pdev = pdev; + drm->dev_private = dsi; + platform_set_drvdata(pdev, drm); + + /* Hardware resources */ + for (i = 0; i < RP1DSI_NUM_CLOCKS; i++) { + static const char * const myclocknames[RP1DSI_NUM_CLOCKS] = { + "cfgclk", "dpiclk", "byteclk", "refclk" + }; + dsi->clocks[i] = devm_clk_get(dev, myclocknames[i]); + if (IS_ERR(dsi->clocks[i])) { + ret = PTR_ERR(dsi->clocks[i]); + dev_err(dev, "Error getting clocks[%d]\n", i); + return ret; + } + } + + for (i = 0; i < RP1DSI_NUM_HW_BLOCKS; i++) { + dsi->hw_base[i] = + devm_ioremap_resource(dev, + platform_get_resource(dsi->pdev, + IORESOURCE_MEM, + i)); + if (IS_ERR(dsi->hw_base[i])) { + ret = PTR_ERR(dsi->hw_base[i]); + dev_err(dev, "Error memory mapping regs[%d]\n", i); + return ret; + } + } + /* We don't need interrupt or DMA - deleted those */ + + /* Enable the MIPI block and set the PHY MUX for DSI */ + rp1dsi_mipicfg_setup(dsi); + + /* XXX Yuck, global variables! */ + mutex_lock(&sysfs_mutex); + the_dsi = dsi; + rp1dsitest_kobj = kobject_create_and_add("rp1_dsi_test", kernel_kobj); + mutex_unlock(&sysfs_mutex); + if (!rp1dsitest_kobj) { + the_dsi = NULL; + return -ENOMEM; + } + + ret = sysfs_create_group(rp1dsitest_kobj, &attr_group); + if (!ret) + return 0; + + kobject_put(rp1dsitest_kobj); + rp1dsitest_kobj = NULL; + +err_free_drm: + dev_err(dev, "%s fail %d\n", __func__, ret); + drm_dev_put(drm); + return ret; + return ret; +} + +static void rp1dsitest_platform_shutdown(struct platform_device *pdev) +{ + struct drm_device *drm = platform_get_drvdata(pdev); + struct rp1_dsi *dsi = drm->dev_private; + + mutex_lock(&sysfs_mutex); + the_dsi = NULL; + if (dsi) + rp1dsitest_teardown_dsihost(dsi); + + mutex_unlock(&sysfs_mutex); + if (rp1dsitest_kobj) { + kobject_put(rp1dsitest_kobj); + rp1dsitest_kobj = NULL; + } +} + +static int rp1dsitest_platform_remove(struct platform_device *pdev) +{ + struct drm_device *drm = platform_get_drvdata(pdev); + + rp1dsitest_platform_shutdown(pdev); + drm_dev_put(drm); + return 0; +} + +static const struct of_device_id rp1dsitest_of_match[] = { + { + .compatible = "raspberrypi,rp1-dsi-test", + }, + { /* sentinel */ }, +}; + +MODULE_DEVICE_TABLE(of, rp1dsitest_of_match); + +static struct platform_driver rp1dsitest_platform_driver = { + .probe = rp1dsitest_platform_probe, + .remove = rp1dsitest_platform_remove, + .shutdown = rp1dsitest_platform_shutdown, + .driver = { + .name = DRIVER_NAME, + .owner = THIS_MODULE, + .of_match_table = rp1dsitest_of_match, + }, +}; + +module_platform_driver(rp1dsitest_platform_driver); + +MODULE_LICENSE("GPL"); +MODULE_DESCRIPTION("DSI loopback test driver for Raspberry Pi RP1"); +MODULE_AUTHOR("Nick Hollinghurst"); diff --git a/drivers/media/platform/raspberrypi/rp1_cfe/Makefile b/drivers/media/platform/raspberrypi/rp1_cfe/Makefile index 9709d6f603e9..da1e9f81793b 100644 --- a/drivers/media/platform/raspberrypi/rp1_cfe/Makefile +++ b/drivers/media/platform/raspberrypi/rp1_cfe/Makefile @@ -3,4 +3,5 @@ # Makefile for RP1 Camera Front End driver # rp1-cfe-objs := cfe.o csi2.o pisp_fe.o dphy.o -obj-$(CONFIG_VIDEO_RP1_CFE) += rp1-cfe.o +rp1-csi-test-objs := csi_test.o dphy.o +obj-$(CONFIG_VIDEO_RP1_CFE) += rp1-cfe.o rp1-csi-test.o diff --git a/drivers/media/platform/raspberrypi/rp1_cfe/csi_test.c b/drivers/media/platform/raspberrypi/rp1_cfe/csi_test.c new file mode 100644 index 000000000000..a4d13f53e7c0 --- /dev/null +++ b/drivers/media/platform/raspberrypi/rp1_cfe/csi_test.c @@ -0,0 +1,452 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * RP1 Camera Front End Driver + * + * Copyright (C) 2021-2022 - Raspberry Pi Ltd. + * + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "dphy.h" + +#define CSI_TEST_MODULE_NAME "rp1-csi-test" +#define CSI_TEST_VERSION "1.0" + +int num_lanes = 4; +module_param_named(num_lanes, num_lanes, int, 0600); +MODULE_PARM_DESC(num_lanes, "Number of lanes to test\n"); + +int mbps = 200; +module_param_named(mbps, mbps, int, 0600); +MODULE_PARM_DESC(pix_clock, "Megabits per second per lane\n"); + +/* MIPICFG registers */ +#define MIPICFG_CFG 0x004 +#define MIPICFG_INTR 0x028 +#define MIPICFG_INTE 0x02c +#define MIPICFG_INTF 0x030 +#define MIPICFG_INTS 0x034 + +#define MIPICFG_CFG_SEL_CSI BIT(0) + +#define MIPICFG_INT_CSI_DMA BIT(0) +#define MIPICFG_INT_CSI_HOST BIT(2) +#define MIPICFG_INT_PISP_FE BIT(4) + +/* CSI2-DMA registers */ +#define CSI2_STATUS 0x000 +#define CSI2_QOS 0x004 +#define CSI2_DISCARDS_OVERFLOW 0x008 +#define CSI2_DISCARDS_INACTIVE 0x00c +#define CSI2_DISCARDS_UNMATCHED 0x010 +#define CSI2_DISCARDS_LEN_LIMIT 0x014 + +#define CSI2_DISCARDS_AMOUNT_SHIFT 0 +#define CSI2_DISCARDS_AMOUNT_MASK GENMASK(23, 0) +#define CSI2_DISCARDS_DT_SHIFT 24 +#define CSI2_DISCARDS_DT_MASK GENMASK(29, 24) +#define CSI2_DISCARDS_VC_SHIFT 30 +#define CSI2_DISCARDS_VC_MASK GENMASK(31, 30) + +#define CSI2_LLEV_PANICS 0x018 +#define CSI2_ULEV_PANICS 0x01c +#define CSI2_IRQ_MASK 0x020 +#define CSI2_IRQ_MASK_IRQ_OVERFLOW BIT(0) +#define CSI2_IRQ_MASK_IRQ_DISCARD_OVERFLOW BIT(1) +#define CSI2_IRQ_MASK_IRQ_DISCARD_LENGTH_LIMIT BIT(2) +#define CSI2_IRQ_MASK_IRQ_DISCARD_UNMATCHED BIT(3) +#define CSI2_IRQ_MASK_IRQ_DISCARD_INACTIVE BIT(4) +#define CSI2_IRQ_MASK_IRQ_ALL \ + (CSI2_IRQ_MASK_IRQ_OVERFLOW | CSI2_IRQ_MASK_IRQ_DISCARD_OVERFLOW | \ + CSI2_IRQ_MASK_IRQ_DISCARD_LENGTH_LIMIT | \ + CSI2_IRQ_MASK_IRQ_DISCARD_UNMATCHED | \ + CSI2_IRQ_MASK_IRQ_DISCARD_INACTIVE) + +#define CSI2_CTRL 0x024 +#define CSI2_CH_CTRL(x) ((x) * 0x40 + 0x28) +#define CSI2_CH_ADDR0(x) ((x) * 0x40 + 0x2c) +#define CSI2_CH_ADDR1(x) ((x) * 0x40 + 0x3c) +#define CSI2_CH_STRIDE(x) ((x) * 0x40 + 0x30) +#define CSI2_CH_LENGTH(x) ((x) * 0x40 + 0x34) +#define CSI2_CH_DEBUG(x) ((x) * 0x40 + 0x38) +#define CSI2_CH_FRAME_SIZE(x) ((x) * 0x40 + 0x40) +#define CSI2_CH_COMP_CTRL(x) ((x) * 0x40 + 0x44) +#define CSI2_CH_FE_FRAME_ID(x) ((x) * 0x40 + 0x48) + +/* CSI2_STATUS */ +#define IRQ_FS(x) (BIT(0) << (x)) +#define IRQ_FE(x) (BIT(4) << (x)) +#define IRQ_FE_ACK(x) (BIT(8) << (x)) +#define IRQ_LE(x) (BIT(12) << (x)) +#define IRQ_LE_ACK(x) (BIT(16) << (x)) +#define IRQ_CH_MASK(x) (0x11111 << (x)) +#define IRQ_OVERFLOW BIT(20) +#define IRQ_DISCARD_OVERFLOW BIT(21) +#define IRQ_DISCARD_LEN_LIMIT BIT(22) +#define IRQ_DISCARD_UNMATCHED BIT(23) +#define IRQ_DISCARD_INACTIVE BIT(24) + +/* CSI2_CTRL */ +#define EOP_IS_EOL BIT(0) + +/* CSI2_CH_CTRL */ +#define DMA_EN BIT(0) +#define FORCE BIT(3) +#define AUTO_ARM BIT(4) +#define IRQ_EN_FS BIT(13) +#define IRQ_EN_FE BIT(14) +#define IRQ_EN_FE_ACK BIT(15) +#define IRQ_EN_LE BIT(16) +#define IRQ_EN_LE_ACK BIT(17) +#define FLUSH_FE BIT(28) +#define PACK_LINES BIT(29) +#define PACK_BYTES BIT(30) +#define CH_MODE_MASK GENMASK(2, 1) +#define VC_MASK GENMASK(6, 5) +#define DT_MASK GENMASK(12, 7) +#define LC_MASK GENMASK(27, 18) + +/* CHx_COMPRESSION_CONTROL */ +#define COMP_OFFSET_MASK GENMASK(15, 0) +#define COMP_SHIFT_MASK GENMASK(19, 16) +#define COMP_MODE_MASK GENMASK(25, 24) + +struct csitest_device { + /* peripheral base addresses */ + void __iomem *mipi_cfg_base; + void __iomem *csi2_base; + + struct clk *clk; + spinlock_t state_lock; /* we don't use this but probably should */ + + /* parent device */ + struct platform_device *pdev; + + struct dphy_data dphy; + + /* working state */ + struct sg_table *sgt; + u8 *buf; +}; + +static inline u32 cfg_reg_read(struct csitest_device *cfe, u32 offset) +{ + return readl(cfe->mipi_cfg_base + offset); +} + +static inline void cfg_reg_write(struct csitest_device *cfe, u32 offset, u32 val) +{ + writel(val, cfe->mipi_cfg_base + offset); +} + +static inline u32 csi2_reg_read(struct csitest_device *cfe, u32 offset) +{ + return readl(cfe->csi2_base + offset); +} + +static inline void csi2_reg_write(struct csitest_device *cfe, u32 offset, u32 val) +{ + writel(val, cfe->csi2_base + offset); +} + +static inline void set_field(u32 *valp, u32 field, u32 mask) +{ + u32 val = *valp; + + val &= ~mask; + val |= (field << __ffs(mask)) & mask; + *valp = val; +} + +static irqreturn_t csitest_isr(int irq, void *dev) +{ + //struct csitest_device *cfe = dev; + //unsigned int i; + + // TODO + return IRQ_HANDLED; +} + +static void csitest_start(struct csitest_device *cfe) +{ + struct sg_dma_page_iter it; + u64 addr; + u32 ctrl = 0; + + csi2_reg_write(cfe, CSI2_STATUS, -1); + csi2_reg_write(cfe, CSI2_IRQ_MASK, 0); + + cfe->dphy.dphy_rate = mbps; + cfe->dphy.active_lanes = num_lanes; + + clk_prepare_enable(cfe->clk); + dphy_start(&cfe->dphy); + + dma_sync_sgtable_for_cpu(&cfe->pdev->dev, cfe->sgt, DMA_FROM_DEVICE); + memset(cfe->buf, '?', PAGE_SIZE); + dma_sync_sgtable_for_device(&cfe->pdev->dev, cfe->sgt, DMA_FROM_DEVICE); + __sg_page_iter_start(&it.base, cfe->sgt->sgl, cfe->sgt->nents, 0); + addr = sg_page_iter_dma_address(&it); + + csi2_reg_write(cfe, CSI2_CTRL, EOP_IS_EOL); + csi2_reg_write(cfe, CSI2_CH_CTRL(1), 0); + csi2_reg_write(cfe, CSI2_CH_CTRL(2), 0); + csi2_reg_write(cfe, CSI2_CH_CTRL(3), 0); + csi2_reg_write(cfe, CSI2_CH_DEBUG(0), 0); + + ctrl = DMA_EN | FORCE | PACK_LINES; + csi2_reg_write(cfe, CSI2_CH_CTRL(0), ctrl); + + csi2_reg_write(cfe, CSI2_CH_LENGTH(0), (PAGE_SIZE - 32) >> 4); + csi2_reg_write(cfe, CSI2_CH_STRIDE(0), 64 >> 4); + + addr >>= 4; + csi2_reg_write(cfe, CSI2_CH_ADDR1(0), addr >> 32); + csi2_reg_write(cfe, CSI2_CH_ADDR0(0), addr & 0xffffffff); +} + +static void csitest_stop(struct csitest_device *cfe) +{ + csi2_reg_write(cfe, CSI2_CH_CTRL(0), FORCE); + csi2_reg_write(cfe, CSI2_CH_ADDR1(0), 0); + csi2_reg_write(cfe, CSI2_CH_ADDR0(0), 0); + csi2_reg_write(cfe, CSI2_CH_ADDR0(0), 0); + dphy_stop(&cfe->dphy); + //clk_disable_unprepare(cfe->clk); +} + +static size_t csitest_get_buffer_content(struct csitest_device *cfe, char *buf) +{ + u32 p, q; + + dma_sync_sgtable_for_cpu(&cfe->pdev->dev, cfe->sgt, DMA_FROM_DEVICE); + memcpy(buf, cfe->buf, PAGE_SIZE); + buf[PAGE_SIZE - 1] = '\0'; + dma_sync_sgtable_for_device(&cfe->pdev->dev, cfe->sgt, DMA_FROM_DEVICE); + + p = readl(cfe->dphy.base + 0x048); + q = readl(cfe->dphy.base + 0x04c); + dev_info(&cfe->pdev->dev, "CSI: phy_rx=0x%08x, stopstate=0x%08x\n", p, q); + dev_info(&cfe->pdev->dev, "CSI: status 0x%08x, discards 0x%08x, 0x%08x, 0x%08x, 0x%08x\n", + csi2_reg_read(cfe, CSI2_STATUS), + csi2_reg_read(cfe, 0x8), + csi2_reg_read(cfe, 0xc), + csi2_reg_read(cfe, 0x10), + csi2_reg_read(cfe, 0x14)); + dev_info(&cfe->pdev->dev, "CSI: get_buffer_content, CTRL=0x%08x, DEBUG=0x%08x\n", + csi2_reg_read(cfe, CSI2_CH_CTRL(0)), + csi2_reg_read(cfe, CSI2_CH_DEBUG(0))); + return PAGE_SIZE - 1; +} + +/* SYSFS interface for running tests */ + +static struct csitest_device *the_cfe; +static DEFINE_MUTEX(sysfs_mutex); + +static ssize_t csitest_show(struct kobject *kobj, struct kobj_attribute *attr, char *buf) +{ + size_t sz = 0; + struct csitest_device *my_cfe; + + mutex_lock(&sysfs_mutex); + my_cfe = the_cfe; + *buf = '\0'; + if (my_cfe) + sz = csitest_get_buffer_content(my_cfe, buf); + mutex_unlock(&sysfs_mutex); + + return sz; +} + +static ssize_t csitest_store(struct kobject *kobj, struct kobj_attribute *attr, + const char *buf, size_t count) +{ + struct csitest_device *my_cfe; + + mutex_lock(&sysfs_mutex); + my_cfe = the_cfe; + if (my_cfe) { + csitest_stop(my_cfe); + csitest_start(my_cfe); + } + mutex_unlock(&sysfs_mutex); + + return (count > PAGE_SIZE) ? PAGE_SIZE : count; +} + +static struct kobj_attribute kobj_attr = + __ATTR(rp1_csi_test, 0644, csitest_show, csitest_store); + +static struct attribute *attrs[] = { + &kobj_attr.attr, + NULL +}; + +static struct attribute_group attr_group = { + .attrs = attrs, +}; + +static struct kobject *csitest_kobj; + +static int csitest_probe(struct platform_device *pdev) +{ + struct csitest_device *cfe; + int ret; + + cfe = devm_kzalloc(&pdev->dev, sizeof(*cfe), GFP_KERNEL); + if (!cfe) + return -ENOMEM; + + platform_set_drvdata(pdev, cfe); + cfe->pdev = pdev; + spin_lock_init(&cfe->state_lock); + + cfe->csi2_base = devm_platform_ioremap_resource(pdev, 0); + if (IS_ERR(cfe->csi2_base)) { + dev_err(&pdev->dev, "Failed to get dma io block\n"); + ret = PTR_ERR(cfe->csi2_base); + goto err_cfe_put; + } + + cfe->dphy.base = devm_platform_ioremap_resource(pdev, 1); + if (IS_ERR(cfe->dphy.base)) { + dev_err(&pdev->dev, "Failed to get host io block\n"); + ret = PTR_ERR(cfe->dphy.base); + goto err_cfe_put; + } + + cfe->mipi_cfg_base = devm_platform_ioremap_resource(pdev, 2); + if (IS_ERR(cfe->mipi_cfg_base)) { + dev_err(&pdev->dev, "Failed to get mipi cfg io block\n"); + ret = PTR_ERR(cfe->mipi_cfg_base); + goto err_cfe_put; + } + + ret = platform_get_irq(pdev, 0); + if (ret <= 0) { + dev_err(&pdev->dev, "No IRQ resource\n"); + ret = -EINVAL; + goto err_cfe_put; + } + + ret = devm_request_irq(&pdev->dev, ret, csitest_isr, 0, "rp1-cfe", cfe); + if (ret) { + dev_err(&pdev->dev, "Unable to request interrupt\n"); + ret = -EINVAL; + goto err_cfe_put; + } + + ret = dma_set_mask_and_coherent(&pdev->dev, DMA_BIT_MASK(64)); + if (ret) { + dev_err(&pdev->dev, "DMA enable failed\n"); + goto err_cfe_put; + } + cfe->sgt = dma_alloc_noncontiguous(&pdev->dev, PAGE_SIZE, + DMA_FROM_DEVICE, GFP_KERNEL, + DMA_ATTR_ALLOC_SINGLE_PAGES); + if (!cfe->sgt) { + ret = -ENOMEM; + goto err_cfe_put; + } + cfe->buf = dma_vmap_noncontiguous(&pdev->dev, PAGE_SIZE, + cfe->sgt); + if (!cfe->buf) { + ret = -ENOMEM; + goto err_cfe_put; + } + + /* TODO: Enable clock only when running. */ + cfe->clk = devm_clk_get(&pdev->dev, NULL); + if (IS_ERR(cfe->clk)) + return dev_err_probe(&pdev->dev, PTR_ERR(cfe->clk), + "clock not found\n"); + + /* Enable the MIPI block, set the PHY MUX for CSI2, probe the PHY */ + cfg_reg_write(cfe, MIPICFG_CFG, MIPICFG_CFG_SEL_CSI); + cfg_reg_write(cfe, MIPICFG_INTE, MIPICFG_INT_CSI_DMA); + cfe->dphy.dev = &pdev->dev; + cfe->dphy.dphy_rate = mbps; + cfe->dphy.max_lanes = 4; + cfe->dphy.active_lanes = num_lanes; + dphy_probe(&cfe->dphy); + + /* Start the test running. Any write to the sysfs file will reset the buffer. */ + csitest_start(cfe); + + /* XXX Yuck, global variables! */ + mutex_lock(&sysfs_mutex); + the_cfe = cfe; + csitest_kobj = kobject_create_and_add("rp1_csi_test", kernel_kobj); + mutex_unlock(&sysfs_mutex); + if (!csitest_kobj) { + the_cfe = NULL; + return -ENOMEM; + } + + ret = sysfs_create_group(csitest_kobj, &attr_group); + if (!ret) + return 0; + +err_cfe_put: + return ret; +} + +static int csitest_remove(struct platform_device *pdev) +{ + struct csitest_device *cfe = platform_get_drvdata(pdev); + + csitest_stop(cfe); + + mutex_lock(&sysfs_mutex); + the_cfe = NULL; + mutex_unlock(&sysfs_mutex); + if (csitest_kobj) { + kobject_put(csitest_kobj); + csitest_kobj = NULL; + } + return 0; +} + +static const struct of_device_id csitest_of_match[] = { + { .compatible = "raspberrypi,rp1-csi-test" }, + { /* sentinel */ }, +}; +MODULE_DEVICE_TABLE(of, csitest_of_match); + +static struct platform_driver csi_test_driver = { + .probe = csitest_probe, + .remove = csitest_remove, + .driver = { + .name = CSI_TEST_MODULE_NAME, + .of_match_table = csitest_of_match, + }, +}; + +module_platform_driver(csi_test_driver); + +MODULE_DESCRIPTION("RP1 CSI test driver"); +MODULE_LICENSE("GPL");