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