Back to home page

OSCL-LXR
 
 

     

0001 // SPDX-License-Identifier: GPL-2.0
0002 /*
0003  * Driver for STM32 Digital Camera Memory Interface
0004  *
0005  * Copyright (C) STMicroelectronics SA 2017
0006  * Authors: Yannick Fertre <yannick.fertre@st.com>
0007  *          Hugues Fruchet <hugues.fruchet@st.com>
0008  *          for STMicroelectronics.
0009  *
0010  * This driver is based on atmel_isi.c
0011  *
0012  */
0013 
0014 #include <linux/clk.h>
0015 #include <linux/completion.h>
0016 #include <linux/delay.h>
0017 #include <linux/dmaengine.h>
0018 #include <linux/init.h>
0019 #include <linux/interrupt.h>
0020 #include <linux/kernel.h>
0021 #include <linux/module.h>
0022 #include <linux/of.h>
0023 #include <linux/of_device.h>
0024 #include <linux/of_graph.h>
0025 #include <linux/pinctrl/consumer.h>
0026 #include <linux/platform_device.h>
0027 #include <linux/pm_runtime.h>
0028 #include <linux/reset.h>
0029 #include <linux/videodev2.h>
0030 
0031 #include <media/v4l2-ctrls.h>
0032 #include <media/v4l2-dev.h>
0033 #include <media/v4l2-device.h>
0034 #include <media/v4l2-event.h>
0035 #include <media/v4l2-fwnode.h>
0036 #include <media/v4l2-image-sizes.h>
0037 #include <media/v4l2-ioctl.h>
0038 #include <media/v4l2-rect.h>
0039 #include <media/videobuf2-dma-contig.h>
0040 
0041 #define DRV_NAME "stm32-dcmi"
0042 
0043 /* Registers offset for DCMI */
0044 #define DCMI_CR     0x00 /* Control Register */
0045 #define DCMI_SR     0x04 /* Status Register */
0046 #define DCMI_RIS    0x08 /* Raw Interrupt Status register */
0047 #define DCMI_IER    0x0C /* Interrupt Enable Register */
0048 #define DCMI_MIS    0x10 /* Masked Interrupt Status register */
0049 #define DCMI_ICR    0x14 /* Interrupt Clear Register */
0050 #define DCMI_ESCR   0x18 /* Embedded Synchronization Code Register */
0051 #define DCMI_ESUR   0x1C /* Embedded Synchronization Unmask Register */
0052 #define DCMI_CWSTRT 0x20 /* Crop Window STaRT */
0053 #define DCMI_CWSIZE 0x24 /* Crop Window SIZE */
0054 #define DCMI_DR     0x28 /* Data Register */
0055 #define DCMI_IDR    0x2C /* IDentifier Register */
0056 
0057 /* Bits definition for control register (DCMI_CR) */
0058 #define CR_CAPTURE  BIT(0)
0059 #define CR_CM       BIT(1)
0060 #define CR_CROP     BIT(2)
0061 #define CR_JPEG     BIT(3)
0062 #define CR_ESS      BIT(4)
0063 #define CR_PCKPOL   BIT(5)
0064 #define CR_HSPOL    BIT(6)
0065 #define CR_VSPOL    BIT(7)
0066 #define CR_FCRC_0   BIT(8)
0067 #define CR_FCRC_1   BIT(9)
0068 #define CR_EDM_0    BIT(10)
0069 #define CR_EDM_1    BIT(11)
0070 #define CR_ENABLE   BIT(14)
0071 
0072 /* Bits definition for status register (DCMI_SR) */
0073 #define SR_HSYNC    BIT(0)
0074 #define SR_VSYNC    BIT(1)
0075 #define SR_FNE      BIT(2)
0076 
0077 /*
0078  * Bits definition for interrupt registers
0079  * (DCMI_RIS, DCMI_IER, DCMI_MIS, DCMI_ICR)
0080  */
0081 #define IT_FRAME    BIT(0)
0082 #define IT_OVR      BIT(1)
0083 #define IT_ERR      BIT(2)
0084 #define IT_VSYNC    BIT(3)
0085 #define IT_LINE     BIT(4)
0086 
0087 enum state {
0088     STOPPED = 0,
0089     WAIT_FOR_BUFFER,
0090     RUNNING,
0091 };
0092 
0093 #define MIN_WIDTH   16U
0094 #define MAX_WIDTH   2592U
0095 #define MIN_HEIGHT  16U
0096 #define MAX_HEIGHT  2592U
0097 
0098 #define TIMEOUT_MS  1000
0099 
0100 #define OVERRUN_ERROR_THRESHOLD 3
0101 
0102 struct dcmi_format {
0103     u32 fourcc;
0104     u32 mbus_code;
0105     u8  bpp;
0106 };
0107 
0108 struct dcmi_framesize {
0109     u32 width;
0110     u32 height;
0111 };
0112 
0113 struct dcmi_buf {
0114     struct vb2_v4l2_buffer  vb;
0115     bool            prepared;
0116     struct sg_table     sgt;
0117     size_t          size;
0118     struct list_head    list;
0119 };
0120 
0121 struct stm32_dcmi {
0122     /* Protects the access of variables shared within the interrupt */
0123     spinlock_t          irqlock;
0124     struct device           *dev;
0125     void __iomem            *regs;
0126     struct resource         *res;
0127     struct reset_control        *rstc;
0128     int             sequence;
0129     struct list_head        buffers;
0130     struct dcmi_buf         *active;
0131     int         irq;
0132 
0133     struct v4l2_device      v4l2_dev;
0134     struct video_device     *vdev;
0135     struct v4l2_async_notifier  notifier;
0136     struct v4l2_subdev      *source;
0137     struct v4l2_format      fmt;
0138     struct v4l2_rect        crop;
0139     bool                do_crop;
0140 
0141     const struct dcmi_format    **sd_formats;
0142     unsigned int            num_of_sd_formats;
0143     const struct dcmi_format    *sd_format;
0144     struct dcmi_framesize       *sd_framesizes;
0145     unsigned int            num_of_sd_framesizes;
0146     struct dcmi_framesize       sd_framesize;
0147     struct v4l2_rect        sd_bounds;
0148 
0149     /* Protect this data structure */
0150     struct mutex            lock;
0151     struct vb2_queue        queue;
0152 
0153     struct v4l2_mbus_config_parallel    bus;
0154     enum v4l2_mbus_type     bus_type;
0155     struct completion       complete;
0156     struct clk          *mclk;
0157     enum state          state;
0158     struct dma_chan         *dma_chan;
0159     dma_cookie_t            dma_cookie;
0160     u32             dma_max_burst;
0161     u32             misr;
0162     int             errors_count;
0163     int             overrun_count;
0164     int             buffers_count;
0165 
0166     /* Ensure DMA operations atomicity */
0167     struct mutex            dma_lock;
0168 
0169     struct media_device     mdev;
0170     struct media_pad        vid_cap_pad;
0171     struct media_pipeline       pipeline;
0172 };
0173 
0174 static inline struct stm32_dcmi *notifier_to_dcmi(struct v4l2_async_notifier *n)
0175 {
0176     return container_of(n, struct stm32_dcmi, notifier);
0177 }
0178 
0179 static inline u32 reg_read(void __iomem *base, u32 reg)
0180 {
0181     return readl_relaxed(base + reg);
0182 }
0183 
0184 static inline void reg_write(void __iomem *base, u32 reg, u32 val)
0185 {
0186     writel_relaxed(val, base + reg);
0187 }
0188 
0189 static inline void reg_set(void __iomem *base, u32 reg, u32 mask)
0190 {
0191     reg_write(base, reg, reg_read(base, reg) | mask);
0192 }
0193 
0194 static inline void reg_clear(void __iomem *base, u32 reg, u32 mask)
0195 {
0196     reg_write(base, reg, reg_read(base, reg) & ~mask);
0197 }
0198 
0199 static int dcmi_start_capture(struct stm32_dcmi *dcmi, struct dcmi_buf *buf);
0200 
0201 static void dcmi_buffer_done(struct stm32_dcmi *dcmi,
0202                  struct dcmi_buf *buf,
0203                  size_t bytesused,
0204                  int err)
0205 {
0206     struct vb2_v4l2_buffer *vbuf;
0207 
0208     if (!buf)
0209         return;
0210 
0211     list_del_init(&buf->list);
0212 
0213     vbuf = &buf->vb;
0214 
0215     vbuf->sequence = dcmi->sequence++;
0216     vbuf->field = V4L2_FIELD_NONE;
0217     vbuf->vb2_buf.timestamp = ktime_get_ns();
0218     vb2_set_plane_payload(&vbuf->vb2_buf, 0, bytesused);
0219     vb2_buffer_done(&vbuf->vb2_buf,
0220             err ? VB2_BUF_STATE_ERROR : VB2_BUF_STATE_DONE);
0221     dev_dbg(dcmi->dev, "buffer[%d] done seq=%d, bytesused=%zu\n",
0222         vbuf->vb2_buf.index, vbuf->sequence, bytesused);
0223 
0224     dcmi->buffers_count++;
0225     dcmi->active = NULL;
0226 }
0227 
0228 static int dcmi_restart_capture(struct stm32_dcmi *dcmi)
0229 {
0230     struct dcmi_buf *buf;
0231 
0232     spin_lock_irq(&dcmi->irqlock);
0233 
0234     if (dcmi->state != RUNNING) {
0235         spin_unlock_irq(&dcmi->irqlock);
0236         return -EINVAL;
0237     }
0238 
0239     /* Restart a new DMA transfer with next buffer */
0240     if (list_empty(&dcmi->buffers)) {
0241         dev_dbg(dcmi->dev, "Capture restart is deferred to next buffer queueing\n");
0242         dcmi->state = WAIT_FOR_BUFFER;
0243         spin_unlock_irq(&dcmi->irqlock);
0244         return 0;
0245     }
0246     buf = list_entry(dcmi->buffers.next, struct dcmi_buf, list);
0247     dcmi->active = buf;
0248 
0249     spin_unlock_irq(&dcmi->irqlock);
0250 
0251     return dcmi_start_capture(dcmi, buf);
0252 }
0253 
0254 static void dcmi_dma_callback(void *param)
0255 {
0256     struct stm32_dcmi *dcmi = (struct stm32_dcmi *)param;
0257     struct dma_tx_state state;
0258     enum dma_status status;
0259     struct dcmi_buf *buf = dcmi->active;
0260 
0261     spin_lock_irq(&dcmi->irqlock);
0262 
0263     /* Check DMA status */
0264     status = dmaengine_tx_status(dcmi->dma_chan, dcmi->dma_cookie, &state);
0265 
0266     switch (status) {
0267     case DMA_IN_PROGRESS:
0268         dev_dbg(dcmi->dev, "%s: Received DMA_IN_PROGRESS\n", __func__);
0269         break;
0270     case DMA_PAUSED:
0271         dev_err(dcmi->dev, "%s: Received DMA_PAUSED\n", __func__);
0272         break;
0273     case DMA_ERROR:
0274         dev_err(dcmi->dev, "%s: Received DMA_ERROR\n", __func__);
0275 
0276         /* Return buffer to V4L2 in error state */
0277         dcmi_buffer_done(dcmi, buf, 0, -EIO);
0278         break;
0279     case DMA_COMPLETE:
0280         dev_dbg(dcmi->dev, "%s: Received DMA_COMPLETE\n", __func__);
0281 
0282         /* Return buffer to V4L2 */
0283         dcmi_buffer_done(dcmi, buf, buf->size, 0);
0284 
0285         spin_unlock_irq(&dcmi->irqlock);
0286 
0287         /* Restart capture */
0288         if (dcmi_restart_capture(dcmi))
0289             dev_err(dcmi->dev, "%s: Cannot restart capture on DMA complete\n",
0290                 __func__);
0291         return;
0292     default:
0293         dev_err(dcmi->dev, "%s: Received unknown status\n", __func__);
0294         break;
0295     }
0296 
0297     spin_unlock_irq(&dcmi->irqlock);
0298 }
0299 
0300 static int dcmi_start_dma(struct stm32_dcmi *dcmi,
0301               struct dcmi_buf *buf)
0302 {
0303     struct dma_async_tx_descriptor *desc = NULL;
0304     struct dma_slave_config config;
0305     int ret;
0306 
0307     memset(&config, 0, sizeof(config));
0308 
0309     config.src_addr = (dma_addr_t)dcmi->res->start + DCMI_DR;
0310     config.src_addr_width = DMA_SLAVE_BUSWIDTH_4_BYTES;
0311     config.dst_addr_width = DMA_SLAVE_BUSWIDTH_4_BYTES;
0312     config.dst_maxburst = 4;
0313 
0314     /* Configure DMA channel */
0315     ret = dmaengine_slave_config(dcmi->dma_chan, &config);
0316     if (ret < 0) {
0317         dev_err(dcmi->dev, "%s: DMA channel config failed (%d)\n",
0318             __func__, ret);
0319         return ret;
0320     }
0321 
0322     /*
0323      * Avoid call of dmaengine_terminate_sync() between
0324      * dmaengine_prep_slave_single() and dmaengine_submit()
0325      * by locking the whole DMA submission sequence
0326      */
0327     mutex_lock(&dcmi->dma_lock);
0328 
0329     /* Prepare a DMA transaction */
0330     desc = dmaengine_prep_slave_sg(dcmi->dma_chan, buf->sgt.sgl, buf->sgt.nents,
0331                        DMA_DEV_TO_MEM,
0332                        DMA_PREP_INTERRUPT);
0333     if (!desc) {
0334         dev_err(dcmi->dev, "%s: DMA dmaengine_prep_slave_sg failed\n", __func__);
0335         mutex_unlock(&dcmi->dma_lock);
0336         return -EINVAL;
0337     }
0338 
0339     /* Set completion callback routine for notification */
0340     desc->callback = dcmi_dma_callback;
0341     desc->callback_param = dcmi;
0342 
0343     /* Push current DMA transaction in the pending queue */
0344     dcmi->dma_cookie = dmaengine_submit(desc);
0345     if (dma_submit_error(dcmi->dma_cookie)) {
0346         dev_err(dcmi->dev, "%s: DMA submission failed\n", __func__);
0347         mutex_unlock(&dcmi->dma_lock);
0348         return -ENXIO;
0349     }
0350 
0351     mutex_unlock(&dcmi->dma_lock);
0352 
0353     dma_async_issue_pending(dcmi->dma_chan);
0354 
0355     return 0;
0356 }
0357 
0358 static int dcmi_start_capture(struct stm32_dcmi *dcmi, struct dcmi_buf *buf)
0359 {
0360     int ret;
0361 
0362     if (!buf)
0363         return -EINVAL;
0364 
0365     ret = dcmi_start_dma(dcmi, buf);
0366     if (ret) {
0367         dcmi->errors_count++;
0368         return ret;
0369     }
0370 
0371     /* Enable capture */
0372     reg_set(dcmi->regs, DCMI_CR, CR_CAPTURE);
0373 
0374     return 0;
0375 }
0376 
0377 static void dcmi_set_crop(struct stm32_dcmi *dcmi)
0378 {
0379     u32 size, start;
0380 
0381     /* Crop resolution */
0382     size = ((dcmi->crop.height - 1) << 16) |
0383         ((dcmi->crop.width << 1) - 1);
0384     reg_write(dcmi->regs, DCMI_CWSIZE, size);
0385 
0386     /* Crop start point */
0387     start = ((dcmi->crop.top) << 16) |
0388          ((dcmi->crop.left << 1));
0389     reg_write(dcmi->regs, DCMI_CWSTRT, start);
0390 
0391     dev_dbg(dcmi->dev, "Cropping to %ux%u@%u:%u\n",
0392         dcmi->crop.width, dcmi->crop.height,
0393         dcmi->crop.left, dcmi->crop.top);
0394 
0395     /* Enable crop */
0396     reg_set(dcmi->regs, DCMI_CR, CR_CROP);
0397 }
0398 
0399 static void dcmi_process_jpeg(struct stm32_dcmi *dcmi)
0400 {
0401     struct dma_tx_state state;
0402     enum dma_status status;
0403     struct dcmi_buf *buf = dcmi->active;
0404 
0405     if (!buf)
0406         return;
0407 
0408     /*
0409      * Because of variable JPEG buffer size sent by sensor,
0410      * DMA transfer never completes due to transfer size never reached.
0411      * In order to ensure that all the JPEG data are transferred
0412      * in active buffer memory, DMA is drained.
0413      * Then DMA tx status gives the amount of data transferred
0414      * to memory, which is then returned to V4L2 through the active
0415      * buffer payload.
0416      */
0417 
0418     /* Drain DMA */
0419     dmaengine_synchronize(dcmi->dma_chan);
0420 
0421     /* Get DMA residue to get JPEG size */
0422     status = dmaengine_tx_status(dcmi->dma_chan, dcmi->dma_cookie, &state);
0423     if (status != DMA_ERROR && state.residue < buf->size) {
0424         /* Return JPEG buffer to V4L2 with received JPEG buffer size */
0425         dcmi_buffer_done(dcmi, buf, buf->size - state.residue, 0);
0426     } else {
0427         dcmi->errors_count++;
0428         dev_err(dcmi->dev, "%s: Cannot get JPEG size from DMA\n",
0429             __func__);
0430         /* Return JPEG buffer to V4L2 in ERROR state */
0431         dcmi_buffer_done(dcmi, buf, 0, -EIO);
0432     }
0433 
0434     /* Abort DMA operation */
0435     dmaengine_terminate_sync(dcmi->dma_chan);
0436 
0437     /* Restart capture */
0438     if (dcmi_restart_capture(dcmi))
0439         dev_err(dcmi->dev, "%s: Cannot restart capture on JPEG received\n",
0440             __func__);
0441 }
0442 
0443 static irqreturn_t dcmi_irq_thread(int irq, void *arg)
0444 {
0445     struct stm32_dcmi *dcmi = arg;
0446 
0447     spin_lock_irq(&dcmi->irqlock);
0448 
0449     if (dcmi->misr & IT_OVR) {
0450         dcmi->overrun_count++;
0451         if (dcmi->overrun_count > OVERRUN_ERROR_THRESHOLD)
0452             dcmi->errors_count++;
0453     }
0454     if (dcmi->misr & IT_ERR)
0455         dcmi->errors_count++;
0456 
0457     if (dcmi->sd_format->fourcc == V4L2_PIX_FMT_JPEG &&
0458         dcmi->misr & IT_FRAME) {
0459         /* JPEG received */
0460         spin_unlock_irq(&dcmi->irqlock);
0461         dcmi_process_jpeg(dcmi);
0462         return IRQ_HANDLED;
0463     }
0464 
0465     spin_unlock_irq(&dcmi->irqlock);
0466     return IRQ_HANDLED;
0467 }
0468 
0469 static irqreturn_t dcmi_irq_callback(int irq, void *arg)
0470 {
0471     struct stm32_dcmi *dcmi = arg;
0472     unsigned long flags;
0473 
0474     spin_lock_irqsave(&dcmi->irqlock, flags);
0475 
0476     dcmi->misr = reg_read(dcmi->regs, DCMI_MIS);
0477 
0478     /* Clear interrupt */
0479     reg_set(dcmi->regs, DCMI_ICR, IT_FRAME | IT_OVR | IT_ERR);
0480 
0481     spin_unlock_irqrestore(&dcmi->irqlock, flags);
0482 
0483     return IRQ_WAKE_THREAD;
0484 }
0485 
0486 static int dcmi_queue_setup(struct vb2_queue *vq,
0487                 unsigned int *nbuffers,
0488                 unsigned int *nplanes,
0489                 unsigned int sizes[],
0490                 struct device *alloc_devs[])
0491 {
0492     struct stm32_dcmi *dcmi = vb2_get_drv_priv(vq);
0493     unsigned int size;
0494 
0495     size = dcmi->fmt.fmt.pix.sizeimage;
0496 
0497     /* Make sure the image size is large enough */
0498     if (*nplanes)
0499         return sizes[0] < size ? -EINVAL : 0;
0500 
0501     *nplanes = 1;
0502     sizes[0] = size;
0503 
0504     dev_dbg(dcmi->dev, "Setup queue, count=%d, size=%d\n",
0505         *nbuffers, size);
0506 
0507     return 0;
0508 }
0509 
0510 static int dcmi_buf_init(struct vb2_buffer *vb)
0511 {
0512     struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
0513     struct dcmi_buf *buf = container_of(vbuf, struct dcmi_buf, vb);
0514 
0515     INIT_LIST_HEAD(&buf->list);
0516 
0517     return 0;
0518 }
0519 
0520 static int dcmi_buf_prepare(struct vb2_buffer *vb)
0521 {
0522     struct stm32_dcmi *dcmi =  vb2_get_drv_priv(vb->vb2_queue);
0523     struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
0524     struct dcmi_buf *buf = container_of(vbuf, struct dcmi_buf, vb);
0525     unsigned long size;
0526     unsigned int num_sgs = 1;
0527     dma_addr_t dma_buf;
0528     struct scatterlist *sg;
0529     int i, ret;
0530 
0531     size = dcmi->fmt.fmt.pix.sizeimage;
0532 
0533     if (vb2_plane_size(vb, 0) < size) {
0534         dev_err(dcmi->dev, "%s data will not fit into plane (%lu < %lu)\n",
0535             __func__, vb2_plane_size(vb, 0), size);
0536         return -EINVAL;
0537     }
0538 
0539     vb2_set_plane_payload(vb, 0, size);
0540 
0541     if (!buf->prepared) {
0542         /* Get memory addresses */
0543         buf->size = vb2_plane_size(&buf->vb.vb2_buf, 0);
0544         if (buf->size > dcmi->dma_max_burst)
0545             num_sgs = DIV_ROUND_UP(buf->size, dcmi->dma_max_burst);
0546 
0547         ret = sg_alloc_table(&buf->sgt, num_sgs, GFP_ATOMIC);
0548         if (ret) {
0549             dev_err(dcmi->dev, "sg table alloc failed\n");
0550             return ret;
0551         }
0552 
0553         dma_buf = vb2_dma_contig_plane_dma_addr(&buf->vb.vb2_buf, 0);
0554 
0555         dev_dbg(dcmi->dev, "buffer[%d] phy=%pad size=%zu\n",
0556             vb->index, &dma_buf, buf->size);
0557 
0558         for_each_sg(buf->sgt.sgl, sg, num_sgs, i) {
0559             size_t bytes = min_t(size_t, size, dcmi->dma_max_burst);
0560 
0561             sg_dma_address(sg) = dma_buf;
0562             sg_dma_len(sg) = bytes;
0563             dma_buf += bytes;
0564             size -= bytes;
0565         }
0566 
0567         buf->prepared = true;
0568 
0569         vb2_set_plane_payload(&buf->vb.vb2_buf, 0, buf->size);
0570     }
0571 
0572     return 0;
0573 }
0574 
0575 static void dcmi_buf_queue(struct vb2_buffer *vb)
0576 {
0577     struct stm32_dcmi *dcmi =  vb2_get_drv_priv(vb->vb2_queue);
0578     struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
0579     struct dcmi_buf *buf = container_of(vbuf, struct dcmi_buf, vb);
0580 
0581     spin_lock_irq(&dcmi->irqlock);
0582 
0583     /* Enqueue to video buffers list */
0584     list_add_tail(&buf->list, &dcmi->buffers);
0585 
0586     if (dcmi->state == WAIT_FOR_BUFFER) {
0587         dcmi->state = RUNNING;
0588         dcmi->active = buf;
0589 
0590         dev_dbg(dcmi->dev, "Starting capture on buffer[%d] queued\n",
0591             buf->vb.vb2_buf.index);
0592 
0593         spin_unlock_irq(&dcmi->irqlock);
0594         if (dcmi_start_capture(dcmi, buf))
0595             dev_err(dcmi->dev, "%s: Cannot restart capture on overflow or error\n",
0596                 __func__);
0597         return;
0598     }
0599 
0600     spin_unlock_irq(&dcmi->irqlock);
0601 }
0602 
0603 static struct media_entity *dcmi_find_source(struct stm32_dcmi *dcmi)
0604 {
0605     struct media_entity *entity = &dcmi->vdev->entity;
0606     struct media_pad *pad;
0607 
0608     /* Walk searching for entity having no sink */
0609     while (1) {
0610         pad = &entity->pads[0];
0611         if (!(pad->flags & MEDIA_PAD_FL_SINK))
0612             break;
0613 
0614         pad = media_pad_remote_pad_first(pad);
0615         if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
0616             break;
0617 
0618         entity = pad->entity;
0619     }
0620 
0621     return entity;
0622 }
0623 
0624 static int dcmi_pipeline_s_fmt(struct stm32_dcmi *dcmi,
0625                    struct v4l2_subdev_format *format)
0626 {
0627     struct media_entity *entity = &dcmi->source->entity;
0628     struct v4l2_subdev *subdev;
0629     struct media_pad *sink_pad = NULL;
0630     struct media_pad *src_pad = NULL;
0631     struct media_pad *pad = NULL;
0632     struct v4l2_subdev_format fmt = *format;
0633     bool found = false;
0634     int ret;
0635 
0636     /*
0637      * Starting from sensor subdevice, walk within
0638      * pipeline and set format on each subdevice
0639      */
0640     while (1) {
0641         unsigned int i;
0642 
0643         /* Search if current entity has a source pad */
0644         for (i = 0; i < entity->num_pads; i++) {
0645             pad = &entity->pads[i];
0646             if (pad->flags & MEDIA_PAD_FL_SOURCE) {
0647                 src_pad = pad;
0648                 found = true;
0649                 break;
0650             }
0651         }
0652         if (!found)
0653             break;
0654 
0655         subdev = media_entity_to_v4l2_subdev(entity);
0656 
0657         /* Propagate format on sink pad if any, otherwise source pad */
0658         if (sink_pad)
0659             pad = sink_pad;
0660 
0661         dev_dbg(dcmi->dev, "\"%s\":%d pad format set to 0x%x %ux%u\n",
0662             subdev->name, pad->index, format->format.code,
0663             format->format.width, format->format.height);
0664 
0665         fmt.pad = pad->index;
0666         ret = v4l2_subdev_call(subdev, pad, set_fmt, NULL, &fmt);
0667         if (ret < 0) {
0668             dev_err(dcmi->dev, "%s: Failed to set format 0x%x %ux%u on \"%s\":%d pad (%d)\n",
0669                 __func__, format->format.code,
0670                 format->format.width, format->format.height,
0671                 subdev->name, pad->index, ret);
0672             return ret;
0673         }
0674 
0675         if (fmt.format.code != format->format.code ||
0676             fmt.format.width != format->format.width ||
0677             fmt.format.height != format->format.height) {
0678             dev_dbg(dcmi->dev, "\"%s\":%d pad format has been changed to 0x%x %ux%u\n",
0679                 subdev->name, pad->index, fmt.format.code,
0680                 fmt.format.width, fmt.format.height);
0681         }
0682 
0683         /* Walk to next entity */
0684         sink_pad = media_pad_remote_pad_first(src_pad);
0685         if (!sink_pad || !is_media_entity_v4l2_subdev(sink_pad->entity))
0686             break;
0687 
0688         entity = sink_pad->entity;
0689     }
0690     *format = fmt;
0691 
0692     return 0;
0693 }
0694 
0695 static int dcmi_pipeline_s_stream(struct stm32_dcmi *dcmi, int state)
0696 {
0697     struct media_entity *entity = &dcmi->vdev->entity;
0698     struct v4l2_subdev *subdev;
0699     struct media_pad *pad;
0700     int ret;
0701 
0702     /* Start/stop all entities within pipeline */
0703     while (1) {
0704         pad = &entity->pads[0];
0705         if (!(pad->flags & MEDIA_PAD_FL_SINK))
0706             break;
0707 
0708         pad = media_pad_remote_pad_first(pad);
0709         if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
0710             break;
0711 
0712         entity = pad->entity;
0713         subdev = media_entity_to_v4l2_subdev(entity);
0714 
0715         ret = v4l2_subdev_call(subdev, video, s_stream, state);
0716         if (ret < 0 && ret != -ENOIOCTLCMD) {
0717             dev_err(dcmi->dev, "%s: \"%s\" failed to %s streaming (%d)\n",
0718                 __func__, subdev->name,
0719                 state ? "start" : "stop", ret);
0720             return ret;
0721         }
0722 
0723         dev_dbg(dcmi->dev, "\"%s\" is %s\n",
0724             subdev->name, state ? "started" : "stopped");
0725     }
0726 
0727     return 0;
0728 }
0729 
0730 static int dcmi_pipeline_start(struct stm32_dcmi *dcmi)
0731 {
0732     return dcmi_pipeline_s_stream(dcmi, 1);
0733 }
0734 
0735 static void dcmi_pipeline_stop(struct stm32_dcmi *dcmi)
0736 {
0737     dcmi_pipeline_s_stream(dcmi, 0);
0738 }
0739 
0740 static int dcmi_start_streaming(struct vb2_queue *vq, unsigned int count)
0741 {
0742     struct stm32_dcmi *dcmi = vb2_get_drv_priv(vq);
0743     struct dcmi_buf *buf, *node;
0744     u32 val = 0;
0745     int ret;
0746 
0747     ret = pm_runtime_resume_and_get(dcmi->dev);
0748     if (ret < 0) {
0749         dev_err(dcmi->dev, "%s: Failed to start streaming, cannot get sync (%d)\n",
0750             __func__, ret);
0751         goto err_unlocked;
0752     }
0753 
0754     ret = media_pipeline_start(&dcmi->vdev->entity, &dcmi->pipeline);
0755     if (ret < 0) {
0756         dev_err(dcmi->dev, "%s: Failed to start streaming, media pipeline start error (%d)\n",
0757             __func__, ret);
0758         goto err_pm_put;
0759     }
0760 
0761     ret = dcmi_pipeline_start(dcmi);
0762     if (ret)
0763         goto err_media_pipeline_stop;
0764 
0765     spin_lock_irq(&dcmi->irqlock);
0766 
0767     /* Set bus width */
0768     switch (dcmi->bus.bus_width) {
0769     case 14:
0770         val |= CR_EDM_0 | CR_EDM_1;
0771         break;
0772     case 12:
0773         val |= CR_EDM_1;
0774         break;
0775     case 10:
0776         val |= CR_EDM_0;
0777         break;
0778     default:
0779         /* Set bus width to 8 bits by default */
0780         break;
0781     }
0782 
0783     /* Set vertical synchronization polarity */
0784     if (dcmi->bus.flags & V4L2_MBUS_VSYNC_ACTIVE_HIGH)
0785         val |= CR_VSPOL;
0786 
0787     /* Set horizontal synchronization polarity */
0788     if (dcmi->bus.flags & V4L2_MBUS_HSYNC_ACTIVE_HIGH)
0789         val |= CR_HSPOL;
0790 
0791     /* Set pixel clock polarity */
0792     if (dcmi->bus.flags & V4L2_MBUS_PCLK_SAMPLE_RISING)
0793         val |= CR_PCKPOL;
0794 
0795     /*
0796      * BT656 embedded synchronisation bus mode.
0797      *
0798      * Default SAV/EAV mode is supported here with default codes
0799      * SAV=0xff000080 & EAV=0xff00009d.
0800      * With DCMI this means LSC=SAV=0x80 & LEC=EAV=0x9d.
0801      */
0802     if (dcmi->bus_type == V4L2_MBUS_BT656) {
0803         val |= CR_ESS;
0804 
0805         /* Unmask all codes */
0806         reg_write(dcmi->regs, DCMI_ESUR, 0xffffffff);/* FEC:LEC:LSC:FSC */
0807 
0808         /* Trig on LSC=0x80 & LEC=0x9d codes, ignore FSC and FEC */
0809         reg_write(dcmi->regs, DCMI_ESCR, 0xff9d80ff);/* FEC:LEC:LSC:FSC */
0810     }
0811 
0812     reg_write(dcmi->regs, DCMI_CR, val);
0813 
0814     /* Set crop */
0815     if (dcmi->do_crop)
0816         dcmi_set_crop(dcmi);
0817 
0818     /* Enable jpeg capture */
0819     if (dcmi->sd_format->fourcc == V4L2_PIX_FMT_JPEG)
0820         reg_set(dcmi->regs, DCMI_CR, CR_CM);/* Snapshot mode */
0821 
0822     /* Enable dcmi */
0823     reg_set(dcmi->regs, DCMI_CR, CR_ENABLE);
0824 
0825     dcmi->sequence = 0;
0826     dcmi->errors_count = 0;
0827     dcmi->overrun_count = 0;
0828     dcmi->buffers_count = 0;
0829 
0830     /*
0831      * Start transfer if at least one buffer has been queued,
0832      * otherwise transfer is deferred at buffer queueing
0833      */
0834     if (list_empty(&dcmi->buffers)) {
0835         dev_dbg(dcmi->dev, "Start streaming is deferred to next buffer queueing\n");
0836         dcmi->state = WAIT_FOR_BUFFER;
0837         spin_unlock_irq(&dcmi->irqlock);
0838         return 0;
0839     }
0840 
0841     buf = list_entry(dcmi->buffers.next, struct dcmi_buf, list);
0842     dcmi->active = buf;
0843 
0844     dcmi->state = RUNNING;
0845 
0846     dev_dbg(dcmi->dev, "Start streaming, starting capture\n");
0847 
0848     spin_unlock_irq(&dcmi->irqlock);
0849     ret = dcmi_start_capture(dcmi, buf);
0850     if (ret) {
0851         dev_err(dcmi->dev, "%s: Start streaming failed, cannot start capture\n",
0852             __func__);
0853         goto err_pipeline_stop;
0854     }
0855 
0856     /* Enable interruptions */
0857     if (dcmi->sd_format->fourcc == V4L2_PIX_FMT_JPEG)
0858         reg_set(dcmi->regs, DCMI_IER, IT_FRAME | IT_OVR | IT_ERR);
0859     else
0860         reg_set(dcmi->regs, DCMI_IER, IT_OVR | IT_ERR);
0861 
0862     return 0;
0863 
0864 err_pipeline_stop:
0865     dcmi_pipeline_stop(dcmi);
0866 
0867 err_media_pipeline_stop:
0868     media_pipeline_stop(&dcmi->vdev->entity);
0869 
0870 err_pm_put:
0871     pm_runtime_put(dcmi->dev);
0872 err_unlocked:
0873     spin_lock_irq(&dcmi->irqlock);
0874     /*
0875      * Return all buffers to vb2 in QUEUED state.
0876      * This will give ownership back to userspace
0877      */
0878     list_for_each_entry_safe(buf, node, &dcmi->buffers, list) {
0879         list_del_init(&buf->list);
0880         vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_QUEUED);
0881     }
0882     dcmi->active = NULL;
0883     spin_unlock_irq(&dcmi->irqlock);
0884 
0885     return ret;
0886 }
0887 
0888 static void dcmi_stop_streaming(struct vb2_queue *vq)
0889 {
0890     struct stm32_dcmi *dcmi = vb2_get_drv_priv(vq);
0891     struct dcmi_buf *buf, *node;
0892 
0893     dcmi_pipeline_stop(dcmi);
0894 
0895     media_pipeline_stop(&dcmi->vdev->entity);
0896 
0897     spin_lock_irq(&dcmi->irqlock);
0898 
0899     /* Disable interruptions */
0900     reg_clear(dcmi->regs, DCMI_IER, IT_FRAME | IT_OVR | IT_ERR);
0901 
0902     /* Disable DCMI */
0903     reg_clear(dcmi->regs, DCMI_CR, CR_ENABLE);
0904 
0905     /* Return all queued buffers to vb2 in ERROR state */
0906     list_for_each_entry_safe(buf, node, &dcmi->buffers, list) {
0907         list_del_init(&buf->list);
0908         vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
0909     }
0910 
0911     dcmi->active = NULL;
0912     dcmi->state = STOPPED;
0913 
0914     spin_unlock_irq(&dcmi->irqlock);
0915 
0916     /* Stop all pending DMA operations */
0917     mutex_lock(&dcmi->dma_lock);
0918     dmaengine_terminate_sync(dcmi->dma_chan);
0919     mutex_unlock(&dcmi->dma_lock);
0920 
0921     pm_runtime_put(dcmi->dev);
0922 
0923     if (dcmi->errors_count)
0924         dev_warn(dcmi->dev, "Some errors found while streaming: errors=%d (overrun=%d), buffers=%d\n",
0925              dcmi->errors_count, dcmi->overrun_count,
0926              dcmi->buffers_count);
0927     dev_dbg(dcmi->dev, "Stop streaming, errors=%d (overrun=%d), buffers=%d\n",
0928         dcmi->errors_count, dcmi->overrun_count,
0929         dcmi->buffers_count);
0930 }
0931 
0932 static const struct vb2_ops dcmi_video_qops = {
0933     .queue_setup        = dcmi_queue_setup,
0934     .buf_init       = dcmi_buf_init,
0935     .buf_prepare        = dcmi_buf_prepare,
0936     .buf_queue      = dcmi_buf_queue,
0937     .start_streaming    = dcmi_start_streaming,
0938     .stop_streaming     = dcmi_stop_streaming,
0939     .wait_prepare       = vb2_ops_wait_prepare,
0940     .wait_finish        = vb2_ops_wait_finish,
0941 };
0942 
0943 static int dcmi_g_fmt_vid_cap(struct file *file, void *priv,
0944                   struct v4l2_format *fmt)
0945 {
0946     struct stm32_dcmi *dcmi = video_drvdata(file);
0947 
0948     *fmt = dcmi->fmt;
0949 
0950     return 0;
0951 }
0952 
0953 static const struct dcmi_format *find_format_by_fourcc(struct stm32_dcmi *dcmi,
0954                                unsigned int fourcc)
0955 {
0956     unsigned int num_formats = dcmi->num_of_sd_formats;
0957     const struct dcmi_format *fmt;
0958     unsigned int i;
0959 
0960     for (i = 0; i < num_formats; i++) {
0961         fmt = dcmi->sd_formats[i];
0962         if (fmt->fourcc == fourcc)
0963             return fmt;
0964     }
0965 
0966     return NULL;
0967 }
0968 
0969 static void __find_outer_frame_size(struct stm32_dcmi *dcmi,
0970                     struct v4l2_pix_format *pix,
0971                     struct dcmi_framesize *framesize)
0972 {
0973     struct dcmi_framesize *match = NULL;
0974     unsigned int i;
0975     unsigned int min_err = UINT_MAX;
0976 
0977     for (i = 0; i < dcmi->num_of_sd_framesizes; i++) {
0978         struct dcmi_framesize *fsize = &dcmi->sd_framesizes[i];
0979         int w_err = (fsize->width - pix->width);
0980         int h_err = (fsize->height - pix->height);
0981         int err = w_err + h_err;
0982 
0983         if (w_err >= 0 && h_err >= 0 && err < min_err) {
0984             min_err = err;
0985             match = fsize;
0986         }
0987     }
0988     if (!match)
0989         match = &dcmi->sd_framesizes[0];
0990 
0991     *framesize = *match;
0992 }
0993 
0994 static int dcmi_try_fmt(struct stm32_dcmi *dcmi, struct v4l2_format *f,
0995             const struct dcmi_format **sd_format,
0996             struct dcmi_framesize *sd_framesize)
0997 {
0998     const struct dcmi_format *sd_fmt;
0999     struct dcmi_framesize sd_fsize;
1000     struct v4l2_pix_format *pix = &f->fmt.pix;
1001     struct v4l2_subdev_format format = {
1002         .which = V4L2_SUBDEV_FORMAT_TRY,
1003     };
1004     bool do_crop;
1005     int ret;
1006 
1007     sd_fmt = find_format_by_fourcc(dcmi, pix->pixelformat);
1008     if (!sd_fmt) {
1009         if (!dcmi->num_of_sd_formats)
1010             return -ENODATA;
1011 
1012         sd_fmt = dcmi->sd_formats[dcmi->num_of_sd_formats - 1];
1013         pix->pixelformat = sd_fmt->fourcc;
1014     }
1015 
1016     /* Limit to hardware capabilities */
1017     pix->width = clamp(pix->width, MIN_WIDTH, MAX_WIDTH);
1018     pix->height = clamp(pix->height, MIN_HEIGHT, MAX_HEIGHT);
1019 
1020     /* No crop if JPEG is requested */
1021     do_crop = dcmi->do_crop && (pix->pixelformat != V4L2_PIX_FMT_JPEG);
1022 
1023     if (do_crop && dcmi->num_of_sd_framesizes) {
1024         struct dcmi_framesize outer_sd_fsize;
1025         /*
1026          * If crop is requested and sensor have discrete frame sizes,
1027          * select the frame size that is just larger than request
1028          */
1029         __find_outer_frame_size(dcmi, pix, &outer_sd_fsize);
1030         pix->width = outer_sd_fsize.width;
1031         pix->height = outer_sd_fsize.height;
1032     }
1033 
1034     v4l2_fill_mbus_format(&format.format, pix, sd_fmt->mbus_code);
1035     ret = v4l2_subdev_call_state_try(dcmi->source, pad, set_fmt, &format);
1036     if (ret < 0)
1037         return ret;
1038 
1039     /* Update pix regarding to what sensor can do */
1040     v4l2_fill_pix_format(pix, &format.format);
1041 
1042     /* Save resolution that sensor can actually do */
1043     sd_fsize.width = pix->width;
1044     sd_fsize.height = pix->height;
1045 
1046     if (do_crop) {
1047         struct v4l2_rect c = dcmi->crop;
1048         struct v4l2_rect max_rect;
1049 
1050         /*
1051          * Adjust crop by making the intersection between
1052          * format resolution request and crop request
1053          */
1054         max_rect.top = 0;
1055         max_rect.left = 0;
1056         max_rect.width = pix->width;
1057         max_rect.height = pix->height;
1058         v4l2_rect_map_inside(&c, &max_rect);
1059         c.top  = clamp_t(s32, c.top, 0, pix->height - c.height);
1060         c.left = clamp_t(s32, c.left, 0, pix->width - c.width);
1061         dcmi->crop = c;
1062 
1063         /* Adjust format resolution request to crop */
1064         pix->width = dcmi->crop.width;
1065         pix->height = dcmi->crop.height;
1066     }
1067 
1068     pix->field = V4L2_FIELD_NONE;
1069     pix->bytesperline = pix->width * sd_fmt->bpp;
1070     pix->sizeimage = pix->bytesperline * pix->height;
1071 
1072     if (sd_format)
1073         *sd_format = sd_fmt;
1074     if (sd_framesize)
1075         *sd_framesize = sd_fsize;
1076 
1077     return 0;
1078 }
1079 
1080 static int dcmi_set_fmt(struct stm32_dcmi *dcmi, struct v4l2_format *f)
1081 {
1082     struct v4l2_subdev_format format = {
1083         .which = V4L2_SUBDEV_FORMAT_ACTIVE,
1084     };
1085     const struct dcmi_format *sd_format;
1086     struct dcmi_framesize sd_framesize;
1087     struct v4l2_mbus_framefmt *mf = &format.format;
1088     struct v4l2_pix_format *pix = &f->fmt.pix;
1089     int ret;
1090 
1091     /*
1092      * Try format, fmt.width/height could have been changed
1093      * to match sensor capability or crop request
1094      * sd_format & sd_framesize will contain what subdev
1095      * can do for this request.
1096      */
1097     ret = dcmi_try_fmt(dcmi, f, &sd_format, &sd_framesize);
1098     if (ret)
1099         return ret;
1100 
1101     /* Disable crop if JPEG is requested or BT656 bus is selected */
1102     if (pix->pixelformat == V4L2_PIX_FMT_JPEG &&
1103         dcmi->bus_type != V4L2_MBUS_BT656)
1104         dcmi->do_crop = false;
1105 
1106     /* pix to mbus format */
1107     v4l2_fill_mbus_format(mf, pix,
1108                   sd_format->mbus_code);
1109     mf->width = sd_framesize.width;
1110     mf->height = sd_framesize.height;
1111 
1112     ret = dcmi_pipeline_s_fmt(dcmi, &format);
1113     if (ret < 0)
1114         return ret;
1115 
1116     dev_dbg(dcmi->dev, "Sensor format set to 0x%x %ux%u\n",
1117         mf->code, mf->width, mf->height);
1118     dev_dbg(dcmi->dev, "Buffer format set to %4.4s %ux%u\n",
1119         (char *)&pix->pixelformat,
1120         pix->width, pix->height);
1121 
1122     dcmi->fmt = *f;
1123     dcmi->sd_format = sd_format;
1124     dcmi->sd_framesize = sd_framesize;
1125 
1126     return 0;
1127 }
1128 
1129 static int dcmi_s_fmt_vid_cap(struct file *file, void *priv,
1130                   struct v4l2_format *f)
1131 {
1132     struct stm32_dcmi *dcmi = video_drvdata(file);
1133 
1134     if (vb2_is_streaming(&dcmi->queue))
1135         return -EBUSY;
1136 
1137     return dcmi_set_fmt(dcmi, f);
1138 }
1139 
1140 static int dcmi_try_fmt_vid_cap(struct file *file, void *priv,
1141                 struct v4l2_format *f)
1142 {
1143     struct stm32_dcmi *dcmi = video_drvdata(file);
1144 
1145     return dcmi_try_fmt(dcmi, f, NULL, NULL);
1146 }
1147 
1148 static int dcmi_enum_fmt_vid_cap(struct file *file, void  *priv,
1149                  struct v4l2_fmtdesc *f)
1150 {
1151     struct stm32_dcmi *dcmi = video_drvdata(file);
1152 
1153     if (f->index >= dcmi->num_of_sd_formats)
1154         return -EINVAL;
1155 
1156     f->pixelformat = dcmi->sd_formats[f->index]->fourcc;
1157     return 0;
1158 }
1159 
1160 static int dcmi_get_sensor_format(struct stm32_dcmi *dcmi,
1161                   struct v4l2_pix_format *pix)
1162 {
1163     struct v4l2_subdev_format fmt = {
1164         .which = V4L2_SUBDEV_FORMAT_ACTIVE,
1165     };
1166     int ret;
1167 
1168     ret = v4l2_subdev_call(dcmi->source, pad, get_fmt, NULL, &fmt);
1169     if (ret)
1170         return ret;
1171 
1172     v4l2_fill_pix_format(pix, &fmt.format);
1173 
1174     return 0;
1175 }
1176 
1177 static int dcmi_set_sensor_format(struct stm32_dcmi *dcmi,
1178                   struct v4l2_pix_format *pix)
1179 {
1180     const struct dcmi_format *sd_fmt;
1181     struct v4l2_subdev_format format = {
1182         .which = V4L2_SUBDEV_FORMAT_TRY,
1183     };
1184     int ret;
1185 
1186     sd_fmt = find_format_by_fourcc(dcmi, pix->pixelformat);
1187     if (!sd_fmt) {
1188         if (!dcmi->num_of_sd_formats)
1189             return -ENODATA;
1190 
1191         sd_fmt = dcmi->sd_formats[dcmi->num_of_sd_formats - 1];
1192         pix->pixelformat = sd_fmt->fourcc;
1193     }
1194 
1195     v4l2_fill_mbus_format(&format.format, pix, sd_fmt->mbus_code);
1196     ret = v4l2_subdev_call_state_try(dcmi->source, pad, set_fmt, &format);
1197     if (ret < 0)
1198         return ret;
1199 
1200     return 0;
1201 }
1202 
1203 static int dcmi_get_sensor_bounds(struct stm32_dcmi *dcmi,
1204                   struct v4l2_rect *r)
1205 {
1206     struct v4l2_subdev_selection bounds = {
1207         .which = V4L2_SUBDEV_FORMAT_ACTIVE,
1208         .target = V4L2_SEL_TGT_CROP_BOUNDS,
1209     };
1210     unsigned int max_width, max_height, max_pixsize;
1211     struct v4l2_pix_format pix;
1212     unsigned int i;
1213     int ret;
1214 
1215     /*
1216      * Get sensor bounds first
1217      */
1218     ret = v4l2_subdev_call(dcmi->source, pad, get_selection,
1219                    NULL, &bounds);
1220     if (!ret)
1221         *r = bounds.r;
1222     if (ret != -ENOIOCTLCMD)
1223         return ret;
1224 
1225     /*
1226      * If selection is not implemented,
1227      * fallback by enumerating sensor frame sizes
1228      * and take the largest one
1229      */
1230     max_width = 0;
1231     max_height = 0;
1232     max_pixsize = 0;
1233     for (i = 0; i < dcmi->num_of_sd_framesizes; i++) {
1234         struct dcmi_framesize *fsize = &dcmi->sd_framesizes[i];
1235         unsigned int pixsize = fsize->width * fsize->height;
1236 
1237         if (pixsize > max_pixsize) {
1238             max_pixsize = pixsize;
1239             max_width = fsize->width;
1240             max_height = fsize->height;
1241         }
1242     }
1243     if (max_pixsize > 0) {
1244         r->top = 0;
1245         r->left = 0;
1246         r->width = max_width;
1247         r->height = max_height;
1248         return 0;
1249     }
1250 
1251     /*
1252      * If frame sizes enumeration is not implemented,
1253      * fallback by getting current sensor frame size
1254      */
1255     ret = dcmi_get_sensor_format(dcmi, &pix);
1256     if (ret)
1257         return ret;
1258 
1259     r->top = 0;
1260     r->left = 0;
1261     r->width = pix.width;
1262     r->height = pix.height;
1263 
1264     return 0;
1265 }
1266 
1267 static int dcmi_g_selection(struct file *file, void *fh,
1268                 struct v4l2_selection *s)
1269 {
1270     struct stm32_dcmi *dcmi = video_drvdata(file);
1271 
1272     if (s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
1273         return -EINVAL;
1274 
1275     switch (s->target) {
1276     case V4L2_SEL_TGT_CROP_DEFAULT:
1277     case V4L2_SEL_TGT_CROP_BOUNDS:
1278         s->r = dcmi->sd_bounds;
1279         return 0;
1280     case V4L2_SEL_TGT_CROP:
1281         if (dcmi->do_crop) {
1282             s->r = dcmi->crop;
1283         } else {
1284             s->r.top = 0;
1285             s->r.left = 0;
1286             s->r.width = dcmi->fmt.fmt.pix.width;
1287             s->r.height = dcmi->fmt.fmt.pix.height;
1288         }
1289         break;
1290     default:
1291         return -EINVAL;
1292     }
1293 
1294     return 0;
1295 }
1296 
1297 static int dcmi_s_selection(struct file *file, void *priv,
1298                 struct v4l2_selection *s)
1299 {
1300     struct stm32_dcmi *dcmi = video_drvdata(file);
1301     struct v4l2_rect r = s->r;
1302     struct v4l2_rect max_rect;
1303     struct v4l2_pix_format pix;
1304 
1305     if (s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE ||
1306         s->target != V4L2_SEL_TGT_CROP)
1307         return -EINVAL;
1308 
1309     /* Reset sensor resolution to max resolution */
1310     pix.pixelformat = dcmi->fmt.fmt.pix.pixelformat;
1311     pix.width = dcmi->sd_bounds.width;
1312     pix.height = dcmi->sd_bounds.height;
1313     dcmi_set_sensor_format(dcmi, &pix);
1314 
1315     /*
1316      * Make the intersection between
1317      * sensor resolution
1318      * and crop request
1319      */
1320     max_rect.top = 0;
1321     max_rect.left = 0;
1322     max_rect.width = pix.width;
1323     max_rect.height = pix.height;
1324     v4l2_rect_map_inside(&r, &max_rect);
1325     r.top  = clamp_t(s32, r.top, 0, pix.height - r.height);
1326     r.left = clamp_t(s32, r.left, 0, pix.width - r.width);
1327 
1328     if (!(r.top == dcmi->sd_bounds.top &&
1329           r.left == dcmi->sd_bounds.left &&
1330           r.width == dcmi->sd_bounds.width &&
1331           r.height == dcmi->sd_bounds.height)) {
1332         /* Crop if request is different than sensor resolution */
1333         dcmi->do_crop = true;
1334         dcmi->crop = r;
1335         dev_dbg(dcmi->dev, "s_selection: crop %ux%u@(%u,%u) from %ux%u\n",
1336             r.width, r.height, r.left, r.top,
1337             pix.width, pix.height);
1338     } else {
1339         /* Disable crop */
1340         dcmi->do_crop = false;
1341         dev_dbg(dcmi->dev, "s_selection: crop is disabled\n");
1342     }
1343 
1344     s->r = r;
1345     return 0;
1346 }
1347 
1348 static int dcmi_querycap(struct file *file, void *priv,
1349              struct v4l2_capability *cap)
1350 {
1351     strscpy(cap->driver, DRV_NAME, sizeof(cap->driver));
1352     strscpy(cap->card, "STM32 Camera Memory Interface",
1353         sizeof(cap->card));
1354     strscpy(cap->bus_info, "platform:dcmi", sizeof(cap->bus_info));
1355     return 0;
1356 }
1357 
1358 static int dcmi_enum_input(struct file *file, void *priv,
1359                struct v4l2_input *i)
1360 {
1361     if (i->index != 0)
1362         return -EINVAL;
1363 
1364     i->type = V4L2_INPUT_TYPE_CAMERA;
1365     strscpy(i->name, "Camera", sizeof(i->name));
1366     return 0;
1367 }
1368 
1369 static int dcmi_g_input(struct file *file, void *priv, unsigned int *i)
1370 {
1371     *i = 0;
1372     return 0;
1373 }
1374 
1375 static int dcmi_s_input(struct file *file, void *priv, unsigned int i)
1376 {
1377     if (i > 0)
1378         return -EINVAL;
1379     return 0;
1380 }
1381 
1382 static int dcmi_enum_framesizes(struct file *file, void *fh,
1383                 struct v4l2_frmsizeenum *fsize)
1384 {
1385     struct stm32_dcmi *dcmi = video_drvdata(file);
1386     const struct dcmi_format *sd_fmt;
1387     struct v4l2_subdev_frame_size_enum fse = {
1388         .index = fsize->index,
1389         .which = V4L2_SUBDEV_FORMAT_ACTIVE,
1390     };
1391     int ret;
1392 
1393     sd_fmt = find_format_by_fourcc(dcmi, fsize->pixel_format);
1394     if (!sd_fmt)
1395         return -EINVAL;
1396 
1397     fse.code = sd_fmt->mbus_code;
1398 
1399     ret = v4l2_subdev_call(dcmi->source, pad, enum_frame_size,
1400                    NULL, &fse);
1401     if (ret)
1402         return ret;
1403 
1404     fsize->type = V4L2_FRMSIZE_TYPE_DISCRETE;
1405     fsize->discrete.width = fse.max_width;
1406     fsize->discrete.height = fse.max_height;
1407 
1408     return 0;
1409 }
1410 
1411 static int dcmi_g_parm(struct file *file, void *priv,
1412                struct v4l2_streamparm *p)
1413 {
1414     struct stm32_dcmi *dcmi = video_drvdata(file);
1415 
1416     return v4l2_g_parm_cap(video_devdata(file), dcmi->source, p);
1417 }
1418 
1419 static int dcmi_s_parm(struct file *file, void *priv,
1420                struct v4l2_streamparm *p)
1421 {
1422     struct stm32_dcmi *dcmi = video_drvdata(file);
1423 
1424     return v4l2_s_parm_cap(video_devdata(file), dcmi->source, p);
1425 }
1426 
1427 static int dcmi_enum_frameintervals(struct file *file, void *fh,
1428                     struct v4l2_frmivalenum *fival)
1429 {
1430     struct stm32_dcmi *dcmi = video_drvdata(file);
1431     const struct dcmi_format *sd_fmt;
1432     struct v4l2_subdev_frame_interval_enum fie = {
1433         .index = fival->index,
1434         .width = fival->width,
1435         .height = fival->height,
1436         .which = V4L2_SUBDEV_FORMAT_ACTIVE,
1437     };
1438     int ret;
1439 
1440     sd_fmt = find_format_by_fourcc(dcmi, fival->pixel_format);
1441     if (!sd_fmt)
1442         return -EINVAL;
1443 
1444     fie.code = sd_fmt->mbus_code;
1445 
1446     ret = v4l2_subdev_call(dcmi->source, pad,
1447                    enum_frame_interval, NULL, &fie);
1448     if (ret)
1449         return ret;
1450 
1451     fival->type = V4L2_FRMIVAL_TYPE_DISCRETE;
1452     fival->discrete = fie.interval;
1453 
1454     return 0;
1455 }
1456 
1457 static const struct of_device_id stm32_dcmi_of_match[] = {
1458     { .compatible = "st,stm32-dcmi"},
1459     { /* end node */ },
1460 };
1461 MODULE_DEVICE_TABLE(of, stm32_dcmi_of_match);
1462 
1463 static int dcmi_open(struct file *file)
1464 {
1465     struct stm32_dcmi *dcmi = video_drvdata(file);
1466     struct v4l2_subdev *sd = dcmi->source;
1467     int ret;
1468 
1469     if (mutex_lock_interruptible(&dcmi->lock))
1470         return -ERESTARTSYS;
1471 
1472     ret = v4l2_fh_open(file);
1473     if (ret < 0)
1474         goto unlock;
1475 
1476     if (!v4l2_fh_is_singular_file(file))
1477         goto fh_rel;
1478 
1479     ret = v4l2_subdev_call(sd, core, s_power, 1);
1480     if (ret < 0 && ret != -ENOIOCTLCMD)
1481         goto fh_rel;
1482 
1483     ret = dcmi_set_fmt(dcmi, &dcmi->fmt);
1484     if (ret)
1485         v4l2_subdev_call(sd, core, s_power, 0);
1486 fh_rel:
1487     if (ret)
1488         v4l2_fh_release(file);
1489 unlock:
1490     mutex_unlock(&dcmi->lock);
1491     return ret;
1492 }
1493 
1494 static int dcmi_release(struct file *file)
1495 {
1496     struct stm32_dcmi *dcmi = video_drvdata(file);
1497     struct v4l2_subdev *sd = dcmi->source;
1498     bool fh_singular;
1499     int ret;
1500 
1501     mutex_lock(&dcmi->lock);
1502 
1503     fh_singular = v4l2_fh_is_singular_file(file);
1504 
1505     ret = _vb2_fop_release(file, NULL);
1506 
1507     if (fh_singular)
1508         v4l2_subdev_call(sd, core, s_power, 0);
1509 
1510     mutex_unlock(&dcmi->lock);
1511 
1512     return ret;
1513 }
1514 
1515 static const struct v4l2_ioctl_ops dcmi_ioctl_ops = {
1516     .vidioc_querycap        = dcmi_querycap,
1517 
1518     .vidioc_try_fmt_vid_cap     = dcmi_try_fmt_vid_cap,
1519     .vidioc_g_fmt_vid_cap       = dcmi_g_fmt_vid_cap,
1520     .vidioc_s_fmt_vid_cap       = dcmi_s_fmt_vid_cap,
1521     .vidioc_enum_fmt_vid_cap    = dcmi_enum_fmt_vid_cap,
1522     .vidioc_g_selection     = dcmi_g_selection,
1523     .vidioc_s_selection     = dcmi_s_selection,
1524 
1525     .vidioc_enum_input      = dcmi_enum_input,
1526     .vidioc_g_input         = dcmi_g_input,
1527     .vidioc_s_input         = dcmi_s_input,
1528 
1529     .vidioc_g_parm          = dcmi_g_parm,
1530     .vidioc_s_parm          = dcmi_s_parm,
1531 
1532     .vidioc_enum_framesizes     = dcmi_enum_framesizes,
1533     .vidioc_enum_frameintervals = dcmi_enum_frameintervals,
1534 
1535     .vidioc_reqbufs         = vb2_ioctl_reqbufs,
1536     .vidioc_create_bufs     = vb2_ioctl_create_bufs,
1537     .vidioc_querybuf        = vb2_ioctl_querybuf,
1538     .vidioc_qbuf            = vb2_ioctl_qbuf,
1539     .vidioc_dqbuf           = vb2_ioctl_dqbuf,
1540     .vidioc_expbuf          = vb2_ioctl_expbuf,
1541     .vidioc_prepare_buf     = vb2_ioctl_prepare_buf,
1542     .vidioc_streamon        = vb2_ioctl_streamon,
1543     .vidioc_streamoff       = vb2_ioctl_streamoff,
1544 
1545     .vidioc_log_status      = v4l2_ctrl_log_status,
1546     .vidioc_subscribe_event     = v4l2_ctrl_subscribe_event,
1547     .vidioc_unsubscribe_event   = v4l2_event_unsubscribe,
1548 };
1549 
1550 static const struct v4l2_file_operations dcmi_fops = {
1551     .owner      = THIS_MODULE,
1552     .unlocked_ioctl = video_ioctl2,
1553     .open       = dcmi_open,
1554     .release    = dcmi_release,
1555     .poll       = vb2_fop_poll,
1556     .mmap       = vb2_fop_mmap,
1557 #ifndef CONFIG_MMU
1558     .get_unmapped_area = vb2_fop_get_unmapped_area,
1559 #endif
1560     .read       = vb2_fop_read,
1561 };
1562 
1563 static int dcmi_set_default_fmt(struct stm32_dcmi *dcmi)
1564 {
1565     struct v4l2_format f = {
1566         .type = V4L2_BUF_TYPE_VIDEO_CAPTURE,
1567         .fmt.pix = {
1568             .width      = CIF_WIDTH,
1569             .height     = CIF_HEIGHT,
1570             .field      = V4L2_FIELD_NONE,
1571             .pixelformat    = dcmi->sd_formats[0]->fourcc,
1572         },
1573     };
1574     int ret;
1575 
1576     ret = dcmi_try_fmt(dcmi, &f, NULL, NULL);
1577     if (ret)
1578         return ret;
1579     dcmi->sd_format = dcmi->sd_formats[0];
1580     dcmi->fmt = f;
1581     return 0;
1582 }
1583 
1584 static const struct dcmi_format dcmi_formats[] = {
1585     {
1586         .fourcc = V4L2_PIX_FMT_RGB565,
1587         .mbus_code = MEDIA_BUS_FMT_RGB565_2X8_LE,
1588         .bpp = 2,
1589     }, {
1590         .fourcc = V4L2_PIX_FMT_RGB565,
1591         .mbus_code = MEDIA_BUS_FMT_RGB565_1X16,
1592         .bpp = 2,
1593     }, {
1594         .fourcc = V4L2_PIX_FMT_YUYV,
1595         .mbus_code = MEDIA_BUS_FMT_YUYV8_2X8,
1596         .bpp = 2,
1597     }, {
1598         .fourcc = V4L2_PIX_FMT_YUYV,
1599         .mbus_code = MEDIA_BUS_FMT_YUYV8_1X16,
1600         .bpp = 2,
1601     }, {
1602         .fourcc = V4L2_PIX_FMT_UYVY,
1603         .mbus_code = MEDIA_BUS_FMT_UYVY8_2X8,
1604         .bpp = 2,
1605     }, {
1606         .fourcc = V4L2_PIX_FMT_UYVY,
1607         .mbus_code = MEDIA_BUS_FMT_UYVY8_1X16,
1608         .bpp = 2,
1609     }, {
1610         .fourcc = V4L2_PIX_FMT_JPEG,
1611         .mbus_code = MEDIA_BUS_FMT_JPEG_1X8,
1612         .bpp = 1,
1613     }, {
1614         .fourcc = V4L2_PIX_FMT_SBGGR8,
1615         .mbus_code = MEDIA_BUS_FMT_SBGGR8_1X8,
1616         .bpp = 1,
1617     }, {
1618         .fourcc = V4L2_PIX_FMT_SGBRG8,
1619         .mbus_code = MEDIA_BUS_FMT_SGBRG8_1X8,
1620         .bpp = 1,
1621     }, {
1622         .fourcc = V4L2_PIX_FMT_SGRBG8,
1623         .mbus_code = MEDIA_BUS_FMT_SGRBG8_1X8,
1624         .bpp = 1,
1625     }, {
1626         .fourcc = V4L2_PIX_FMT_SRGGB8,
1627         .mbus_code = MEDIA_BUS_FMT_SRGGB8_1X8,
1628         .bpp = 1,
1629     }, {
1630         .fourcc = V4L2_PIX_FMT_SBGGR10,
1631         .mbus_code = MEDIA_BUS_FMT_SBGGR10_1X10,
1632         .bpp = 2,
1633     }, {
1634         .fourcc = V4L2_PIX_FMT_SGBRG10,
1635         .mbus_code = MEDIA_BUS_FMT_SGBRG10_1X10,
1636         .bpp = 2,
1637     }, {
1638         .fourcc = V4L2_PIX_FMT_SGRBG10,
1639         .mbus_code = MEDIA_BUS_FMT_SGRBG10_1X10,
1640         .bpp = 2,
1641     }, {
1642         .fourcc = V4L2_PIX_FMT_SRGGB10,
1643         .mbus_code = MEDIA_BUS_FMT_SRGGB10_1X10,
1644         .bpp = 2,
1645     }, {
1646         .fourcc = V4L2_PIX_FMT_SBGGR12,
1647         .mbus_code = MEDIA_BUS_FMT_SBGGR12_1X12,
1648         .bpp = 2,
1649     }, {
1650         .fourcc = V4L2_PIX_FMT_SGBRG12,
1651         .mbus_code = MEDIA_BUS_FMT_SGBRG12_1X12,
1652         .bpp = 2,
1653     }, {
1654         .fourcc = V4L2_PIX_FMT_SGRBG12,
1655         .mbus_code = MEDIA_BUS_FMT_SGRBG12_1X12,
1656         .bpp = 2,
1657     }, {
1658         .fourcc = V4L2_PIX_FMT_SRGGB12,
1659         .mbus_code = MEDIA_BUS_FMT_SRGGB12_1X12,
1660         .bpp = 2,
1661     }, {
1662         .fourcc = V4L2_PIX_FMT_SBGGR14,
1663         .mbus_code = MEDIA_BUS_FMT_SBGGR14_1X14,
1664         .bpp = 2,
1665     }, {
1666         .fourcc = V4L2_PIX_FMT_SGBRG14,
1667         .mbus_code = MEDIA_BUS_FMT_SGBRG14_1X14,
1668         .bpp = 2,
1669     }, {
1670         .fourcc = V4L2_PIX_FMT_SGRBG14,
1671         .mbus_code = MEDIA_BUS_FMT_SGRBG14_1X14,
1672         .bpp = 2,
1673     }, {
1674         .fourcc = V4L2_PIX_FMT_SRGGB14,
1675         .mbus_code = MEDIA_BUS_FMT_SRGGB14_1X14,
1676         .bpp = 2,
1677     },
1678 };
1679 
1680 static int dcmi_formats_init(struct stm32_dcmi *dcmi)
1681 {
1682     const struct dcmi_format *sd_fmts[ARRAY_SIZE(dcmi_formats)];
1683     unsigned int num_fmts = 0, i, j;
1684     struct v4l2_subdev *subdev = dcmi->source;
1685     struct v4l2_subdev_mbus_code_enum mbus_code = {
1686         .which = V4L2_SUBDEV_FORMAT_ACTIVE,
1687     };
1688 
1689     while (!v4l2_subdev_call(subdev, pad, enum_mbus_code,
1690                  NULL, &mbus_code)) {
1691         for (i = 0; i < ARRAY_SIZE(dcmi_formats); i++) {
1692             if (dcmi_formats[i].mbus_code != mbus_code.code)
1693                 continue;
1694 
1695             /* Exclude JPEG if BT656 bus is selected */
1696             if (dcmi_formats[i].fourcc == V4L2_PIX_FMT_JPEG &&
1697                 dcmi->bus_type == V4L2_MBUS_BT656)
1698                 continue;
1699 
1700             /* Code supported, have we got this fourcc yet? */
1701             for (j = 0; j < num_fmts; j++)
1702                 if (sd_fmts[j]->fourcc ==
1703                         dcmi_formats[i].fourcc) {
1704                     /* Already available */
1705                     dev_dbg(dcmi->dev, "Skipping fourcc/code: %4.4s/0x%x\n",
1706                         (char *)&sd_fmts[j]->fourcc,
1707                         mbus_code.code);
1708                     break;
1709                 }
1710             if (j == num_fmts) {
1711                 /* New */
1712                 sd_fmts[num_fmts++] = dcmi_formats + i;
1713                 dev_dbg(dcmi->dev, "Supported fourcc/code: %4.4s/0x%x\n",
1714                     (char *)&sd_fmts[num_fmts - 1]->fourcc,
1715                     sd_fmts[num_fmts - 1]->mbus_code);
1716             }
1717         }
1718         mbus_code.index++;
1719     }
1720 
1721     if (!num_fmts)
1722         return -ENXIO;
1723 
1724     dcmi->num_of_sd_formats = num_fmts;
1725     dcmi->sd_formats = devm_kcalloc(dcmi->dev,
1726                     num_fmts, sizeof(struct dcmi_format *),
1727                     GFP_KERNEL);
1728     if (!dcmi->sd_formats) {
1729         dev_err(dcmi->dev, "Could not allocate memory\n");
1730         return -ENOMEM;
1731     }
1732 
1733     memcpy(dcmi->sd_formats, sd_fmts,
1734            num_fmts * sizeof(struct dcmi_format *));
1735     dcmi->sd_format = dcmi->sd_formats[0];
1736 
1737     return 0;
1738 }
1739 
1740 static int dcmi_framesizes_init(struct stm32_dcmi *dcmi)
1741 {
1742     unsigned int num_fsize = 0;
1743     struct v4l2_subdev *subdev = dcmi->source;
1744     struct v4l2_subdev_frame_size_enum fse = {
1745         .which = V4L2_SUBDEV_FORMAT_ACTIVE,
1746         .code = dcmi->sd_format->mbus_code,
1747     };
1748     unsigned int ret;
1749     unsigned int i;
1750 
1751     /* Allocate discrete framesizes array */
1752     while (!v4l2_subdev_call(subdev, pad, enum_frame_size,
1753                  NULL, &fse))
1754         fse.index++;
1755 
1756     num_fsize = fse.index;
1757     if (!num_fsize)
1758         return 0;
1759 
1760     dcmi->num_of_sd_framesizes = num_fsize;
1761     dcmi->sd_framesizes = devm_kcalloc(dcmi->dev, num_fsize,
1762                        sizeof(struct dcmi_framesize),
1763                        GFP_KERNEL);
1764     if (!dcmi->sd_framesizes) {
1765         dev_err(dcmi->dev, "Could not allocate memory\n");
1766         return -ENOMEM;
1767     }
1768 
1769     /* Fill array with sensor supported framesizes */
1770     dev_dbg(dcmi->dev, "Sensor supports %u frame sizes:\n", num_fsize);
1771     for (i = 0; i < dcmi->num_of_sd_framesizes; i++) {
1772         fse.index = i;
1773         ret = v4l2_subdev_call(subdev, pad, enum_frame_size,
1774                        NULL, &fse);
1775         if (ret)
1776             return ret;
1777         dcmi->sd_framesizes[fse.index].width = fse.max_width;
1778         dcmi->sd_framesizes[fse.index].height = fse.max_height;
1779         dev_dbg(dcmi->dev, "%ux%u\n", fse.max_width, fse.max_height);
1780     }
1781 
1782     return 0;
1783 }
1784 
1785 static int dcmi_graph_notify_complete(struct v4l2_async_notifier *notifier)
1786 {
1787     struct stm32_dcmi *dcmi = notifier_to_dcmi(notifier);
1788     int ret;
1789 
1790     /*
1791      * Now that the graph is complete,
1792      * we search for the source subdevice
1793      * in order to expose it through V4L2 interface
1794      */
1795     dcmi->source = media_entity_to_v4l2_subdev(dcmi_find_source(dcmi));
1796     if (!dcmi->source) {
1797         dev_err(dcmi->dev, "Source subdevice not found\n");
1798         return -ENODEV;
1799     }
1800 
1801     dcmi->vdev->ctrl_handler = dcmi->source->ctrl_handler;
1802 
1803     ret = dcmi_formats_init(dcmi);
1804     if (ret) {
1805         dev_err(dcmi->dev, "No supported mediabus format found\n");
1806         return ret;
1807     }
1808 
1809     ret = dcmi_framesizes_init(dcmi);
1810     if (ret) {
1811         dev_err(dcmi->dev, "Could not initialize framesizes\n");
1812         return ret;
1813     }
1814 
1815     ret = dcmi_get_sensor_bounds(dcmi, &dcmi->sd_bounds);
1816     if (ret) {
1817         dev_err(dcmi->dev, "Could not get sensor bounds\n");
1818         return ret;
1819     }
1820 
1821     ret = dcmi_set_default_fmt(dcmi);
1822     if (ret) {
1823         dev_err(dcmi->dev, "Could not set default format\n");
1824         return ret;
1825     }
1826 
1827     ret = devm_request_threaded_irq(dcmi->dev, dcmi->irq, dcmi_irq_callback,
1828                     dcmi_irq_thread, IRQF_ONESHOT,
1829                     dev_name(dcmi->dev), dcmi);
1830     if (ret) {
1831         dev_err(dcmi->dev, "Unable to request irq %d\n", dcmi->irq);
1832         return ret;
1833     }
1834 
1835     return 0;
1836 }
1837 
1838 static void dcmi_graph_notify_unbind(struct v4l2_async_notifier *notifier,
1839                      struct v4l2_subdev *sd,
1840                      struct v4l2_async_subdev *asd)
1841 {
1842     struct stm32_dcmi *dcmi = notifier_to_dcmi(notifier);
1843 
1844     dev_dbg(dcmi->dev, "Removing %s\n", video_device_node_name(dcmi->vdev));
1845 
1846     /* Checks internally if vdev has been init or not */
1847     video_unregister_device(dcmi->vdev);
1848 }
1849 
1850 static int dcmi_graph_notify_bound(struct v4l2_async_notifier *notifier,
1851                    struct v4l2_subdev *subdev,
1852                    struct v4l2_async_subdev *asd)
1853 {
1854     struct stm32_dcmi *dcmi = notifier_to_dcmi(notifier);
1855     unsigned int ret;
1856     int src_pad;
1857 
1858     dev_dbg(dcmi->dev, "Subdev \"%s\" bound\n", subdev->name);
1859 
1860     /*
1861      * Link this sub-device to DCMI, it could be
1862      * a parallel camera sensor or a bridge
1863      */
1864     src_pad = media_entity_get_fwnode_pad(&subdev->entity,
1865                           subdev->fwnode,
1866                           MEDIA_PAD_FL_SOURCE);
1867 
1868     ret = media_create_pad_link(&subdev->entity, src_pad,
1869                     &dcmi->vdev->entity, 0,
1870                     MEDIA_LNK_FL_IMMUTABLE |
1871                     MEDIA_LNK_FL_ENABLED);
1872     if (ret)
1873         dev_err(dcmi->dev, "Failed to create media pad link with subdev \"%s\"\n",
1874             subdev->name);
1875     else
1876         dev_dbg(dcmi->dev, "DCMI is now linked to \"%s\"\n",
1877             subdev->name);
1878 
1879     return ret;
1880 }
1881 
1882 static const struct v4l2_async_notifier_operations dcmi_graph_notify_ops = {
1883     .bound = dcmi_graph_notify_bound,
1884     .unbind = dcmi_graph_notify_unbind,
1885     .complete = dcmi_graph_notify_complete,
1886 };
1887 
1888 static int dcmi_graph_init(struct stm32_dcmi *dcmi)
1889 {
1890     struct v4l2_async_subdev *asd;
1891     struct device_node *ep;
1892     int ret;
1893 
1894     ep = of_graph_get_next_endpoint(dcmi->dev->of_node, NULL);
1895     if (!ep) {
1896         dev_err(dcmi->dev, "Failed to get next endpoint\n");
1897         return -EINVAL;
1898     }
1899 
1900     v4l2_async_nf_init(&dcmi->notifier);
1901 
1902     asd = v4l2_async_nf_add_fwnode_remote(&dcmi->notifier,
1903                           of_fwnode_handle(ep),
1904                           struct v4l2_async_subdev);
1905 
1906     of_node_put(ep);
1907 
1908     if (IS_ERR(asd)) {
1909         dev_err(dcmi->dev, "Failed to add subdev notifier\n");
1910         return PTR_ERR(asd);
1911     }
1912 
1913     dcmi->notifier.ops = &dcmi_graph_notify_ops;
1914 
1915     ret = v4l2_async_nf_register(&dcmi->v4l2_dev, &dcmi->notifier);
1916     if (ret < 0) {
1917         dev_err(dcmi->dev, "Failed to register notifier\n");
1918         v4l2_async_nf_cleanup(&dcmi->notifier);
1919         return ret;
1920     }
1921 
1922     return 0;
1923 }
1924 
1925 static int dcmi_probe(struct platform_device *pdev)
1926 {
1927     struct device_node *np = pdev->dev.of_node;
1928     const struct of_device_id *match = NULL;
1929     struct v4l2_fwnode_endpoint ep = { .bus_type = 0 };
1930     struct stm32_dcmi *dcmi;
1931     struct vb2_queue *q;
1932     struct dma_chan *chan;
1933     struct dma_slave_caps caps;
1934     struct clk *mclk;
1935     int irq;
1936     int ret = 0;
1937 
1938     match = of_match_device(of_match_ptr(stm32_dcmi_of_match), &pdev->dev);
1939     if (!match) {
1940         dev_err(&pdev->dev, "Could not find a match in devicetree\n");
1941         return -ENODEV;
1942     }
1943 
1944     dcmi = devm_kzalloc(&pdev->dev, sizeof(struct stm32_dcmi), GFP_KERNEL);
1945     if (!dcmi)
1946         return -ENOMEM;
1947 
1948     dcmi->rstc = devm_reset_control_get_exclusive(&pdev->dev, NULL);
1949     if (IS_ERR(dcmi->rstc)) {
1950         if (PTR_ERR(dcmi->rstc) != -EPROBE_DEFER)
1951             dev_err(&pdev->dev, "Could not get reset control\n");
1952 
1953         return PTR_ERR(dcmi->rstc);
1954     }
1955 
1956     /* Get bus characteristics from devicetree */
1957     np = of_graph_get_next_endpoint(np, NULL);
1958     if (!np) {
1959         dev_err(&pdev->dev, "Could not find the endpoint\n");
1960         return -ENODEV;
1961     }
1962 
1963     ret = v4l2_fwnode_endpoint_parse(of_fwnode_handle(np), &ep);
1964     of_node_put(np);
1965     if (ret) {
1966         dev_err(&pdev->dev, "Could not parse the endpoint\n");
1967         return ret;
1968     }
1969 
1970     if (ep.bus_type == V4L2_MBUS_CSI2_DPHY) {
1971         dev_err(&pdev->dev, "CSI bus not supported\n");
1972         return -ENODEV;
1973     }
1974 
1975     if (ep.bus_type == V4L2_MBUS_BT656 &&
1976         ep.bus.parallel.bus_width != 8) {
1977         dev_err(&pdev->dev, "BT656 bus conflicts with %u bits bus width (8 bits required)\n",
1978             ep.bus.parallel.bus_width);
1979         return -ENODEV;
1980     }
1981 
1982     dcmi->bus.flags = ep.bus.parallel.flags;
1983     dcmi->bus.bus_width = ep.bus.parallel.bus_width;
1984     dcmi->bus.data_shift = ep.bus.parallel.data_shift;
1985     dcmi->bus_type = ep.bus_type;
1986 
1987     irq = platform_get_irq(pdev, 0);
1988     if (irq <= 0)
1989         return irq ? irq : -ENXIO;
1990 
1991     dcmi->irq = irq;
1992 
1993     dcmi->res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
1994     if (!dcmi->res) {
1995         dev_err(&pdev->dev, "Could not get resource\n");
1996         return -ENODEV;
1997     }
1998 
1999     dcmi->regs = devm_ioremap_resource(&pdev->dev, dcmi->res);
2000     if (IS_ERR(dcmi->regs)) {
2001         dev_err(&pdev->dev, "Could not map registers\n");
2002         return PTR_ERR(dcmi->regs);
2003     }
2004 
2005     mclk = devm_clk_get(&pdev->dev, "mclk");
2006     if (IS_ERR(mclk)) {
2007         if (PTR_ERR(mclk) != -EPROBE_DEFER)
2008             dev_err(&pdev->dev, "Unable to get mclk\n");
2009         return PTR_ERR(mclk);
2010     }
2011 
2012     chan = dma_request_chan(&pdev->dev, "tx");
2013     if (IS_ERR(chan)) {
2014         ret = PTR_ERR(chan);
2015         if (ret != -EPROBE_DEFER)
2016             dev_err(&pdev->dev,
2017                 "Failed to request DMA channel: %d\n", ret);
2018         return ret;
2019     }
2020 
2021     dcmi->dma_max_burst = UINT_MAX;
2022     ret = dma_get_slave_caps(chan, &caps);
2023     if (!ret && caps.max_sg_burst)
2024         dcmi->dma_max_burst = caps.max_sg_burst * DMA_SLAVE_BUSWIDTH_4_BYTES;
2025 
2026     spin_lock_init(&dcmi->irqlock);
2027     mutex_init(&dcmi->lock);
2028     mutex_init(&dcmi->dma_lock);
2029     init_completion(&dcmi->complete);
2030     INIT_LIST_HEAD(&dcmi->buffers);
2031 
2032     dcmi->dev = &pdev->dev;
2033     dcmi->mclk = mclk;
2034     dcmi->state = STOPPED;
2035     dcmi->dma_chan = chan;
2036 
2037     q = &dcmi->queue;
2038 
2039     dcmi->v4l2_dev.mdev = &dcmi->mdev;
2040 
2041     /* Initialize media device */
2042     strscpy(dcmi->mdev.model, DRV_NAME, sizeof(dcmi->mdev.model));
2043     dcmi->mdev.dev = &pdev->dev;
2044     media_device_init(&dcmi->mdev);
2045 
2046     /* Initialize the top-level structure */
2047     ret = v4l2_device_register(&pdev->dev, &dcmi->v4l2_dev);
2048     if (ret)
2049         goto err_media_device_cleanup;
2050 
2051     dcmi->vdev = video_device_alloc();
2052     if (!dcmi->vdev) {
2053         ret = -ENOMEM;
2054         goto err_device_unregister;
2055     }
2056 
2057     /* Video node */
2058     dcmi->vdev->fops = &dcmi_fops;
2059     dcmi->vdev->v4l2_dev = &dcmi->v4l2_dev;
2060     dcmi->vdev->queue = &dcmi->queue;
2061     strscpy(dcmi->vdev->name, KBUILD_MODNAME, sizeof(dcmi->vdev->name));
2062     dcmi->vdev->release = video_device_release;
2063     dcmi->vdev->ioctl_ops = &dcmi_ioctl_ops;
2064     dcmi->vdev->lock = &dcmi->lock;
2065     dcmi->vdev->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING |
2066                   V4L2_CAP_READWRITE;
2067     video_set_drvdata(dcmi->vdev, dcmi);
2068 
2069     /* Media entity pads */
2070     dcmi->vid_cap_pad.flags = MEDIA_PAD_FL_SINK;
2071     ret = media_entity_pads_init(&dcmi->vdev->entity,
2072                      1, &dcmi->vid_cap_pad);
2073     if (ret) {
2074         dev_err(dcmi->dev, "Failed to init media entity pad\n");
2075         goto err_device_release;
2076     }
2077     dcmi->vdev->entity.flags |= MEDIA_ENT_FL_DEFAULT;
2078 
2079     ret = video_register_device(dcmi->vdev, VFL_TYPE_VIDEO, -1);
2080     if (ret) {
2081         dev_err(dcmi->dev, "Failed to register video device\n");
2082         goto err_media_entity_cleanup;
2083     }
2084 
2085     dev_dbg(dcmi->dev, "Device registered as %s\n",
2086         video_device_node_name(dcmi->vdev));
2087 
2088     /* Buffer queue */
2089     q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
2090     q->io_modes = VB2_MMAP | VB2_READ | VB2_DMABUF;
2091     q->lock = &dcmi->lock;
2092     q->drv_priv = dcmi;
2093     q->buf_struct_size = sizeof(struct dcmi_buf);
2094     q->ops = &dcmi_video_qops;
2095     q->mem_ops = &vb2_dma_contig_memops;
2096     q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
2097     q->min_buffers_needed = 2;
2098     q->dev = &pdev->dev;
2099 
2100     ret = vb2_queue_init(q);
2101     if (ret < 0) {
2102         dev_err(&pdev->dev, "Failed to initialize vb2 queue\n");
2103         goto err_media_entity_cleanup;
2104     }
2105 
2106     ret = dcmi_graph_init(dcmi);
2107     if (ret < 0)
2108         goto err_media_entity_cleanup;
2109 
2110     /* Reset device */
2111     ret = reset_control_assert(dcmi->rstc);
2112     if (ret) {
2113         dev_err(&pdev->dev, "Failed to assert the reset line\n");
2114         goto err_cleanup;
2115     }
2116 
2117     usleep_range(3000, 5000);
2118 
2119     ret = reset_control_deassert(dcmi->rstc);
2120     if (ret) {
2121         dev_err(&pdev->dev, "Failed to deassert the reset line\n");
2122         goto err_cleanup;
2123     }
2124 
2125     dev_info(&pdev->dev, "Probe done\n");
2126 
2127     platform_set_drvdata(pdev, dcmi);
2128 
2129     pm_runtime_enable(&pdev->dev);
2130 
2131     return 0;
2132 
2133 err_cleanup:
2134     v4l2_async_nf_cleanup(&dcmi->notifier);
2135 err_media_entity_cleanup:
2136     media_entity_cleanup(&dcmi->vdev->entity);
2137 err_device_release:
2138     video_device_release(dcmi->vdev);
2139 err_device_unregister:
2140     v4l2_device_unregister(&dcmi->v4l2_dev);
2141 err_media_device_cleanup:
2142     media_device_cleanup(&dcmi->mdev);
2143     dma_release_channel(dcmi->dma_chan);
2144 
2145     return ret;
2146 }
2147 
2148 static int dcmi_remove(struct platform_device *pdev)
2149 {
2150     struct stm32_dcmi *dcmi = platform_get_drvdata(pdev);
2151 
2152     pm_runtime_disable(&pdev->dev);
2153 
2154     v4l2_async_nf_unregister(&dcmi->notifier);
2155     v4l2_async_nf_cleanup(&dcmi->notifier);
2156     media_entity_cleanup(&dcmi->vdev->entity);
2157     v4l2_device_unregister(&dcmi->v4l2_dev);
2158     media_device_cleanup(&dcmi->mdev);
2159 
2160     dma_release_channel(dcmi->dma_chan);
2161 
2162     return 0;
2163 }
2164 
2165 static __maybe_unused int dcmi_runtime_suspend(struct device *dev)
2166 {
2167     struct stm32_dcmi *dcmi = dev_get_drvdata(dev);
2168 
2169     clk_disable_unprepare(dcmi->mclk);
2170 
2171     return 0;
2172 }
2173 
2174 static __maybe_unused int dcmi_runtime_resume(struct device *dev)
2175 {
2176     struct stm32_dcmi *dcmi = dev_get_drvdata(dev);
2177     int ret;
2178 
2179     ret = clk_prepare_enable(dcmi->mclk);
2180     if (ret)
2181         dev_err(dev, "%s: Failed to prepare_enable clock\n", __func__);
2182 
2183     return ret;
2184 }
2185 
2186 static __maybe_unused int dcmi_suspend(struct device *dev)
2187 {
2188     /* disable clock */
2189     pm_runtime_force_suspend(dev);
2190 
2191     /* change pinctrl state */
2192     pinctrl_pm_select_sleep_state(dev);
2193 
2194     return 0;
2195 }
2196 
2197 static __maybe_unused int dcmi_resume(struct device *dev)
2198 {
2199     /* restore pinctl default state */
2200     pinctrl_pm_select_default_state(dev);
2201 
2202     /* clock enable */
2203     pm_runtime_force_resume(dev);
2204 
2205     return 0;
2206 }
2207 
2208 static const struct dev_pm_ops dcmi_pm_ops = {
2209     SET_SYSTEM_SLEEP_PM_OPS(dcmi_suspend, dcmi_resume)
2210     SET_RUNTIME_PM_OPS(dcmi_runtime_suspend,
2211                dcmi_runtime_resume, NULL)
2212 };
2213 
2214 static struct platform_driver stm32_dcmi_driver = {
2215     .probe      = dcmi_probe,
2216     .remove     = dcmi_remove,
2217     .driver     = {
2218         .name = DRV_NAME,
2219         .of_match_table = of_match_ptr(stm32_dcmi_of_match),
2220         .pm = &dcmi_pm_ops,
2221     },
2222 };
2223 
2224 module_platform_driver(stm32_dcmi_driver);
2225 
2226 MODULE_AUTHOR("Yannick Fertre <yannick.fertre@st.com>");
2227 MODULE_AUTHOR("Hugues Fruchet <hugues.fruchet@st.com>");
2228 MODULE_DESCRIPTION("STMicroelectronics STM32 Digital Camera Memory Interface driver");
2229 MODULE_LICENSE("GPL");
[ Source navigation ] [ Diff markup ] [ Identifier search ] [ general search ]

This page was automatically generated by the 2.1.0 LXR engine.

The LXR team
Valid CSS 2.1! Valid HTML 4.01!