2740 lines · c
1// SPDX-License-Identifier: GPL-2.0-only2/*3 * BCM283x / BCM271x Unicam Capture Driver4 *5 * Copyright (C) 2017-2020 - Raspberry Pi (Trading) Ltd.6 * Copyright (C) 2024 - Ideas on Board7 *8 * Dave Stevenson <dave.stevenson@raspberrypi.com>9 *10 * Based on TI am437x driver by11 * Benoit Parrot <bparrot@ti.com>12 * Lad, Prabhakar <prabhakar.csengg@gmail.com>13 *14 * and TI CAL camera interface driver by15 * Benoit Parrot <bparrot@ti.com>16 *17 *18 * There are two camera drivers in the kernel for BCM283x - this one and19 * bcm2835-camera (currently in staging).20 *21 * This driver directly controls the Unicam peripheral - there is no22 * involvement with the VideoCore firmware. Unicam receives CSI-2 or CCP2 data23 * and writes it into SDRAM. The only potential processing options are to24 * repack Bayer data into an alternate format, and applying windowing. The25 * repacking does not shift the data, so can repack V4L2_PIX_FMT_Sxxxx10P to26 * V4L2_PIX_FMT_Sxxxx10, or V4L2_PIX_FMT_Sxxxx12P to V4L2_PIX_FMT_Sxxxx12, but27 * not generically up to V4L2_PIX_FMT_Sxxxx16. Support for windowing may be28 * added later.29 *30 * It should be possible to connect this driver to any sensor with a suitable31 * output interface and V4L2 subdevice driver.32 */33 34#include <linux/clk.h>35#include <linux/delay.h>36#include <linux/device.h>37#include <linux/dma-mapping.h>38#include <linux/err.h>39#include <linux/interrupt.h>40#include <linux/io.h>41#include <linux/module.h>42#include <linux/of.h>43#include <linux/of_device.h>44#include <linux/platform_device.h>45#include <linux/pm_runtime.h>46#include <linux/slab.h>47#include <linux/videodev2.h>48 49#include <media/mipi-csi2.h>50#include <media/v4l2-async.h>51#include <media/v4l2-common.h>52#include <media/v4l2-dev.h>53#include <media/v4l2-device.h>54#include <media/v4l2-event.h>55#include <media/v4l2-ioctl.h>56#include <media/v4l2-fwnode.h>57#include <media/v4l2-mc.h>58#include <media/v4l2-subdev.h>59#include <media/videobuf2-dma-contig.h>60 61#include "bcm2835-unicam-regs.h"62 63#define UNICAM_MODULE_NAME "unicam"64 65/*66 * Unicam must request a minimum of 250Mhz from the VPU clock.67 * Otherwise the input FIFOs overrun and cause image corruption.68 */69#define UNICAM_MIN_VPU_CLOCK_RATE (250 * 1000 * 1000)70 71/* Unicam has an internal DMA alignment constraint of 16 bytes for each line. */72#define UNICAM_DMA_BPL_ALIGNMENT 1673 74/*75 * The image stride is stored in a 16 bit register, and needs to be aligned to76 * the DMA constraint. As the ISP in the same SoC has a 32 bytes alignment77 * constraint on its input, set the image stride alignment to 32 bytes here as78 * well to avoid incompatible configurations.79 */80#define UNICAM_IMAGE_BPL_ALIGNMENT 3281#define UNICAM_IMAGE_MAX_BPL ((1U << 16) - UNICAM_IMAGE_BPL_ALIGNMENT)82 83/*84 * Max width is therefore determined by the max stride divided by the number of85 * bits per pixel. Take 32bpp as a worst case. No imposed limit on the height,86 * so adopt a square image for want of anything better.87 */88#define UNICAM_IMAGE_MIN_WIDTH 1689#define UNICAM_IMAGE_MIN_HEIGHT 1690#define UNICAM_IMAGE_MAX_WIDTH (UNICAM_IMAGE_MAX_BPL / 4)91#define UNICAM_IMAGE_MAX_HEIGHT UNICAM_IMAGE_MAX_WIDTH92 93/*94 * There's no intrinsic limits on the width and height for embedded data. Use95 * the same maximum values as for the image, to avoid overflows in the image96 * size computation.97 */98#define UNICAM_META_MIN_WIDTH 199#define UNICAM_META_MIN_HEIGHT 1100#define UNICAM_META_MAX_WIDTH UNICAM_IMAGE_MAX_WIDTH101#define UNICAM_META_MAX_HEIGHT UNICAM_IMAGE_MAX_HEIGHT102 103/*104 * Size of the dummy buffer. Can be any size really, but the DMA105 * allocation works in units of page sizes.106 */107#define UNICAM_DUMMY_BUF_SIZE PAGE_SIZE108 109enum unicam_pad {110 UNICAM_SD_PAD_SINK,111 UNICAM_SD_PAD_SOURCE_IMAGE,112 UNICAM_SD_PAD_SOURCE_METADATA,113 UNICAM_SD_NUM_PADS114};115 116enum unicam_node_type {117 UNICAM_IMAGE_NODE,118 UNICAM_METADATA_NODE,119 UNICAM_MAX_NODES120};121 122/*123 * struct unicam_format_info - Unicam media bus format information124 * @fourcc: V4L2 pixel format FCC identifier. 0 if n/a.125 * @unpacked_fourcc: V4L2 pixel format FCC identifier if the data is expanded126 * out to 16bpp. 0 if n/a.127 * @code: V4L2 media bus format code.128 * @depth: Bits per pixel as delivered from the source.129 * @csi_dt: CSI data type.130 * @unpack: PUM value when unpacking to @unpacked_fourcc131 */132struct unicam_format_info {133 u32 fourcc;134 u32 unpacked_fourcc;135 u32 code;136 u8 depth;137 u8 csi_dt;138 u8 unpack;139};140 141struct unicam_buffer {142 struct vb2_v4l2_buffer vb;143 struct list_head list;144 dma_addr_t dma_addr;145 unsigned int size;146};147 148static inline struct unicam_buffer *to_unicam_buffer(struct vb2_buffer *vb)149{150 return container_of(vb, struct unicam_buffer, vb.vb2_buf);151}152 153struct unicam_node {154 bool registered;155 unsigned int id;156 157 /* Pointer to the current v4l2_buffer */158 struct unicam_buffer *cur_frm;159 /* Pointer to the next v4l2_buffer */160 struct unicam_buffer *next_frm;161 /* Used to store current pixel format */162 struct v4l2_format fmt;163 /* Buffer queue used in video-buf */164 struct vb2_queue buffer_queue;165 /* Queue of filled frames */166 struct list_head dma_queue;167 /* IRQ lock for DMA queue */168 spinlock_t dma_queue_lock;169 /* Identifies video device for this channel */170 struct video_device video_dev;171 /* Pointer to the parent handle */172 struct unicam_device *dev;173 struct media_pad pad;174 /*175 * Dummy buffer intended to be used by unicam176 * if we have no other queued buffers to swap to.177 */178 struct unicam_buffer dummy_buf;179 void *dummy_buf_cpu_addr;180};181 182struct unicam_device {183 struct kref kref;184 185 /* peripheral base address */186 void __iomem *base;187 /* clock gating base address */188 void __iomem *clk_gate_base;189 /* lp clock handle */190 struct clk *clock;191 /* vpu clock handle */192 struct clk *vpu_clock;193 /* V4l2 device */194 struct v4l2_device v4l2_dev;195 struct media_device mdev;196 197 /* parent device */198 struct device *dev;199 /* subdevice async notifier */200 struct v4l2_async_notifier notifier;201 unsigned int sequence;202 203 /* Sensor node */204 struct {205 struct v4l2_subdev *subdev;206 struct media_pad *pad;207 } sensor;208 209 /* Internal subdev */210 struct {211 struct v4l2_subdev sd;212 struct media_pad pads[UNICAM_SD_NUM_PADS];213 unsigned int enabled_streams;214 } subdev;215 216 enum v4l2_mbus_type bus_type;217 /*218 * Stores bus.mipi_csi2.flags for CSI2 sensors, or219 * bus.mipi_csi1.strobe for CCP2.220 */221 unsigned int bus_flags;222 unsigned int max_data_lanes;223 224 struct {225 struct media_pipeline pipe;226 unsigned int num_data_lanes;227 unsigned int nodes;228 } pipe;229 230 /* Lock used for the video devices of both nodes */231 struct mutex lock;232 struct unicam_node node[UNICAM_MAX_NODES];233};234 235static inline struct unicam_device *236notifier_to_unicam_device(struct v4l2_async_notifier *notifier)237{238 return container_of(notifier, struct unicam_device, notifier);239}240 241static inline struct unicam_device *242sd_to_unicam_device(struct v4l2_subdev *sd)243{244 return container_of(sd, struct unicam_device, subdev.sd);245}246 247static void unicam_release(struct kref *kref)248{249 struct unicam_device *unicam =250 container_of(kref, struct unicam_device, kref);251 252 if (unicam->mdev.dev)253 media_device_cleanup(&unicam->mdev);254 255 mutex_destroy(&unicam->lock);256 kfree(unicam);257}258 259static struct unicam_device *unicam_get(struct unicam_device *unicam)260{261 kref_get(&unicam->kref);262 263 return unicam;264}265 266static void unicam_put(struct unicam_device *unicam)267{268 kref_put(&unicam->kref, unicam_release);269}270 271/* -----------------------------------------------------------------------------272 * Misc helper functions273 */274 275static inline bool unicam_sd_pad_is_source(u32 pad)276{277 /* Camera RX has 1 sink pad, and N source pads */278 return pad != UNICAM_SD_PAD_SINK;279}280 281static inline bool is_metadata_node(struct unicam_node *node)282{283 return node->video_dev.device_caps & V4L2_CAP_META_CAPTURE;284}285 286static inline bool is_image_node(struct unicam_node *node)287{288 return node->video_dev.device_caps & V4L2_CAP_VIDEO_CAPTURE;289}290 291/* -----------------------------------------------------------------------------292 * Format data table and helper functions293 */294 295static const struct v4l2_mbus_framefmt unicam_default_image_format = {296 .width = 640,297 .height = 480,298 .code = MEDIA_BUS_FMT_UYVY8_1X16,299 .field = V4L2_FIELD_NONE,300 .colorspace = V4L2_COLORSPACE_SRGB,301 .ycbcr_enc = V4L2_YCBCR_ENC_601,302 .quantization = V4L2_QUANTIZATION_LIM_RANGE,303 .xfer_func = V4L2_XFER_FUNC_SRGB,304 .flags = 0,305};306 307static const struct v4l2_mbus_framefmt unicam_default_meta_format = {308 .width = 640,309 .height = 2,310 .code = MEDIA_BUS_FMT_META_8,311 .field = V4L2_FIELD_NONE,312};313 314static const struct unicam_format_info unicam_image_formats[] = {315 /* YUV Formats */316 {317 .fourcc = V4L2_PIX_FMT_YUYV,318 .code = MEDIA_BUS_FMT_YUYV8_1X16,319 .depth = 16,320 .csi_dt = MIPI_CSI2_DT_YUV422_8B,321 }, {322 .fourcc = V4L2_PIX_FMT_UYVY,323 .code = MEDIA_BUS_FMT_UYVY8_1X16,324 .depth = 16,325 .csi_dt = MIPI_CSI2_DT_YUV422_8B,326 }, {327 .fourcc = V4L2_PIX_FMT_YVYU,328 .code = MEDIA_BUS_FMT_YVYU8_1X16,329 .depth = 16,330 .csi_dt = MIPI_CSI2_DT_YUV422_8B,331 }, {332 .fourcc = V4L2_PIX_FMT_VYUY,333 .code = MEDIA_BUS_FMT_VYUY8_1X16,334 .depth = 16,335 .csi_dt = MIPI_CSI2_DT_YUV422_8B,336 }, {337 /* RGB Formats */338 .fourcc = V4L2_PIX_FMT_RGB565, /* gggbbbbb rrrrrggg */339 .code = MEDIA_BUS_FMT_RGB565_1X16,340 .depth = 16,341 .csi_dt = MIPI_CSI2_DT_RGB565,342 }, {343 .fourcc = V4L2_PIX_FMT_RGB24, /* rgb */344 .code = MEDIA_BUS_FMT_RGB888_1X24,345 .depth = 24,346 .csi_dt = MIPI_CSI2_DT_RGB888,347 }, {348 .fourcc = V4L2_PIX_FMT_BGR24, /* bgr */349 .code = MEDIA_BUS_FMT_BGR888_1X24,350 .depth = 24,351 .csi_dt = MIPI_CSI2_DT_RGB888,352 }, {353 /* Bayer Formats */354 .fourcc = V4L2_PIX_FMT_SBGGR8,355 .code = MEDIA_BUS_FMT_SBGGR8_1X8,356 .depth = 8,357 .csi_dt = MIPI_CSI2_DT_RAW8,358 }, {359 .fourcc = V4L2_PIX_FMT_SGBRG8,360 .code = MEDIA_BUS_FMT_SGBRG8_1X8,361 .depth = 8,362 .csi_dt = MIPI_CSI2_DT_RAW8,363 }, {364 .fourcc = V4L2_PIX_FMT_SGRBG8,365 .code = MEDIA_BUS_FMT_SGRBG8_1X8,366 .depth = 8,367 .csi_dt = MIPI_CSI2_DT_RAW8,368 }, {369 .fourcc = V4L2_PIX_FMT_SRGGB8,370 .code = MEDIA_BUS_FMT_SRGGB8_1X8,371 .depth = 8,372 .csi_dt = MIPI_CSI2_DT_RAW8,373 }, {374 .fourcc = V4L2_PIX_FMT_SBGGR10P,375 .unpacked_fourcc = V4L2_PIX_FMT_SBGGR10,376 .code = MEDIA_BUS_FMT_SBGGR10_1X10,377 .depth = 10,378 .csi_dt = MIPI_CSI2_DT_RAW10,379 .unpack = UNICAM_PUM_UNPACK10,380 }, {381 .fourcc = V4L2_PIX_FMT_SGBRG10P,382 .unpacked_fourcc = V4L2_PIX_FMT_SGBRG10,383 .code = MEDIA_BUS_FMT_SGBRG10_1X10,384 .depth = 10,385 .csi_dt = MIPI_CSI2_DT_RAW10,386 .unpack = UNICAM_PUM_UNPACK10,387 }, {388 .fourcc = V4L2_PIX_FMT_SGRBG10P,389 .unpacked_fourcc = V4L2_PIX_FMT_SGRBG10,390 .code = MEDIA_BUS_FMT_SGRBG10_1X10,391 .depth = 10,392 .csi_dt = MIPI_CSI2_DT_RAW10,393 .unpack = UNICAM_PUM_UNPACK10,394 }, {395 .fourcc = V4L2_PIX_FMT_SRGGB10P,396 .unpacked_fourcc = V4L2_PIX_FMT_SRGGB10,397 .code = MEDIA_BUS_FMT_SRGGB10_1X10,398 .depth = 10,399 .csi_dt = MIPI_CSI2_DT_RAW10,400 .unpack = UNICAM_PUM_UNPACK10,401 }, {402 .fourcc = V4L2_PIX_FMT_SBGGR12P,403 .unpacked_fourcc = V4L2_PIX_FMT_SBGGR12,404 .code = MEDIA_BUS_FMT_SBGGR12_1X12,405 .depth = 12,406 .csi_dt = MIPI_CSI2_DT_RAW12,407 .unpack = UNICAM_PUM_UNPACK12,408 }, {409 .fourcc = V4L2_PIX_FMT_SGBRG12P,410 .unpacked_fourcc = V4L2_PIX_FMT_SGBRG12,411 .code = MEDIA_BUS_FMT_SGBRG12_1X12,412 .depth = 12,413 .csi_dt = MIPI_CSI2_DT_RAW12,414 .unpack = UNICAM_PUM_UNPACK12,415 }, {416 .fourcc = V4L2_PIX_FMT_SGRBG12P,417 .unpacked_fourcc = V4L2_PIX_FMT_SGRBG12,418 .code = MEDIA_BUS_FMT_SGRBG12_1X12,419 .depth = 12,420 .csi_dt = MIPI_CSI2_DT_RAW12,421 .unpack = UNICAM_PUM_UNPACK12,422 }, {423 .fourcc = V4L2_PIX_FMT_SRGGB12P,424 .unpacked_fourcc = V4L2_PIX_FMT_SRGGB12,425 .code = MEDIA_BUS_FMT_SRGGB12_1X12,426 .depth = 12,427 .csi_dt = MIPI_CSI2_DT_RAW12,428 .unpack = UNICAM_PUM_UNPACK12,429 }, {430 .fourcc = V4L2_PIX_FMT_SBGGR14P,431 .unpacked_fourcc = V4L2_PIX_FMT_SBGGR14,432 .code = MEDIA_BUS_FMT_SBGGR14_1X14,433 .depth = 14,434 .csi_dt = MIPI_CSI2_DT_RAW14,435 .unpack = UNICAM_PUM_UNPACK14,436 }, {437 .fourcc = V4L2_PIX_FMT_SGBRG14P,438 .unpacked_fourcc = V4L2_PIX_FMT_SGBRG14,439 .code = MEDIA_BUS_FMT_SGBRG14_1X14,440 .depth = 14,441 .csi_dt = MIPI_CSI2_DT_RAW14,442 .unpack = UNICAM_PUM_UNPACK14,443 }, {444 .fourcc = V4L2_PIX_FMT_SGRBG14P,445 .unpacked_fourcc = V4L2_PIX_FMT_SGRBG14,446 .code = MEDIA_BUS_FMT_SGRBG14_1X14,447 .depth = 14,448 .csi_dt = MIPI_CSI2_DT_RAW14,449 .unpack = UNICAM_PUM_UNPACK14,450 }, {451 .fourcc = V4L2_PIX_FMT_SRGGB14P,452 .unpacked_fourcc = V4L2_PIX_FMT_SRGGB14,453 .code = MEDIA_BUS_FMT_SRGGB14_1X14,454 .depth = 14,455 .csi_dt = MIPI_CSI2_DT_RAW14,456 .unpack = UNICAM_PUM_UNPACK14,457 }, {458 /* 16 bit Bayer formats could be supported. */459 460 /* Greyscale formats */461 .fourcc = V4L2_PIX_FMT_GREY,462 .code = MEDIA_BUS_FMT_Y8_1X8,463 .depth = 8,464 .csi_dt = MIPI_CSI2_DT_RAW8,465 }, {466 .fourcc = V4L2_PIX_FMT_Y10P,467 .unpacked_fourcc = V4L2_PIX_FMT_Y10,468 .code = MEDIA_BUS_FMT_Y10_1X10,469 .depth = 10,470 .csi_dt = MIPI_CSI2_DT_RAW10,471 .unpack = UNICAM_PUM_UNPACK10,472 }, {473 .fourcc = V4L2_PIX_FMT_Y12P,474 .unpacked_fourcc = V4L2_PIX_FMT_Y12,475 .code = MEDIA_BUS_FMT_Y12_1X12,476 .depth = 12,477 .csi_dt = MIPI_CSI2_DT_RAW12,478 .unpack = UNICAM_PUM_UNPACK12,479 }, {480 .fourcc = V4L2_PIX_FMT_Y14P,481 .unpacked_fourcc = V4L2_PIX_FMT_Y14,482 .code = MEDIA_BUS_FMT_Y14_1X14,483 .depth = 14,484 .csi_dt = MIPI_CSI2_DT_RAW14,485 .unpack = UNICAM_PUM_UNPACK14,486 },487};488 489static const struct unicam_format_info unicam_meta_formats[] = {490 {491 .fourcc = V4L2_META_FMT_GENERIC_8,492 .code = MEDIA_BUS_FMT_META_8,493 .depth = 8,494 }, {495 .fourcc = V4L2_META_FMT_GENERIC_CSI2_10,496 .code = MEDIA_BUS_FMT_META_10,497 .depth = 10,498 }, {499 .fourcc = V4L2_META_FMT_GENERIC_CSI2_12,500 .code = MEDIA_BUS_FMT_META_12,501 .depth = 12,502 }, {503 .fourcc = V4L2_META_FMT_GENERIC_CSI2_14,504 .code = MEDIA_BUS_FMT_META_14,505 .depth = 14,506 },507};508 509/* Format setup functions */510static const struct unicam_format_info *511unicam_find_format_by_code(u32 code, u32 pad)512{513 const struct unicam_format_info *formats;514 unsigned int num_formats;515 unsigned int i;516 517 if (pad == UNICAM_SD_PAD_SOURCE_IMAGE) {518 formats = unicam_image_formats;519 num_formats = ARRAY_SIZE(unicam_image_formats);520 } else {521 formats = unicam_meta_formats;522 num_formats = ARRAY_SIZE(unicam_meta_formats);523 }524 525 for (i = 0; i < num_formats; i++) {526 if (formats[i].code == code)527 return &formats[i];528 }529 530 return NULL;531}532 533static const struct unicam_format_info *534unicam_find_format_by_fourcc(u32 fourcc, u32 pad)535{536 const struct unicam_format_info *formats;537 unsigned int num_formats;538 unsigned int i;539 540 if (pad == UNICAM_SD_PAD_SOURCE_IMAGE) {541 formats = unicam_image_formats;542 num_formats = ARRAY_SIZE(unicam_image_formats);543 } else {544 formats = unicam_meta_formats;545 num_formats = ARRAY_SIZE(unicam_meta_formats);546 }547 548 for (i = 0; i < num_formats; ++i) {549 if (formats[i].fourcc == fourcc)550 return &formats[i];551 }552 553 return NULL;554}555 556static void unicam_calc_image_size_bpl(struct unicam_device *unicam,557 const struct unicam_format_info *fmtinfo,558 struct v4l2_pix_format *pix)559{560 u32 min_bpl;561 562 v4l_bound_align_image(&pix->width, UNICAM_IMAGE_MIN_WIDTH,563 UNICAM_IMAGE_MAX_WIDTH, 2,564 &pix->height, UNICAM_IMAGE_MIN_HEIGHT,565 UNICAM_IMAGE_MAX_HEIGHT, 0, 0);566 567 /* Unpacking always goes to 16bpp */568 if (pix->pixelformat == fmtinfo->unpacked_fourcc)569 min_bpl = pix->width * 2;570 else571 min_bpl = pix->width * fmtinfo->depth / 8;572 min_bpl = ALIGN(min_bpl, UNICAM_IMAGE_BPL_ALIGNMENT);573 574 pix->bytesperline = ALIGN(pix->bytesperline, UNICAM_IMAGE_BPL_ALIGNMENT);575 pix->bytesperline = clamp_t(unsigned int, pix->bytesperline, min_bpl,576 UNICAM_IMAGE_MAX_BPL);577 578 pix->sizeimage = pix->height * pix->bytesperline;579}580 581static void unicam_calc_meta_size_bpl(struct unicam_device *unicam,582 const struct unicam_format_info *fmtinfo,583 struct v4l2_meta_format *meta)584{585 v4l_bound_align_image(&meta->width, UNICAM_META_MIN_WIDTH,586 UNICAM_META_MAX_WIDTH, 0,587 &meta->height, UNICAM_META_MIN_HEIGHT,588 UNICAM_META_MAX_HEIGHT, 0, 0);589 590 meta->bytesperline = ALIGN(meta->width * fmtinfo->depth / 8,591 UNICAM_DMA_BPL_ALIGNMENT);592 meta->buffersize = meta->height * meta->bytesperline;593}594 595/* -----------------------------------------------------------------------------596 * Hardware handling597 */598 599static inline void unicam_clk_write(struct unicam_device *unicam, u32 val)600{601 /* Pass the CM_PASSWORD along with the value. */602 writel(val | 0x5a000000, unicam->clk_gate_base);603}604 605static inline u32 unicam_reg_read(struct unicam_device *unicam, u32 offset)606{607 return readl(unicam->base + offset);608}609 610static inline void unicam_reg_write(struct unicam_device *unicam, u32 offset, u32 val)611{612 writel(val, unicam->base + offset);613}614 615static inline int unicam_get_field(u32 value, u32 mask)616{617 return (value & mask) >> __ffs(mask);618}619 620static inline void unicam_set_field(u32 *valp, u32 field, u32 mask)621{622 u32 val = *valp;623 624 val &= ~mask;625 val |= (field << __ffs(mask)) & mask;626 *valp = val;627}628 629static inline void unicam_reg_write_field(struct unicam_device *unicam, u32 offset,630 u32 field, u32 mask)631{632 u32 val = unicam_reg_read(unicam, offset);633 634 unicam_set_field(&val, field, mask);635 unicam_reg_write(unicam, offset, val);636}637 638static void unicam_wr_dma_addr(struct unicam_node *node,639 struct unicam_buffer *buf)640{641 dma_addr_t endaddr = buf->dma_addr + buf->size;642 643 if (node->id == UNICAM_IMAGE_NODE) {644 unicam_reg_write(node->dev, UNICAM_IBSA0, buf->dma_addr);645 unicam_reg_write(node->dev, UNICAM_IBEA0, endaddr);646 } else {647 unicam_reg_write(node->dev, UNICAM_DBSA0, buf->dma_addr);648 unicam_reg_write(node->dev, UNICAM_DBEA0, endaddr);649 }650}651 652static unsigned int unicam_get_lines_done(struct unicam_device *unicam)653{654 struct unicam_node *node = &unicam->node[UNICAM_IMAGE_NODE];655 unsigned int stride = node->fmt.fmt.pix.bytesperline;656 struct unicam_buffer *frm = node->cur_frm;657 dma_addr_t cur_addr;658 659 if (!frm)660 return 0;661 662 cur_addr = unicam_reg_read(unicam, UNICAM_IBWP);663 return (unsigned int)(cur_addr - frm->dma_addr) / stride;664}665 666static void unicam_schedule_next_buffer(struct unicam_node *node)667{668 struct unicam_buffer *buf;669 670 buf = list_first_entry(&node->dma_queue, struct unicam_buffer, list);671 node->next_frm = buf;672 list_del(&buf->list);673 674 unicam_wr_dma_addr(node, buf);675}676 677static void unicam_schedule_dummy_buffer(struct unicam_node *node)678{679 int node_id = is_image_node(node) ? UNICAM_IMAGE_NODE : UNICAM_METADATA_NODE;680 681 dev_dbg(node->dev->dev, "Scheduling dummy buffer for node %d\n", node_id);682 683 unicam_wr_dma_addr(node, &node->dummy_buf);684 685 node->next_frm = NULL;686}687 688static void unicam_process_buffer_complete(struct unicam_node *node,689 unsigned int sequence)690{691 node->cur_frm->vb.field = node->fmt.fmt.pix.field;692 node->cur_frm->vb.sequence = sequence;693 694 vb2_buffer_done(&node->cur_frm->vb.vb2_buf, VB2_BUF_STATE_DONE);695}696 697static void unicam_queue_event_sof(struct unicam_device *unicam)698{699 struct unicam_node *node = &unicam->node[UNICAM_IMAGE_NODE];700 struct v4l2_event event = {701 .type = V4L2_EVENT_FRAME_SYNC,702 .u.frame_sync.frame_sequence = unicam->sequence,703 };704 705 v4l2_event_queue(&node->video_dev, &event);706}707 708static irqreturn_t unicam_isr(int irq, void *dev)709{710 struct unicam_device *unicam = dev;711 unsigned int lines_done = unicam_get_lines_done(dev);712 unsigned int sequence = unicam->sequence;713 unsigned int i;714 u32 ista, sta;715 bool fe;716 u64 ts;717 718 sta = unicam_reg_read(unicam, UNICAM_STA);719 /* Write value back to clear the interrupts */720 unicam_reg_write(unicam, UNICAM_STA, sta);721 722 ista = unicam_reg_read(unicam, UNICAM_ISTA);723 /* Write value back to clear the interrupts */724 unicam_reg_write(unicam, UNICAM_ISTA, ista);725 726 dev_dbg(unicam->dev, "ISR: ISTA: 0x%X, STA: 0x%X, sequence %d, lines done %d\n",727 ista, sta, sequence, lines_done);728 729 if (!(sta & (UNICAM_IS | UNICAM_PI0)))730 return IRQ_HANDLED;731 732 /*733 * Look for either the Frame End interrupt or the Packet Capture status734 * to signal a frame end.735 */736 fe = ista & UNICAM_FEI || sta & UNICAM_PI0;737 738 /*739 * We must run the frame end handler first. If we have a valid next_frm740 * and we get a simultaneout FE + FS interrupt, running the FS handler741 * first would null out the next_frm ptr and we would have lost the742 * buffer forever.743 */744 if (fe) {745 /*746 * Ensure we have swapped buffers already as we can't747 * stop the peripheral. If no buffer is available, use a748 * dummy buffer to dump out frames until we get a new buffer749 * to use.750 */751 for (i = 0; i < ARRAY_SIZE(unicam->node); i++) {752 struct unicam_node *node = &unicam->node[i];753 754 if (!vb2_start_streaming_called(&node->buffer_queue))755 continue;756 757 /*758 * If cur_frm == next_frm, it means we have not had759 * a chance to swap buffers, likely due to having760 * multiple interrupts occurring simultaneously (like FE761 * + FS + LS). In this case, we cannot signal the buffer762 * as complete, as the HW will reuse that buffer.763 */764 if (node->cur_frm && node->cur_frm != node->next_frm)765 unicam_process_buffer_complete(node, sequence);766 node->cur_frm = node->next_frm;767 }768 unicam->sequence++;769 }770 771 if (ista & UNICAM_FSI) {772 /*773 * Timestamp is to be when the first data byte was captured,774 * aka frame start.775 */776 ts = ktime_get_ns();777 for (i = 0; i < ARRAY_SIZE(unicam->node); i++) {778 struct unicam_node *node = &unicam->node[i];779 780 if (!vb2_start_streaming_called(&node->buffer_queue))781 continue;782 783 if (node->cur_frm)784 node->cur_frm->vb.vb2_buf.timestamp = ts;785 else786 dev_dbg(unicam->v4l2_dev.dev,787 "ISR: [%d] Dropping frame, buffer not available at FS\n",788 i);789 /*790 * Set the next frame output to go to a dummy frame791 * if we have not managed to obtain another frame792 * from the queue.793 */794 unicam_schedule_dummy_buffer(node);795 }796 797 unicam_queue_event_sof(unicam);798 }799 800 /*801 * Cannot swap buffer at frame end, there may be a race condition802 * where the HW does not actually swap it if the new frame has803 * already started.804 */805 if (ista & (UNICAM_FSI | UNICAM_LCI) && !fe) {806 for (i = 0; i < ARRAY_SIZE(unicam->node); i++) {807 struct unicam_node *node = &unicam->node[i];808 809 if (!vb2_start_streaming_called(&node->buffer_queue))810 continue;811 812 spin_lock(&node->dma_queue_lock);813 if (!list_empty(&node->dma_queue) && !node->next_frm)814 unicam_schedule_next_buffer(node);815 spin_unlock(&node->dma_queue_lock);816 }817 }818 819 if (unicam_reg_read(unicam, UNICAM_ICTL) & UNICAM_FCM) {820 /* Switch out of trigger mode if selected */821 unicam_reg_write_field(unicam, UNICAM_ICTL, 1, UNICAM_TFC);822 unicam_reg_write_field(unicam, UNICAM_ICTL, 0, UNICAM_FCM);823 }824 return IRQ_HANDLED;825}826 827static void unicam_set_packing_config(struct unicam_device *unicam,828 const struct unicam_format_info *fmtinfo)829{830 struct unicam_node *node = &unicam->node[UNICAM_IMAGE_NODE];831 u32 pack, unpack;832 u32 val;833 834 if (node->fmt.fmt.pix.pixelformat == fmtinfo->fourcc) {835 unpack = UNICAM_PUM_NONE;836 pack = UNICAM_PPM_NONE;837 } else {838 unpack = fmtinfo->unpack;839 /* Repacking is always to 16bpp */840 pack = UNICAM_PPM_PACK16;841 }842 843 val = 0;844 unicam_set_field(&val, unpack, UNICAM_PUM_MASK);845 unicam_set_field(&val, pack, UNICAM_PPM_MASK);846 unicam_reg_write(unicam, UNICAM_IPIPE, val);847}848 849static void unicam_cfg_image_id(struct unicam_device *unicam, u8 vc, u8 dt)850{851 if (unicam->bus_type == V4L2_MBUS_CSI2_DPHY) {852 /* CSI2 mode */853 unicam_reg_write(unicam, UNICAM_IDI0, (vc << 6) | dt);854 } else {855 /* CCP2 mode */856 unicam_reg_write(unicam, UNICAM_IDI0, 0x80 | dt);857 }858}859 860static void unicam_enable_ed(struct unicam_device *unicam)861{862 u32 val = unicam_reg_read(unicam, UNICAM_DCS);863 864 unicam_set_field(&val, 2, UNICAM_EDL_MASK);865 /* Do not wrap at the end of the embedded data buffer */866 unicam_set_field(&val, 0, UNICAM_DBOB);867 868 unicam_reg_write(unicam, UNICAM_DCS, val);869}870 871static int unicam_get_image_vc_dt(struct unicam_device *unicam,872 struct v4l2_subdev_state *state,873 u8 *vc, u8 *dt)874{875 struct v4l2_mbus_frame_desc fd;876 u32 stream;877 int ret;878 879 ret = v4l2_subdev_routing_find_opposite_end(&state->routing,880 UNICAM_SD_PAD_SOURCE_IMAGE,881 0, NULL, &stream);882 if (ret)883 return ret;884 885 ret = v4l2_subdev_call(unicam->sensor.subdev, pad, get_frame_desc,886 unicam->sensor.pad->index, &fd);887 if (ret)888 return ret;889 890 /* Only CSI-2 supports DTs. */891 if (fd.type != V4L2_MBUS_FRAME_DESC_TYPE_CSI2)892 return -EINVAL;893 894 for (unsigned int i = 0; i < fd.num_entries; ++i) {895 const struct v4l2_mbus_frame_desc_entry *fde = &fd.entry[i];896 897 if (fde->stream == stream) {898 *vc = fde->bus.csi2.vc;899 *dt = fde->bus.csi2.dt;900 return 0;901 }902 }903 904 return -EINVAL;905}906 907static void unicam_start_rx(struct unicam_device *unicam,908 struct v4l2_subdev_state *state)909{910 struct unicam_node *node = &unicam->node[UNICAM_IMAGE_NODE];911 const struct unicam_format_info *fmtinfo;912 const struct v4l2_mbus_framefmt *fmt;913 unsigned int line_int_freq;914 u8 vc, dt;915 u32 val;916 int ret;917 918 fmt = v4l2_subdev_state_get_format(state, UNICAM_SD_PAD_SOURCE_IMAGE, 0);919 fmtinfo = unicam_find_format_by_code(fmt->code,920 UNICAM_SD_PAD_SOURCE_IMAGE);921 if (WARN_ON(!fmtinfo))922 return;923 924 /*925 * Enable lane clocks. The register is structured as follows:926 *927 * [9:8] - DAT3928 * [7:6] - DAT2929 * [5:4] - DAT1930 * [3:2] - DAT0931 * [1:0] - CLK932 *933 * Enabled lane must be set to b01, and disabled lanes to b00. The clock934 * lane is always enabled.935 */936 val = 0x155 & GENMASK(unicam->pipe.num_data_lanes * 2 + 1, 0);937 unicam_clk_write(unicam, val);938 939 /* Basic init */940 unicam_reg_write(unicam, UNICAM_CTRL, UNICAM_MEM);941 942 /* Enable analogue control, and leave in reset. */943 val = UNICAM_AR;944 unicam_set_field(&val, 7, UNICAM_CTATADJ_MASK);945 unicam_set_field(&val, 7, UNICAM_PTATADJ_MASK);946 unicam_reg_write(unicam, UNICAM_ANA, val);947 usleep_range(1000, 2000);948 949 /* Come out of reset */950 unicam_reg_write_field(unicam, UNICAM_ANA, 0, UNICAM_AR);951 952 /* Peripheral reset */953 unicam_reg_write_field(unicam, UNICAM_CTRL, 1, UNICAM_CPR);954 unicam_reg_write_field(unicam, UNICAM_CTRL, 0, UNICAM_CPR);955 956 unicam_reg_write_field(unicam, UNICAM_CTRL, 0, UNICAM_CPE);957 958 /* Enable Rx control. */959 val = unicam_reg_read(unicam, UNICAM_CTRL);960 if (unicam->bus_type == V4L2_MBUS_CSI2_DPHY) {961 unicam_set_field(&val, UNICAM_CPM_CSI2, UNICAM_CPM_MASK);962 unicam_set_field(&val, UNICAM_DCM_STROBE, UNICAM_DCM_MASK);963 } else {964 unicam_set_field(&val, UNICAM_CPM_CCP2, UNICAM_CPM_MASK);965 unicam_set_field(&val, unicam->bus_flags, UNICAM_DCM_MASK);966 }967 /* Packet framer timeout */968 unicam_set_field(&val, 0xf, UNICAM_PFT_MASK);969 unicam_set_field(&val, 128, UNICAM_OET_MASK);970 unicam_reg_write(unicam, UNICAM_CTRL, val);971 972 unicam_reg_write(unicam, UNICAM_IHWIN, 0);973 unicam_reg_write(unicam, UNICAM_IVWIN, 0);974 975 /* AXI bus access QoS setup */976 val = unicam_reg_read(unicam, UNICAM_PRI);977 unicam_set_field(&val, 0, UNICAM_BL_MASK);978 unicam_set_field(&val, 0, UNICAM_BS_MASK);979 unicam_set_field(&val, 0xe, UNICAM_PP_MASK);980 unicam_set_field(&val, 8, UNICAM_NP_MASK);981 unicam_set_field(&val, 2, UNICAM_PT_MASK);982 unicam_set_field(&val, 1, UNICAM_PE);983 unicam_reg_write(unicam, UNICAM_PRI, val);984 985 unicam_reg_write_field(unicam, UNICAM_ANA, 0, UNICAM_DDL);986 987 /* Always start in trigger frame capture mode (UNICAM_FCM set) */988 val = UNICAM_FSIE | UNICAM_FEIE | UNICAM_FCM | UNICAM_IBOB;989 line_int_freq = max(fmt->height >> 2, 128);990 unicam_set_field(&val, line_int_freq, UNICAM_LCIE_MASK);991 unicam_reg_write(unicam, UNICAM_ICTL, val);992 unicam_reg_write(unicam, UNICAM_STA, UNICAM_STA_MASK_ALL);993 unicam_reg_write(unicam, UNICAM_ISTA, UNICAM_ISTA_MASK_ALL);994 995 /* tclk_term_en */996 unicam_reg_write_field(unicam, UNICAM_CLT, 2, UNICAM_CLT1_MASK);997 /* tclk_settle */998 unicam_reg_write_field(unicam, UNICAM_CLT, 6, UNICAM_CLT2_MASK);999 /* td_term_en */1000 unicam_reg_write_field(unicam, UNICAM_DLT, 2, UNICAM_DLT1_MASK);1001 /* ths_settle */1002 unicam_reg_write_field(unicam, UNICAM_DLT, 6, UNICAM_DLT2_MASK);1003 /* trx_enable */1004 unicam_reg_write_field(unicam, UNICAM_DLT, 0, UNICAM_DLT3_MASK);1005 1006 unicam_reg_write_field(unicam, UNICAM_CTRL, 0, UNICAM_SOE);1007 1008 /* Packet compare setup - required to avoid missing frame ends */1009 val = 0;1010 unicam_set_field(&val, 1, UNICAM_PCE);1011 unicam_set_field(&val, 1, UNICAM_GI);1012 unicam_set_field(&val, 1, UNICAM_CPH);1013 unicam_set_field(&val, 0, UNICAM_PCVC_MASK);1014 unicam_set_field(&val, 1, UNICAM_PCDT_MASK);1015 unicam_reg_write(unicam, UNICAM_CMP0, val);1016 1017 /* Enable clock lane and set up terminations */1018 val = 0;1019 if (unicam->bus_type == V4L2_MBUS_CSI2_DPHY) {1020 /* CSI2 */1021 unicam_set_field(&val, 1, UNICAM_CLE);1022 unicam_set_field(&val, 1, UNICAM_CLLPE);1023 if (!(unicam->bus_flags & V4L2_MBUS_CSI2_NONCONTINUOUS_CLOCK)) {1024 unicam_set_field(&val, 1, UNICAM_CLTRE);1025 unicam_set_field(&val, 1, UNICAM_CLHSE);1026 }1027 } else {1028 /* CCP2 */1029 unicam_set_field(&val, 1, UNICAM_CLE);1030 unicam_set_field(&val, 1, UNICAM_CLHSE);1031 unicam_set_field(&val, 1, UNICAM_CLTRE);1032 }1033 unicam_reg_write(unicam, UNICAM_CLK, val);1034 1035 /*1036 * Enable required data lanes with appropriate terminations.1037 * The same value needs to be written to UNICAM_DATn registers for1038 * the active lanes, and 0 for inactive ones.1039 */1040 val = 0;1041 if (unicam->bus_type == V4L2_MBUS_CSI2_DPHY) {1042 /* CSI2 */1043 unicam_set_field(&val, 1, UNICAM_DLE);1044 unicam_set_field(&val, 1, UNICAM_DLLPE);1045 if (!(unicam->bus_flags & V4L2_MBUS_CSI2_NONCONTINUOUS_CLOCK)) {1046 unicam_set_field(&val, 1, UNICAM_DLTRE);1047 unicam_set_field(&val, 1, UNICAM_DLHSE);1048 }1049 } else {1050 /* CCP2 */1051 unicam_set_field(&val, 1, UNICAM_DLE);1052 unicam_set_field(&val, 1, UNICAM_DLHSE);1053 unicam_set_field(&val, 1, UNICAM_DLTRE);1054 }1055 unicam_reg_write(unicam, UNICAM_DAT0, val);1056 1057 if (unicam->pipe.num_data_lanes == 1)1058 val = 0;1059 unicam_reg_write(unicam, UNICAM_DAT1, val);1060 1061 if (unicam->max_data_lanes > 2) {1062 /*1063 * Registers UNICAM_DAT2 and UNICAM_DAT3 only valid if the1064 * instance supports more than 2 data lanes.1065 */1066 if (unicam->pipe.num_data_lanes == 2)1067 val = 0;1068 unicam_reg_write(unicam, UNICAM_DAT2, val);1069 1070 if (unicam->pipe.num_data_lanes == 3)1071 val = 0;1072 unicam_reg_write(unicam, UNICAM_DAT3, val);1073 }1074 1075 unicam_reg_write(unicam, UNICAM_IBLS,1076 node->fmt.fmt.pix.bytesperline);1077 unicam_wr_dma_addr(node, node->cur_frm);1078 unicam_set_packing_config(unicam, fmtinfo);1079 1080 ret = unicam_get_image_vc_dt(unicam, state, &vc, &dt);1081 if (ret) {1082 /*1083 * If the source doesn't support frame descriptors, default to1084 * VC 0 and use the DT corresponding to the format.1085 */1086 vc = 0;1087 dt = fmtinfo->csi_dt;1088 }1089 1090 unicam_cfg_image_id(unicam, vc, dt);1091 1092 val = unicam_reg_read(unicam, UNICAM_MISC);1093 unicam_set_field(&val, 1, UNICAM_FL0);1094 unicam_set_field(&val, 1, UNICAM_FL1);1095 unicam_reg_write(unicam, UNICAM_MISC, val);1096 1097 /* Enable peripheral */1098 unicam_reg_write_field(unicam, UNICAM_CTRL, 1, UNICAM_CPE);1099 1100 /* Load image pointers */1101 unicam_reg_write_field(unicam, UNICAM_ICTL, 1, UNICAM_LIP_MASK);1102 1103 /*1104 * Enable trigger only for the first frame to1105 * sync correctly to the FS from the source.1106 */1107 unicam_reg_write_field(unicam, UNICAM_ICTL, 1, UNICAM_TFC);1108}1109 1110static void unicam_start_metadata(struct unicam_device *unicam)1111{1112 struct unicam_node *node = &unicam->node[UNICAM_METADATA_NODE];1113 1114 unicam_enable_ed(unicam);1115 unicam_wr_dma_addr(node, node->cur_frm);1116 unicam_reg_write_field(unicam, UNICAM_DCS, 1, UNICAM_LDP);1117}1118 1119static void unicam_disable(struct unicam_device *unicam)1120{1121 /* Analogue lane control disable */1122 unicam_reg_write_field(unicam, UNICAM_ANA, 1, UNICAM_DDL);1123 1124 /* Stop the output engine */1125 unicam_reg_write_field(unicam, UNICAM_CTRL, 1, UNICAM_SOE);1126 1127 /* Disable the data lanes. */1128 unicam_reg_write(unicam, UNICAM_DAT0, 0);1129 unicam_reg_write(unicam, UNICAM_DAT1, 0);1130 1131 if (unicam->max_data_lanes > 2) {1132 unicam_reg_write(unicam, UNICAM_DAT2, 0);1133 unicam_reg_write(unicam, UNICAM_DAT3, 0);1134 }1135 1136 /* Peripheral reset */1137 unicam_reg_write_field(unicam, UNICAM_CTRL, 1, UNICAM_CPR);1138 usleep_range(50, 100);1139 unicam_reg_write_field(unicam, UNICAM_CTRL, 0, UNICAM_CPR);1140 1141 /* Disable peripheral */1142 unicam_reg_write_field(unicam, UNICAM_CTRL, 0, UNICAM_CPE);1143 1144 /* Clear ED setup */1145 unicam_reg_write(unicam, UNICAM_DCS, 0);1146 1147 /* Disable all lane clocks */1148 unicam_clk_write(unicam, 0);1149}1150 1151/* -----------------------------------------------------------------------------1152 * V4L2 subdev operations1153 */1154 1155static int __unicam_subdev_set_routing(struct v4l2_subdev *sd,1156 struct v4l2_subdev_state *state,1157 struct v4l2_subdev_krouting *routing)1158{1159 struct v4l2_subdev_route *route;1160 int ret;1161 1162 ret = v4l2_subdev_routing_validate(sd, routing,1163 V4L2_SUBDEV_ROUTING_ONLY_1_TO_1);1164 if (ret)1165 return ret;1166 1167 ret = v4l2_subdev_set_routing(sd, state, routing);1168 if (ret)1169 return ret;1170 1171 for_each_active_route(&state->routing, route) {1172 const struct v4l2_mbus_framefmt *def_fmt;1173 struct v4l2_mbus_framefmt *fmt;1174 1175 if (route->source_pad == UNICAM_SD_PAD_SOURCE_IMAGE)1176 def_fmt = &unicam_default_image_format;1177 else1178 def_fmt = &unicam_default_meta_format;1179 1180 fmt = v4l2_subdev_state_get_format(state, route->sink_pad,1181 route->sink_stream);1182 *fmt = *def_fmt;1183 fmt = v4l2_subdev_state_get_format(state, route->source_pad,1184 route->source_stream);1185 *fmt = *def_fmt;1186 }1187 1188 return 0;1189}1190 1191static int unicam_subdev_init_state(struct v4l2_subdev *sd,1192 struct v4l2_subdev_state *state)1193{1194 struct v4l2_subdev_route routes[] = {1195 {1196 .sink_pad = UNICAM_SD_PAD_SINK,1197 .sink_stream = 0,1198 .source_pad = UNICAM_SD_PAD_SOURCE_IMAGE,1199 .source_stream = 0,1200 .flags = V4L2_SUBDEV_ROUTE_FL_ACTIVE,1201 },1202 };1203 1204 struct v4l2_subdev_krouting routing = {1205 .len_routes = ARRAY_SIZE(routes),1206 .num_routes = ARRAY_SIZE(routes),1207 .routes = routes,1208 };1209 1210 /* Initialize routing to single route to the fist source pad. */1211 return __unicam_subdev_set_routing(sd, state, &routing);1212}1213 1214static int unicam_subdev_enum_mbus_code(struct v4l2_subdev *sd,1215 struct v4l2_subdev_state *state,1216 struct v4l2_subdev_mbus_code_enum *code)1217{1218 u32 pad, stream;1219 int ret;1220 1221 ret = v4l2_subdev_routing_find_opposite_end(&state->routing,1222 code->pad, code->stream,1223 &pad, &stream);1224 if (ret)1225 return ret;1226 1227 if (unicam_sd_pad_is_source(code->pad)) {1228 /* No transcoding, source and sink codes must match. */1229 const struct v4l2_mbus_framefmt *fmt;1230 1231 fmt = v4l2_subdev_state_get_format(state, pad, stream);1232 if (!fmt)1233 return -EINVAL;1234 1235 if (code->index > 0)1236 return -EINVAL;1237 1238 code->code = fmt->code;1239 } else {1240 const struct unicam_format_info *formats;1241 unsigned int num_formats;1242 1243 if (pad == UNICAM_SD_PAD_SOURCE_IMAGE) {1244 formats = unicam_image_formats;1245 num_formats = ARRAY_SIZE(unicam_image_formats);1246 } else {1247 formats = unicam_meta_formats;1248 num_formats = ARRAY_SIZE(unicam_meta_formats);1249 }1250 1251 if (code->index >= num_formats)1252 return -EINVAL;1253 1254 code->code = formats[code->index].code;1255 }1256 1257 return 0;1258}1259 1260static int unicam_subdev_enum_frame_size(struct v4l2_subdev *sd,1261 struct v4l2_subdev_state *state,1262 struct v4l2_subdev_frame_size_enum *fse)1263{1264 u32 pad, stream;1265 int ret;1266 1267 if (fse->index > 0)1268 return -EINVAL;1269 1270 ret = v4l2_subdev_routing_find_opposite_end(&state->routing, fse->pad,1271 fse->stream, &pad,1272 &stream);1273 if (ret)1274 return ret;1275 1276 if (unicam_sd_pad_is_source(fse->pad)) {1277 /* No transcoding, source and sink formats must match. */1278 const struct v4l2_mbus_framefmt *fmt;1279 1280 fmt = v4l2_subdev_state_get_format(state, pad, stream);1281 if (!fmt)1282 return -EINVAL;1283 1284 if (fse->code != fmt->code)1285 return -EINVAL;1286 1287 fse->min_width = fmt->width;1288 fse->max_width = fmt->width;1289 fse->min_height = fmt->height;1290 fse->max_height = fmt->height;1291 } else {1292 const struct unicam_format_info *fmtinfo;1293 1294 fmtinfo = unicam_find_format_by_code(fse->code, pad);1295 if (!fmtinfo)1296 return -EINVAL;1297 1298 if (pad == UNICAM_SD_PAD_SOURCE_IMAGE) {1299 fse->min_width = UNICAM_IMAGE_MIN_WIDTH;1300 fse->max_width = UNICAM_IMAGE_MAX_WIDTH;1301 fse->min_height = UNICAM_IMAGE_MIN_HEIGHT;1302 fse->max_height = UNICAM_IMAGE_MAX_HEIGHT;1303 } else {1304 fse->min_width = UNICAM_META_MIN_WIDTH;1305 fse->max_width = UNICAM_META_MAX_WIDTH;1306 fse->min_height = UNICAM_META_MIN_HEIGHT;1307 fse->max_height = UNICAM_META_MAX_HEIGHT;1308 }1309 }1310 1311 return 0;1312}1313 1314static int unicam_subdev_set_format(struct v4l2_subdev *sd,1315 struct v4l2_subdev_state *state,1316 struct v4l2_subdev_format *format)1317{1318 struct unicam_device *unicam = sd_to_unicam_device(sd);1319 struct v4l2_mbus_framefmt *sink_format, *source_format;1320 const struct unicam_format_info *fmtinfo;1321 u32 source_pad, source_stream;1322 int ret;1323 1324 if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE &&1325 unicam->subdev.enabled_streams)1326 return -EBUSY;1327 1328 /* No transcoding, source and sink formats must match. */1329 if (unicam_sd_pad_is_source(format->pad))1330 return v4l2_subdev_get_fmt(sd, state, format);1331 1332 /*1333 * Allowed formats for the stream on the sink pad depend on what source1334 * pad the stream is routed to. Find the corresponding source pad and1335 * use it to validate the media bus code.1336 */1337 ret = v4l2_subdev_routing_find_opposite_end(&state->routing,1338 format->pad, format->stream,1339 &source_pad, &source_stream);1340 if (ret)1341 return ret;1342 1343 fmtinfo = unicam_find_format_by_code(format->format.code, source_pad);1344 if (!fmtinfo) {1345 fmtinfo = source_pad == UNICAM_SD_PAD_SOURCE_IMAGE1346 ? &unicam_image_formats[0] : &unicam_meta_formats[0];1347 format->format.code = fmtinfo->code;1348 }1349 1350 if (source_pad == UNICAM_SD_PAD_SOURCE_IMAGE) {1351 format->format.width = clamp_t(unsigned int,1352 format->format.width,1353 UNICAM_IMAGE_MIN_WIDTH,1354 UNICAM_IMAGE_MAX_WIDTH);1355 format->format.height = clamp_t(unsigned int,1356 format->format.height,1357 UNICAM_IMAGE_MIN_HEIGHT,1358 UNICAM_IMAGE_MAX_HEIGHT);1359 format->format.field = V4L2_FIELD_NONE;1360 } else {1361 format->format.width = clamp_t(unsigned int,1362 format->format.width,1363 UNICAM_META_MIN_WIDTH,1364 UNICAM_META_MAX_WIDTH);1365 format->format.height = clamp_t(unsigned int,1366 format->format.height,1367 UNICAM_META_MIN_HEIGHT,1368 UNICAM_META_MAX_HEIGHT);1369 format->format.field = V4L2_FIELD_NONE;1370 1371 /* Colorspace don't apply to metadata. */1372 format->format.colorspace = 0;1373 format->format.ycbcr_enc = 0;1374 format->format.quantization = 0;1375 format->format.xfer_func = 0;1376 }1377 1378 sink_format = v4l2_subdev_state_get_format(state, format->pad,1379 format->stream);1380 source_format = v4l2_subdev_state_get_format(state, source_pad,1381 source_stream);1382 *sink_format = format->format;1383 *source_format = format->format;1384 1385 return 0;1386}1387 1388static int unicam_subdev_set_routing(struct v4l2_subdev *sd,1389 struct v4l2_subdev_state *state,1390 enum v4l2_subdev_format_whence which,1391 struct v4l2_subdev_krouting *routing)1392{1393 struct unicam_device *unicam = sd_to_unicam_device(sd);1394 1395 if (which == V4L2_SUBDEV_FORMAT_ACTIVE && unicam->subdev.enabled_streams)1396 return -EBUSY;1397 1398 return __unicam_subdev_set_routing(sd, state, routing);1399}1400 1401static int unicam_sd_enable_streams(struct v4l2_subdev *sd,1402 struct v4l2_subdev_state *state, u32 pad,1403 u64 streams_mask)1404{1405 struct unicam_device *unicam = sd_to_unicam_device(sd);1406 u32 other_pad, other_stream;1407 int ret;1408 1409 if (!unicam->subdev.enabled_streams) {1410 /* Configure and start Unicam. */1411 unicam->sequence = 0;1412 1413 if (unicam->pipe.nodes & BIT(UNICAM_METADATA_NODE))1414 unicam_start_metadata(unicam);1415 1416 unicam_start_rx(unicam, state);1417 }1418 1419 ret = v4l2_subdev_routing_find_opposite_end(&state->routing, pad, 0,1420 &other_pad, &other_stream);1421 if (ret)1422 return ret;1423 1424 ret = v4l2_subdev_enable_streams(unicam->sensor.subdev,1425 unicam->sensor.pad->index,1426 BIT(other_stream));1427 if (ret) {1428 dev_err(unicam->dev, "stream on failed in subdev\n");1429 return ret;1430 }1431 1432 unicam->subdev.enabled_streams |= BIT(other_stream);1433 1434 return 0;1435}1436 1437static int unicam_sd_disable_streams(struct v4l2_subdev *sd,1438 struct v4l2_subdev_state *state, u32 pad,1439 u64 streams_mask)1440{1441 struct unicam_device *unicam = sd_to_unicam_device(sd);1442 u32 other_pad, other_stream;1443 int ret;1444 1445 ret = v4l2_subdev_routing_find_opposite_end(&state->routing, pad, 0,1446 &other_pad, &other_stream);1447 if (ret)1448 return ret;1449 1450 v4l2_subdev_disable_streams(unicam->sensor.subdev,1451 unicam->sensor.pad->index,1452 BIT(other_stream));1453 1454 unicam->subdev.enabled_streams &= ~BIT(other_stream);1455 1456 if (!unicam->subdev.enabled_streams)1457 unicam_disable(unicam);1458 1459 return 0;1460}1461 1462static const struct v4l2_subdev_pad_ops unicam_subdev_pad_ops = {1463 .enum_mbus_code = unicam_subdev_enum_mbus_code,1464 .enum_frame_size = unicam_subdev_enum_frame_size,1465 .get_fmt = v4l2_subdev_get_fmt,1466 .set_fmt = unicam_subdev_set_format,1467 .set_routing = unicam_subdev_set_routing,1468 .enable_streams = unicam_sd_enable_streams,1469 .disable_streams = unicam_sd_disable_streams,1470};1471 1472static const struct v4l2_subdev_ops unicam_subdev_ops = {1473 .pad = &unicam_subdev_pad_ops,1474};1475 1476static const struct v4l2_subdev_internal_ops unicam_subdev_internal_ops = {1477 .init_state = unicam_subdev_init_state,1478};1479 1480static const struct media_entity_operations unicam_subdev_media_ops = {1481 .link_validate = v4l2_subdev_link_validate,1482 .has_pad_interdep = v4l2_subdev_has_pad_interdep,1483};1484 1485static int unicam_subdev_init(struct unicam_device *unicam)1486{1487 struct v4l2_subdev *sd = &unicam->subdev.sd;1488 int ret;1489 1490 v4l2_subdev_init(sd, &unicam_subdev_ops);1491 sd->internal_ops = &unicam_subdev_internal_ops;1492 v4l2_set_subdevdata(sd, unicam);1493 1494 sd->entity.function = MEDIA_ENT_F_VID_IF_BRIDGE;1495 sd->entity.ops = &unicam_subdev_media_ops;1496 sd->dev = unicam->dev;1497 sd->owner = THIS_MODULE;1498 sd->flags = V4L2_SUBDEV_FL_HAS_DEVNODE | V4L2_SUBDEV_FL_STREAMS;1499 1500 strscpy(sd->name, "unicam", sizeof(sd->name));1501 1502 unicam->subdev.pads[UNICAM_SD_PAD_SINK].flags = MEDIA_PAD_FL_SINK;1503 unicam->subdev.pads[UNICAM_SD_PAD_SOURCE_IMAGE].flags = MEDIA_PAD_FL_SOURCE;1504 unicam->subdev.pads[UNICAM_SD_PAD_SOURCE_METADATA].flags = MEDIA_PAD_FL_SOURCE;1505 1506 ret = media_entity_pads_init(&sd->entity, ARRAY_SIZE(unicam->subdev.pads),1507 unicam->subdev.pads);1508 if (ret) {1509 dev_err(unicam->dev, "Failed to initialize media entity: %d\n",1510 ret);1511 return ret;1512 }1513 1514 ret = v4l2_subdev_init_finalize(sd);1515 if (ret) {1516 dev_err(unicam->dev, "Failed to initialize subdev: %d\n", ret);1517 goto err_entity;1518 }1519 1520 ret = v4l2_device_register_subdev(&unicam->v4l2_dev, sd);1521 if (ret) {1522 dev_err(unicam->dev, "Failed to register subdev: %d\n", ret);1523 goto err_subdev;1524 }1525 1526 return 0;1527 1528err_subdev:1529 v4l2_subdev_cleanup(sd);1530err_entity:1531 media_entity_cleanup(&sd->entity);1532 return ret;1533}1534 1535static void unicam_subdev_cleanup(struct unicam_device *unicam)1536{1537 v4l2_subdev_cleanup(&unicam->subdev.sd);1538 media_entity_cleanup(&unicam->subdev.sd.entity);1539}1540 1541/* -----------------------------------------------------------------------------1542 * Videobuf2 queue operations1543 */1544 1545static int unicam_queue_setup(struct vb2_queue *vq, unsigned int *nbuffers,1546 unsigned int *nplanes, unsigned int sizes[],1547 struct device *alloc_devs[])1548{1549 struct unicam_node *node = vb2_get_drv_priv(vq);1550 u32 size = is_image_node(node) ? node->fmt.fmt.pix.sizeimage1551 : node->fmt.fmt.meta.buffersize;1552 1553 if (*nplanes) {1554 if (sizes[0] < size) {1555 dev_dbg(node->dev->dev, "sizes[0] %i < size %u\n",1556 sizes[0], size);1557 return -EINVAL;1558 }1559 size = sizes[0];1560 }1561 1562 *nplanes = 1;1563 sizes[0] = size;1564 1565 return 0;1566}1567 1568static int unicam_buffer_prepare(struct vb2_buffer *vb)1569{1570 struct unicam_node *node = vb2_get_drv_priv(vb->vb2_queue);1571 struct unicam_buffer *buf = to_unicam_buffer(vb);1572 u32 size = is_image_node(node) ? node->fmt.fmt.pix.sizeimage1573 : node->fmt.fmt.meta.buffersize;1574 1575 if (vb2_plane_size(vb, 0) < size) {1576 dev_dbg(node->dev->dev,1577 "data will not fit into plane (%lu < %u)\n",1578 vb2_plane_size(vb, 0), size);1579 return -EINVAL;1580 }1581 1582 buf->dma_addr = vb2_dma_contig_plane_dma_addr(&buf->vb.vb2_buf, 0);1583 buf->size = size;1584 1585 vb2_set_plane_payload(&buf->vb.vb2_buf, 0, size);1586 1587 return 0;1588}1589 1590static void unicam_return_buffers(struct unicam_node *node,1591 enum vb2_buffer_state state)1592{1593 struct unicam_buffer *buf, *tmp;1594 1595 list_for_each_entry_safe(buf, tmp, &node->dma_queue, list) {1596 list_del(&buf->list);1597 vb2_buffer_done(&buf->vb.vb2_buf, state);1598 }1599 1600 if (node->cur_frm)1601 vb2_buffer_done(&node->cur_frm->vb.vb2_buf,1602 state);1603 if (node->next_frm && node->cur_frm != node->next_frm)1604 vb2_buffer_done(&node->next_frm->vb.vb2_buf,1605 state);1606 1607 node->cur_frm = NULL;1608 node->next_frm = NULL;1609}1610 1611static int unicam_num_data_lanes(struct unicam_device *unicam)1612{1613 struct v4l2_mbus_config mbus_config = { 0 };1614 unsigned int num_data_lanes;1615 int ret;1616 1617 if (unicam->bus_type != V4L2_MBUS_CSI2_DPHY)1618 return unicam->max_data_lanes;1619 1620 ret = v4l2_subdev_call(unicam->sensor.subdev, pad, get_mbus_config,1621 unicam->sensor.pad->index, &mbus_config);1622 if (ret == -ENOIOCTLCMD)1623 return unicam->max_data_lanes;1624 1625 if (ret < 0) {1626 dev_err(unicam->dev, "Failed to get mbus config: %d\n", ret);1627 return ret;1628 }1629 1630 num_data_lanes = mbus_config.bus.mipi_csi2.num_data_lanes;1631 1632 if (num_data_lanes != 1 && num_data_lanes != 2 && num_data_lanes != 4) {1633 dev_err(unicam->dev,1634 "Device %s has requested %u data lanes, invalid\n",1635 unicam->sensor.subdev->name, num_data_lanes);1636 return -EINVAL;1637 }1638 1639 if (num_data_lanes > unicam->max_data_lanes) {1640 dev_err(unicam->dev,1641 "Device %s has requested %u data lanes, >%u configured in DT\n",1642 unicam->sensor.subdev->name, num_data_lanes,1643 unicam->max_data_lanes);1644 return -EINVAL;1645 }1646 1647 return num_data_lanes;1648}1649 1650static int unicam_start_streaming(struct vb2_queue *vq, unsigned int count)1651{1652 struct unicam_node *node = vb2_get_drv_priv(vq);1653 struct unicam_device *unicam = node->dev;1654 struct unicam_buffer *buf;1655 struct media_pipeline_pad_iter iter;1656 struct media_pad *pad;1657 unsigned long flags;1658 int ret;1659 1660 dev_dbg(unicam->dev, "Starting stream on %s device\n",1661 is_metadata_node(node) ? "metadata" : "image");1662 1663 /*1664 * Start the pipeline. This validates all links, and populates the1665 * pipeline structure.1666 */1667 ret = video_device_pipeline_start(&node->video_dev, &unicam->pipe.pipe);1668 if (ret < 0) {1669 dev_dbg(unicam->dev, "Failed to start media pipeline: %d\n", ret);1670 goto err_buffers;1671 }1672 1673 /*1674 * Determine which video nodes are included in the pipeline, and get the1675 * number of data lanes.1676 */1677 if (unicam->pipe.pipe.start_count == 1) {1678 unicam->pipe.nodes = 0;1679 1680 media_pipeline_for_each_pad(&unicam->pipe.pipe, &iter, pad) {1681 if (pad->entity != &unicam->subdev.sd.entity)1682 continue;1683 1684 if (pad->index == UNICAM_SD_PAD_SOURCE_IMAGE)1685 unicam->pipe.nodes |= BIT(UNICAM_IMAGE_NODE);1686 else if (pad->index == UNICAM_SD_PAD_SOURCE_METADATA)1687 unicam->pipe.nodes |= BIT(UNICAM_METADATA_NODE);1688 }1689 1690 if (!(unicam->pipe.nodes & BIT(UNICAM_IMAGE_NODE))) {1691 dev_dbg(unicam->dev,1692 "Pipeline does not include image node\n");1693 ret = -EPIPE;1694 goto err_pipeline;1695 }1696 1697 ret = unicam_num_data_lanes(unicam);1698 if (ret < 0)1699 goto err_pipeline;1700 1701 unicam->pipe.num_data_lanes = ret;1702 1703 dev_dbg(unicam->dev, "Running with %u data lanes, nodes %u\n",1704 unicam->pipe.num_data_lanes, unicam->pipe.nodes);1705 }1706 1707 /* Arm the node with the first buffer from the DMA queue. */1708 spin_lock_irqsave(&node->dma_queue_lock, flags);1709 buf = list_first_entry(&node->dma_queue, struct unicam_buffer, list);1710 node->cur_frm = buf;1711 node->next_frm = buf;1712 list_del(&buf->list);1713 spin_unlock_irqrestore(&node->dma_queue_lock, flags);1714 1715 /*1716 * Wait for all the video devices in the pipeline to have been started1717 * before starting the hardware. In the general case, this would1718 * prevent capturing multiple streams independently. However, the1719 * Unicam DMA engines are not generic, they have been designed to1720 * capture image data and embedded data from the same camera sensor.1721 * Not only does the main use case not benefit from independent1722 * capture, it requires proper synchronization of the streams at start1723 * time.1724 */1725 if (unicam->pipe.pipe.start_count < hweight32(unicam->pipe.nodes))1726 return 0;1727 1728 ret = pm_runtime_resume_and_get(unicam->dev);1729 if (ret < 0) {1730 dev_err(unicam->dev, "PM runtime resume failed: %d\n", ret);1731 goto err_pipeline;1732 }1733 1734 /* Enable the streams on the source. */1735 ret = v4l2_subdev_enable_streams(&unicam->subdev.sd,1736 UNICAM_SD_PAD_SOURCE_IMAGE,1737 BIT(0));1738 if (ret < 0) {1739 dev_err(unicam->dev, "stream on failed in subdev\n");1740 goto err_pm_put;1741 }1742 1743 if (unicam->pipe.nodes & BIT(UNICAM_METADATA_NODE)) {1744 ret = v4l2_subdev_enable_streams(&unicam->subdev.sd,1745 UNICAM_SD_PAD_SOURCE_METADATA,1746 BIT(0));1747 if (ret < 0) {1748 dev_err(unicam->dev, "stream on failed in subdev\n");1749 goto err_disable_streams;1750 }1751 }1752 1753 return 0;1754 1755err_disable_streams:1756 v4l2_subdev_disable_streams(&unicam->subdev.sd,1757 UNICAM_SD_PAD_SOURCE_IMAGE, BIT(0));1758err_pm_put:1759 pm_runtime_put_sync(unicam->dev);1760err_pipeline:1761 video_device_pipeline_stop(&node->video_dev);1762err_buffers:1763 unicam_return_buffers(node, VB2_BUF_STATE_QUEUED);1764 return ret;1765}1766 1767static void unicam_stop_streaming(struct vb2_queue *vq)1768{1769 struct unicam_node *node = vb2_get_drv_priv(vq);1770 struct unicam_device *unicam = node->dev;1771 1772 /* Stop the hardware when the first video device gets stopped. */1773 if (unicam->pipe.pipe.start_count == hweight32(unicam->pipe.nodes)) {1774 if (unicam->pipe.nodes & BIT(UNICAM_METADATA_NODE))1775 v4l2_subdev_disable_streams(&unicam->subdev.sd,1776 UNICAM_SD_PAD_SOURCE_METADATA,1777 BIT(0));1778 1779 v4l2_subdev_disable_streams(&unicam->subdev.sd,1780 UNICAM_SD_PAD_SOURCE_IMAGE,1781 BIT(0));1782 1783 pm_runtime_put(unicam->dev);1784 }1785 1786 video_device_pipeline_stop(&node->video_dev);1787 1788 /* Clear all queued buffers for the node */1789 unicam_return_buffers(node, VB2_BUF_STATE_ERROR);1790}1791 1792static void unicam_buffer_queue(struct vb2_buffer *vb)1793{1794 struct unicam_node *node = vb2_get_drv_priv(vb->vb2_queue);1795 struct unicam_buffer *buf = to_unicam_buffer(vb);1796 1797 spin_lock_irq(&node->dma_queue_lock);1798 list_add_tail(&buf->list, &node->dma_queue);1799 spin_unlock_irq(&node->dma_queue_lock);1800}1801 1802static const struct vb2_ops unicam_video_qops = {1803 .queue_setup = unicam_queue_setup,1804 .wait_prepare = vb2_ops_wait_prepare,1805 .wait_finish = vb2_ops_wait_finish,1806 .buf_prepare = unicam_buffer_prepare,1807 .start_streaming = unicam_start_streaming,1808 .stop_streaming = unicam_stop_streaming,1809 .buf_queue = unicam_buffer_queue,1810};1811 1812/* -----------------------------------------------------------------------------1813 * V4L2 video device operations1814 */1815 1816static int unicam_querycap(struct file *file, void *priv,1817 struct v4l2_capability *cap)1818{1819 strscpy(cap->driver, UNICAM_MODULE_NAME, sizeof(cap->driver));1820 strscpy(cap->card, UNICAM_MODULE_NAME, sizeof(cap->card));1821 1822 cap->capabilities |= V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_META_CAPTURE;1823 1824 return 0;1825}1826 1827static int unicam_enum_fmt_vid(struct file *file, void *priv,1828 struct v4l2_fmtdesc *f)1829{1830 unsigned int index;1831 unsigned int i;1832 1833 for (i = 0, index = 0; i < ARRAY_SIZE(unicam_image_formats); i++) {1834 if (f->mbus_code && unicam_image_formats[i].code != f->mbus_code)1835 continue;1836 1837 if (index == f->index) {1838 f->pixelformat = unicam_image_formats[i].fourcc;1839 return 0;1840 }1841 1842 index++;1843 1844 if (!unicam_image_formats[i].unpacked_fourcc)1845 continue;1846 1847 if (index == f->index) {1848 f->pixelformat = unicam_image_formats[i].unpacked_fourcc;1849 return 0;1850 }1851 1852 index++;1853 }1854 1855 return -EINVAL;1856}1857 1858static int unicam_g_fmt_vid(struct file *file, void *priv,1859 struct v4l2_format *f)1860{1861 struct unicam_node *node = video_drvdata(file);1862 1863 *f = node->fmt;1864 1865 return 0;1866}1867 1868static void __unicam_try_fmt_vid(struct unicam_node *node,1869 struct v4l2_pix_format *pix)1870{1871 const struct unicam_format_info *fmtinfo;1872 1873 /*1874 * Default to the first format if the requested pixel format code isn't1875 * supported.1876 */1877 fmtinfo = unicam_find_format_by_fourcc(pix->pixelformat,1878 UNICAM_SD_PAD_SOURCE_IMAGE);1879 if (!fmtinfo) {1880 fmtinfo = &unicam_image_formats[0];1881 pix->pixelformat = fmtinfo->fourcc;1882 }1883 1884 unicam_calc_image_size_bpl(node->dev, fmtinfo, pix);1885 1886 if (pix->field == V4L2_FIELD_ANY)1887 pix->field = V4L2_FIELD_NONE;1888}1889 1890static int unicam_try_fmt_vid(struct file *file, void *priv,1891 struct v4l2_format *f)1892{1893 struct unicam_node *node = video_drvdata(file);1894 1895 __unicam_try_fmt_vid(node, &f->fmt.pix);1896 return 0;1897}1898 1899static int unicam_s_fmt_vid(struct file *file, void *priv,1900 struct v4l2_format *f)1901{1902 struct unicam_node *node = video_drvdata(file);1903 1904 if (vb2_is_busy(&node->buffer_queue))1905 return -EBUSY;1906 1907 __unicam_try_fmt_vid(node, &f->fmt.pix);1908 node->fmt = *f;1909 1910 return 0;1911}1912 1913static int unicam_enum_fmt_meta(struct file *file, void *priv,1914 struct v4l2_fmtdesc *f)1915{1916 unsigned int i, index;1917 1918 for (i = 0, index = 0; i < ARRAY_SIZE(unicam_meta_formats); i++) {1919 if (f->mbus_code && unicam_meta_formats[i].code != f->mbus_code)1920 continue;1921 1922 if (index == f->index) {1923 f->pixelformat = unicam_meta_formats[i].fourcc;1924 f->type = V4L2_BUF_TYPE_META_CAPTURE;1925 f->flags = V4L2_FMT_FLAG_META_LINE_BASED;1926 return 0;1927 }1928 1929 index++;1930 }1931 1932 return -EINVAL;1933}1934 1935static int unicam_g_fmt_meta(struct file *file, void *priv,1936 struct v4l2_format *f)1937{1938 struct unicam_node *node = video_drvdata(file);1939 1940 f->fmt.meta = node->fmt.fmt.meta;1941 1942 return 0;1943}1944 1945static const struct unicam_format_info *1946__unicam_try_fmt_meta(struct unicam_node *node, struct v4l2_meta_format *meta)1947{1948 const struct unicam_format_info *fmtinfo;1949 1950 /*1951 * Default to the first format if the requested pixel format code isn't1952 * supported.1953 */1954 fmtinfo = unicam_find_format_by_fourcc(meta->dataformat,1955 UNICAM_SD_PAD_SOURCE_METADATA);1956 if (!fmtinfo) {1957 fmtinfo = &unicam_meta_formats[0];1958 meta->dataformat = fmtinfo->fourcc;1959 }1960 1961 unicam_calc_meta_size_bpl(node->dev, fmtinfo, meta);1962 1963 return fmtinfo;1964}1965 1966static int unicam_try_fmt_meta(struct file *file, void *priv,1967 struct v4l2_format *f)1968{1969 struct unicam_node *node = video_drvdata(file);1970 1971 __unicam_try_fmt_meta(node, &f->fmt.meta);1972 return 0;1973}1974 1975static int unicam_s_fmt_meta(struct file *file, void *priv,1976 struct v4l2_format *f)1977{1978 struct unicam_node *node = video_drvdata(file);1979 1980 if (vb2_is_busy(&node->buffer_queue))1981 return -EBUSY;1982 1983 __unicam_try_fmt_meta(node, &f->fmt.meta);1984 node->fmt = *f;1985 1986 return 0;1987}1988 1989static int unicam_enum_framesizes(struct file *file, void *fh,1990 struct v4l2_frmsizeenum *fsize)1991{1992 struct unicam_node *node = video_drvdata(file);1993 int ret = -EINVAL;1994 1995 if (fsize->index > 0)1996 return ret;1997 1998 if (is_image_node(node)) {1999 if (!unicam_find_format_by_fourcc(fsize->pixel_format,2000 UNICAM_SD_PAD_SOURCE_IMAGE))2001 return ret;2002 2003 fsize->type = V4L2_FRMSIZE_TYPE_STEPWISE;2004 fsize->stepwise.min_width = UNICAM_IMAGE_MIN_WIDTH;2005 fsize->stepwise.max_width = UNICAM_IMAGE_MAX_WIDTH;2006 fsize->stepwise.step_width = 1;2007 fsize->stepwise.min_height = UNICAM_IMAGE_MIN_HEIGHT;2008 fsize->stepwise.max_height = UNICAM_IMAGE_MAX_HEIGHT;2009 fsize->stepwise.step_height = 1;2010 } else {2011 if (!unicam_find_format_by_fourcc(fsize->pixel_format,2012 UNICAM_SD_PAD_SOURCE_METADATA))2013 return ret;2014 2015 fsize->type = V4L2_FRMSIZE_TYPE_STEPWISE;2016 fsize->stepwise.min_width = UNICAM_META_MIN_WIDTH;2017 fsize->stepwise.max_width = UNICAM_META_MAX_WIDTH;2018 fsize->stepwise.step_width = 1;2019 fsize->stepwise.min_height = UNICAM_META_MIN_HEIGHT;2020 fsize->stepwise.max_height = UNICAM_META_MAX_HEIGHT;2021 fsize->stepwise.step_height = 1;2022 }2023 2024 return 0;2025}2026 2027static int unicam_log_status(struct file *file, void *fh)2028{2029 struct unicam_node *node = video_drvdata(file);2030 struct unicam_device *unicam = node->dev;2031 u32 reg;2032 2033 /* status for sub devices */2034 v4l2_device_call_all(&unicam->v4l2_dev, 0, core, log_status);2035 2036 dev_info(unicam->dev, "-----Receiver status-----\n");2037 dev_info(unicam->dev, "V4L2 width/height: %ux%u\n",2038 node->fmt.fmt.pix.width, node->fmt.fmt.pix.height);2039 dev_info(unicam->dev, "V4L2 format: %08x\n",2040 node->fmt.fmt.pix.pixelformat);2041 reg = unicam_reg_read(unicam, UNICAM_IPIPE);2042 dev_info(unicam->dev, "Unpacking/packing: %u / %u\n",2043 unicam_get_field(reg, UNICAM_PUM_MASK),2044 unicam_get_field(reg, UNICAM_PPM_MASK));2045 dev_info(unicam->dev, "----Live data----\n");2046 dev_info(unicam->dev, "Programmed stride: %4u\n",2047 unicam_reg_read(unicam, UNICAM_IBLS));2048 dev_info(unicam->dev, "Detected resolution: %ux%u\n",2049 unicam_reg_read(unicam, UNICAM_IHSTA),2050 unicam_reg_read(unicam, UNICAM_IVSTA));2051 dev_info(unicam->dev, "Write pointer: %08x\n",2052 unicam_reg_read(unicam, UNICAM_IBWP));2053 2054 return 0;2055}2056 2057static int unicam_subscribe_event(struct v4l2_fh *fh,2058 const struct v4l2_event_subscription *sub)2059{2060 switch (sub->type) {2061 case V4L2_EVENT_FRAME_SYNC:2062 return v4l2_event_subscribe(fh, sub, 2, NULL);2063 default:2064 return -EINVAL;2065 }2066}2067 2068static const struct v4l2_ioctl_ops unicam_ioctl_ops = {2069 .vidioc_querycap = unicam_querycap,2070 2071 .vidioc_enum_fmt_vid_cap = unicam_enum_fmt_vid,2072 .vidioc_g_fmt_vid_cap = unicam_g_fmt_vid,2073 .vidioc_try_fmt_vid_cap = unicam_try_fmt_vid,2074 .vidioc_s_fmt_vid_cap = unicam_s_fmt_vid,2075 2076 .vidioc_enum_fmt_meta_cap = unicam_enum_fmt_meta,2077 .vidioc_g_fmt_meta_cap = unicam_g_fmt_meta,2078 .vidioc_try_fmt_meta_cap = unicam_try_fmt_meta,2079 .vidioc_s_fmt_meta_cap = unicam_s_fmt_meta,2080 2081 .vidioc_enum_framesizes = unicam_enum_framesizes,2082 2083 .vidioc_reqbufs = vb2_ioctl_reqbufs,2084 .vidioc_create_bufs = vb2_ioctl_create_bufs,2085 .vidioc_prepare_buf = vb2_ioctl_prepare_buf,2086 .vidioc_querybuf = vb2_ioctl_querybuf,2087 .vidioc_qbuf = vb2_ioctl_qbuf,2088 .vidioc_dqbuf = vb2_ioctl_dqbuf,2089 .vidioc_expbuf = vb2_ioctl_expbuf,2090 .vidioc_streamon = vb2_ioctl_streamon,2091 .vidioc_streamoff = vb2_ioctl_streamoff,2092 2093 .vidioc_log_status = unicam_log_status,2094 .vidioc_subscribe_event = unicam_subscribe_event,2095 .vidioc_unsubscribe_event = v4l2_event_unsubscribe,2096};2097 2098/* unicam capture driver file operations */2099static const struct v4l2_file_operations unicam_fops = {2100 .owner = THIS_MODULE,2101 .open = v4l2_fh_open,2102 .release = vb2_fop_release,2103 .poll = vb2_fop_poll,2104 .unlocked_ioctl = video_ioctl2,2105 .mmap = vb2_fop_mmap,2106};2107 2108static int unicam_video_link_validate(struct media_link *link)2109{2110 struct video_device *vdev =2111 media_entity_to_video_device(link->sink->entity);2112 struct v4l2_subdev *sd =2113 media_entity_to_v4l2_subdev(link->source->entity);2114 struct unicam_node *node = video_get_drvdata(vdev);2115 const u32 pad = is_image_node(node) ? UNICAM_SD_PAD_SOURCE_IMAGE2116 : UNICAM_SD_PAD_SOURCE_METADATA;2117 const struct v4l2_mbus_framefmt *format;2118 struct v4l2_subdev_state *state;2119 int ret = 0;2120 2121 state = v4l2_subdev_lock_and_get_active_state(sd);2122 2123 format = v4l2_subdev_state_get_format(state, pad, 0);2124 if (!format) {2125 ret = -EINVAL;2126 goto out;2127 }2128 2129 if (is_image_node(node)) {2130 const struct v4l2_pix_format *fmt = &node->fmt.fmt.pix;2131 const struct unicam_format_info *fmtinfo;2132 2133 fmtinfo = unicam_find_format_by_fourcc(fmt->pixelformat,2134 UNICAM_SD_PAD_SOURCE_IMAGE);2135 if (WARN_ON(!fmtinfo)) {2136 ret = -EPIPE;2137 goto out;2138 }2139 2140 if (fmtinfo->code != format->code ||2141 fmt->height != format->height ||2142 fmt->width != format->width ||2143 fmt->field != format->field) {2144 dev_dbg(node->dev->dev,2145 "image: (%u x %u) 0x%08x %s != (%u x %u) 0x%08x %s\n",2146 fmt->width, fmt->height, fmtinfo->code,2147 v4l2_field_names[fmt->field],2148 format->width, format->height, format->code,2149 v4l2_field_names[format->field]);2150 ret = -EPIPE;2151 }2152 } else {2153 const struct v4l2_meta_format *fmt = &node->fmt.fmt.meta;2154 2155 const struct unicam_format_info *fmtinfo;2156 2157 fmtinfo = unicam_find_format_by_fourcc(fmt->dataformat,2158 UNICAM_SD_PAD_SOURCE_METADATA);2159 if (WARN_ON(!fmtinfo)) {2160 ret = -EPIPE;2161 goto out;2162 }2163 2164 if (fmtinfo->code != format->code ||2165 fmt->height != format->height ||2166 fmt->width != format->width) {2167 dev_dbg(node->dev->dev,2168 "meta: (%u x %u) 0x%04x != (%u x %u) 0x%04x\n",2169 fmt->width, fmt->height, fmtinfo->code,2170 format->width, format->height, format->code);2171 ret = -EPIPE;2172 }2173 }2174 2175out:2176 v4l2_subdev_unlock_state(state);2177 return ret;2178}2179 2180static const struct media_entity_operations unicam_video_media_ops = {2181 .link_validate = unicam_video_link_validate,2182};2183 2184static void unicam_node_release(struct video_device *vdev)2185{2186 struct unicam_node *node = video_get_drvdata(vdev);2187 2188 unicam_put(node->dev);2189}2190 2191static void unicam_set_default_format(struct unicam_node *node)2192{2193 if (is_image_node(node)) {2194 struct v4l2_pix_format *fmt = &node->fmt.fmt.pix;2195 const struct unicam_format_info *fmtinfo =2196 &unicam_image_formats[0];2197 2198 node->fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;2199 2200 v4l2_fill_pix_format(fmt, &unicam_default_image_format);2201 fmt->pixelformat = fmtinfo->fourcc;2202 unicam_calc_image_size_bpl(node->dev, fmtinfo, fmt);2203 } else {2204 struct v4l2_meta_format *fmt = &node->fmt.fmt.meta;2205 const struct unicam_format_info *fmtinfo =2206 &unicam_meta_formats[0];2207 2208 node->fmt.type = V4L2_BUF_TYPE_META_CAPTURE;2209 2210 fmt->dataformat = fmtinfo->fourcc;2211 fmt->width = unicam_default_meta_format.width;2212 fmt->height = unicam_default_meta_format.height;2213 unicam_calc_meta_size_bpl(node->dev, fmtinfo, fmt);2214 }2215}2216 2217static int unicam_register_node(struct unicam_device *unicam,2218 enum unicam_node_type type)2219{2220 const u32 pad_index = type == UNICAM_IMAGE_NODE2221 ? UNICAM_SD_PAD_SOURCE_IMAGE2222 : UNICAM_SD_PAD_SOURCE_METADATA;2223 struct unicam_node *node = &unicam->node[type];2224 struct video_device *vdev = &node->video_dev;2225 struct vb2_queue *q = &node->buffer_queue;2226 int ret;2227 2228 node->dev = unicam_get(unicam);2229 node->id = type;2230 2231 spin_lock_init(&node->dma_queue_lock);2232 2233 INIT_LIST_HEAD(&node->dma_queue);2234 2235 /* Initialize the videobuf2 queue. */2236 q->type = type == UNICAM_IMAGE_NODE ? V4L2_BUF_TYPE_VIDEO_CAPTURE2237 : V4L2_BUF_TYPE_META_CAPTURE;2238 q->io_modes = VB2_MMAP | VB2_DMABUF;2239 q->drv_priv = node;2240 q->ops = &unicam_video_qops;2241 q->mem_ops = &vb2_dma_contig_memops;2242 q->buf_struct_size = sizeof(struct unicam_buffer);2243 q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;2244 q->lock = &unicam->lock;2245 q->min_queued_buffers = 1;2246 q->dev = unicam->dev;2247 2248 ret = vb2_queue_init(q);2249 if (ret) {2250 dev_err(unicam->dev, "vb2_queue_init() failed\n");2251 goto err_unicam_put;2252 }2253 2254 /* Initialize the video device. */2255 vdev->release = unicam_node_release;2256 vdev->fops = &unicam_fops;2257 vdev->ioctl_ops = &unicam_ioctl_ops;2258 vdev->v4l2_dev = &unicam->v4l2_dev;2259 vdev->vfl_dir = VFL_DIR_RX;2260 vdev->queue = q;2261 vdev->lock = &unicam->lock;2262 vdev->device_caps = type == UNICAM_IMAGE_NODE2263 ? V4L2_CAP_VIDEO_CAPTURE : V4L2_CAP_META_CAPTURE;2264 vdev->device_caps |= V4L2_CAP_STREAMING | V4L2_CAP_IO_MC;2265 vdev->entity.ops = &unicam_video_media_ops;2266 2267 snprintf(vdev->name, sizeof(vdev->name), "%s-%s", UNICAM_MODULE_NAME,2268 type == UNICAM_IMAGE_NODE ? "image" : "embedded");2269 2270 video_set_drvdata(vdev, node);2271 2272 if (type == UNICAM_IMAGE_NODE)2273 vdev->entity.flags |= MEDIA_ENT_FL_DEFAULT;2274 2275 node->pad.flags = MEDIA_PAD_FL_SINK;2276 2277 ret = media_entity_pads_init(&vdev->entity, 1, &node->pad);2278 if (ret)2279 goto err_unicam_put;2280 2281 node->dummy_buf.size = UNICAM_DUMMY_BUF_SIZE;2282 node->dummy_buf_cpu_addr = dma_alloc_coherent(unicam->dev,2283 node->dummy_buf.size,2284 &node->dummy_buf.dma_addr,2285 GFP_KERNEL);2286 if (!node->dummy_buf_cpu_addr) {2287 dev_err(unicam->dev, "Unable to allocate dummy buffer.\n");2288 ret = -ENOMEM;2289 goto err_entity_cleanup;2290 }2291 2292 unicam_set_default_format(node);2293 2294 ret = video_register_device(vdev, VFL_TYPE_VIDEO, -1);2295 if (ret) {2296 dev_err(unicam->dev, "Unable to register video device %s\n",2297 vdev->name);2298 goto err_dma_free;2299 }2300 2301 node->registered = true;2302 2303 ret = media_create_pad_link(&unicam->subdev.sd.entity,2304 pad_index,2305 &node->video_dev.entity,2306 0,2307 MEDIA_LNK_FL_ENABLED |2308 MEDIA_LNK_FL_IMMUTABLE);2309 if (ret) {2310 /*2311 * No need for cleanup, the caller will unregister the2312 * video device, which will drop the reference on the2313 * device and trigger the cleanup.2314 */2315 dev_err(unicam->dev, "Unable to create pad link for %s\n",2316 unicam->sensor.subdev->name);2317 return ret;2318 }2319 2320 return 0;2321 2322err_dma_free:2323 dma_free_coherent(unicam->dev, node->dummy_buf.size,2324 node->dummy_buf_cpu_addr,2325 node->dummy_buf.dma_addr);2326err_entity_cleanup:2327 media_entity_cleanup(&vdev->entity);2328err_unicam_put:2329 unicam_put(unicam);2330 return ret;2331}2332 2333static void unicam_unregister_nodes(struct unicam_device *unicam)2334{2335 unsigned int i;2336 2337 for (i = 0; i < ARRAY_SIZE(unicam->node); i++) {2338 struct unicam_node *node = &unicam->node[i];2339 2340 if (node->registered) {2341 vb2_video_unregister_device(&node->video_dev);2342 node->registered = false;2343 }2344 2345 if (node->dummy_buf_cpu_addr)2346 dma_free_coherent(unicam->dev, node->dummy_buf.size,2347 node->dummy_buf_cpu_addr,2348 node->dummy_buf.dma_addr);2349 }2350}2351 2352/* -----------------------------------------------------------------------------2353 * Power management2354 */2355 2356static int unicam_runtime_resume(struct device *dev)2357{2358 struct unicam_device *unicam = dev_get_drvdata(dev);2359 int ret;2360 2361 ret = clk_set_min_rate(unicam->vpu_clock, UNICAM_MIN_VPU_CLOCK_RATE);2362 if (ret) {2363 dev_err(unicam->dev, "failed to set up VPU clock\n");2364 return ret;2365 }2366 2367 ret = clk_prepare_enable(unicam->vpu_clock);2368 if (ret) {2369 dev_err(unicam->dev, "Failed to enable VPU clock: %d\n", ret);2370 goto err_vpu_clock;2371 }2372 2373 ret = clk_set_rate(unicam->clock, 100 * 1000 * 1000);2374 if (ret) {2375 dev_err(unicam->dev, "failed to set up CSI clock\n");2376 goto err_vpu_prepare;2377 }2378 2379 ret = clk_prepare_enable(unicam->clock);2380 if (ret) {2381 dev_err(unicam->dev, "Failed to enable CSI clock: %d\n", ret);2382 goto err_vpu_prepare;2383 }2384 2385 return 0;2386 2387err_vpu_prepare:2388 clk_disable_unprepare(unicam->vpu_clock);2389err_vpu_clock:2390 if (clk_set_min_rate(unicam->vpu_clock, 0))2391 dev_err(unicam->dev, "failed to reset the VPU clock\n");2392 2393 return ret;2394}2395 2396static int unicam_runtime_suspend(struct device *dev)2397{2398 struct unicam_device *unicam = dev_get_drvdata(dev);2399 2400 clk_disable_unprepare(unicam->clock);2401 2402 if (clk_set_min_rate(unicam->vpu_clock, 0))2403 dev_err(unicam->dev, "failed to reset the VPU clock\n");2404 2405 clk_disable_unprepare(unicam->vpu_clock);2406 2407 return 0;2408}2409 2410static const struct dev_pm_ops unicam_pm_ops = {2411 RUNTIME_PM_OPS(unicam_runtime_suspend, unicam_runtime_resume, NULL)2412};2413 2414/* -----------------------------------------------------------------------------2415 * V4L2 async notifier2416 */2417 2418static int unicam_async_bound(struct v4l2_async_notifier *notifier,2419 struct v4l2_subdev *subdev,2420 struct v4l2_async_connection *asc)2421{2422 struct unicam_device *unicam = notifier_to_unicam_device(notifier);2423 struct media_pad *sink = &unicam->subdev.pads[UNICAM_SD_PAD_SINK];2424 struct media_pad *source;2425 int ret;2426 2427 dev_dbg(unicam->dev, "Using sensor %s for capture\n",2428 subdev->name);2429 2430 ret = v4l2_create_fwnode_links_to_pad(subdev, sink, MEDIA_LNK_FL_ENABLED |2431 MEDIA_LNK_FL_IMMUTABLE);2432 if (ret)2433 return ret;2434 2435 source = media_pad_remote_pad_unique(sink);2436 if (IS_ERR(source)) {2437 dev_err(unicam->dev, "No connected sensor pad\n");2438 return PTR_ERR(source);2439 }2440 2441 unicam->sensor.subdev = subdev;2442 unicam->sensor.pad = source;2443 2444 return 0;2445}2446 2447static int unicam_async_complete(struct v4l2_async_notifier *notifier)2448{2449 struct unicam_device *unicam = notifier_to_unicam_device(notifier);2450 int ret;2451 2452 ret = unicam_register_node(unicam, UNICAM_IMAGE_NODE);2453 if (ret) {2454 dev_err(unicam->dev, "Unable to register image video device.\n");2455 goto unregister;2456 }2457 2458 ret = unicam_register_node(unicam, UNICAM_METADATA_NODE);2459 if (ret) {2460 dev_err(unicam->dev, "Unable to register metadata video device.\n");2461 goto unregister;2462 }2463 2464 ret = v4l2_device_register_subdev_nodes(&unicam->v4l2_dev);2465 if (ret) {2466 dev_err(unicam->dev, "Unable to register subdev nodes.\n");2467 goto unregister;2468 }2469 2470 return 0;2471 2472unregister:2473 unicam_unregister_nodes(unicam);2474 unicam_put(unicam);2475 2476 return ret;2477}2478 2479static const struct v4l2_async_notifier_operations unicam_async_ops = {2480 .bound = unicam_async_bound,2481 .complete = unicam_async_complete,2482};2483 2484static int unicam_async_nf_init(struct unicam_device *unicam)2485{2486 struct v4l2_fwnode_endpoint ep = { };2487 struct fwnode_handle *ep_handle;2488 struct v4l2_async_connection *asc;2489 int ret;2490 2491 ret = of_property_read_u32(unicam->dev->of_node, "brcm,num-data-lanes",2492 &unicam->max_data_lanes);2493 if (ret < 0) {2494 dev_err(unicam->dev, "Missing %s DT property\n",2495 "brcm,num-data-lanes");2496 return -EINVAL;2497 }2498 2499 /* Get and parse the local endpoint. */2500 ep_handle = fwnode_graph_get_endpoint_by_id(dev_fwnode(unicam->dev), 0, 0,2501 FWNODE_GRAPH_ENDPOINT_NEXT);2502 if (!ep_handle) {2503 dev_err(unicam->dev, "No endpoint found\n");2504 return -ENODEV;2505 }2506 2507 ret = v4l2_fwnode_endpoint_parse(ep_handle, &ep);2508 if (ret) {2509 dev_err(unicam->dev, "Failed to parse endpoint: %d\n", ret);2510 goto error;2511 }2512 2513 unicam->bus_type = ep.bus_type;2514 2515 switch (ep.bus_type) {2516 case V4L2_MBUS_CSI2_DPHY: {2517 unsigned int num_data_lanes = ep.bus.mipi_csi2.num_data_lanes;2518 2519 if (num_data_lanes != 1 && num_data_lanes != 2 &&2520 num_data_lanes != 4) {2521 dev_err(unicam->dev, "%u data lanes not supported\n",2522 num_data_lanes);2523 ret = -EINVAL;2524 goto error;2525 }2526 2527 if (num_data_lanes > unicam->max_data_lanes) {2528 dev_err(unicam->dev,2529 "Endpoint uses %u data lanes when %u are supported\n",2530 num_data_lanes, unicam->max_data_lanes);2531 ret = -EINVAL;2532 goto error;2533 }2534 2535 unicam->max_data_lanes = num_data_lanes;2536 unicam->bus_flags = ep.bus.mipi_csi2.flags;2537 break;2538 }2539 2540 case V4L2_MBUS_CCP2:2541 unicam->max_data_lanes = 1;2542 unicam->bus_flags = ep.bus.mipi_csi1.strobe;2543 break;2544 2545 default:2546 /* Unsupported bus type */2547 dev_err(unicam->dev, "Unsupported bus type %u\n", ep.bus_type);2548 ret = -EINVAL;2549 goto error;2550 }2551 2552 /* Initialize and register the async notifier. */2553 v4l2_async_nf_init(&unicam->notifier, &unicam->v4l2_dev);2554 2555 asc = v4l2_async_nf_add_fwnode_remote(&unicam->notifier, ep_handle,2556 struct v4l2_async_connection);2557 fwnode_handle_put(ep_handle);2558 ep_handle = NULL;2559 2560 if (IS_ERR(asc)) {2561 ret = PTR_ERR(asc);2562 dev_err(unicam->dev, "Failed to add entry to notifier: %d\n",2563 ret);2564 goto error;2565 }2566 2567 unicam->notifier.ops = &unicam_async_ops;2568 2569 ret = v4l2_async_nf_register(&unicam->notifier);2570 if (ret) {2571 dev_err(unicam->dev, "Error registering device notifier: %d\n",2572 ret);2573 goto error;2574 }2575 2576 return 0;2577 2578error:2579 fwnode_handle_put(ep_handle);2580 return ret;2581}2582 2583/* -----------------------------------------------------------------------------2584 * Probe & remove2585 */2586 2587static int unicam_media_init(struct unicam_device *unicam)2588{2589 int ret;2590 2591 unicam->mdev.dev = unicam->dev;2592 strscpy(unicam->mdev.model, UNICAM_MODULE_NAME,2593 sizeof(unicam->mdev.model));2594 unicam->mdev.hw_revision = 0;2595 2596 media_device_init(&unicam->mdev);2597 2598 unicam->v4l2_dev.mdev = &unicam->mdev;2599 2600 ret = v4l2_device_register(unicam->dev, &unicam->v4l2_dev);2601 if (ret < 0) {2602 dev_err(unicam->dev, "Unable to register v4l2 device\n");2603 goto err_media_cleanup;2604 }2605 2606 ret = media_device_register(&unicam->mdev);2607 if (ret < 0) {2608 dev_err(unicam->dev,2609 "Unable to register media-controller device\n");2610 goto err_v4l2_unregister;2611 }2612 2613 return 0;2614 2615err_v4l2_unregister:2616 v4l2_device_unregister(&unicam->v4l2_dev);2617err_media_cleanup:2618 media_device_cleanup(&unicam->mdev);2619 return ret;2620}2621 2622static int unicam_probe(struct platform_device *pdev)2623{2624 struct unicam_device *unicam;2625 int ret;2626 2627 unicam = kzalloc(sizeof(*unicam), GFP_KERNEL);2628 if (!unicam)2629 return -ENOMEM;2630 2631 kref_init(&unicam->kref);2632 mutex_init(&unicam->lock);2633 2634 unicam->dev = &pdev->dev;2635 platform_set_drvdata(pdev, unicam);2636 2637 unicam->base = devm_platform_ioremap_resource_byname(pdev, "unicam");2638 if (IS_ERR(unicam->base)) {2639 ret = PTR_ERR(unicam->base);2640 goto err_unicam_put;2641 }2642 2643 unicam->clk_gate_base = devm_platform_ioremap_resource_byname(pdev, "cmi");2644 if (IS_ERR(unicam->clk_gate_base)) {2645 ret = PTR_ERR(unicam->clk_gate_base);2646 goto err_unicam_put;2647 }2648 2649 unicam->clock = devm_clk_get(&pdev->dev, "lp");2650 if (IS_ERR(unicam->clock)) {2651 dev_err(unicam->dev, "Failed to get lp clock\n");2652 ret = PTR_ERR(unicam->clock);2653 goto err_unicam_put;2654 }2655 2656 unicam->vpu_clock = devm_clk_get(&pdev->dev, "vpu");2657 if (IS_ERR(unicam->vpu_clock)) {2658 dev_err(unicam->dev, "Failed to get vpu clock\n");2659 ret = PTR_ERR(unicam->vpu_clock);2660 goto err_unicam_put;2661 }2662 2663 ret = platform_get_irq(pdev, 0);2664 if (ret < 0)2665 goto err_unicam_put;2666 2667 ret = devm_request_irq(&pdev->dev, ret, unicam_isr, 0,2668 "unicam_capture0", unicam);2669 if (ret) {2670 dev_err(&pdev->dev, "Unable to request interrupt\n");2671 goto err_unicam_put;2672 }2673 2674 /* Enable the block power domain. */2675 pm_runtime_enable(&pdev->dev);2676 2677 ret = unicam_media_init(unicam);2678 if (ret)2679 goto err_pm_runtime;2680 2681 ret = unicam_subdev_init(unicam);2682 if (ret)2683 goto err_media_unregister;2684 2685 ret = unicam_async_nf_init(unicam);2686 if (ret)2687 goto err_subdev_unregister;2688 2689 return 0;2690 2691err_subdev_unregister:2692 unicam_subdev_cleanup(unicam);2693err_media_unregister:2694 media_device_unregister(&unicam->mdev);2695err_pm_runtime:2696 pm_runtime_disable(&pdev->dev);2697err_unicam_put:2698 unicam_put(unicam);2699 2700 return ret;2701}2702 2703static void unicam_remove(struct platform_device *pdev)2704{2705 struct unicam_device *unicam = platform_get_drvdata(pdev);2706 2707 unicam_unregister_nodes(unicam);2708 v4l2_device_unregister(&unicam->v4l2_dev);2709 media_device_unregister(&unicam->mdev);2710 v4l2_async_nf_unregister(&unicam->notifier);2711 2712 unicam_subdev_cleanup(unicam);2713 2714 unicam_put(unicam);2715 2716 pm_runtime_disable(&pdev->dev);2717}2718 2719static const struct of_device_id unicam_of_match[] = {2720 { .compatible = "brcm,bcm2835-unicam", },2721 { /* sentinel */ },2722};2723MODULE_DEVICE_TABLE(of, unicam_of_match);2724 2725static struct platform_driver unicam_driver = {2726 .probe = unicam_probe,2727 .remove_new = unicam_remove,2728 .driver = {2729 .name = UNICAM_MODULE_NAME,2730 .pm = pm_ptr(&unicam_pm_ops),2731 .of_match_table = unicam_of_match,2732 },2733};2734 2735module_platform_driver(unicam_driver);2736 2737MODULE_AUTHOR("Dave Stevenson <dave.stevenson@raspberrypi.com>");2738MODULE_DESCRIPTION("BCM2835 Unicam driver");2739MODULE_LICENSE("GPL");2740