1// SPDX-License-Identifier: GPL-2.0-only 2/* 3 * BCM283x / BCM271x Unicam Capture Driver 4 * 5 * Copyright (C) 2017-2020 - Raspberry Pi (Trading) Ltd. 6 * Copyright (C) 2024 - Ideas on Board 7 * 8 * Dave Stevenson <dave.stevenson@raspberrypi.com> 9 * 10 * Based on TI am437x driver by 11 * Benoit Parrot <bparrot@ti.com> 12 * Lad, Prabhakar <prabhakar.csengg@gmail.com> 13 * 14 * and TI CAL camera interface driver by 15 * Benoit Parrot <bparrot@ti.com> 16 * 17 * 18 * There are two camera drivers in the kernel for BCM283x - this one and 19 * bcm2835-camera (currently in staging). 20 * 21 * This driver directly controls the Unicam peripheral - there is no 22 * involvement with the VideoCore firmware. Unicam receives CSI-2 or CCP2 data 23 * and writes it into SDRAM. The only potential processing options are to 24 * repack Bayer data into an alternate format, and applying windowing. The 25 * repacking does not shift the data, so can repack V4L2_PIX_FMT_Sxxxx10P to 26 * V4L2_PIX_FMT_Sxxxx10, or V4L2_PIX_FMT_Sxxxx12P to V4L2_PIX_FMT_Sxxxx12, but 27 * not generically up to V4L2_PIX_FMT_Sxxxx16. Support for windowing may be 28 * added later. 29 * 30 * It should be possible to connect this driver to any sensor with a suitable 31 * 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 16 73 74/* 75 * The image stride is stored in a 16 bit register, and needs to be aligned to 76 * the DMA constraint. As the ISP in the same SoC has a 32 bytes alignment 77 * constraint on its input, set the image stride alignment to 32 bytes here as 78 * well to avoid incompatible configurations. 79 */ 80#define UNICAM_IMAGE_BPL_ALIGNMENT 32 81#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 of 85 * 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 16 89#define UNICAM_IMAGE_MIN_HEIGHT 16 90#define UNICAM_IMAGE_MAX_WIDTH (UNICAM_IMAGE_MAX_BPL / 4) 91#define UNICAM_IMAGE_MAX_HEIGHT UNICAM_IMAGE_MAX_WIDTH 92 93/* 94 * There's no intrinsic limits on the width and height for embedded data. Use 95 * the same maximum values as for the image, to avoid overflows in the image 96 * size computation. 97 */ 98#define UNICAM_META_MIN_WIDTH 1 99#define UNICAM_META_MIN_HEIGHT 1 100#define UNICAM_META_MAX_WIDTH UNICAM_IMAGE_MAX_WIDTH 101#define UNICAM_META_MAX_HEIGHT UNICAM_IMAGE_MAX_HEIGHT 102 103/* 104 * Size of the dummy buffer. Can be any size really, but the DMA 105 * allocation works in units of page sizes. 106 */ 107#define UNICAM_DUMMY_BUF_SIZE PAGE_SIZE 108 109enum unicam_pad { 110 UNICAM_SD_PAD_SINK, 111 UNICAM_SD_PAD_SOURCE_IMAGE, 112 UNICAM_SD_PAD_SOURCE_METADATA, 113 UNICAM_SD_NUM_PADS 114}; 115 116enum unicam_node_type { 117 UNICAM_IMAGE_NODE, 118 UNICAM_METADATA_NODE, 119 UNICAM_MAX_NODES 120}; 121 122/* 123 * struct unicam_format_info - Unicam media bus format information 124 * @fourcc: V4L2 pixel format FCC identifier. 0 if n/a. 125 * @unpacked_fourcc: V4L2 pixel format FCC identifier if the data is expanded 126 * 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_fourcc 131 */ 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 unicam 176 * 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, or 219 * 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 functions 273 */ 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 functions 293 */ 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 else 571 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 handling 597 */ 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 status 734 * 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_frm 740 * and we get a simultaneout FE + FS interrupt, running the FS handler 741 * first would null out the next_frm ptr and we would have lost the 742 * buffer forever. 743 */ 744 if (fe) { 745 /* 746 * Ensure we have swapped buffers already as we can't 747 * stop the peripheral. If no buffer is available, use a 748 * dummy buffer to dump out frames until we get a new buffer 749 * 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 had 759 * a chance to swap buffers, likely due to having 760 * multiple interrupts occurring simultaneously (like FE 761 * + FS + LS). In this case, we cannot signal the buffer 762 * 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 else 786 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 frame 791 * if we have not managed to obtain another frame 792 * 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 condition 802 * where the HW does not actually swap it if the new frame has 803 * 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] - DAT3 928 * [7:6] - DAT2 929 * [5:4] - DAT1 930 * [3:2] - DAT0 931 * [1:0] - CLK 932 * 933 * Enabled lane must be set to b01, and disabled lanes to b00. The clock 934 * 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 for 1038 * 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 the 1064 * 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 to 1084 * 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 to 1105 * 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 operations 1153 */ 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 else 1178 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 source 1334 * pad the stream is routed to. Find the corresponding source pad and 1335 * 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_IMAGE 1346 ? &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 operations 1543 */ 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.sizeimage 1551 : 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.sizeimage 1573 : 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 the 1665 * 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 the 1675 * 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 started 1717 * before starting the hardware. In the general case, this would 1718 * prevent capturing multiple streams independently. However, the 1719 * Unicam DMA engines are not generic, they have been designed to 1720 * capture image data and embedded data from the same camera sensor. 1721 * Not only does the main use case not benefit from independent 1722 * capture, it requires proper synchronization of the streams at start 1723 * 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 operations 1814 */ 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't 1875 * 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't 1952 * 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_IMAGE 2116 : 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_NODE 2221 ? UNICAM_SD_PAD_SOURCE_IMAGE 2222 : 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_CAPTURE 2237 : 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_NODE 2263 ? 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 the 2312 * video device, which will drop the reference on the 2313 * 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 management 2354 */ 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 notifier 2416 */ 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 & remove 2585 */ 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