Quelle  rzg2l-video.c   Sprache: C

// SPDX-License-Identifier: GPL-2.0+
/*
 * Driver for Renesas RZ/G2L CRU
 *
 * Copyright (C) 2022 Renesas Electronics Corp.
 *
 * Based on Renesas R-Car VIN
 * Copyright (C) 2016 Renesas Electronics Corp.
 * Copyright (C) 2011-2013 Renesas Solutions Corp.
 * Copyright (C) 2013 Cogent Embedded, Inc., <source@cogentembedded.com>
 * Copyright (C) 2008 Magnus Damm
 */

#include <linux/clk.h>
#include <linux/delay.h>
#include <linux/pm_runtime.h>

#include <media/mipi-csi2.h>
#include <media/v4l2-ioctl.h>
#include <media/videobuf2-dma-contig.h>

#include "rzg2l-cru.h"
#include "rzg2l-cru-regs.h"

#define RZG2L_TIMEOUT_MS  100
#define RZG2L_RETRIES   10

#define RZG2L_CRU_DEFAULT_FORMAT V4L2_PIX_FMT_UYVY
#define RZG2L_CRU_DEFAULT_WIDTH  RZG2L_CRU_MIN_INPUT_WIDTH
#define RZG2L_CRU_DEFAULT_HEIGHT RZG2L_CRU_MIN_INPUT_HEIGHT
#define RZG2L_CRU_DEFAULT_FIELD  V4L2_FIELD_NONE
#define RZG2L_CRU_DEFAULT_COLORSPACE V4L2_COLORSPACE_SRGB

#define RZG2L_CRU_STRIDE_MAX  32640
#define RZG2L_CRU_STRIDE_ALIGN  128

struct rzg2l_cru_buffer {
 struct vb2_v4l2_buffer vb;
 struct list_head list;
};

#define to_buf_list(vb2_buffer) \
 (&container_of(vb2_buffer, struct rzg2l_cru_buffer, vb)->list)

/* -----------------------------------------------------------------------------
 * DMA operations
 */
static void __rzg2l_cru_write(struct rzg2l_cru_dev *cru, u32 offset, u32 value)
{
 const u16 *regs = cru->info->regs;

 /*
  * CRUnCTRL is a first register on all CRU supported SoCs so validate
  * rest of the registers have valid offset being set in cru->info->regs.
 */
 if (WARN_ON(offset >= RZG2L_CRU_MAX_REG) ||
     WARN_ON(offset != CRUnCTRL && regs[offset] == 0))
  return;

 iowrite32(value, cru->base + regs[offset]);
}

static u32 __rzg2l_cru_read(struct rzg2l_cru_dev *cru, u32 offset)
{
 const u16 *regs = cru->info->regs;

 /*
  * CRUnCTRL is a first register on all CRU supported SoCs so validate
  * rest of the registers have valid offset being set in cru->info->regs.
 */
 if (WARN_ON(offset >= RZG2L_CRU_MAX_REG) ||
     WARN_ON(offset != CRUnCTRL && regs[offset] == 0))
  return 0;

 return ioread32(cru->base + regs[offset]);
}

static __always_inline void
__rzg2l_cru_write_constant(struct rzg2l_cru_dev *cru, u32 offset, u32 value)
{
 const u16 *regs = cru->info->regs;

 BUILD_BUG_ON(offset >= RZG2L_CRU_MAX_REG);

 iowrite32(value, cru->base + regs[offset]);
}

static __always_inline u32
__rzg2l_cru_read_constant(struct rzg2l_cru_dev *cru, u32 offset)
{
 const u16 *regs = cru->info->regs;

 BUILD_BUG_ON(offset >= RZG2L_CRU_MAX_REG);

 return ioread32(cru->base + regs[offset]);
}

#define rzg2l_cru_write(cru, offset, value) \
 (__builtin_constant_p(offset) ? \
  __rzg2l_cru_write_constant(cru, offset, value) : \
  __rzg2l_cru_write(cru, offset, value))

#define rzg2l_cru_read(cru, offset) \
 (__builtin_constant_p(offset) ? \
  __rzg2l_cru_read_constant(cru, offset) : \
  __rzg2l_cru_read(cru, offset))

/* Need to hold qlock before calling */
static void return_unused_buffers(struct rzg2l_cru_dev *cru,
      enum vb2_buffer_state state)
{
 struct rzg2l_cru_buffer *buf, *node;
 unsigned long flags;
 unsigned int i;

 spin_lock_irqsave(&cru->qlock, flags);
 for (i = 0; i < cru->num_buf; i++) {
  if (cru->queue_buf[i]) {
   vb2_buffer_done(&cru->queue_buf[i]->vb2_buf,
     state);
   cru->queue_buf[i] = NULL;
  }
 }

 list_for_each_entry_safe(buf, node, &cru->buf_list, list) {
  vb2_buffer_done(&buf->vb.vb2_buf, state);
  list_del(&buf->list);
 }
 spin_unlock_irqrestore(&cru->qlock, flags);
}

static int rzg2l_cru_queue_setup(struct vb2_queue *vq, unsigned int *nbuffers,
     unsigned int *nplanes, unsigned int sizes[],
     struct device *alloc_devs[])
{
 struct rzg2l_cru_dev *cru = vb2_get_drv_priv(vq);

 /* Make sure the image size is large enough. */
 if (*nplanes)
  return sizes[0] < cru->format.sizeimage ? -EINVAL : 0;

 *nplanes = 1;
 sizes[0] = cru->format.sizeimage;

 return 0;
};

static int rzg2l_cru_buffer_prepare(struct vb2_buffer *vb)
{
 struct rzg2l_cru_dev *cru = vb2_get_drv_priv(vb->vb2_queue);
 unsigned long size = cru->format.sizeimage;

 if (vb2_plane_size(vb, 0) < size) {
  dev_err(cru->dev, "buffer too small (%lu < %lu)\n",
   vb2_plane_size(vb, 0), size);
  return -EINVAL;
 }

 vb2_set_plane_payload(vb, 0, size);

 return 0;
}

static void rzg2l_cru_buffer_queue(struct vb2_buffer *vb)
{
 struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
 struct rzg2l_cru_dev *cru = vb2_get_drv_priv(vb->vb2_queue);
 unsigned long flags;

 spin_lock_irqsave(&cru->qlock, flags);

 list_add_tail(to_buf_list(vbuf), &cru->buf_list);

 spin_unlock_irqrestore(&cru->qlock, flags);
}

static void rzg2l_cru_set_slot_addr(struct rzg2l_cru_dev *cru,
        int slot, dma_addr_t addr)
{
 /*
  * The address needs to be 512 bytes aligned. Driver should never accept
  * settings that do not satisfy this in the first place...
 */
 if (WARN_ON((addr) & RZG2L_CRU_HW_BUFFER_MASK))
  return;

 /* Currently, we just use the buffer in 32 bits address */
 rzg2l_cru_write(cru, AMnMBxADDRL(slot), addr);
 rzg2l_cru_write(cru, AMnMBxADDRH(slot), 0);

 cru->buf_addr[slot] = addr;
}

/*
 * Moves a buffer from the queue to the HW slot. If no buffer is
 * available use the scratch buffer. The scratch buffer is never
 * returned to userspace, its only function is to enable the capture
 * loop to keep running.
 */
static void rzg2l_cru_fill_hw_slot(struct rzg2l_cru_dev *cru, int slot)
{
 struct vb2_v4l2_buffer *vbuf;
 struct rzg2l_cru_buffer *buf;
 dma_addr_t phys_addr;

 /* A already populated slot shall never be overwritten. */
 if (WARN_ON(cru->queue_buf[slot]))
  return;

 dev_dbg(cru->dev, "Filling HW slot: %d\n", slot);

 if (list_empty(&cru->buf_list)) {
  cru->queue_buf[slot] = NULL;
  phys_addr = cru->scratch_phys;
 } else {
  /* Keep track of buffer we give to HW */
  buf = list_entry(cru->buf_list.next,
     struct rzg2l_cru_buffer, list);
  vbuf = &buf->vb;
  list_del_init(to_buf_list(vbuf));
  cru->queue_buf[slot] = vbuf;

  /* Setup DMA */
  phys_addr = vb2_dma_contig_plane_dma_addr(&vbuf->vb2_buf, 0);
 }

 rzg2l_cru_set_slot_addr(cru, slot, phys_addr);
}

static void rzg2l_cru_initialize_axi(struct rzg2l_cru_dev *cru)
{
 const struct rzg2l_cru_info *info = cru->info;
 unsigned int slot;
 u32 amnaxiattr;

 /*
  * Set image data memory banks.
  * Currently, we will use maximum address.
 */
 rzg2l_cru_write(cru, AMnMBVALID, AMnMBVALID_MBVALID(cru->num_buf - 1));

 for (slot = 0; slot < cru->num_buf; slot++)
  rzg2l_cru_fill_hw_slot(cru, slot);

 if (info->has_stride) {
  u32 stride = cru->format.bytesperline;
  u32 amnis;

  stride /= RZG2L_CRU_STRIDE_ALIGN;
  amnis = rzg2l_cru_read(cru, AMnIS) & ~AMnIS_IS_MASK;
  rzg2l_cru_write(cru, AMnIS, amnis | AMnIS_IS(stride));
 }

 /* Set AXI burst max length to recommended setting */
 amnaxiattr = rzg2l_cru_read(cru, AMnAXIATTR) & ~AMnAXIATTR_AXILEN_MASK;
 amnaxiattr |= AMnAXIATTR_AXILEN;
 rzg2l_cru_write(cru, AMnAXIATTR, amnaxiattr);
}

void rzg3e_cru_csi2_setup(struct rzg2l_cru_dev *cru,
     const struct rzg2l_cru_ip_format *ip_fmt,
     u8 csi_vc)
{
 const struct rzg2l_cru_info *info = cru->info;
 u32 icnmc = ICnMC_INF(ip_fmt->datatype);

 icnmc |= rzg2l_cru_read(cru, info->image_conv) & ~ICnMC_INF_MASK;

 /* Set virtual channel CSI2 */
 icnmc |= ICnMC_VCSEL(csi_vc);

 rzg2l_cru_write(cru, ICnSVCNUM, csi_vc);
 rzg2l_cru_write(cru, ICnSVC, ICnSVC_SVC0(0) | ICnSVC_SVC1(1) |
   ICnSVC_SVC2(2) | ICnSVC_SVC3(3));
 rzg2l_cru_write(cru, info->image_conv, icnmc);
}

void rzg2l_cru_csi2_setup(struct rzg2l_cru_dev *cru,
     const struct rzg2l_cru_ip_format *ip_fmt,
     u8 csi_vc)
{
 const struct rzg2l_cru_info *info = cru->info;
 u32 icnmc = ICnMC_INF(ip_fmt->datatype);

 icnmc |= rzg2l_cru_read(cru, info->image_conv) & ~ICnMC_INF_MASK;

 /* Set virtual channel CSI2 */
 icnmc |= ICnMC_VCSEL(csi_vc);

 rzg2l_cru_write(cru, info->image_conv, icnmc);
}

static int rzg2l_cru_initialize_image_conv(struct rzg2l_cru_dev *cru,
        struct v4l2_mbus_framefmt *ip_sd_fmt,
        u8 csi_vc)
{
 const struct rzg2l_cru_info *info = cru->info;
 const struct rzg2l_cru_ip_format *cru_video_fmt;
 const struct rzg2l_cru_ip_format *cru_ip_fmt;

 cru_ip_fmt = rzg2l_cru_ip_code_to_fmt(ip_sd_fmt->code);
 info->csi_setup(cru, cru_ip_fmt, csi_vc);

 /* Output format */
 cru_video_fmt = rzg2l_cru_ip_format_to_fmt(cru->format.pixelformat);
 if (!cru_video_fmt) {
  dev_err(cru->dev, "Invalid pixelformat (0x%x)\n",
   cru->format.pixelformat);
  return -EINVAL;
 }

 /* If input and output use same colorspace, do bypass mode */
 if (cru_ip_fmt->yuv == cru_video_fmt->yuv)
  rzg2l_cru_write(cru, info->image_conv,
    rzg2l_cru_read(cru, info->image_conv) | ICnMC_CSCTHR);
 else
  rzg2l_cru_write(cru, info->image_conv,
    rzg2l_cru_read(cru, info->image_conv) & ~ICnMC_CSCTHR);

 /* Set output data format */
 rzg2l_cru_write(cru, ICnDMR, cru_video_fmt->icndmr);

 return 0;
}

bool rzg3e_fifo_empty(struct rzg2l_cru_dev *cru)
{
 u32 amnfifopntr = rzg2l_cru_read(cru, AMnFIFOPNTR);

 if ((((amnfifopntr & AMnFIFOPNTR_FIFORPNTR_B1) >> 24) ==
      ((amnfifopntr & AMnFIFOPNTR_FIFOWPNTR_B1) >> 8)) &&
     (((amnfifopntr & AMnFIFOPNTR_FIFORPNTR_B0) >> 16) ==
      (amnfifopntr & AMnFIFOPNTR_FIFOWPNTR_B0)))
  return true;

 return false;
}

bool rzg2l_fifo_empty(struct rzg2l_cru_dev *cru)
{
 u32 amnfifopntr, amnfifopntr_w, amnfifopntr_r_y;

 amnfifopntr = rzg2l_cru_read(cru, AMnFIFOPNTR);

 amnfifopntr_w = amnfifopntr & AMnFIFOPNTR_FIFOWPNTR;
 amnfifopntr_r_y =
  (amnfifopntr & AMnFIFOPNTR_FIFORPNTR_Y) >> 16;

 return amnfifopntr_w == amnfifopntr_r_y;
}

void rzg2l_cru_stop_image_processing(struct rzg2l_cru_dev *cru)
{
 unsigned int retries = 0;
 unsigned long flags;
 u32 icnms;

 spin_lock_irqsave(&cru->qlock, flags);

 /* Disable and clear the interrupt */
 cru->info->disable_interrupts(cru);

 /* Stop the operation of image conversion */
 rzg2l_cru_write(cru, ICnEN, 0);

 /* Wait for streaming to stop */
 while ((rzg2l_cru_read(cru, ICnMS) & ICnMS_IA) && retries++ < RZG2L_RETRIES) {
  spin_unlock_irqrestore(&cru->qlock, flags);
  msleep(RZG2L_TIMEOUT_MS);
  spin_lock_irqsave(&cru->qlock, flags);
 }

 icnms = rzg2l_cru_read(cru, ICnMS) & ICnMS_IA;
 if (icnms)
  dev_err(cru->dev, "Failed stop HW, something is seriously broken\n");

 cru->state = RZG2L_CRU_DMA_STOPPED;

 /* Wait until the FIFO becomes empty */
 for (retries = 5; retries > 0; retries--) {
  if (cru->info->fifo_empty(cru))
   break;

  usleep_range(10, 20);
 }

 /* Notify that FIFO is not empty here */
 if (!retries)
  dev_err(cru->dev, "Failed to empty FIFO\n");

 /* Stop AXI bus */
 rzg2l_cru_write(cru, AMnAXISTP, AMnAXISTP_AXI_STOP);

 /* Wait until the AXI bus stop */
 for (retries = 5; retries > 0; retries--) {
  if (rzg2l_cru_read(cru, AMnAXISTPACK) &
   AMnAXISTPACK_AXI_STOP_ACK)
   break;

  usleep_range(10, 20);
 }

 /* Notify that AXI bus can not stop here */
 if (!retries)
  dev_err(cru->dev, "Failed to stop AXI bus\n");

 /* Cancel the AXI bus stop request */
 rzg2l_cru_write(cru, AMnAXISTP, 0);

 /* Reset the CRU (AXI-master) */
 reset_control_assert(cru->aresetn);

 /* Resets the image processing module */
 rzg2l_cru_write(cru, CRUnRST, 0);

 spin_unlock_irqrestore(&cru->qlock, flags);
}

static int rzg2l_cru_get_virtual_channel(struct rzg2l_cru_dev *cru)
{
 struct v4l2_mbus_frame_desc fd = { };
 struct media_pad *remote_pad;
 int ret;

 remote_pad = media_pad_remote_pad_unique(&cru->ip.pads[RZG2L_CRU_IP_SINK]);
 ret = v4l2_subdev_call(cru->ip.remote, pad, get_frame_desc, remote_pad->index, &fd);
 if (ret < 0 && ret != -ENOIOCTLCMD) {
  dev_err(cru->dev, "get_frame_desc failed on IP remote subdev\n");
  return ret;
 }
 /* If remote subdev does not implement .get_frame_desc default to VC0. */
 if (ret == -ENOIOCTLCMD)
  return 0;

 if (fd.type != V4L2_MBUS_FRAME_DESC_TYPE_CSI2) {
  dev_err(cru->dev, "get_frame_desc returned invalid bus type %d\n", fd.type);
  return -EINVAL;
 }

 if (!fd.num_entries) {
  dev_err(cru->dev, "get_frame_desc returned zero entries\n");
  return -EINVAL;
 }

 return fd.entry[0].bus.csi2.vc;
}

void rzg3e_cru_enable_interrupts(struct rzg2l_cru_dev *cru)
{
 rzg2l_cru_write(cru, CRUnIE2, CRUnIE2_FSxE(cru->svc_channel));
 rzg2l_cru_write(cru, CRUnIE2, CRUnIE2_FExE(cru->svc_channel));
}

void rzg3e_cru_disable_interrupts(struct rzg2l_cru_dev *cru)
{
 rzg2l_cru_write(cru, CRUnIE, 0);
 rzg2l_cru_write(cru, CRUnIE2, 0);
 rzg2l_cru_write(cru, CRUnINTS, rzg2l_cru_read(cru, CRUnINTS));
 rzg2l_cru_write(cru, CRUnINTS2, rzg2l_cru_read(cru, CRUnINTS2));
}

void rzg2l_cru_enable_interrupts(struct rzg2l_cru_dev *cru)
{
 rzg2l_cru_write(cru, CRUnIE, CRUnIE_EFE);
}

void rzg2l_cru_disable_interrupts(struct rzg2l_cru_dev *cru)
{
 rzg2l_cru_write(cru, CRUnIE, 0);
 rzg2l_cru_write(cru, CRUnINTS, 0x001f000f);
}

int rzg2l_cru_start_image_processing(struct rzg2l_cru_dev *cru)
{
 struct v4l2_mbus_framefmt *fmt = rzg2l_cru_ip_get_src_fmt(cru);
 unsigned long flags;
 u8 csi_vc;
 int ret;

 ret = rzg2l_cru_get_virtual_channel(cru);
 if (ret < 0)
  return ret;
 csi_vc = ret;
 cru->svc_channel = csi_vc;

 spin_lock_irqsave(&cru->qlock, flags);

 /* Select a video input */
 rzg2l_cru_write(cru, CRUnCTRL, CRUnCTRL_VINSEL(0));

 /* Cancel the software reset for image processing block */
 rzg2l_cru_write(cru, CRUnRST, CRUnRST_VRESETN);

 /* Disable and clear the interrupt before using */
 cru->info->disable_interrupts(cru);

 /* Initialize the AXI master */
 rzg2l_cru_initialize_axi(cru);

 /* Initialize image convert */
 ret = rzg2l_cru_initialize_image_conv(cru, fmt, csi_vc);
 if (ret) {
  spin_unlock_irqrestore(&cru->qlock, flags);
  return ret;
 }

 /* Enable interrupt */
 cru->info->enable_interrupts(cru);

 /* Enable image processing reception */
 rzg2l_cru_write(cru, ICnEN, ICnEN_ICEN);

 spin_unlock_irqrestore(&cru->qlock, flags);

 return 0;
}

static int rzg2l_cru_set_stream(struct rzg2l_cru_dev *cru, int on)
{
 struct media_pipeline *pipe;
 struct v4l2_subdev *sd;
 struct media_pad *pad;
 int ret;

 pad = media_pad_remote_pad_first(&cru->pad);
 if (!pad)
  return -EPIPE;

 sd = media_entity_to_v4l2_subdev(pad->entity);

 if (!on) {
  int stream_off_ret = 0;

  ret = v4l2_subdev_call(sd, video, s_stream, 0);
  if (ret)
   stream_off_ret = ret;

  ret = v4l2_subdev_call(sd, video, post_streamoff);
  if (ret == -ENOIOCTLCMD)
   ret = 0;
  if (ret && !stream_off_ret)
   stream_off_ret = ret;

  video_device_pipeline_stop(&cru->vdev);

  return stream_off_ret;
 }

 pipe = media_entity_pipeline(&sd->entity) ? : &cru->vdev.pipe;
 ret = video_device_pipeline_start(&cru->vdev, pipe);
 if (ret)
  return ret;

 ret = v4l2_subdev_call(sd, video, pre_streamon, 0);
 if (ret && ret != -ENOIOCTLCMD)
  goto pipe_line_stop;

 ret = v4l2_subdev_call(sd, video, s_stream, 1);
 if (ret && ret != -ENOIOCTLCMD)
  goto err_s_stream;

 return 0;

err_s_stream:
 v4l2_subdev_call(sd, video, post_streamoff);

pipe_line_stop:
 video_device_pipeline_stop(&cru->vdev);

 return ret;
}

static void rzg2l_cru_stop_streaming(struct rzg2l_cru_dev *cru)
{
 cru->state = RZG2L_CRU_DMA_STOPPING;

 rzg2l_cru_set_stream(cru, 0);
}

irqreturn_t rzg2l_cru_irq(int irq, void *data)
{
 struct rzg2l_cru_dev *cru = data;
 unsigned int handled = 0;
 unsigned long flags;
 u32 irq_status;
 u32 amnmbs;
 int slot;

 spin_lock_irqsave(&cru->qlock, flags);

 irq_status = rzg2l_cru_read(cru, CRUnINTS);
 if (!irq_status)
  goto done;

 handled = 1;

 rzg2l_cru_write(cru, CRUnINTS, rzg2l_cru_read(cru, CRUnINTS));

 /* Nothing to do if capture status is 'RZG2L_CRU_DMA_STOPPED' */
 if (cru->state == RZG2L_CRU_DMA_STOPPED) {
  dev_dbg(cru->dev, "IRQ while state stopped\n");
  goto done;
 }

 /* Increase stop retries if capture status is 'RZG2L_CRU_DMA_STOPPING' */
 if (cru->state == RZG2L_CRU_DMA_STOPPING) {
  if (irq_status & CRUnINTS_SFS)
   dev_dbg(cru->dev, "IRQ while state stopping\n");
  goto done;
 }

 /* Prepare for capture and update state */
 amnmbs = rzg2l_cru_read(cru, AMnMBS);
 slot = amnmbs & AMnMBS_MBSTS;

 /*
  * AMnMBS.MBSTS indicates the destination of Memory Bank (MB).
  * Recalculate to get the current transfer complete MB.
 */
 if (slot == 0)
  slot = cru->num_buf - 1;
 else
  slot--;

 /*
  * To hand buffers back in a known order to userspace start
  * to capture first from slot 0.
 */
 if (cru->state == RZG2L_CRU_DMA_STARTING) {
  if (slot != 0) {
   dev_dbg(cru->dev, "Starting sync slot: %d\n", slot);
   goto done;
  }

  dev_dbg(cru->dev, "Capture start synced!\n");
  cru->state = RZG2L_CRU_DMA_RUNNING;
 }

 /* Capture frame */
 if (cru->queue_buf[slot]) {
  cru->queue_buf[slot]->field = cru->format.field;
  cru->queue_buf[slot]->sequence = cru->sequence;
  cru->queue_buf[slot]->vb2_buf.timestamp = ktime_get_ns();
  vb2_buffer_done(&cru->queue_buf[slot]->vb2_buf,
    VB2_BUF_STATE_DONE);
  cru->queue_buf[slot] = NULL;
 } else {
  /* Scratch buffer was used, dropping frame. */
  dev_dbg(cru->dev, "Dropping frame %u\n", cru->sequence);
 }

 cru->sequence++;

 /* Prepare for next frame */
 rzg2l_cru_fill_hw_slot(cru, slot);

done:
 spin_unlock_irqrestore(&cru->qlock, flags);

 return IRQ_RETVAL(handled);
}

static int rzg3e_cru_get_current_slot(struct rzg2l_cru_dev *cru)
{
 u64 amnmadrs;
 int slot;

 /*
  * When AMnMADRSL is read, AMnMADRSH of the higher-order
  * address also latches the address.
  *
  * AMnMADRSH must be read after AMnMADRSL has been read.
 */
 amnmadrs = rzg2l_cru_read(cru, AMnMADRSL);
 amnmadrs |= (u64)rzg2l_cru_read(cru, AMnMADRSH) << 32;

 /* Ensure amnmadrs is within this buffer range */
 for (slot = 0; slot < cru->num_buf; slot++) {
  if (amnmadrs >= cru->buf_addr[slot] &&
      amnmadrs < cru->buf_addr[slot] + cru->format.sizeimage)
   return slot;
 }

 dev_err(cru->dev, "Invalid MB address 0x%llx (out of range)\n", amnmadrs);
 return -EINVAL;
}

irqreturn_t rzg3e_cru_irq(int irq, void *data)
{
 struct rzg2l_cru_dev *cru = data;
 u32 irq_status;
 int slot;

 scoped_guard(spinlock, &cru->qlock) {
  irq_status = rzg2l_cru_read(cru, CRUnINTS2);
  if (!irq_status)
   return IRQ_NONE;

  dev_dbg(cru->dev, "CRUnINTS2 0x%x\n", irq_status);

  rzg2l_cru_write(cru, CRUnINTS2, rzg2l_cru_read(cru, CRUnINTS2));

  /* Nothing to do if capture status is 'RZG2L_CRU_DMA_STOPPED' */
  if (cru->state == RZG2L_CRU_DMA_STOPPED) {
   dev_dbg(cru->dev, "IRQ while state stopped\n");
   return IRQ_HANDLED;
  }

  if (cru->state == RZG2L_CRU_DMA_STOPPING) {
   if (irq_status & CRUnINTS2_FSxS(0) ||
       irq_status & CRUnINTS2_FSxS(1) ||
       irq_status & CRUnINTS2_FSxS(2) ||
       irq_status & CRUnINTS2_FSxS(3))
    dev_dbg(cru->dev, "IRQ while state stopping\n");
   return IRQ_HANDLED;
  }

  slot = rzg3e_cru_get_current_slot(cru);
  if (slot < 0)
   return IRQ_HANDLED;

  dev_dbg(cru->dev, "Current written slot: %d\n", slot);
  cru->buf_addr[slot] = 0;

  /*
   * To hand buffers back in a known order to userspace start
   * to capture first from slot 0.
 */
  if (cru->state == RZG2L_CRU_DMA_STARTING) {
   if (slot != 0) {
    dev_dbg(cru->dev, "Starting sync slot: %d\n", slot);
    return IRQ_HANDLED;
   }
   dev_dbg(cru->dev, "Capture start synced!\n");
   cru->state = RZG2L_CRU_DMA_RUNNING;
  }

  /* Capture frame */
  if (cru->queue_buf[slot]) {
   struct vb2_v4l2_buffer *buf = cru->queue_buf[slot];

   buf->field = cru->format.field;
   buf->sequence = cru->sequence;
   buf->vb2_buf.timestamp = ktime_get_ns();
   vb2_buffer_done(&buf->vb2_buf, VB2_BUF_STATE_DONE);
   cru->queue_buf[slot] = NULL;
  } else {
   /* Scratch buffer was used, dropping frame. */
   dev_dbg(cru->dev, "Dropping frame %u\n", cru->sequence);
  }

  cru->sequence++;

  /* Prepare for next frame */
  rzg2l_cru_fill_hw_slot(cru, slot);
 }

 return IRQ_HANDLED;
}

static int rzg2l_cru_start_streaming_vq(struct vb2_queue *vq, unsigned int count)
{
 struct rzg2l_cru_dev *cru = vb2_get_drv_priv(vq);
 int ret;

 ret = pm_runtime_resume_and_get(cru->dev);
 if (ret)
  return ret;

 ret = clk_prepare_enable(cru->vclk);
 if (ret)
  goto err_pm_put;

 /* Release reset state */
 ret = reset_control_deassert(cru->aresetn);
 if (ret) {
  dev_err(cru->dev, "failed to deassert aresetn\n");
  goto err_vclk_disable;
 }

 ret = reset_control_deassert(cru->presetn);
 if (ret) {
  reset_control_assert(cru->aresetn);
  dev_err(cru->dev, "failed to deassert presetn\n");
  goto assert_aresetn;
 }

 /* Allocate scratch buffer */
 cru->scratch = dma_alloc_coherent(cru->dev, cru->format.sizeimage,
       &cru->scratch_phys, GFP_KERNEL);
 if (!cru->scratch) {
  return_unused_buffers(cru, VB2_BUF_STATE_QUEUED);
  dev_err(cru->dev, "Failed to allocate scratch buffer\n");
  ret = -ENOMEM;
  goto assert_presetn;
 }

 cru->sequence = 0;

 ret = rzg2l_cru_set_stream(cru, 1);
 if (ret) {
  return_unused_buffers(cru, VB2_BUF_STATE_QUEUED);
  goto out;
 }

 cru->state = RZG2L_CRU_DMA_STARTING;
 dev_dbg(cru->dev, "Starting to capture\n");
 return 0;

out:
 if (ret)
  dma_free_coherent(cru->dev, cru->format.sizeimage, cru->scratch,
      cru->scratch_phys);
assert_presetn:
 reset_control_assert(cru->presetn);

assert_aresetn:
 reset_control_assert(cru->aresetn);

err_vclk_disable:
 clk_disable_unprepare(cru->vclk);

err_pm_put:
 pm_runtime_put_sync(cru->dev);

 return ret;
}

static void rzg2l_cru_stop_streaming_vq(struct vb2_queue *vq)
{
 struct rzg2l_cru_dev *cru = vb2_get_drv_priv(vq);

 rzg2l_cru_stop_streaming(cru);

 /* Free scratch buffer */
 dma_free_coherent(cru->dev, cru->format.sizeimage,
     cru->scratch, cru->scratch_phys);

 return_unused_buffers(cru, VB2_BUF_STATE_ERROR);

 reset_control_assert(cru->presetn);
 clk_disable_unprepare(cru->vclk);
 pm_runtime_put_sync(cru->dev);
}

static const struct vb2_ops rzg2l_cru_qops = {
 .queue_setup  = rzg2l_cru_queue_setup,
 .buf_prepare  = rzg2l_cru_buffer_prepare,
 .buf_queue  = rzg2l_cru_buffer_queue,
 .start_streaming = rzg2l_cru_start_streaming_vq,
 .stop_streaming  = rzg2l_cru_stop_streaming_vq,
};

void rzg2l_cru_dma_unregister(struct rzg2l_cru_dev *cru)
{
 mutex_destroy(&cru->lock);

 v4l2_device_unregister(&cru->v4l2_dev);
 vb2_queue_release(&cru->queue);
}

int rzg2l_cru_dma_register(struct rzg2l_cru_dev *cru)
{
 struct vb2_queue *q = &cru->queue;
 unsigned int i;
 int ret;

 /* Initialize the top-level structure */
 ret = v4l2_device_register(cru->dev, &cru->v4l2_dev);
 if (ret)
  return ret;

 mutex_init(&cru->lock);
 INIT_LIST_HEAD(&cru->buf_list);

 spin_lock_init(&cru->qlock);

 cru->state = RZG2L_CRU_DMA_STOPPED;

 for (i = 0; i < RZG2L_CRU_HW_BUFFER_MAX; i++)
  cru->queue_buf[i] = NULL;

 /* buffer queue */
 q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 q->io_modes = VB2_MMAP | VB2_DMABUF;
 q->lock = &cru->lock;
 q->drv_priv = cru;
 q->buf_struct_size = sizeof(struct rzg2l_cru_buffer);
 q->ops = &rzg2l_cru_qops;
 q->mem_ops = &vb2_dma_contig_memops;
 q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
 q->min_queued_buffers = 4;
 q->dev = cru->dev;

 ret = vb2_queue_init(q);
 if (ret < 0) {
  dev_err(cru->dev, "failed to initialize VB2 queue\n");
  goto error;
 }

 return 0;

error:
 mutex_destroy(&cru->lock);
 v4l2_device_unregister(&cru->v4l2_dev);
 return ret;
}

/* -----------------------------------------------------------------------------
 * V4L2 stuff
 */

static void rzg2l_cru_format_align(struct rzg2l_cru_dev *cru,
       struct v4l2_pix_format *pix)
{
 const struct rzg2l_cru_info *info = cru->info;
 const struct rzg2l_cru_ip_format *fmt;

 fmt = rzg2l_cru_ip_format_to_fmt(pix->pixelformat);
 if (!fmt) {
  pix->pixelformat = RZG2L_CRU_DEFAULT_FORMAT;
  fmt = rzg2l_cru_ip_format_to_fmt(pix->pixelformat);
 }

 switch (pix->field) {
 case V4L2_FIELD_TOP:
 case V4L2_FIELD_BOTTOM:
 case V4L2_FIELD_NONE:
 case V4L2_FIELD_INTERLACED_TB:
 case V4L2_FIELD_INTERLACED_BT:
 case V4L2_FIELD_INTERLACED:
  break;
 default:
  pix->field = RZG2L_CRU_DEFAULT_FIELD;
  break;
 }

 /* Limit to CRU capabilities */
 v4l_bound_align_image(&pix->width, 320, info->max_width, 1,
         &pix->height, 240, info->max_height, 2, 0);

 v4l2_fill_pixfmt(pix, pix->pixelformat, pix->width, pix->height);

 dev_dbg(cru->dev, "Format %ux%u bpl: %u size: %u\n",
  pix->width, pix->height, pix->bytesperline, pix->sizeimage);
}

static void rzg2l_cru_try_format(struct rzg2l_cru_dev *cru,
     struct v4l2_pix_format *pix)
{
 /*
  * The V4L2 specification clearly documents the colorspace fields
  * as being set by drivers for capture devices. Using the values
  * supplied by userspace thus wouldn't comply with the API. Until
  * the API is updated force fixed values.
 */
 pix->colorspace = RZG2L_CRU_DEFAULT_COLORSPACE;
 pix->xfer_func = V4L2_MAP_XFER_FUNC_DEFAULT(pix->colorspace);
 pix->ycbcr_enc = V4L2_MAP_YCBCR_ENC_DEFAULT(pix->colorspace);
 pix->quantization = V4L2_MAP_QUANTIZATION_DEFAULT(true, pix->colorspace,
         pix->ycbcr_enc);

 rzg2l_cru_format_align(cru, pix);
}

static int rzg2l_cru_querycap(struct file *file, void *priv,
         struct v4l2_capability *cap)
{
 strscpy(cap->driver, KBUILD_MODNAME, sizeof(cap->driver));
 strscpy(cap->card, "RZG2L_CRU", sizeof(cap->card));

 return 0;
}

static int rzg2l_cru_try_fmt_vid_cap(struct file *file, void *priv,
         struct v4l2_format *f)
{
 struct rzg2l_cru_dev *cru = video_drvdata(file);

 rzg2l_cru_try_format(cru, &f->fmt.pix);

 return 0;
}

static int rzg2l_cru_s_fmt_vid_cap(struct file *file, void *priv,
       struct v4l2_format *f)
{
 struct rzg2l_cru_dev *cru = video_drvdata(file);

 if (vb2_is_busy(&cru->queue))
  return -EBUSY;

 rzg2l_cru_try_format(cru, &f->fmt.pix);

 cru->format = f->fmt.pix;

 return 0;
}

static int rzg2l_cru_g_fmt_vid_cap(struct file *file, void *priv,
       struct v4l2_format *f)
{
 struct rzg2l_cru_dev *cru = video_drvdata(file);

 f->fmt.pix = cru->format;

 return 0;
}

static int rzg2l_cru_enum_fmt_vid_cap(struct file *file, void *priv,
          struct v4l2_fmtdesc *f)
{
 const struct rzg2l_cru_ip_format *fmt;

 fmt = rzg2l_cru_ip_index_to_fmt(f->index);
 if (!fmt)
  return -EINVAL;

 f->pixelformat = fmt->format;

 return 0;
}

static int rzg2l_cru_enum_framesizes(struct file *file, void *fh,
         struct v4l2_frmsizeenum *fsize)
{
 struct rzg2l_cru_dev *cru = video_drvdata(file);
 const struct rzg2l_cru_info *info = cru->info;
 const struct rzg2l_cru_ip_format *fmt;

 if (fsize->index)
  return -EINVAL;

 fmt = rzg2l_cru_ip_format_to_fmt(fsize->pixel_format);
 if (!fmt)
  return -EINVAL;

 fsize->type = V4L2_FRMIVAL_TYPE_CONTINUOUS;
 fsize->stepwise.min_width = RZG2L_CRU_MIN_INPUT_WIDTH;
 fsize->stepwise.max_width = info->max_width;
 fsize->stepwise.step_width = 1;
 fsize->stepwise.min_height = RZG2L_CRU_MIN_INPUT_HEIGHT;
 fsize->stepwise.max_height = info->max_height;
 fsize->stepwise.step_height = 1;

 return 0;
}

static const struct v4l2_ioctl_ops rzg2l_cru_ioctl_ops = {
 .vidioc_querycap  = rzg2l_cru_querycap,
 .vidioc_try_fmt_vid_cap  = rzg2l_cru_try_fmt_vid_cap,
 .vidioc_g_fmt_vid_cap  = rzg2l_cru_g_fmt_vid_cap,
 .vidioc_s_fmt_vid_cap  = rzg2l_cru_s_fmt_vid_cap,
 .vidioc_enum_fmt_vid_cap = rzg2l_cru_enum_fmt_vid_cap,

 .vidioc_reqbufs   = vb2_ioctl_reqbufs,
 .vidioc_create_bufs  = vb2_ioctl_create_bufs,
 .vidioc_querybuf  = vb2_ioctl_querybuf,
 .vidioc_qbuf   = vb2_ioctl_qbuf,
 .vidioc_dqbuf   = vb2_ioctl_dqbuf,
 .vidioc_expbuf   = vb2_ioctl_expbuf,
 .vidioc_prepare_buf  = vb2_ioctl_prepare_buf,
 .vidioc_streamon  = vb2_ioctl_streamon,
 .vidioc_streamoff  = vb2_ioctl_streamoff,
 .vidioc_enum_framesizes  = rzg2l_cru_enum_framesizes,
};

/* -----------------------------------------------------------------------------
 * Media controller file operations
 */

static int rzg2l_cru_open(struct file *file)
{
 struct rzg2l_cru_dev *cru = video_drvdata(file);
 int ret;

 ret = mutex_lock_interruptible(&cru->lock);
 if (ret)
  return ret;

 file->private_data = cru;
 ret = v4l2_fh_open(file);
 if (ret)
  goto err_unlock;

 mutex_unlock(&cru->lock);

 return 0;

err_unlock:
 mutex_unlock(&cru->lock);

 return ret;
}

static int rzg2l_cru_release(struct file *file)
{
 struct rzg2l_cru_dev *cru = video_drvdata(file);
 int ret;

 mutex_lock(&cru->lock);

 /* the release helper will cleanup any on-going streaming. */
 ret = _vb2_fop_release(file, NULL);

 mutex_unlock(&cru->lock);

 return ret;
}

static const struct v4l2_file_operations rzg2l_cru_fops = {
 .owner  = THIS_MODULE,
 .unlocked_ioctl = video_ioctl2,
 .open  = rzg2l_cru_open,
 .release = rzg2l_cru_release,
 .poll  = vb2_fop_poll,
 .mmap  = vb2_fop_mmap,
 .read  = vb2_fop_read,
};

/* -----------------------------------------------------------------------------
 * Media entity operations
 */

static int rzg2l_cru_video_link_validate(struct media_link *link)
{
 struct v4l2_subdev_format fmt = {
  .which = V4L2_SUBDEV_FORMAT_ACTIVE,
 };
 const struct rzg2l_cru_ip_format *video_fmt;
 struct v4l2_subdev *subdev;
 struct rzg2l_cru_dev *cru;
 int ret;

 subdev = media_entity_to_v4l2_subdev(link->source->entity);
 fmt.pad = link->source->index;
 ret = v4l2_subdev_call(subdev, pad, get_fmt, NULL, &fmt);
 if (ret < 0)
  return ret == -ENOIOCTLCMD ? -EINVAL : ret;

 cru = container_of(media_entity_to_video_device(link->sink->entity),
      struct rzg2l_cru_dev, vdev);
 video_fmt = rzg2l_cru_ip_format_to_fmt(cru->format.pixelformat);

 if (fmt.format.width != cru->format.width ||
     fmt.format.height != cru->format.height ||
     fmt.format.field != cru->format.field ||
     !rzg2l_cru_ip_fmt_supports_mbus_code(video_fmt, fmt.format.code))
  return -EPIPE;

 return 0;
}

static const struct media_entity_operations rzg2l_cru_video_media_ops = {
 .link_validate = rzg2l_cru_video_link_validate,
};

static void rzg2l_cru_v4l2_init(struct rzg2l_cru_dev *cru)
{
 struct video_device *vdev = &cru->vdev;

 vdev->v4l2_dev = &cru->v4l2_dev;
 vdev->queue = &cru->queue;
 snprintf(vdev->name, sizeof(vdev->name), "CRU output");
 vdev->release = video_device_release_empty;
 vdev->lock = &cru->lock;
 vdev->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
 vdev->device_caps |= V4L2_CAP_IO_MC;
 vdev->entity.ops = &rzg2l_cru_video_media_ops;
 vdev->fops = &rzg2l_cru_fops;
 vdev->ioctl_ops = &rzg2l_cru_ioctl_ops;

 /* Set a default format */
 cru->format.pixelformat = RZG2L_CRU_DEFAULT_FORMAT;
 cru->format.width = RZG2L_CRU_DEFAULT_WIDTH;
 cru->format.height = RZG2L_CRU_DEFAULT_HEIGHT;
 cru->format.field = RZG2L_CRU_DEFAULT_FIELD;
 cru->format.colorspace = RZG2L_CRU_DEFAULT_COLORSPACE;
 rzg2l_cru_format_align(cru, &cru->format);
}

void rzg2l_cru_video_unregister(struct rzg2l_cru_dev *cru)
{
 media_device_unregister(&cru->mdev);
 video_unregister_device(&cru->vdev);
}

int rzg2l_cru_video_register(struct rzg2l_cru_dev *cru)
{
 struct video_device *vdev = &cru->vdev;
 int ret;

 if (video_is_registered(&cru->vdev)) {
  struct media_entity *entity;

  entity = &cru->vdev.entity;
  if (!entity->graph_obj.mdev)
   entity->graph_obj.mdev = &cru->mdev;
  return 0;
 }

 rzg2l_cru_v4l2_init(cru);
 video_set_drvdata(vdev, cru);
 ret = video_register_device(vdev, VFL_TYPE_VIDEO, -1);
 if (ret) {
  dev_err(cru->dev, "Failed to register video device\n");
  return ret;
 }

 ret = media_device_register(&cru->mdev);
 if (ret) {
  video_unregister_device(&cru->vdev);
  return ret;
 }

 return 0;
}