1 // SPDX-License-Identifier: GPL-2.0 2 /* 3 * ARM Mali-C55 ISP Driver - Video capture devices 4 * 5 * Copyright (C) 2025 Ideas on Board Oy 6 */ 7 8 #include <linux/cleanup.h> 9 #include <linux/minmax.h> 10 #include <linux/lockdep.h> 11 #include <linux/pm_runtime.h> 12 #include <linux/string.h> 13 #include <linux/videodev2.h> 14 15 #include <media/v4l2-dev.h> 16 #include <media/v4l2-event.h> 17 #include <media/v4l2-ioctl.h> 18 #include <media/v4l2-subdev.h> 19 #include <media/videobuf2-core.h> 20 #include <media/videobuf2-dma-contig.h> 21 22 #include "mali-c55-common.h" 23 #include "mali-c55-registers.h" 24 25 static const struct mali_c55_format_info mali_c55_fmts[] = { 26 /* 27 * This table is missing some entries which need further work or 28 * investigation: 29 * 30 * Base mode 5 is "Generic Data" 31 * Base mode 8 is a backwards V4L2_PIX_FMT_XYUV32 - no V4L2 equivalent 32 * Base mode 9 seems to have no V4L2 equivalent 33 * Base mode 17, 19 and 20 describe formats which seem to have no V4L2 34 * equivalent 35 */ 36 { 37 .fourcc = V4L2_PIX_FMT_XBGR32, 38 .mbus_codes = { 39 MEDIA_BUS_FMT_RGB121212_1X36, 40 MEDIA_BUS_FMT_RGB202020_1X60, 41 }, 42 .is_raw = false, 43 .registers = { 44 .base_mode = MALI_C55_OUTPUT_RGB32, 45 .uv_plane = MALI_C55_OUTPUT_PLANE_ALT0 46 } 47 }, 48 { 49 .fourcc = V4L2_PIX_FMT_ARGB2101010, 50 .mbus_codes = { 51 MEDIA_BUS_FMT_RGB121212_1X36, 52 MEDIA_BUS_FMT_RGB202020_1X60, 53 }, 54 .is_raw = false, 55 .registers = { 56 .base_mode = MALI_C55_OUTPUT_A2R10G10B10, 57 .uv_plane = MALI_C55_OUTPUT_PLANE_ALT0 58 } 59 }, 60 { 61 .fourcc = V4L2_PIX_FMT_RGB565, 62 .mbus_codes = { 63 MEDIA_BUS_FMT_RGB121212_1X36, 64 MEDIA_BUS_FMT_RGB202020_1X60, 65 }, 66 .is_raw = false, 67 .registers = { 68 .base_mode = MALI_C55_OUTPUT_RGB565, 69 .uv_plane = MALI_C55_OUTPUT_PLANE_ALT0 70 } 71 }, 72 { 73 .fourcc = V4L2_PIX_FMT_BGR24, 74 .mbus_codes = { 75 MEDIA_BUS_FMT_RGB121212_1X36, 76 MEDIA_BUS_FMT_RGB202020_1X60, 77 }, 78 .is_raw = false, 79 .registers = { 80 .base_mode = MALI_C55_OUTPUT_RGB24, 81 .uv_plane = MALI_C55_OUTPUT_PLANE_ALT0 82 } 83 }, 84 { 85 .fourcc = V4L2_PIX_FMT_YUYV, 86 .mbus_codes = { 87 MEDIA_BUS_FMT_YUV10_1X30, 88 }, 89 .is_raw = false, 90 .registers = { 91 .base_mode = MALI_C55_OUTPUT_YUY2, 92 .uv_plane = MALI_C55_OUTPUT_PLANE_ALT0 93 } 94 }, 95 { 96 .fourcc = V4L2_PIX_FMT_UYVY, 97 .mbus_codes = { 98 MEDIA_BUS_FMT_YUV10_1X30, 99 }, 100 .is_raw = false, 101 .registers = { 102 .base_mode = MALI_C55_OUTPUT_UYVY, 103 .uv_plane = MALI_C55_OUTPUT_PLANE_ALT0 104 } 105 }, 106 { 107 .fourcc = V4L2_PIX_FMT_Y210, 108 .mbus_codes = { 109 MEDIA_BUS_FMT_YUV10_1X30, 110 }, 111 .is_raw = false, 112 .registers = { 113 .base_mode = MALI_C55_OUTPUT_Y210, 114 .uv_plane = MALI_C55_OUTPUT_PLANE_ALT0 115 } 116 }, 117 /* 118 * This is something of a hack, the ISP thinks it's running NV12M but 119 * by setting uv_plane = 0 we simply discard that planes and only output 120 * the Y-plane. 121 */ 122 { 123 .fourcc = V4L2_PIX_FMT_GREY, 124 .mbus_codes = { 125 MEDIA_BUS_FMT_YUV10_1X30, 126 }, 127 .is_raw = false, 128 .registers = { 129 .base_mode = MALI_C55_OUTPUT_NV12_21, 130 .uv_plane = MALI_C55_OUTPUT_PLANE_ALT0 131 } 132 }, 133 { 134 .fourcc = V4L2_PIX_FMT_NV12M, 135 .mbus_codes = { 136 MEDIA_BUS_FMT_YUV10_1X30, 137 }, 138 .is_raw = false, 139 .registers = { 140 .base_mode = MALI_C55_OUTPUT_NV12_21, 141 .uv_plane = MALI_C55_OUTPUT_PLANE_ALT1 142 } 143 }, 144 { 145 .fourcc = V4L2_PIX_FMT_NV21M, 146 .mbus_codes = { 147 MEDIA_BUS_FMT_YUV10_1X30, 148 }, 149 .is_raw = false, 150 .registers = { 151 .base_mode = MALI_C55_OUTPUT_NV12_21, 152 .uv_plane = MALI_C55_OUTPUT_PLANE_ALT2 153 } 154 }, 155 /* 156 * RAW uncompressed formats are all packed in 16 bpp. 157 * TODO: Expand this list to encompass all possible RAW formats. 158 */ 159 { 160 .fourcc = V4L2_PIX_FMT_SRGGB16, 161 .mbus_codes = { 162 MEDIA_BUS_FMT_SRGGB16_1X16, 163 }, 164 .is_raw = true, 165 .registers = { 166 .base_mode = MALI_C55_OUTPUT_RAW16, 167 .uv_plane = MALI_C55_OUTPUT_PLANE_ALT0 168 } 169 }, 170 { 171 .fourcc = V4L2_PIX_FMT_SBGGR16, 172 .mbus_codes = { 173 MEDIA_BUS_FMT_SBGGR16_1X16, 174 }, 175 .is_raw = true, 176 .registers = { 177 .base_mode = MALI_C55_OUTPUT_RAW16, 178 .uv_plane = MALI_C55_OUTPUT_PLANE_ALT0 179 } 180 }, 181 { 182 .fourcc = V4L2_PIX_FMT_SGBRG16, 183 .mbus_codes = { 184 MEDIA_BUS_FMT_SGBRG16_1X16, 185 }, 186 .is_raw = true, 187 .registers = { 188 .base_mode = MALI_C55_OUTPUT_RAW16, 189 .uv_plane = MALI_C55_OUTPUT_PLANE_ALT0 190 } 191 }, 192 { 193 .fourcc = V4L2_PIX_FMT_SGRBG16, 194 .mbus_codes = { 195 MEDIA_BUS_FMT_SGRBG16_1X16, 196 }, 197 .is_raw = true, 198 .registers = { 199 .base_mode = MALI_C55_OUTPUT_RAW16, 200 .uv_plane = MALI_C55_OUTPUT_PLANE_ALT0 201 } 202 }, 203 }; 204 205 void mali_c55_cap_dev_write(struct mali_c55_cap_dev *cap_dev, unsigned int addr, 206 u32 val) 207 { 208 mali_c55_ctx_write(cap_dev->mali_c55, addr + cap_dev->reg_offset, val); 209 } 210 211 static u32 mali_c55_cap_dev_read(struct mali_c55_cap_dev *cap_dev, unsigned int addr) 212 { 213 return mali_c55_ctx_read(cap_dev->mali_c55, addr + cap_dev->reg_offset); 214 } 215 216 static void mali_c55_cap_dev_update_bits(struct mali_c55_cap_dev *cap_dev, 217 unsigned int addr, u32 mask, u32 val) 218 { 219 u32 orig, tmp; 220 221 orig = mali_c55_cap_dev_read(cap_dev, addr); 222 223 tmp = orig & ~mask; 224 tmp |= val & mask; 225 226 if (tmp != orig) 227 mali_c55_cap_dev_write(cap_dev, addr, tmp); 228 } 229 230 static bool 231 mali_c55_mbus_code_can_produce_fmt(const struct mali_c55_format_info *fmt, 232 u32 code) 233 { 234 unsigned int i; 235 236 for (i = 0; i < ARRAY_SIZE(fmt->mbus_codes); i++) { 237 if (fmt->mbus_codes[i] == code) 238 return true; 239 } 240 241 return false; 242 } 243 244 bool mali_c55_format_is_raw(unsigned int mbus_code) 245 { 246 unsigned int i; 247 248 for (i = 0; i < ARRAY_SIZE(mali_c55_fmts); i++) { 249 if (mali_c55_fmts[i].mbus_codes[0] == mbus_code) 250 return mali_c55_fmts[i].is_raw; 251 } 252 253 return false; 254 } 255 256 static const struct mali_c55_format_info * 257 mali_c55_format_from_pix(const u32 pixelformat) 258 { 259 unsigned int i; 260 261 for (i = 0; i < ARRAY_SIZE(mali_c55_fmts); i++) { 262 if (mali_c55_fmts[i].fourcc == pixelformat) 263 return &mali_c55_fmts[i]; 264 } 265 266 /* 267 * If we find no matching pixelformat, we'll just default to the first 268 * one for now. 269 */ 270 271 return &mali_c55_fmts[0]; 272 } 273 274 static const char * const capture_device_names[] = { 275 "mali-c55 fr", 276 "mali-c55 ds", 277 }; 278 279 static int mali_c55_link_validate(struct media_link *link) 280 { 281 struct video_device *vdev = 282 media_entity_to_video_device(link->sink->entity); 283 struct mali_c55_cap_dev *cap_dev = video_get_drvdata(vdev); 284 struct v4l2_subdev *sd = 285 media_entity_to_v4l2_subdev(link->source->entity); 286 const struct v4l2_pix_format_mplane *pix_mp; 287 const struct mali_c55_format_info *cap_fmt; 288 struct v4l2_subdev_format sd_fmt = { 289 .which = V4L2_SUBDEV_FORMAT_ACTIVE, 290 .pad = link->source->index, 291 }; 292 int ret; 293 294 ret = v4l2_subdev_call(sd, pad, get_fmt, NULL, &sd_fmt); 295 if (ret) 296 return ret; 297 298 pix_mp = &cap_dev->format.format; 299 cap_fmt = cap_dev->format.info; 300 301 if (sd_fmt.format.width != pix_mp->width || 302 sd_fmt.format.height != pix_mp->height) { 303 dev_dbg(cap_dev->mali_c55->dev, 304 "link '%s':%u -> '%s':%u not valid: %ux%u != %ux%u\n", 305 link->source->entity->name, link->source->index, 306 link->sink->entity->name, link->sink->index, 307 sd_fmt.format.width, sd_fmt.format.height, 308 pix_mp->width, pix_mp->height); 309 return -EPIPE; 310 } 311 312 if (!mali_c55_mbus_code_can_produce_fmt(cap_fmt, sd_fmt.format.code)) { 313 dev_dbg(cap_dev->mali_c55->dev, 314 "link '%s':%u -> '%s':%u not valid: mbus_code 0x%04x cannot produce pixel format %p4cc\n", 315 link->source->entity->name, link->source->index, 316 link->sink->entity->name, link->sink->index, 317 sd_fmt.format.code, &pix_mp->pixelformat); 318 return -EPIPE; 319 } 320 321 return 0; 322 } 323 324 static const struct media_entity_operations mali_c55_media_ops = { 325 .link_validate = mali_c55_link_validate, 326 }; 327 328 static int mali_c55_vb2_queue_setup(struct vb2_queue *q, unsigned int *num_buffers, 329 unsigned int *num_planes, unsigned int sizes[], 330 struct device *alloc_devs[]) 331 { 332 struct mali_c55_cap_dev *cap_dev = q->drv_priv; 333 unsigned int i; 334 335 if (*num_planes) { 336 if (*num_planes != cap_dev->format.format.num_planes) 337 return -EINVAL; 338 339 for (i = 0; i < cap_dev->format.format.num_planes; i++) 340 if (sizes[i] < cap_dev->format.format.plane_fmt[i].sizeimage) 341 return -EINVAL; 342 } else { 343 *num_planes = cap_dev->format.format.num_planes; 344 for (i = 0; i < cap_dev->format.format.num_planes; i++) 345 sizes[i] = cap_dev->format.format.plane_fmt[i].sizeimage; 346 } 347 348 return 0; 349 } 350 351 static void mali_c55_buf_queue(struct vb2_buffer *vb) 352 { 353 struct mali_c55_cap_dev *cap_dev = vb2_get_drv_priv(vb->vb2_queue); 354 struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb); 355 struct mali_c55_buffer *buf = container_of(vbuf, 356 struct mali_c55_buffer, vb); 357 unsigned int i; 358 359 buf->planes_pending = cap_dev->format.format.num_planes; 360 361 for (i = 0; i < cap_dev->format.format.num_planes; i++) { 362 unsigned long size = cap_dev->format.format.plane_fmt[i].sizeimage; 363 364 vb2_set_plane_payload(vb, i, size); 365 } 366 367 buf->vb.field = V4L2_FIELD_NONE; 368 369 guard(spinlock)(&cap_dev->buffers.lock); 370 list_add_tail(&buf->queue, &cap_dev->buffers.input); 371 } 372 373 static int mali_c55_buf_init(struct vb2_buffer *vb) 374 { 375 struct mali_c55_cap_dev *cap_dev = vb2_get_drv_priv(vb->vb2_queue); 376 struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb); 377 struct mali_c55_buffer *buf = container_of(vbuf, 378 struct mali_c55_buffer, vb); 379 unsigned int i; 380 381 for (i = 0; i < cap_dev->format.format.num_planes; i++) 382 buf->addrs[i] = vb2_dma_contig_plane_dma_addr(vb, i); 383 384 return 0; 385 } 386 387 void mali_c55_set_next_buffer(struct mali_c55_cap_dev *cap_dev) 388 { 389 struct v4l2_pix_format_mplane *pix_mp; 390 struct mali_c55_buffer *buf; 391 dma_addr_t *addrs; 392 393 scoped_guard(spinlock, &cap_dev->buffers.lock) { 394 buf = list_first_entry_or_null(&cap_dev->buffers.input, 395 struct mali_c55_buffer, queue); 396 if (buf) 397 list_del(&buf->queue); 398 } 399 400 if (!buf) { 401 /* 402 * If we underflow then we can tell the ISP that we don't want 403 * to write out the next frame. 404 */ 405 mali_c55_cap_dev_update_bits(cap_dev, 406 MALI_C55_REG_Y_WRITER_MODE, 407 MALI_C55_WRITER_FRAME_WRITE_MASK, 408 0x00); 409 mali_c55_cap_dev_update_bits(cap_dev, 410 MALI_C55_REG_UV_WRITER_MODE, 411 MALI_C55_WRITER_FRAME_WRITE_MASK, 412 0x00); 413 return; 414 } 415 416 pix_mp = &cap_dev->format.format; 417 418 mali_c55_cap_dev_update_bits(cap_dev, MALI_C55_REG_Y_WRITER_MODE, 419 MALI_C55_WRITER_FRAME_WRITE_MASK, 420 MALI_C55_WRITER_FRAME_WRITE_ENABLE); 421 if (cap_dev->format.info->registers.uv_plane) 422 mali_c55_cap_dev_update_bits(cap_dev, 423 MALI_C55_REG_UV_WRITER_MODE, 424 MALI_C55_WRITER_FRAME_WRITE_MASK, 425 MALI_C55_WRITER_FRAME_WRITE_ENABLE); 426 427 addrs = buf->addrs; 428 mali_c55_cap_dev_write(cap_dev, 429 MALI_C55_REG_Y_WRITER_BANKS_BASE, 430 addrs[MALI_C55_PLANE_Y]); 431 mali_c55_cap_dev_write(cap_dev, 432 MALI_C55_REG_UV_WRITER_BANKS_BASE, 433 addrs[MALI_C55_PLANE_UV]); 434 435 mali_c55_cap_dev_write(cap_dev, 436 MALI_C55_REG_Y_WRITER_OFFSET, 437 pix_mp->plane_fmt[MALI_C55_PLANE_Y].bytesperline); 438 mali_c55_cap_dev_write(cap_dev, 439 MALI_C55_REG_UV_WRITER_OFFSET, 440 pix_mp->plane_fmt[MALI_C55_PLANE_UV].bytesperline); 441 442 guard(spinlock)(&cap_dev->buffers.processing_lock); 443 list_add_tail(&buf->queue, &cap_dev->buffers.processing); 444 } 445 446 /** 447 * mali_c55_set_plane_done - mark the plane as written and process the buffer if 448 * both planes are finished. 449 * @cap_dev: pointer to the fr or ds pipe output 450 * @plane: the plane to mark as completed 451 * 452 * The Mali C55 ISP has muliplanar outputs for some formats that come with two 453 * separate "buffer write completed" interrupts - we need to flag each plane's 454 * completion and check whether both planes are done - if so, complete the buf 455 * in vb2. 456 */ 457 void mali_c55_set_plane_done(struct mali_c55_cap_dev *cap_dev, 458 enum mali_c55_planes plane) 459 { 460 struct mali_c55_isp *isp = &cap_dev->mali_c55->isp; 461 struct mali_c55_buffer *buf; 462 463 scoped_guard(spinlock, &cap_dev->buffers.processing_lock) { 464 buf = list_first_entry_or_null(&cap_dev->buffers.processing, 465 struct mali_c55_buffer, queue); 466 467 /* 468 * If the stream was stopped, the buffer might have been sent 469 * back to userspace already. 470 */ 471 if (!buf || --buf->planes_pending) 472 return; 473 474 list_del(&buf->queue); 475 } 476 477 /* If the other plane is also done... */ 478 buf->vb.vb2_buf.timestamp = ktime_get_boottime_ns(); 479 buf->vb.sequence = isp->frame_sequence++; 480 vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_DONE); 481 } 482 483 static void mali_c55_cap_dev_stream_disable(struct mali_c55_cap_dev *cap_dev) 484 { 485 mali_c55_cap_dev_update_bits(cap_dev, MALI_C55_REG_Y_WRITER_MODE, 486 MALI_C55_WRITER_FRAME_WRITE_MASK, 0x00); 487 mali_c55_cap_dev_update_bits(cap_dev, MALI_C55_REG_UV_WRITER_MODE, 488 MALI_C55_WRITER_FRAME_WRITE_MASK, 0x00); 489 } 490 491 static void mali_c55_cap_dev_stream_enable(struct mali_c55_cap_dev *cap_dev) 492 { 493 /* 494 * The Mali ISP can hold up to 5 buffer addresses and simply cycle 495 * through them, but it's not clear to me that the vb2 queue _guarantees_ 496 * it will queue buffers to the driver in a fixed order, and ensuring 497 * we call vb2_buffer_done() for the right buffer seems to me to add 498 * pointless complexity given in multi-context mode we'd need to 499 * re-write those registers every frame anyway...so we tell the ISP to 500 * use a single register and update it for each frame. 501 */ 502 mali_c55_cap_dev_update_bits(cap_dev, 503 MALI_C55_REG_Y_WRITER_BANKS_CONFIG, 504 MALI_C55_REG_Y_WRITER_MAX_BANKS_MASK, 0); 505 mali_c55_cap_dev_update_bits(cap_dev, 506 MALI_C55_REG_UV_WRITER_BANKS_CONFIG, 507 MALI_C55_REG_UV_WRITER_MAX_BANKS_MASK, 0); 508 509 mali_c55_set_next_buffer(cap_dev); 510 } 511 512 static void mali_c55_cap_dev_return_buffers(struct mali_c55_cap_dev *cap_dev, 513 enum vb2_buffer_state state) 514 { 515 struct mali_c55_buffer *buf, *tmp; 516 517 scoped_guard(spinlock, &cap_dev->buffers.lock) { 518 list_for_each_entry_safe(buf, tmp, &cap_dev->buffers.input, 519 queue) { 520 list_del(&buf->queue); 521 vb2_buffer_done(&buf->vb.vb2_buf, state); 522 } 523 } 524 525 guard(spinlock)(&cap_dev->buffers.processing_lock); 526 list_for_each_entry_safe(buf, tmp, &cap_dev->buffers.processing, queue) { 527 list_del(&buf->queue); 528 vb2_buffer_done(&buf->vb.vb2_buf, state); 529 } 530 } 531 532 static void mali_c55_cap_dev_format_configure(struct mali_c55_cap_dev *cap_dev) 533 { 534 const struct mali_c55_format_info *capture_format = cap_dev->format.info; 535 const struct v4l2_pix_format_mplane *pix_mp = &cap_dev->format.format; 536 const struct v4l2_format_info *info; 537 538 info = v4l2_format_info(pix_mp->pixelformat); 539 if (WARN_ON(!info)) 540 return; 541 542 mali_c55_cap_dev_write(cap_dev, MALI_C55_REG_Y_WRITER_MODE, 543 capture_format->registers.base_mode); 544 mali_c55_cap_dev_write(cap_dev, MALI_C55_REG_ACTIVE_OUT_Y_SIZE, 545 MALI_C55_REG_ACTIVE_OUT_SIZE_W(pix_mp->width) | 546 MALI_C55_REG_ACTIVE_OUT_SIZE_H(pix_mp->height)); 547 548 if (info->mem_planes > 1) { 549 mali_c55_cap_dev_write(cap_dev, MALI_C55_REG_UV_WRITER_MODE, 550 capture_format->registers.base_mode); 551 mali_c55_cap_dev_update_bits(cap_dev, 552 MALI_C55_REG_UV_WRITER_MODE, 553 MALI_C55_WRITER_SUBMODE_MASK, 554 MALI_C55_WRITER_SUBMODE(capture_format->registers.uv_plane)); 555 556 mali_c55_cap_dev_write(cap_dev, MALI_C55_REG_ACTIVE_OUT_UV_SIZE, 557 MALI_C55_REG_ACTIVE_OUT_SIZE_W(pix_mp->width) | 558 MALI_C55_REG_ACTIVE_OUT_SIZE_H(pix_mp->height)); 559 } 560 561 if (info->pixel_enc == V4L2_PIXEL_ENC_YUV) { 562 mali_c55_cap_dev_write(cap_dev, MALI_C55_REG_CS_CONV_CONFIG, 563 MALI_C55_CS_CONV_MATRIX_MASK); 564 565 if (info->hdiv > 1) 566 mali_c55_cap_dev_update_bits(cap_dev, 567 MALI_C55_REG_CS_CONV_CONFIG, 568 MALI_C55_CS_CONV_HORZ_DOWNSAMPLE_MASK, 569 MALI_C55_CS_CONV_HORZ_DOWNSAMPLE_ENABLE); 570 if (info->vdiv > 1) 571 mali_c55_cap_dev_update_bits(cap_dev, 572 MALI_C55_REG_CS_CONV_CONFIG, 573 MALI_C55_CS_CONV_VERT_DOWNSAMPLE_MASK, 574 MALI_C55_CS_CONV_VERT_DOWNSAMPLE_ENABLE); 575 if (info->hdiv > 1 || info->vdiv > 1) 576 mali_c55_cap_dev_update_bits(cap_dev, 577 MALI_C55_REG_CS_CONV_CONFIG, 578 MALI_C55_CS_CONV_FILTER_MASK, 579 MALI_C55_CS_CONV_FILTER_ENABLE); 580 } 581 } 582 583 static int mali_c55_vb2_start_streaming(struct vb2_queue *q, unsigned int count) 584 { 585 struct mali_c55_cap_dev *cap_dev = q->drv_priv; 586 struct mali_c55 *mali_c55 = cap_dev->mali_c55; 587 struct mali_c55_resizer *rsz = cap_dev->rsz; 588 struct mali_c55_isp *isp = &mali_c55->isp; 589 int ret; 590 591 guard(mutex)(&isp->capture_lock); 592 593 ret = pm_runtime_resume_and_get(mali_c55->dev); 594 if (ret) 595 goto err_return_buffers; 596 597 ret = video_device_pipeline_alloc_start(&cap_dev->vdev); 598 if (ret) { 599 dev_dbg(mali_c55->dev, "%s failed to start media pipeline\n", 600 cap_dev->vdev.name); 601 goto err_pm_put; 602 } 603 604 mali_c55_cap_dev_format_configure(cap_dev); 605 mali_c55_cap_dev_stream_enable(cap_dev); 606 607 ret = v4l2_subdev_enable_streams(&rsz->sd, MALI_C55_RSZ_SOURCE_PAD, 608 BIT(0)); 609 if (ret) 610 goto err_disable_cap_dev; 611 612 if (mali_c55_pipeline_ready(mali_c55)) { 613 ret = v4l2_subdev_enable_streams(&mali_c55->isp.sd, 614 MALI_C55_ISP_PAD_SOURCE_VIDEO, 615 BIT(0)); 616 if (ret < 0) 617 goto err_disable_rsz; 618 } 619 620 return 0; 621 622 err_disable_rsz: 623 v4l2_subdev_disable_streams(&rsz->sd, MALI_C55_RSZ_SOURCE_PAD, BIT(0)); 624 err_disable_cap_dev: 625 mali_c55_cap_dev_stream_disable(cap_dev); 626 video_device_pipeline_stop(&cap_dev->vdev); 627 err_pm_put: 628 pm_runtime_put_autosuspend(mali_c55->dev); 629 err_return_buffers: 630 mali_c55_cap_dev_return_buffers(cap_dev, VB2_BUF_STATE_QUEUED); 631 632 return ret; 633 } 634 635 static void mali_c55_vb2_stop_streaming(struct vb2_queue *q) 636 { 637 struct mali_c55_cap_dev *cap_dev = q->drv_priv; 638 struct mali_c55 *mali_c55 = cap_dev->mali_c55; 639 struct mali_c55_resizer *rsz = cap_dev->rsz; 640 struct mali_c55_isp *isp = &mali_c55->isp; 641 642 guard(mutex)(&isp->capture_lock); 643 644 if (mali_c55_pipeline_ready(mali_c55)) { 645 if (v4l2_subdev_is_streaming(&isp->sd)) 646 v4l2_subdev_disable_streams(&isp->sd, 647 MALI_C55_ISP_PAD_SOURCE_VIDEO, 648 BIT(0)); 649 } 650 651 v4l2_subdev_disable_streams(&rsz->sd, MALI_C55_RSZ_SOURCE_PAD, BIT(0)); 652 mali_c55_cap_dev_stream_disable(cap_dev); 653 mali_c55_cap_dev_return_buffers(cap_dev, VB2_BUF_STATE_ERROR); 654 video_device_pipeline_stop(&cap_dev->vdev); 655 pm_runtime_put_autosuspend(mali_c55->dev); 656 } 657 658 static const struct vb2_ops mali_c55_vb2_ops = { 659 .queue_setup = &mali_c55_vb2_queue_setup, 660 .buf_queue = &mali_c55_buf_queue, 661 .buf_init = &mali_c55_buf_init, 662 .start_streaming = &mali_c55_vb2_start_streaming, 663 .stop_streaming = &mali_c55_vb2_stop_streaming, 664 }; 665 666 static const struct v4l2_file_operations mali_c55_v4l2_fops = { 667 .owner = THIS_MODULE, 668 .unlocked_ioctl = video_ioctl2, 669 .open = v4l2_fh_open, 670 .release = vb2_fop_release, 671 .poll = vb2_fop_poll, 672 .mmap = vb2_fop_mmap, 673 }; 674 675 static void mali_c55_try_fmt(struct v4l2_pix_format_mplane *pix_mp) 676 { 677 const struct mali_c55_format_info *capture_format; 678 const struct v4l2_format_info *info; 679 struct v4l2_plane_pix_format *plane, *y_plane; 680 unsigned int padding; 681 unsigned int i; 682 683 capture_format = mali_c55_format_from_pix(pix_mp->pixelformat); 684 pix_mp->pixelformat = capture_format->fourcc; 685 686 pix_mp->width = clamp(pix_mp->width, MALI_C55_MIN_WIDTH, 687 MALI_C55_MAX_WIDTH); 688 pix_mp->height = clamp(pix_mp->height, MALI_C55_MIN_HEIGHT, 689 MALI_C55_MAX_HEIGHT); 690 691 pix_mp->field = V4L2_FIELD_NONE; 692 pix_mp->colorspace = V4L2_COLORSPACE_DEFAULT; 693 pix_mp->ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT; 694 pix_mp->quantization = V4L2_QUANTIZATION_DEFAULT; 695 696 info = v4l2_format_info(pix_mp->pixelformat); 697 pix_mp->num_planes = info->mem_planes; 698 memset(pix_mp->plane_fmt, 0, sizeof(pix_mp->plane_fmt)); 699 700 y_plane = &pix_mp->plane_fmt[0]; 701 y_plane->bytesperline = clamp(y_plane->bytesperline, 702 info->bpp[0] * pix_mp->width, 65535U); 703 704 /* 705 * The ISP requires that the stride be aligned to 16-bytes. This is not 706 * detailed in the documentation but has been verified experimentally. 707 */ 708 y_plane->bytesperline = ALIGN(y_plane->bytesperline, 16); 709 y_plane->sizeimage = y_plane->bytesperline * pix_mp->height; 710 711 padding = y_plane->bytesperline - (pix_mp->width * info->bpp[0]); 712 713 for (i = 1; i < info->comp_planes; i++) { 714 plane = &pix_mp->plane_fmt[i]; 715 716 plane->bytesperline = DIV_ROUND_UP(info->bpp[i] * pix_mp->width, 717 info->hdiv) + padding; 718 plane->sizeimage = DIV_ROUND_UP(plane->bytesperline * 719 pix_mp->height, info->vdiv); 720 } 721 722 if (info->mem_planes == 1) { 723 for (i = 1; i < info->comp_planes; i++) { 724 plane = &pix_mp->plane_fmt[i]; 725 y_plane->sizeimage += plane->sizeimage; 726 } 727 } 728 } 729 730 static int mali_c55_try_fmt_vid_cap_mplane(struct file *file, void *fh, 731 struct v4l2_format *f) 732 { 733 mali_c55_try_fmt(&f->fmt.pix_mp); 734 735 return 0; 736 } 737 738 static void mali_c55_set_format(struct mali_c55_cap_dev *cap_dev, 739 struct v4l2_pix_format_mplane *pix_mp) 740 { 741 mali_c55_try_fmt(pix_mp); 742 743 cap_dev->format.format = *pix_mp; 744 cap_dev->format.info = mali_c55_format_from_pix(pix_mp->pixelformat); 745 } 746 747 static int mali_c55_s_fmt_vid_cap_mplane(struct file *file, void *fh, 748 struct v4l2_format *f) 749 { 750 struct mali_c55_cap_dev *cap_dev = video_drvdata(file); 751 752 if (vb2_is_busy(&cap_dev->queue)) 753 return -EBUSY; 754 755 mali_c55_set_format(cap_dev, &f->fmt.pix_mp); 756 757 return 0; 758 } 759 760 static int mali_c55_g_fmt_vid_cap_mplane(struct file *file, void *fh, 761 struct v4l2_format *f) 762 { 763 struct mali_c55_cap_dev *cap_dev = video_drvdata(file); 764 765 f->fmt.pix_mp = cap_dev->format.format; 766 767 return 0; 768 } 769 770 static int mali_c55_enum_fmt_vid_cap_mplane(struct file *file, void *fh, 771 struct v4l2_fmtdesc *f) 772 { 773 struct mali_c55_cap_dev *cap_dev = video_drvdata(file); 774 unsigned int j = 0; 775 unsigned int i; 776 777 for (i = 0; i < ARRAY_SIZE(mali_c55_fmts); i++) { 778 if (f->mbus_code && 779 !mali_c55_mbus_code_can_produce_fmt(&mali_c55_fmts[i], 780 f->mbus_code)) 781 continue; 782 783 /* Downscale pipe can't output RAW formats */ 784 if (mali_c55_fmts[i].is_raw && 785 cap_dev->reg_offset == MALI_C55_CAP_DEV_DS_REG_OFFSET) 786 continue; 787 788 if (j++ == f->index) { 789 f->pixelformat = mali_c55_fmts[i].fourcc; 790 return 0; 791 } 792 } 793 794 return -EINVAL; 795 } 796 797 static int mali_c55_querycap(struct file *file, void *fh, 798 struct v4l2_capability *cap) 799 { 800 strscpy(cap->driver, MALI_C55_DRIVER_NAME, sizeof(cap->driver)); 801 strscpy(cap->card, "ARM Mali-C55 ISP", sizeof(cap->card)); 802 803 return 0; 804 } 805 806 static const struct v4l2_ioctl_ops mali_c55_v4l2_ioctl_ops = { 807 .vidioc_reqbufs = vb2_ioctl_reqbufs, 808 .vidioc_querybuf = vb2_ioctl_querybuf, 809 .vidioc_create_bufs = vb2_ioctl_create_bufs, 810 .vidioc_qbuf = vb2_ioctl_qbuf, 811 .vidioc_expbuf = vb2_ioctl_expbuf, 812 .vidioc_dqbuf = vb2_ioctl_dqbuf, 813 .vidioc_prepare_buf = vb2_ioctl_prepare_buf, 814 .vidioc_streamon = vb2_ioctl_streamon, 815 .vidioc_streamoff = vb2_ioctl_streamoff, 816 .vidioc_try_fmt_vid_cap_mplane = mali_c55_try_fmt_vid_cap_mplane, 817 .vidioc_s_fmt_vid_cap_mplane = mali_c55_s_fmt_vid_cap_mplane, 818 .vidioc_g_fmt_vid_cap_mplane = mali_c55_g_fmt_vid_cap_mplane, 819 .vidioc_enum_fmt_vid_cap = mali_c55_enum_fmt_vid_cap_mplane, 820 .vidioc_querycap = mali_c55_querycap, 821 .vidioc_subscribe_event = v4l2_ctrl_subscribe_event, 822 .vidioc_unsubscribe_event = v4l2_event_unsubscribe, 823 }; 824 825 static int mali_c55_register_cap_dev(struct mali_c55 *mali_c55, 826 enum mali_c55_cap_devs cap_dev_id) 827 { 828 struct mali_c55_cap_dev *cap_dev = &mali_c55->cap_devs[cap_dev_id]; 829 struct v4l2_pix_format_mplane pix_mp; 830 struct video_device *vdev; 831 struct vb2_queue *vb2q; 832 int ret; 833 834 vdev = &cap_dev->vdev; 835 vb2q = &cap_dev->queue; 836 837 cap_dev->mali_c55 = mali_c55; 838 mutex_init(&cap_dev->lock); 839 INIT_LIST_HEAD(&cap_dev->buffers.input); 840 INIT_LIST_HEAD(&cap_dev->buffers.processing); 841 spin_lock_init(&cap_dev->buffers.lock); 842 spin_lock_init(&cap_dev->buffers.processing_lock); 843 844 switch (cap_dev_id) { 845 case MALI_C55_CAP_DEV_FR: 846 cap_dev->rsz = &mali_c55->resizers[MALI_C55_RSZ_FR]; 847 cap_dev->reg_offset = MALI_C55_CAP_DEV_FR_REG_OFFSET; 848 break; 849 case MALI_C55_CAP_DEV_DS: 850 cap_dev->rsz = &mali_c55->resizers[MALI_C55_RSZ_DS]; 851 cap_dev->reg_offset = MALI_C55_CAP_DEV_DS_REG_OFFSET; 852 break; 853 default: 854 ret = -EINVAL; 855 goto err_destroy_mutex; 856 } 857 858 cap_dev->pad.flags = MEDIA_PAD_FL_SINK; 859 ret = media_entity_pads_init(&cap_dev->vdev.entity, 1, &cap_dev->pad); 860 if (ret) { 861 mutex_destroy(&cap_dev->lock); 862 goto err_destroy_mutex; 863 } 864 865 vb2q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE; 866 vb2q->io_modes = VB2_MMAP | VB2_DMABUF; 867 vb2q->drv_priv = cap_dev; 868 vb2q->mem_ops = &vb2_dma_contig_memops; 869 vb2q->ops = &mali_c55_vb2_ops; 870 vb2q->buf_struct_size = sizeof(struct mali_c55_buffer); 871 vb2q->min_queued_buffers = 1; 872 vb2q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC; 873 vb2q->lock = &cap_dev->lock; 874 vb2q->dev = mali_c55->dev; 875 876 ret = vb2_queue_init(vb2q); 877 if (ret) { 878 dev_err(mali_c55->dev, "%s vb2 queue init failed\n", 879 cap_dev->vdev.name); 880 goto err_cleanup_media_entity; 881 } 882 883 strscpy(cap_dev->vdev.name, capture_device_names[cap_dev_id], 884 sizeof(cap_dev->vdev.name)); 885 vdev->release = video_device_release_empty; 886 vdev->fops = &mali_c55_v4l2_fops; 887 vdev->ioctl_ops = &mali_c55_v4l2_ioctl_ops; 888 vdev->lock = &cap_dev->lock; 889 vdev->v4l2_dev = &mali_c55->v4l2_dev; 890 vdev->queue = &cap_dev->queue; 891 vdev->device_caps = V4L2_CAP_VIDEO_CAPTURE_MPLANE | 892 V4L2_CAP_STREAMING | V4L2_CAP_IO_MC; 893 vdev->entity.ops = &mali_c55_media_ops; 894 video_set_drvdata(vdev, cap_dev); 895 896 memset(&pix_mp, 0, sizeof(pix_mp)); 897 pix_mp.pixelformat = V4L2_PIX_FMT_RGB565; 898 pix_mp.width = MALI_C55_DEFAULT_WIDTH; 899 pix_mp.height = MALI_C55_DEFAULT_HEIGHT; 900 mali_c55_set_format(cap_dev, &pix_mp); 901 902 ret = video_register_device(vdev, VFL_TYPE_VIDEO, -1); 903 if (ret) { 904 dev_err(mali_c55->dev, 905 "%s failed to register video device\n", 906 cap_dev->vdev.name); 907 goto err_release_vb2q; 908 } 909 910 return 0; 911 912 err_release_vb2q: 913 vb2_queue_release(vb2q); 914 err_cleanup_media_entity: 915 media_entity_cleanup(&cap_dev->vdev.entity); 916 err_destroy_mutex: 917 mutex_destroy(&cap_dev->lock); 918 919 return ret; 920 } 921 922 int mali_c55_register_capture_devs(struct mali_c55 *mali_c55) 923 { 924 int ret; 925 926 ret = mali_c55_register_cap_dev(mali_c55, MALI_C55_CAP_DEV_FR); 927 if (ret) 928 return ret; 929 930 if (mali_c55->capabilities & MALI_C55_GPS_DS_PIPE_FITTED) { 931 ret = mali_c55_register_cap_dev(mali_c55, MALI_C55_CAP_DEV_DS); 932 if (ret) { 933 mali_c55_unregister_capture_devs(mali_c55); 934 return ret; 935 } 936 } 937 938 return 0; 939 } 940 941 static void mali_c55_unregister_cap_dev(struct mali_c55 *mali_c55, 942 enum mali_c55_cap_devs cap_dev_id) 943 { 944 struct mali_c55_cap_dev *cap_dev = &mali_c55->cap_devs[cap_dev_id]; 945 946 if (!video_is_registered(&cap_dev->vdev)) 947 return; 948 949 vb2_video_unregister_device(&cap_dev->vdev); 950 media_entity_cleanup(&cap_dev->vdev.entity); 951 mutex_destroy(&cap_dev->lock); 952 } 953 954 void mali_c55_unregister_capture_devs(struct mali_c55 *mali_c55) 955 { 956 mali_c55_unregister_cap_dev(mali_c55, MALI_C55_CAP_DEV_FR); 957 if (mali_c55->capabilities & MALI_C55_GPS_DS_PIPE_FITTED) 958 mali_c55_unregister_cap_dev(mali_c55, MALI_C55_CAP_DEV_DS); 959 } 960