Raspberry Pi Forums

Code: Select all

/dts-v1/;
/plugin/;
/ {
	compatible = "brcm,bcm2712";
	fragment@0 {
		/* * Target the CSI1 node (CAM0 connector) */
		target = <&csi1>;
		__overlay__ {
			compatible = "acme,fpga-csi-rx", "brcm,rp1-csi";
			status = "okay";
			acme,raw-bits = <10>; /* set to <8> for RAW8, or to <10> for RAW10 */
			acme,dt = <0x2b>; /* MIPI CSI-2 Data Type: set to <0x2a> for RAW8, <0x2b> for RAW10 */
			acme,mbps = <437>; /* lane rate in Mbps */
			acme,nlanes = <2>; /* number of data lanes */
		};
	};
};

Code: Select all

// fpga-csi.cleaned.c — CSI-2 RAW8/RAW10 char device for Raspberry Pi 5 (RP1, Cam1)
//
#include <linux/module.h>
#include <linux/platform_device.h>
#include <linux/of_address.h>
#include <linux/of_device.h>
#include <linux/of_clk.h>
#include <linux/interrupt.h>
#include <linux/io.h>
#include <linux/miscdevice.h>
#include <linux/fs.h>
#include <linux/uaccess.h>
#include <linux/poll.h>
#include <linux/wait.h>
#include <linux/spinlock.h>
#include <linux/vmalloc.h>
#include <linux/mutex.h>
#include <linux/pm_runtime.h>
#include <linux/dma-mapping.h>
#include <linux/clk.h>
#include <linux/reset.h>
#include <linux/kernel.h>
#include <linux/delay.h>
#include "fpga_csi.h"
#define DRV_NAME "fpga-csi"
#define DEV_NAME "csi_stream0"
/* Module param: drop-oldest vs block producer when ring is full */
static bool drop_oldest;
module_param(drop_oldest, bool, 0444);
MODULE_PARM_DESC(drop_oldest, "Drop oldest data when ring is full (default: 0=block)");
/* ========= Userspace byte ring ========= */
#define RING_ORDER 7 /* 512 KiB default */
#define RING_PAGES (1u << RING_ORDER)
#define RING_SIZE (RING_PAGES << PAGE_SHIFT)
struct byte_ring {
 u8 *data;
 size_t size; /* power-of-two */
 size_t rpos; /* modulo size */
 size_t wpos; /* modulo size */
 spinlock_t lock;
 wait_queue_head_t wq_read; /* readers wait for data */
 wait_queue_head_t wq_space; /* producer waits for space */
};
static inline size_t r_used(const struct byte_ring *r)
{ return (r->wpos - r->rpos) & (r->size - 1); }
static inline size_t r_space(const struct byte_ring *r)
{ return r->size - 1 - r_used(r); }
static size_t r_write(struct byte_ring *r, const u8 *src, size_t len, bool allow_drop, u64 *overflows)
{
 size_t written = 0;
 while (len) {
 size_t space = r_space(r);
 if (!space) {
 if (allow_drop) {
 size_t give = r->size >> 1; /* amortize wakeups */
 r->rpos = (r->rpos + give) & (r->size - 1);
 if (overflows) (*overflows)++;
 wake_up_interruptible(&r->wq_read);
 continue;
 }
 break; /* caller should sleep */
 }
 size_t chunk = min(len, space);
 size_t tail = r->size - r->wpos;
 size_t n1 = min(chunk, tail);
 memcpy(&r->data[r->wpos], src, n1);
 r->wpos = (r->wpos + n1) & (r->size - 1);
 src += n1; len -= n1; written += n1;
 if (chunk > n1) {
 size_t n2 = chunk - n1;
 memcpy(&r->data[r->wpos], src, n2);
 r->wpos = (r->wpos + n2) & (r->size - 1);
 src += n2; len -= n2; written += n2;
 }
 }
 if (written) wake_up_interruptible(&r->wq_read);
 return written;
}
/* ========= RP1 CSI2-DMA registers ========= */
struct rp1_regs {
 void __iomem *csi2; /* reg[0] CSI2-DMA */
 void __iomem *dphy; /* reg[1] D-PHY */
 void __iomem *mipic; /* reg[2] MIPI CFG */
 void __iomem *fe; /* reg[3] FE */
};
static inline void __iomem *ch_base(void __iomem *csi2, u32 ch)
{ return csi2 + (0x028 + (ch * 0x40)); }
/* CSI2-DMA global regs (base = csi2) */
#define CSI2_STATUS 0x000
#define CSI2_QOS 0x004
#define CSI2_DISCARDS_DT 0x008
#define CSI2_DISCARDS_VC 0x00c
#define CSI2_DISCARDS_FIFO 0x010
#define CSI2_DISCARDS_CHKS 0x014
#define CSI2_LLEV_PANICS 0x018
#define CSI2_ULEV_PANICS 0x01c
#define CSI2_IRQ_MASK 0x020
#define CSI2_CTRL 0x024
#define CSI2_CTRL_EOP_IS_EOL BIT(0)
/* Per-channel register block offsets (relative to channel base) */
#define CH_CTRL 0x000
#define CH_ADDR0 0x004
#define CH_LENGTH 0x00c /* units of 16 bytes */
#define CH_STRIDE 0x008 /* units of 16 bytes */
#define CH_DEBUG 0x010 /* frame/line counters (ro) */
#define CH_ADDR1 0x014 /* upper address bits (>>36) */
#define CH_FRAME_SIZE 0x018 /* (h<<16)|w when CH_MODE != 0 */
#define CH_COMP_CTRL 0x01c
#define CH_FE_FRAME_ID 0x020
/* MIPI CFG (base = mipic) */
#define MIPIC_CFG 0x004
#define MIPIC_CFG_SEL_CSI BIT(0)
#define MIPIC_INTR 0x028 /* Raw interrupt status */
#define MIPIC_INTE 0x02c /* Interrupt Enable */
#define MIPIC_INTF 0x030 /* Interrupt Force */
#define MIPIC_INTS 0x034 /* Masked Status (R) / Clear (W1C) */
#define MIPIC_INT_CSI_DMA BIT(0)
#define MIPIC_INT_CSI_HOST BIT(2)
#define MIPIC_INT_PISP_FE BIT(4)
/* CSI2_STATUS per-channel bits */
#define CSI2_STATUS_IRQ_FS(ch) (BIT(0) << (ch))
#define CSI2_STATUS_IRQ_FE(ch) (BIT(4) << (ch))
#define CSI2_STATUS_IRQ_FE_ACK(ch) (BIT(8) << (ch))
#define CSI2_STATUS_IRQ_LE(ch) (BIT(12) << (ch))
#define CSI2_STATUS_IRQ_LE_ACK(ch) (BIT(16) << (ch))
/* CH_CTRL layout */
#define CH_CTRL_DMA_EN BIT(0)
#define CH_CTRL_FORCE BIT(3)
#define CH_CTRL_AUTO_ARM BIT(4)
#define CH_CTRL_MODE_SHIFT 1
#define CH_CTRL_MODE_MASK (0x3 << CH_CTRL_MODE_SHIFT)
#define CH_CTRL_VC_SHIFT 5
#define CH_CTRL_DT_SHIFT 7
#define CH_CTRL_DT_MASK (0x3f << CH_CTRL_DT_SHIFT)
#define CH_CTRL_IRQ_FS_EN BIT(13)
#define CH_CTRL_IRQ_FE_EN BIT(14)
#define CH_CTRL_IRQ_FE_ACK_EN BIT(15)
#define CH_CTRL_IRQ_LE_EN BIT(16)
#define CH_CTRL_IRQ_LE_ACK_EN BIT(17)
#define CH_CTRL_LC_SHIFT 18
#define CH_CTRL_LC_MASK (0x3ff << CH_CTRL_LC_SHIFT)
#define CH_CTRL_FLUSH_FE BIT(28)
#define CH_CTRL_PACK_LINE BIT(29)
#define CH_CTRL_PACK_BYTES BIT(30) /* RAW8 only */
#define CSI2_MODE_NORMAL 0
#define CSI2_MODE_REMAP 1
#define CSI2_MODE_COMPRESSED 2
#define CSI2_MODE_FE_STREAMING 3
#define FE_INT_EOF BIT(0)
#define FE_INT_SOF BIT(1)
#define CSI_DMA_CHANNEL 0
#define DMA_BUF_COUNT 4
struct dma_buf {
 void *cpu;
 dma_addr_t dma;
 size_t size;
};
struct fpga_csi_dev {
 struct device *dev;
 struct rp1_regs regs;
 int irq;
 struct clk *clk_core; /* DT clocks[0] */
 struct clk *clk_phy; /* DT clocks[1] optional */
 struct reset_control *rst_core; /* optional */
 struct reset_control *rst_phy; /* optional */
 struct byte_ring ring;
 struct mutex read_lock;
 struct dma_buf dbuf[DMA_BUF_COUNT];
 u32 cur_idx;
 size_t dma_span; /* how many bytes the IRQ will push per FE */
 struct csi_filter_cfg filter; /* current VC/DT filter */
 struct csi_geometry geom; /* current geometry */
 /* overlay defaults (read at probe): */
 u32 ov_raw_bits; /* 8 or 10; 0 means "unknown" */
 u32 ov_dt; /* 0x2A/0x2B when provided, else 0 */
 struct csi_stats stats;
 u32 last_dis_dt;
 u32 last_dis_vc;
 u32 last_dis_fifo;
 u32 last_dis_crc;
 u32 last_frame_id;
 /* Diagnostics */
 u64 mipic_irq_total;
 u64 mipic_irq_dma;
 u64 mipic_irq_host;
 u64 mipic_irq_other;
 u64 ch_irq_total;
 u64 ch_irq_fe;
 struct miscdevice miscdev;
};
static inline u32 rd(struct fpga_csi_dev *cd, u32 off)
{ return readl(cd->regs.csi2 + off); }
static inline void wr(struct fpga_csi_dev *cd, u32 off, u32 v)
{ writel(v, cd->regs.csi2 + off); }
static inline u32 rd_ch(struct fpga_csi_dev *cd, u32 ch_off, u32 off)
{ return readl(ch_base(cd->regs.csi2, ch_off) + off); }
static inline void wr_ch(struct fpga_csi_dev *cd, u32 ch_off, u32 off, u32 v)
{ writel(v, ch_base(cd->regs.csi2, ch_off) + off); }
/* MIPI CFG accessors */
static inline u32 rd_mipic(struct fpga_csi_dev *cd, u32 off)
{ return readl(cd->regs.mipic + off); }
static inline void wr_mipic(struct fpga_csi_dev *cd, u32 off, u32 v)
{ writel(v, cd->regs.mipic + off); }
/* ---- devm action helpers ---- */
static void clk_disable_unprepare_put(void *p)
{
 struct clk *c = p;
 clk_disable_unprepare(c);
 clk_put(c);
}
static void reset_assert_action(void *p)
{
 struct reset_control *r = p;
 reset_control_assert(r);
}
/* ---- D-PHY TEST interface helpers ---- */
static uint8_t dphy_xact(void __iomem *d, u8 code, u8 data)
{
 u32 ctrl0, ctrl1;
 ctrl0 = readl(d + 0x050);
 writel((ctrl0 & ~0x2) | 0x2, d + 0x050); /* TESTCLK=1 */
 ctrl1 = readl(d + 0x054);
 writel((ctrl1 & ~(1u << 16)) | 0, d + 0x054); /* TESTEN=0 */
 ctrl1 = readl(d + 0x054);
 writel((ctrl1 & ~0xffu) | code, d + 0x054); /* DATA=code */
 ctrl1 = readl(d + 0x054);
 writel(ctrl1 | (1u << 16), d + 0x054); /* TESTEN=1 */
 ctrl0 = readl(d + 0x050);
 writel(ctrl0 & ~0x2, d + 0x050); /* TESTCLK=0 */
 /* Present data */
 ctrl1 = readl(d + 0x054);
 writel(ctrl1 & ~(1u << 16), d + 0x054); /* TESTEN=0 */
 ctrl1 = readl(d + 0x054);
 writel((ctrl1 & ~0xffu) | data, d + 0x054);
 ctrl0 = readl(d + 0x050);
 writel((ctrl0 & ~0x2) | 0x2, d + 0x050); /* TESTCLK=1 */
 return ((readl(d + 0x054) >> 8) & 0xff);
}
/* ===== D-PHY init ===== */
static int dphy_init(struct fpga_csi_dev *cd)
{
 void __iomem *d = cd->regs.dphy;
 u32 nlanes = 1;
 u32 mbps = 640; /* default lane rate */
 u32 nlanes_reg;
 if (!d)
 return -ENODEV;
 of_property_read_u32(cd->dev->of_node, "acme,nlanes", &nlanes);
 of_property_read_u32(cd->dev->of_node, "acme,mbps", &mbps);
 if (nlanes == 0) nlanes = 1;
 nlanes_reg = (nlanes - 1) & 0x3;
 dev_info(cd->dev, "DPHY VERSION=0x%08x, lanes=%u (N_LANES=%u), %u Mbps",
 readl(d + 0x000), nlanes, nlanes_reg, mbps);
 /* Configure the number of lanes *before* the reset sequence. */
 writel(nlanes_reg, d + 0x004); /* N_LANES = lanes-1 */
 /* Reset sequence */
 writel(0, d + 0x044); /* PHY_RSTZ=0 */
 writel(0, d + 0x040); /* PHY_SHUTDOWNZ=0 */
 /* Pulse TSTCLR while TSTCLK high (per ref driver) */
 {
 u32 c0, c1;
 c0 = readl(d + 0x050);
 writel((c0 & ~2) | 2, d + 0x050); /* TSTCLK=1 */
 c1 = readl(d + 0x054);
 writel(c1 & ~(1u << 16), d + 0x054); /* TESTEN=0 */
 c0 = readl(d + 0x050);
 writel((c0 & ~1) | 1, d + 0x050); /* TSTCLR=1 */
 usleep_range(15, 20);
 c0 = readl(d + 0x050);
 writel(c0 & ~1, d + 0x050); /* TSTCLR=0 */
 usleep_range(15, 20);
 }
 /* HSFREQRANGE table (mbps threshold, code) */
 static const u16 htab[][2] = {
 { 89, 0b000000 }, { 99, 0b010000 }, { 109, 0b100000 },
 { 129, 0b000001 }, { 139, 0b010001 }, { 149, 0b100001 },
 { 169, 0b000010 }, { 179, 0b010010 }, { 199, 0b100010 },
 { 219, 0b000011 }, { 239, 0b010011 }, { 249, 0b100011 },
 { 269, 0b000100 }, { 299, 0b010100 }, { 329, 0b000101 },
 { 359, 0b010101 }, { 399, 0b100101 }, { 449, 0b000110 },
 { 499, 0b010110 }, { 549, 0b000111 }, { 599, 0b010111 },
 { 649, 0b001000 }, { 699, 0b011000 }, { 749, 0b001001 },
 { 799, 0b011001 }, { 849, 0b101001 }, { 899, 0b111001 },
 { 949, 0b001010 }, { 999, 0b011010 }, {1049, 0b101010 },
 {1099, 0b111010 }, {1149, 0b001011 }, {1199, 0b011011 },
 {1249, 0b101011 }, {1299, 0b111011 }, {1349, 0b001100 },
 {1399, 0b011100 }, {1449, 0b101100 }, {1500, 0b111100 },
 };
 u8 code = 0b001000; /* ~640 Mbps default */
 for (size_t i = 0; i < ARRAY_SIZE(htab) - 1; ++i) {
 if (mbps <= htab[i][0]) { code = htab[i][1]; break; }
 }
 dphy_xact(d, 0x44, code << 1); /* HS_RX_CTRL_LANE0 */
 /* Bring PHY out of reset */
 usleep_range(5, 10);
 writel(1, d + 0x040); /* SHUTDOWNZ=1 */
 usleep_range(5, 10);
 writel(1, d + 0x044); /* RSTZ=1 */
 /* De-assert controller reset */
 writel(0xffffffff, d + 0x008); /* RESETN deassert */
 usleep_range(10, 50);
 /* EOP => EOL so FE IRQ aligns to frame end */
 wr(cd, CSI2_CTRL, CSI2_CTRL_EOP_IS_EOL);
 return 0;
}
/* Wait for STOPSTATE = LP-11 on clock + enabled data lanes */
static bool dphy_wait_stopstate(struct fpga_csi_dev *cd, u32 lanes, unsigned int ms)
{
 void __iomem *d = cd->regs.dphy;
 u32 want = BIT(16); /* clock lane */
 if (lanes > 0) want |= BIT(0);
 if (lanes > 1) want |= BIT(1);
 if (lanes > 2) want |= BIT(2);
 if (lanes > 3) want |= BIT(3);
 while (ms--) {
 u32 ss = readl(d + 0x04c);
 if ((ss & want) == want) return true;
 udelay(1000);
 }
 dev_warn(cd->dev, "DPHY STOPSTATE not reached: last=0x%08x want=0x%08x",
 readl(d + 0x04c), want);
 return false;
}
/* ---- RAW helpers ---- */
static inline bool raw8_selected(const struct fpga_csi_dev *cd)
{
 /* Prefer explicit filter.dt if enabled; else fall back to overlay raw-bits */
 if (cd->filter.enable_dt_filter)
 return (cd->filter.dt == 0x2A);
 if (cd->ov_dt)
 return (cd->ov_dt == 0x2A);
 if (cd->ov_raw_bits)
 return (cd->ov_raw_bits == 8);
 /* Unknown => default false (RAW10-style) to match current working path */
 return false;
}
/* Channel programming derived from geometry */
static void csi_start_channel(struct fpga_csi_dev *cd, u32 ch)
{
 void __iomem *base = ch_base(cd->regs.csi2, ch);
 /* IRQs: FS + FE_ACK; pack by line; auto-arm next span */
 u32 ctrl = CH_CTRL_IRQ_FS_EN | CH_CTRL_IRQ_FE_ACK_EN |
 CH_CTRL_PACK_LINE | CH_CTRL_AUTO_ARM;
 /* RAW8 packing byte-wise when selected */
 if (raw8_selected(cd))
 ctrl |= CH_CTRL_PACK_BYTES;
 /* Optional VC/DT filtering (honor user's choice; don't force defaults) */
 if (cd->filter.enable_vc_filter)
 ctrl |= ((cd->filter.vc & 0x3) << CH_CTRL_VC_SHIFT);
 if (cd->filter.enable_dt_filter)
 ctrl |= ((cd->filter.dt & 0x3f) << CH_CTRL_DT_SHIFT);
 /* MODE/FRAME_SIZE hint (MODE=normal; frame size only if lines known) */
 ctrl = (ctrl & ~CH_CTRL_MODE_MASK) | (CSI2_MODE_NORMAL << CH_CTRL_MODE_SHIFT);
 if (cd->geom.lines) {
 /* width hint: for RAW8 use exact bytes_per_line; otherwise 0 */
 u32 width_hint = raw8_selected(cd) ? cd->geom.bytes_per_line : 0;
 writel((cd->geom.lines << 16) | (width_hint & 0xffff), base + CH_FRAME_SIZE);
 }
 /* Length/stride are in units of 16 bytes */
 {
 u32 stride16 = cd->geom.bytes_per_line >> 4; /* floor */
 if (cd->geom.bytes_per_line & 0xF)
 stride16++; /* ceil */
 /* Program one "span" worth of lines per FE; choose span_lines below */
 u32 span_lines = cd->geom.lines ? cd->geom.lines : 540; /* ~1/2 1080p by default */
 u32 len16 = stride16 * span_lines; /* total "words" */
 writel(stride16, base + CH_STRIDE);
 writel(len16, base + CH_LENGTH);
 }
 writel(ctrl | CH_CTRL_DMA_EN, base + CH_CTRL);
}
/* Program one CSI2-DMA channel for the next span */
static void csi_dma_arm(struct fpga_csi_dev *cd, u32 ch, u32 buf_idx)
{
 void __iomem *base = ch_base(cd->regs.csi2, ch);
 u64 addr = cd->dbuf[buf_idx].dma;
 /* Address: CH_ADDR1 then CH_ADDR0 (ADDR0 last latches/arms) */
 writel((u32)(addr >> 36), base + CH_ADDR1);
 wmb();
 writel((u32)((addr >> 4) & 0xffffffffu), base + CH_ADDR0);
 wmb();
 /* Ensure DMA_EN remains set (idempotent) */
 {
 u32 ctrl = readl(base + CH_CTRL);
 if (!(ctrl & CH_CTRL_DMA_EN))
 writel(ctrl | CH_CTRL_DMA_EN, base + CH_CTRL);
 }
}
static __maybe_unused void push_dma_to_ring(struct fpga_csi_dev *cd, u32 buf_idx, size_t nbytes)
{
 const u8 *src = cd->dbuf[buf_idx].cpu;
 spin_lock(&cd->ring.lock);
 if (!drop_oldest) {
 while (r_space(&cd->ring) < nbytes) {
 spin_unlock(&cd->ring.lock);
 if (wait_event_interruptible(cd->ring.wq_space, r_space(&cd->ring) >= nbytes))
 return; /* interrupted */
 spin_lock(&cd->ring.lock);
 }
 }
 cd->stats.bytes_out += r_write(&cd->ring, src, nbytes, drop_oldest, &cd->stats.overflows);
 spin_unlock(&cd->ring.lock);
}
/* IRQ handler */
static irqreturn_t fpga_csi_irq_thread(int irq, void *data)
{
 struct fpga_csi_dev *cd = data;
 void __iomem *ch_reg_base = ch_base(cd->regs.csi2, CSI_DMA_CHANNEL);
 u32 ints = rd_mipic(cd, MIPIC_INTS);
 if (!ints) return IRQ_NONE;
 cd->mipic_irq_total++;
 if (ints & MIPIC_INT_CSI_DMA) cd->mipic_irq_dma++;
 if (ints & MIPIC_INT_CSI_HOST) cd->mipic_irq_host++;
 if (ints & ~(MIPIC_INT_CSI_DMA | MIPIC_INT_CSI_HOST)) cd->mipic_irq_other++;
 wr_mipic(cd, MIPIC_INTS, ints); /* W1C summary */
 if (!(ints & MIPIC_INT_CSI_DMA))
 return IRQ_HANDLED;
 /* Read CSI2 status and clear (W1C) */
 {
 u32 status = rd(cd, CSI2_STATUS);
 if (status) cd->ch_irq_total++;
 wr(cd, CSI2_STATUS, status);
 if (!(status & CSI2_STATUS_IRQ_FE_ACK(CSI_DMA_CHANNEL)))
 return IRQ_HANDLED; /* not our FE completion */
 }
 /* Update discard counters (diff) */
 {
 u32 dt = rd(cd, CSI2_DISCARDS_DT);
 u32 vc = rd(cd, CSI2_DISCARDS_VC);
 u32 fifo = rd(cd, CSI2_DISCARDS_FIFO);
 u32 crc = rd(cd, CSI2_DISCARDS_CHKS);
 cd->stats.discards_dt += (u32)(dt - cd->last_dis_dt);
 cd->stats.discards_vc += (u32)(vc - cd->last_dis_vc);
 cd->stats.discards_fifo += (u32)(fifo - cd->last_dis_fifo);
 cd->stats.discards_crc += (u32)(crc - cd->last_dis_crc);
 cd->last_dis_dt = dt;
 cd->last_dis_vc = vc;
 cd->last_dis_fifo = fifo;
 cd->last_dis_crc = crc;
 }
 /* Frame count via FE_FRAME_ID */
 {
 u32 fid = readl(ch_reg_base + CH_FE_FRAME_ID);
 if (fid != cd->last_frame_id) {
 cd->stats.frame_count++;
 cd->last_frame_id = fid;
 }
 cd->ch_irq_fe++;
 }
 /* Transfer current DMA span to userspace ring and re-arm next buffer */
 {
 u32 idx = cd->cur_idx;
 size_t n = cd->dma_span; /* geometry-based span */
 if (n > cd->dbuf[idx].size)
 n = cd->dbuf[idx].size; /* clamp in case geometry changed */
 push_dma_to_ring(cd, idx, n);
 cd->stats.dma_bytes += n;
 idx = (idx + 1) % DMA_BUF_COUNT;
 cd->cur_idx = idx;
 csi_dma_arm(cd, CSI_DMA_CHANNEL, idx);
 }
 return IRQ_HANDLED;
}
static irqreturn_t fpga_csi_irq_top(int irq, void *data)
{ return IRQ_WAKE_THREAD; }
/* ========== Char device ops ========== */
static ssize_t csi_read(struct file *f, char __user *ubuf, size_t len, loff_t *ppos)
{
 struct miscdevice *mdev = f->private_data;
 struct fpga_csi_dev *cd = container_of(mdev, struct fpga_csi_dev, miscdev);
 ssize_t ret = 0;
 if (!len)
 return 0;
 if (mutex_lock_interruptible(&cd->read_lock))
 return -ERESTARTSYS;
 while (len) {
 size_t used;
 spin_lock_irq(&cd->ring.lock);
 used = r_used(&cd->ring);
 if (!used) {
 spin_unlock_irq(&cd->ring.lock);
 if (ret) break; /* return partial */
 if (f->f_flags & O_NONBLOCK) { ret = -EAGAIN; break; }
 if (wait_event_interruptible(cd->ring.wq_read, r_used(&cd->ring) > 0))
 { ret = -ERESTARTSYS; break; }
 continue; /* data available now */
 }
 /* Snapshot */
 {
 const size_t mask = cd->ring.size - 1;
 size_t to = min(len, used);
 size_t rpos = cd->ring.rpos;
 size_t tail = cd->ring.size - rpos;
 size_t n1 = min(to, tail);
 size_t n2 = to - n1;
 u8 *p1 = &cd->ring.data[rpos];
 u8 *p2 = n2 ? &cd->ring.data[0] : NULL;
 spin_unlock_irq(&cd->ring.lock);
 if (copy_to_user(ubuf, p1, n1)) { ret = ret ? ret : -EFAULT; break; }
 if (n2 && copy_to_user(ubuf + n1, p2, n2)) { ret = ret ? ret : -EFAULT; break; }
 spin_lock_irq(&cd->ring.lock);
 if (cd->ring.rpos == rpos) {
 cd->ring.rpos = (rpos + to) & mask;
 } else {
 size_t cur = cd->ring.rpos;
 size_t new_rpos = (rpos + to) & mask;
 size_t fwd = (new_rpos - cur) & mask;
 if (fwd && fwd < cd->ring.size)
 cd->ring.rpos = new_rpos;
 }
 spin_unlock_irq(&cd->ring.lock);
 ubuf += to; len -= to; ret += to;
 wake_up_interruptible(&cd->ring.wq_space);
 }
 }
 mutex_unlock(&cd->read_lock);
 return ret;
}
static __poll_t csi_poll(struct file *f, poll_table *wait)
{
 struct miscdevice *mdev = f->private_data;
 struct fpga_csi_dev *cd = container_of(mdev, struct fpga_csi_dev, miscdev);
 __poll_t mask = 0;
 poll_wait(f, &cd->ring.wq_read, wait);
 if (r_used(&cd->ring) > 0) mask |= POLLIN | POLLRDNORM;
 return mask;
}
/* ---- DMA buffer sizing helpers ---- */
static size_t compute_span_bytes(const struct csi_geometry *g)
{
 /* If lines known, stream whole frame. If not, stream ~1/2 frame. */
 if (g->lines)
 return (size_t)g->bytes_per_line * (size_t)g->lines;
 /* unknown lines: assume ~1/2 of a 1080p frame worth of the provided width */
 return (size_t)g->bytes_per_line * 540u;
}
static int alloc_dma_buffers(struct fpga_csi_dev *cd, size_t span)
{
 int i, ret;
 size_t alloc_sz = PAGE_ALIGN(span);
 /* Prefer widest DMA mask first */
 ret = dma_set_mask_and_coherent(cd->dev, DMA_BIT_MASK(64));
 if (ret) {
 dev_warn(cd->dev, "64-bit DMA mask failed, trying 40-bit");
 ret = dma_set_mask_and_coherent(cd->dev, DMA_BIT_MASK(40));
 if (ret) {
 dev_warn(cd->dev, "40-bit DMA mask failed, trying 32-bit");
 ret = dma_set_mask_and_coherent(cd->dev, DMA_BIT_MASK(32));
 if (ret) {
 dev_err(cd->dev, "Failed to set DMA mask: %d", ret);
 return ret;
 }
 }
 }
 for (i = 0; i < DMA_BUF_COUNT; ++i) {
 cd->dbuf[i].size = alloc_sz;
 cd->dbuf[i].cpu = dma_alloc_coherent(cd->dev, alloc_sz, &cd->dbuf[i].dma, GFP_KERNEL);
 if (!cd->dbuf[i].cpu) {
 dev_err(cd->dev, "dma_alloc_coherent(%zu) failed at idx %d", alloc_sz, i);
 goto err;
 }
 dev_info(cd->dev, "dbuf[%d]: dma=%pad size=%zu", i, &cd->dbuf[i].dma, cd->dbuf[i].size);
 }
 cd->cur_idx = 0;
 wr(cd, CSI2_IRQ_MASK, 0x00000000);
 dev_info(cd->dev, "DMA buffers allocated: %d x %zu (span=%zu)",
 DMA_BUF_COUNT, alloc_sz, span);
 return 0;
err:
 while (--i >= 0)
 dma_free_coherent(cd->dev, cd->dbuf[i].size, cd->dbuf[i].cpu, cd->dbuf[i].dma);
 memset(cd->dbuf, 0, sizeof(cd->dbuf));
 return -ENOMEM;
}
static void free_dma_buffers(struct fpga_csi_dev *cd)
{
 int i;
 for (i = 0; i < DMA_BUF_COUNT; ++i) {
 if (cd->dbuf[i].cpu)
 dma_free_coherent(cd->dev, cd->dbuf[i].size, cd->dbuf[i].cpu, cd->dbuf[i].dma);
 cd->dbuf[i].cpu = NULL;
 }
}
static long csi_ioctl(struct file *f, unsigned int cmd, unsigned long arg)
{
 struct miscdevice *mdev = f->private_data;
 struct fpga_csi_dev *cd = container_of(mdev, struct fpga_csi_dev, miscdev);
 switch (cmd) {
 case CSI_IOC_SET_FILTER: {
 struct csi_filter_cfg cfg;
 if (copy_from_user(&cfg, (void __user *)arg, sizeof(cfg))) return -EFAULT;
 cfg.vc &= 0x3;
 /* If user disables DT filter, keep a sensible default for later enable */
 if (!cfg.enable_dt_filter) {
 if (cd->ov_dt)
 cfg.dt = cd->ov_dt; /* honor overlay default */
 else
 cfg.dt = 0x2B; /* default to RAW10 */
 }
 cd->filter = cfg;
 return 0;
 }
 case CSI_IOC_GET_STATS: {
 struct csi_stats st = cd->stats;
 if (copy_to_user((void __user *)arg, &st, sizeof(st))) return -EFAULT;
 return 0;
 }
 case CSI_IOC_DBG_PHY: {
 struct csi_debug dbg;
 dbg.phy_rx = readl(cd->regs.dphy + 0x48);
 dbg.stopstate = readl(cd->regs.dphy + 0x4C);
 if (copy_to_user((void __user *)arg, &dbg, sizeof(dbg))) return -EFAULT;
 return 0;
 }
 case CSI_IOC_GET_LINK: {
 struct csi_link_info info = {0};
 void __iomem *d = cd->regs.dphy;
 if (d) {
 info.dphy_version = readl(d + 0x000);
 info.dphy_n_lanes = readl(d + 0x004);
 info.dphy_resetn = readl(d + 0x008);
 info.dphy_shutdownz = readl(d + 0x040);
 info.dphy_rstz = readl(d + 0x044);
 info.dphy_phy_rx = readl(d + 0x048);
 info.dphy_stopstate = readl(d + 0x04c);
 }
 info.csi2_status = rd(cd, CSI2_STATUS);
 info.csi2_discards_dt = rd(cd, CSI2_DISCARDS_DT);
 info.csi2_discards_vc = rd(cd, CSI2_DISCARDS_VC);
 info.csi2_discards_fifo = rd(cd, CSI2_DISCARDS_FIFO);
 info.csi2_discards_crc = rd(cd, CSI2_DISCARDS_CHKS);
 info.mipic_cfg = rd_mipic(cd, MIPIC_CFG);
 info.mipic_intr = rd_mipic(cd, MIPIC_INTR);
 info.mipic_inte = rd_mipic(cd, MIPIC_INTE);
 info.mipic_ints = rd_mipic(cd, MIPIC_INTS);
 info.mipic_irq_total = cd->mipic_irq_total;
 info.mipic_irq_dma = cd->mipic_irq_dma;
 info.mipic_irq_host = cd->mipic_irq_host;
 info.mipic_irq_other = cd->mipic_irq_other;
 info.ch_irq_total = cd->ch_irq_total;
 info.ch_irq_fe = cd->ch_irq_fe;
 if (copy_to_user((void __user *)arg, &info, sizeof(info))) return -EFAULT;
 return 0;
 }
 case CSI_IOC_SET_GEOMETRY: {
 struct csi_geometry g;
 /* Route MIPI-CFG to CSI and enable DMA summary */
 wr_mipic(cd, MIPIC_CFG, MIPIC_CFG_SEL_CSI);
 wr_mipic(cd, MIPIC_INTE, MIPIC_INT_CSI_DMA);
 if (copy_from_user(&g, (void __user *)arg, sizeof(g))) return -EFAULT;
 if (g.bytes_per_line < 64) return -EINVAL;
 /* Init PHY and wait for LP-11 */
 {
 int ret = dphy_init(cd);
 if (ret)
 dev_warn(cd->dev, "DPHY init returned %d; continuing", ret);
 {
 u32 lanes = 1;
 of_property_read_u32(cd->dev->of_node, "acme,nlanes", &lanes);
 if (lanes == 0) lanes = 1;
 if (!dphy_wait_stopstate(cd, lanes, 50))
 dev_warn(cd->dev, "DPHY did not reach LP-11 before DMA arm");
 }
 }
 wr(cd, CSI2_IRQ_MASK, 0x00000000);
 /* (Re)allocate buffers when geometry changes */
 if (cd->dbuf[0].cpu == NULL ||
 cd->geom.bytes_per_line != g.bytes_per_line ||
 cd->geom.lines != g.lines) {
 free_dma_buffers(cd);
 cd->geom = g;
 cd->dma_span = compute_span_bytes(&g);
 if (alloc_dma_buffers(cd, cd->dma_span))
 return -ENOMEM;
 } else {
 cd->geom = g;
 cd->dma_span = compute_span_bytes(&g);
 }
 /* Program channel and arm first buffer */
 cd->cur_idx = 0;
 csi_start_channel(cd, CSI_DMA_CHANNEL);
 csi_dma_arm(cd, CSI_DMA_CHANNEL, cd->cur_idx);
 return 0;
 }
 default:
 return -ENOTTY;
 }
}
static const struct file_operations csi_fops = {
 .owner = THIS_MODULE,
 .read = csi_read,
 .poll = csi_poll,
 .unlocked_ioctl = csi_ioctl,
 .llseek = noop_llseek,
};
/* ========== Probe/remove ========== */
static int fpga_csi_probe(struct platform_device *pdev)
{
 struct fpga_csi_dev *cd;
 struct resource *res;
 int ret;
 cd = devm_kzalloc(&pdev->dev, sizeof(*cd), GFP_KERNEL);
 if (!cd) return -ENOMEM;
 cd->dev = &pdev->dev;
 /* Read overlay defaults (optional) */
 of_property_read_u32(pdev->dev.of_node, "acme,raw-bits", &cd->ov_raw_bits); /* 8 or 10 */
 of_property_read_u32(pdev->dev.of_node, "acme,dt", &cd->ov_dt); /* 0x2A/0x2B */
 /* Clocks */
 cd->clk_core = of_clk_get(pdev->dev.of_node, 0);
 if (IS_ERR(cd->clk_core))
 return dev_err_probe(&pdev->dev, PTR_ERR(cd->clk_core),
 "Failed to get clock[0] (core)\n");
 cd->clk_phy = of_clk_get(pdev->dev.of_node, 1);
 if (IS_ERR(cd->clk_phy)) {
 if (PTR_ERR(cd->clk_phy) == -ENOENT || PTR_ERR(cd->clk_phy) == -EINVAL) {
 dev_warn(&pdev->dev, "No clock[1] (dphy) in DT; treating as optional");
 cd->clk_phy = NULL;
 } else {
 ret = dev_err_probe(&pdev->dev, PTR_ERR(cd->clk_phy),
 "Failed to get clock[1] (dphy)\n");
 clk_put(cd->clk_core);
 return ret;
 }
 }
 ret = clk_prepare_enable(cd->clk_core);
 if (ret) {
 clk_put(cd->clk_core);
 if (cd->clk_phy) clk_put(cd->clk_phy);
 return ret;
 }
 ret = devm_add_action_or_reset(&pdev->dev, clk_disable_unprepare_put, cd->clk_core);
 if (ret) return ret;
 if (cd->clk_phy) {
 ret = clk_prepare_enable(cd->clk_phy);
 if (ret) return ret;
 ret = devm_add_action_or_reset(&pdev->dev, clk_disable_unprepare_put, cd->clk_phy);
 if (ret) return ret;
 }
 /* Optional resets */
 cd->rst_core = devm_reset_control_get_optional_exclusive(&pdev->dev, "core");
 if (IS_ERR(cd->rst_core))
 return dev_err_probe(&pdev->dev, PTR_ERR(cd->rst_core), "Failed to get core reset\n");
 if (cd->rst_core) {
 reset_control_deassert(cd->rst_core);
 ret = devm_add_action_or_reset(&pdev->dev, reset_assert_action, cd->rst_core);
 if (ret) return ret;
 } else {
 dev_warn(&pdev->dev, "No core reset in DT; proceeding without it");
 }
 cd->rst_phy = devm_reset_control_get_optional_exclusive(cd->dev, "dphy");
 if (IS_ERR(cd->rst_phy))
 return dev_err_probe(&pdev->dev, PTR_ERR(cd->rst_phy), "Failed to get dphy reset\n");
 if (cd->rst_phy) {
 reset_control_deassert(cd->rst_phy);
 ret = devm_add_action_or_reset(&pdev->dev, reset_assert_action, cd->rst_phy);
 if (ret) return ret;
 } else {
 dev_warn(&pdev->dev, "No dphy reset in DT; proceeding without it");
 }
 /* Map reg windows; require reg[0] and reg[2] */
 res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 if (!res) return -ENODEV;
 cd->regs.csi2 = devm_ioremap_resource(&pdev->dev, res);
 if (IS_ERR(cd->regs.csi2)) return PTR_ERR(cd->regs.csi2);
 res = platform_get_resource(pdev, IORESOURCE_MEM, 1);
 if (res) cd->regs.dphy = devm_ioremap(&pdev->dev, res->start, resource_size(res));
 res = platform_get_resource(pdev, IORESOURCE_MEM, 2);
 if (!res) return -ENODEV;
 cd->regs.mipic = devm_ioremap_resource(&pdev->dev, res);
 if (IS_ERR(cd->regs.mipic)) return PTR_ERR(cd->regs.mipic);
 res = platform_get_resource(pdev, IORESOURCE_MEM, 3);
 if (res) cd->regs.fe = devm_ioremap(&pdev->dev, res->start, resource_size(res));
 dev_info(cd->dev, "reg0 CSI2=%p, reg1 DPHY=%p, reg2 MIPIC=%p, reg3 FE=%p",
 cd->regs.csi2, cd->regs.dphy, cd->regs.mipic, cd->regs.fe);
 cd->irq = platform_get_irq(pdev, 0);
 if (cd->irq < 0) return cd->irq;
 /* Userspace ring */
 cd->ring.size = RING_SIZE;
 cd->ring.data = vzalloc(cd->ring.size);
 if (!cd->ring.data) return -ENOMEM;
 spin_lock_init(&cd->ring.lock);
 init_waitqueue_head(&cd->ring.wq_read);
 init_waitqueue_head(&cd->ring.wq_space);
 mutex_init(&cd->read_lock);
 /* Defaults */
 cd->filter.enable_vc_filter = 0;
 cd->filter.vc = 0;
 cd->filter.enable_dt_filter = 0; /* off by default */
 if (cd->ov_dt) cd->filter.dt = cd->ov_dt; /* preload from overlay if present */
 else if (cd->ov_raw_bits == 8) cd->filter.dt = 0x2A; else cd->filter.dt = 0x2B;
 cd->geom.bytes_per_line = 3072; /* overridden via IOCTL */
 cd->geom.lines = 0;
 cd->dma_span = compute_span_bytes(&cd->geom);
 pm_runtime_enable(&pdev->dev);
 pm_runtime_get_sync(&pdev->dev);
 cd->miscdev.minor = MISC_DYNAMIC_MINOR;
 cd->miscdev.name = DEV_NAME;
 cd->miscdev.fops = &csi_fops;
 ret = misc_register(&cd->miscdev);
 if (ret) {
 dev_err(&pdev->dev, "misc register failed: %d", ret);
 goto err_misc;
 }
 ret = devm_request_threaded_irq(&pdev->dev, cd->irq, fpga_csi_irq_top, fpga_csi_irq_thread,
 IRQF_ONESHOT, DRV_NAME, cd);
 if (ret) {
 dev_err(&pdev->dev, "irq request failed: %d", ret);
 goto err_irq;
 }
 platform_set_drvdata(pdev, cd);
 dev_info(&pdev->dev, "mapped reg[0..3], irq=%d; /dev/%s ready. DMA deferred to SET_GEOMETRY",
 cd->irq, DEV_NAME);
 return 0;
err_irq:
 misc_deregister(&cd->miscdev);
err_misc:
 vfree(cd->ring.data);
 pm_runtime_put_sync(&pdev->dev);
 pm_runtime_disable(&pdev->dev);
 return ret;
}
static void fpga_csi_remove(struct platform_device *pdev)
{
 struct fpga_csi_dev *cd = platform_get_drvdata(pdev);
 misc_deregister(&cd->miscdev);
 vfree(cd->ring.data);
 free_dma_buffers(cd);
 pm_runtime_put_sync(&pdev->dev);
 pm_runtime_disable(&pdev->dev);
}
static const struct of_device_id fpga_csi_of_match[] = {
 { .compatible = "acme,fpga-csi-rx" },
 { /* sentinel */ }
};
MODULE_DEVICE_TABLE(of, fpga_csi_of_match);
static struct platform_driver fpga_csi_driver = {
 .probe = fpga_csi_probe,
#if defined(CONFIG_PM)
 /* no .pm hooks yet */
#endif
 .remove_new = fpga_csi_remove,
 .driver = {
 .name = DRV_NAME,
 .of_match_table = fpga_csi_of_match,
 },
};
module_platform_driver(fpga_csi_driver);
MODULE_AUTHOR("Martin McCormick");
MODULE_DESCRIPTION("RP1 CSI-2 RAW8/RAW10 DMA char device (/dev/csi_stream0) with geometry-based spans and overlay RAW support");
MODULE_LICENSE("GPL");

Code: Select all

// User-space control utility
// csi_setgeom.c — set CSI geometry, optional DT/VC filter, and print stats/link snapshot
// Build: gcc -O2 -Wall -Wextra -o csi_setgeom csi_setgeom.c
// Usage examples:
// ./csi_setgeom --set-filter --dt 0x2B --vc 0 --raw10-width 2592 --lines 1944
// ./csi_setgeom --bytes-per-line 3240 --lines 1944
// ./csi_setgeom --stats --link
// ./csi_setgeom --dbg-phy # single read of PHY_RX/STOPSTATE via driver ioctl
// ./csi_setgeom --dbg-phy-loop 100 --interval-ms 5 # poll 100x at 5ms
#include <errno.h>
#include <fcntl.h>
#include <getopt.h>
#include <inttypes.h>
#include <stdbool.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/ioctl.h>
#include <sys/time.h>
#include <unistd.h>
#include "fpga_csi.h" // IOCTLs and structs shared with driver
static void usage(const char *prog) {
 fprintf(stderr,
 "Usage:"
 " %s [--bytes-per-line N] [--lines N]"
 " [--set-filter --dt HEX --vc N]"
 " [--raw10-width W] [--stats] [--link]"
 " [--dbg-phy | --dbg-phy-loop N [--interval-ms M]]"
 "Examples:"
 " %s --set-filter --dt 0x2B --vc 0 --raw10-width 2592 --lines 1944"
 " %s --bytes-per-line 3240 --lines 1944"
 " %s --stats --link"
 " %s --dbg-phy-loop 200 --interval-ms 1",
 prog, prog, prog, prog, prog);
}
static unsigned raw10_bpl(unsigned width)
{
 unsigned calc = (5u * width) / 4u; // floor
 if ((5u * width) % 4u) calc++; // ceil
 return calc;
}
static void print_enabled(const char *name, bool on)
{ printf(" %-12s : %s", name, on ? "ON" : "off"); }
static void print_bool(const char *name, bool on)
{ printf(" %-14s : %s", name, on ? "1" : "0"); }
static void print_hex(const char *name, uint32_t v)
{ printf(" %-14s : 0x%08X", name, v); }
// Common DW D-PHY STOPSTATE bit layout used by many SoCs:
// bit[0..3] = data lane[0..3] stop state
// bit[16] = clock lane stop state
static void decode_dphy(uint32_t ver, uint32_t n_lanes, uint32_t resetn,
 uint32_t shutdownz, uint32_t rstz,
 uint32_t phy_rx, uint32_t stopstate)
{
 unsigned lanes = (n_lanes & 0x3) + 1; // per RP1 driver usage
 puts(" D-PHY:");
 print_hex("version", ver);
 printf(" active_lanes : %u", lanes);
 print_hex("phy_rx", phy_rx);
 print_hex("stopstate", stopstate);
 bool ss_d0 = stopstate & (1u<<0);
 bool ss_d1 = stopstate & (1u<<1);
 bool ss_d2 = stopstate & (1u<<2);
 bool ss_d3 = stopstate & (1u<<3);
 bool ss_clk = stopstate & (1u<<16);
 printf(" lanes stop : D0=%c D1=%c D2=%c D3=%c CLK=%c",
 ss_d0?'S':'-', ss_d1?'S':'-', ss_d2?'S':'-', ss_d3?'S':'-', ss_clk?'S':'-');
 print_bool("shutdownz", !!shutdownz);
 print_bool("rstz", !!rstz);
 print_bool("resetn", !!resetn);
 // Heuristic verdict hints
 printf(" verdict : ");
 if (!shutdownz || !rstz) {
 printf("PHY is held in reset/shutdown");
 } else if (ss_clk && ss_d0 && (lanes == 1 || ss_d1) && (lanes <= 2 || ss_d2) && (lanes <= 3 || ss_d3)) {
 printf("All enabled lanes in LP-11 (Stop). No HS activity seen.");
 } else if (!ss_clk) {
 printf("Clock lane not in Stop (likely HS activity present).");
 } else {
 printf("Mixed state (some lanes not in Stop).");
 }
}
static void decode_csi(uint32_t status,
 uint32_t dis_dt, uint32_t dis_vc,
 uint32_t dis_fifo, uint32_t dis_crc)
{
 puts(" CSI-2 receiver:");
 print_hex("status", status);
 printf(" discards : dt=%u vc=%u fifo=%u crc=%u",
 dis_dt, dis_vc, dis_fifo, dis_crc);
}
static void decode_mipic(uint32_t cfg, uint32_t intr, uint32_t inte, uint32_t ints,
 uint64_t total, uint64_t dma, uint64_t host, uint64_t other,
 uint64_t ch_total, uint64_t ch_fe)
{
 puts(" MIPIC (MIPI CFG):");
 print_bool("sel_csi", (cfg & 0x1));
 printf(" intr(latched) : 0x%08X", intr);
 printf(" inte(enabled) : 0x%08X [DMA=%s HOST=%s]",
 inte,
 (inte & (1u<<0))?"Y":"n",
 (inte & (1u<<2))?"Y":"n");
 printf(" ints(masked) : 0x%08X", ints);
 printf(" irq counts : total=%" PRIu64 " dma=%" PRIu64 " host=%" PRIu64 " other=%" PRIu64 "",
 total, dma, host, other);
 printf(" ch irq counts : total=%" PRIu64 " fe=%" PRIu64 "", ch_total, ch_fe);
}
static void print_ts_prefix(void)
{
 struct timeval tv; gettimeofday(&tv, NULL);
 printf("[%ld.%03ld] ", (long)tv.tv_sec, (long)(tv.tv_usec/1000));
}
int main(int argc, char **argv) {
 const char *dev = "/dev/csi_stream0";
 // Options
 bool do_filter = false, do_geom = false, do_stats = false, do_link = false;
 bool do_dbg_phy = false;
 int dbg_loop = 0; // 0 = single shot; >0 = loop count
 int interval_ms = 5; // default interval for loop
 unsigned vc = 0;
 unsigned dt = 0;
 bool have_dt = false, have_vc = false;
 unsigned lines = 0;
 unsigned bpl = 0;
 bool have_lines = false, have_bpl = false;
 unsigned raw10_w = 0;
 static struct option opts[] = {
 {"set-filter", no_argument, 0, 1 },
 {"dt", required_argument, 0, 2 },
 {"vc", required_argument, 0, 3 },
 {"bytes-per-line", required_argument, 0, 4 },
 {"lines", required_argument, 0, 5 },
 {"raw10-width", required_argument, 0, 6 },
 {"stats", no_argument, 0, 7 },
 {"link", no_argument, 0, 8 },
 {"dbg-phy", no_argument, 0, 9 },
 {"dbg-phy-loop", required_argument, 0, 10 },
 {"interval-ms", required_argument, 0, 11 },
 {"help", no_argument, 0, 'h'},
 {0,0,0,0}
 };
 int c, idx;
 while ((c = getopt_long(argc, argv, "h", opts, &idx)) != -1) {
 switch (c) {
 case 1: do_filter = true; break;
 case 2: dt = (unsigned)strtoul(optarg, NULL, 0); have_dt = true; break;
 case 3: vc = (unsigned)strtoul(optarg, NULL, 0); have_vc = true; break;
 case 4: bpl = (unsigned)strtoul(optarg, NULL, 0); have_bpl = true; break;
 case 5: lines = (unsigned)strtoul(optarg, NULL, 0); have_lines = true; break;
 case 6: raw10_w = (unsigned)strtoul(optarg, NULL, 0); break;
 case 7: do_stats = true; break;
 case 8: do_link = true; break;
 case 9: do_dbg_phy = true; break;
 case 10: dbg_loop = (int)strtol(optarg, NULL, 0); do_dbg_phy = true; break;
 case 11: interval_ms = (int)strtol(optarg, NULL, 0); break;
 case 'h': usage(argv[0]); return 0;
 default: usage(argv[0]); return 1;
 }
 }
 // Convenience: compute RAW10 bytes_per_line from width if requested
 if (raw10_w) {
 bpl = raw10_bpl(raw10_w);
 have_bpl = true;
 }
 // We’ll set geometry if either bpl or lines was provided
 do_geom = (have_bpl || have_lines);
 // Minimal guardrails
 if (!do_filter && !do_geom && !do_stats && !do_link && !do_dbg_phy) {
 usage(argv[0]);
 return 1;
 }
 if (have_vc && vc > 3) {
 fprintf(stderr, "VC must be 0..3");
 return 1;
 }
 if (have_dt && dt > 0x3F) {
 fprintf(stderr, "DT must be 0x00..0x3F");
 return 1;
 }
 int fd = open(dev, O_RDONLY | O_NONBLOCK);
 if (fd < 0) { perror("open /dev/csi_stream0"); return 1; }
 // Apply filter (defaults handled in driver)
 if (do_filter) {
 struct csi_filter_cfg cfg = (struct csi_filter_cfg){0};
 cfg.enable_vc_filter = have_vc ? 1 : 0;
 cfg.vc = vc & 0x3;
 cfg.enable_dt_filter = have_dt ? 1 : 0;
 cfg.dt = have_dt ? (dt & 0x3F) : 0x2A; // default RAW8 if enabled but not passed
 if (ioctl(fd, CSI_IOC_SET_FILTER, &cfg) < 0) {
 perror("ioctl SET_FILTER");
 close(fd);
 return 1;
 }
 printf("SET_FILTER ok (enable_dt=%u dt=0x%02X, enable_vc=%u vc=%u)",
 cfg.enable_dt_filter, cfg.dt, cfg.enable_vc_filter, cfg.vc);
 }
 // Apply geometry (this will (re)allocate and arm DMA in the driver)
 if (do_geom) {
 struct csi_geometry g = (struct csi_geometry){0};
 if (!have_bpl) {
 fprintf(stderr, "bytes_per_line not set; use --bytes-per-line N or --raw10-width W");
 close(fd);
 return 1;
 }
 if (!have_lines) {
 fprintf(stderr, "lines not set; use --lines N");
 close(fd);
 return 1;
 }
 g.bytes_per_line = bpl;
 g.lines = lines;
 if (ioctl(fd, CSI_IOC_SET_GEOMETRY, &g) < 0) {
 perror("ioctl SET_GEOMETRY");
 close(fd);
 return 1;
 }
 printf("SET_GEOMETRY ok (bytes_per_line=%u, lines=%u)", g.bytes_per_line, g.lines);
 }
 if (do_stats) {
 struct csi_stats st = (struct csi_stats){0};
 if (ioctl(fd, CSI_IOC_GET_STATS, &st) == 0) {
 printf("STATS: bytes_out=%" PRIu64 ", dma_bytes=%" PRIu64 ", frames=%" PRIu32
 ", overflows=%" PRIu64 ""
 " discards: dt=%" PRIu32 " vc=%" PRIu32 " fifo=%" PRIu32 " crc=%" PRIu32 "",
 st.bytes_out, st.dma_bytes, st.frame_count, st.overflows,
 st.discards_dt, st.discards_vc, st.discards_fifo, st.discards_crc);
 } else {
 perror("ioctl GET_STATS");
 }
 }
 if (do_link) {
 struct csi_link_info li = (struct csi_link_info){0};
 if (ioctl(fd, CSI_IOC_GET_LINK, &li) == 0) {
 puts("LINK snapshot (verbose):");
 decode_dphy(li.dphy_version, li.dphy_n_lanes, li.dphy_resetn,
 li.dphy_shutdownz, li.dphy_rstz,
 li.dphy_phy_rx, li.dphy_stopstate);
 decode_csi(li.csi2_status, li.csi2_discards_dt, li.csi2_discards_vc,
 li.csi2_discards_fifo, li.csi2_discards_crc);
 decode_mipic(li.mipic_cfg, li.mipic_intr, li.mipic_inte, li.mipic_ints,
 li.mipic_irq_total, li.mipic_irq_dma, li.mipic_irq_host, li.mipic_irq_other,
 li.ch_irq_total, li.ch_irq_fe);
 } else {
 perror("ioctl GET_LINK");
 }
 }
 if (do_dbg_phy) {
 struct csi_debug dbg = {0};
 if (dbg_loop <= 0) dbg_loop = 1; // single shot by default
 for (int i = 0; i < dbg_loop; ++i) {
 if (ioctl(fd, CSI_IOC_DBG_PHY, &dbg) == 0) {
 print_ts_prefix();
 printf("DBG_PHY: phy_rx=0x%08X stopstate=0x%08X ", dbg.phy_rx, dbg.stopstate);
 bool ss_d0 = dbg.stopstate & (1u<<0);
 bool ss_d1 = dbg.stopstate & (1u<<1);
 bool ss_d2 = dbg.stopstate & (1u<<2);
 bool ss_d3 = dbg.stopstate & (1u<<3);
 bool ss_clk = dbg.stopstate & (1u<<16);
 printf("[D0=%c D1=%c D2=%c D3=%c CLK=%c]\n",
 ss_d0?'S':'-', ss_d1?'S':'-', ss_d2?'S':'-', ss_d3?'S':'-', ss_clk?'S':'-');
 } else {
 perror("ioctl DBG_PHY");
 break;
 }
 if (i + 1 < dbg_loop) usleep(interval_ms * 1000);
 }
 }
 close(fd);
 return 0;
}

Code: Select all

obj-m += fpga-csi.o
all:
	$(MAKE) -C /lib/modules/$(shell uname -r)/build M=$(PWD) modules
clean:
	$(MAKE) -C /lib/modules/$(shell uname -r)/build M=$(PWD) clean

Code: Select all

# build and copy overlay
sudo dtc -@ -I dts -O dtb -o /boot/firmware/overlays/fpga-csi.dtbo fpga-csi.dts
# in /boot/firmware/config.txt add to [all] section,
camera_auto_detect=0
dtoverlay=fpga-csi
# (reboot)
# load driver
sudo insmod ./fpga-csi.ko
# user space utility to tell driver dimensons
sudo ./csi_setgeom --bytes-per-line 1920 --lines 1080
# print out link status (also --stats)
sudo ./csi_setgeom --link
# test transfer
sudo dd if=/dev/csi_stream0 of=cap.bin count=1000