1 // SPDX-License-Identifier: (GPL-2.0-only OR MIT) 2 /* 3 * Copyright (C) 2024 Amlogic, Inc. All rights reserved 4 */ 5 6 #include <linux/cleanup.h> 7 #include <linux/pm_runtime.h> 8 9 #include <media/v4l2-ctrls.h> 10 #include <media/v4l2-event.h> 11 #include <media/v4l2-ioctl.h> 12 #include <media/v4l2-mc.h> 13 #include <media/videobuf2-dma-contig.h> 14 15 #include "c3-isp-common.h" 16 #include "c3-isp-regs.h" 17 18 #define C3_ISP_WRMIFX3_REG(addr, id) ((addr) + (id) * 0x100) 19 20 static const struct c3_isp_cap_format_info cap_formats[] = { 21 /* YUV formats */ 22 { 23 .mbus_code = MEDIA_BUS_FMT_YUV10_1X30, 24 .fourcc = V4L2_PIX_FMT_GREY, 25 .format = ISP_WRMIFX3_0_FMT_CTRL_MODE_OUT_Y_ONLY, 26 .planes = ISP_WRMIFX3_0_FMT_CTRL_MTX_PLANE_X1, 27 .ch0_pix_bits = ISP_WRMIFX3_0_CH0_CTRL1_PIX_BITS_8BITS, 28 .uv_swap = ISP_WRMIFX3_0_FMT_CTRL_MTX_UV_SWAP_VU, 29 .in_bits = ISP_WRMIFX3_0_FMT_CTRL_MTX_IBITS_8BIT, 30 .hdiv = 1, 31 .vdiv = 1, 32 }, { 33 .mbus_code = MEDIA_BUS_FMT_YUV10_1X30, 34 .fourcc = V4L2_PIX_FMT_NV12M, 35 .format = ISP_WRMIFX3_0_FMT_CTRL_MODE_OUT_YUV420, 36 .planes = ISP_WRMIFX3_0_FMT_CTRL_MTX_PLANE_X2, 37 .ch0_pix_bits = ISP_WRMIFX3_0_CH0_CTRL1_PIX_BITS_8BITS, 38 .uv_swap = ISP_WRMIFX3_0_FMT_CTRL_MTX_UV_SWAP_UV, 39 .in_bits = ISP_WRMIFX3_0_FMT_CTRL_MTX_IBITS_8BIT, 40 .hdiv = 2, 41 .vdiv = 2, 42 }, { 43 .mbus_code = MEDIA_BUS_FMT_YUV10_1X30, 44 .fourcc = V4L2_PIX_FMT_NV21M, 45 .format = ISP_WRMIFX3_0_FMT_CTRL_MODE_OUT_YUV420, 46 .planes = ISP_WRMIFX3_0_FMT_CTRL_MTX_PLANE_X2, 47 .ch0_pix_bits = ISP_WRMIFX3_0_CH0_CTRL1_PIX_BITS_8BITS, 48 .uv_swap = ISP_WRMIFX3_0_FMT_CTRL_MTX_UV_SWAP_VU, 49 .in_bits = ISP_WRMIFX3_0_FMT_CTRL_MTX_IBITS_8BIT, 50 .hdiv = 2, 51 .vdiv = 2, 52 }, { 53 .mbus_code = MEDIA_BUS_FMT_YUV10_1X30, 54 .fourcc = V4L2_PIX_FMT_NV16M, 55 .format = ISP_WRMIFX3_0_FMT_CTRL_MODE_OUT_YUV422, 56 .planes = ISP_WRMIFX3_0_FMT_CTRL_MTX_PLANE_X2, 57 .ch0_pix_bits = ISP_WRMIFX3_0_CH0_CTRL1_PIX_BITS_8BITS, 58 .uv_swap = ISP_WRMIFX3_0_FMT_CTRL_MTX_UV_SWAP_UV, 59 .in_bits = ISP_WRMIFX3_0_FMT_CTRL_MTX_IBITS_8BIT, 60 .hdiv = 1, 61 .vdiv = 2 62 }, { 63 .mbus_code = MEDIA_BUS_FMT_YUV10_1X30, 64 .fourcc = V4L2_PIX_FMT_NV61M, 65 .format = ISP_WRMIFX3_0_FMT_CTRL_MODE_OUT_YUV422, 66 .planes = ISP_WRMIFX3_0_FMT_CTRL_MTX_PLANE_X2, 67 .ch0_pix_bits = ISP_WRMIFX3_0_CH0_CTRL1_PIX_BITS_8BITS, 68 .uv_swap = ISP_WRMIFX3_0_FMT_CTRL_MTX_UV_SWAP_VU, 69 .in_bits = ISP_WRMIFX3_0_FMT_CTRL_MTX_IBITS_8BIT, 70 .hdiv = 1, 71 .vdiv = 2, 72 }, 73 /* RAW formats */ 74 { 75 .mbus_code = MEDIA_BUS_FMT_SRGGB16_1X16, 76 .fourcc = V4L2_PIX_FMT_SRGGB12, 77 .format = ISP_WRMIFX3_0_FMT_CTRL_MODE_OUT_RAW, 78 .planes = ISP_WRMIFX3_0_FMT_CTRL_MTX_PLANE_X1, 79 .ch0_pix_bits = ISP_WRMIFX3_0_CH0_CTRL1_PIX_BITS_16BITS, 80 .uv_swap = ISP_WRMIFX3_0_FMT_CTRL_MTX_UV_SWAP_VU, 81 .in_bits = ISP_WRMIFX3_0_FMT_CTRL_MTX_IBITS_16BIT, 82 .hdiv = 1, 83 .vdiv = 1, 84 }, { 85 .mbus_code = MEDIA_BUS_FMT_SBGGR16_1X16, 86 .fourcc = V4L2_PIX_FMT_SBGGR12, 87 .format = ISP_WRMIFX3_0_FMT_CTRL_MODE_OUT_RAW, 88 .planes = ISP_WRMIFX3_0_FMT_CTRL_MTX_PLANE_X1, 89 .ch0_pix_bits = ISP_WRMIFX3_0_CH0_CTRL1_PIX_BITS_16BITS, 90 .uv_swap = ISP_WRMIFX3_0_FMT_CTRL_MTX_UV_SWAP_VU, 91 .in_bits = ISP_WRMIFX3_0_FMT_CTRL_MTX_IBITS_16BIT, 92 .hdiv = 1, 93 .vdiv = 1, 94 }, { 95 .mbus_code = MEDIA_BUS_FMT_SGRBG16_1X16, 96 .fourcc = V4L2_PIX_FMT_SGRBG12, 97 .format = ISP_WRMIFX3_0_FMT_CTRL_MODE_OUT_RAW, 98 .planes = ISP_WRMIFX3_0_FMT_CTRL_MTX_PLANE_X1, 99 .ch0_pix_bits = ISP_WRMIFX3_0_CH0_CTRL1_PIX_BITS_16BITS, 100 .uv_swap = ISP_WRMIFX3_0_FMT_CTRL_MTX_UV_SWAP_VU, 101 .in_bits = ISP_WRMIFX3_0_FMT_CTRL_MTX_IBITS_16BIT, 102 .hdiv = 1, 103 .vdiv = 1, 104 }, { 105 .mbus_code = MEDIA_BUS_FMT_SGBRG16_1X16, 106 .fourcc = V4L2_PIX_FMT_SGBRG12, 107 .format = ISP_WRMIFX3_0_FMT_CTRL_MODE_OUT_RAW, 108 .planes = ISP_WRMIFX3_0_FMT_CTRL_MTX_PLANE_X1, 109 .ch0_pix_bits = ISP_WRMIFX3_0_CH0_CTRL1_PIX_BITS_16BITS, 110 .uv_swap = ISP_WRMIFX3_0_FMT_CTRL_MTX_UV_SWAP_VU, 111 .in_bits = ISP_WRMIFX3_0_FMT_CTRL_MTX_IBITS_16BIT, 112 .hdiv = 1, 113 .vdiv = 1, 114 }, 115 }; 116 117 /* Hardware configuration */ 118 119 /* Set the address of wrmifx3(write memory interface) */ 120 static void c3_isp_cap_wrmifx3_buff(struct c3_isp_capture *cap) 121 { 122 dma_addr_t y_dma_addr; 123 dma_addr_t uv_dma_addr; 124 125 if (cap->buff) { 126 y_dma_addr = cap->buff->dma_addr[C3_ISP_PLANE_Y]; 127 uv_dma_addr = cap->buff->dma_addr[C3_ISP_PLANE_UV]; 128 } else { 129 y_dma_addr = cap->dummy_buff.dma_addr; 130 uv_dma_addr = cap->dummy_buff.dma_addr; 131 } 132 133 c3_isp_write(cap->isp, 134 C3_ISP_WRMIFX3_REG(ISP_WRMIFX3_0_CH0_BADDR, cap->id), 135 ISP_WRMIFX3_0_CH0_BASE_ADDR(y_dma_addr)); 136 137 c3_isp_write(cap->isp, 138 C3_ISP_WRMIFX3_REG(ISP_WRMIFX3_0_CH1_BADDR, cap->id), 139 ISP_WRMIFX3_0_CH1_BASE_ADDR(uv_dma_addr)); 140 } 141 142 static void c3_isp_cap_wrmifx3_format(struct c3_isp_capture *cap) 143 { 144 struct v4l2_pix_format_mplane *pix_mp = &cap->format.pix_mp; 145 const struct c3_isp_cap_format_info *info = cap->format.info; 146 u32 stride; 147 u32 chrom_h; 148 u32 chrom_v; 149 150 c3_isp_write(cap->isp, 151 C3_ISP_WRMIFX3_REG(ISP_WRMIFX3_0_FMT_SIZE, cap->id), 152 ISP_WRMIFX3_0_FMT_SIZE_HSIZE(pix_mp->width) | 153 ISP_WRMIFX3_0_FMT_SIZE_VSIZE(pix_mp->height)); 154 155 c3_isp_update_bits(cap->isp, 156 C3_ISP_WRMIFX3_REG(ISP_WRMIFX3_0_FMT_CTRL, cap->id), 157 ISP_WRMIFX3_0_FMT_CTRL_MODE_OUT_MASK, info->format); 158 159 c3_isp_update_bits(cap->isp, 160 C3_ISP_WRMIFX3_REG(ISP_WRMIFX3_0_FMT_CTRL, cap->id), 161 ISP_WRMIFX3_0_FMT_CTRL_MTX_IBITS_MASK, 162 info->in_bits); 163 164 c3_isp_update_bits(cap->isp, 165 C3_ISP_WRMIFX3_REG(ISP_WRMIFX3_0_FMT_CTRL, cap->id), 166 ISP_WRMIFX3_0_FMT_CTRL_MTX_PLANE_MASK, info->planes); 167 168 c3_isp_update_bits(cap->isp, 169 C3_ISP_WRMIFX3_REG(ISP_WRMIFX3_0_FMT_CTRL, cap->id), 170 ISP_WRMIFX3_0_FMT_CTRL_MTX_UV_SWAP_MASK, 171 info->uv_swap); 172 173 stride = DIV_ROUND_UP(pix_mp->plane_fmt[C3_ISP_PLANE_Y].bytesperline, 174 C3_ISP_DMA_SIZE_ALIGN_BYTES); 175 c3_isp_update_bits(cap->isp, 176 C3_ISP_WRMIFX3_REG(ISP_WRMIFX3_0_CH0_CTRL0, cap->id), 177 ISP_WRMIFX3_0_CH0_CTRL0_STRIDE_MASK, 178 ISP_WRMIFX3_0_CH0_CTRL0_STRIDE(stride)); 179 180 c3_isp_update_bits(cap->isp, 181 C3_ISP_WRMIFX3_REG(ISP_WRMIFX3_0_CH0_CTRL1, cap->id), 182 ISP_WRMIFX3_0_CH0_CTRL1_PIX_BITS_MODE_MASK, 183 info->ch0_pix_bits); 184 185 c3_isp_write(cap->isp, 186 C3_ISP_WRMIFX3_REG(ISP_WRMIFX3_0_WIN_LUMA_H, cap->id), 187 ISP_WRMIFX3_0_WIN_LUMA_H_LUMA_HEND(pix_mp->width)); 188 189 c3_isp_write(cap->isp, 190 C3_ISP_WRMIFX3_REG(ISP_WRMIFX3_0_WIN_LUMA_V, cap->id), 191 ISP_WRMIFX3_0_WIN_LUMA_V_LUMA_VEND(pix_mp->height)); 192 193 stride = DIV_ROUND_UP(pix_mp->plane_fmt[C3_ISP_PLANE_UV].bytesperline, 194 C3_ISP_DMA_SIZE_ALIGN_BYTES); 195 c3_isp_update_bits(cap->isp, 196 C3_ISP_WRMIFX3_REG(ISP_WRMIFX3_0_CH1_CTRL0, cap->id), 197 ISP_WRMIFX3_0_CH1_CTRL0_STRIDE_MASK, 198 ISP_WRMIFX3_0_CH1_CTRL0_STRIDE(stride)); 199 200 c3_isp_update_bits(cap->isp, 201 C3_ISP_WRMIFX3_REG(ISP_WRMIFX3_0_CH1_CTRL1, cap->id), 202 ISP_WRMIFX3_0_CH1_CTRL1_PIX_BITS_MODE_MASK, 203 ISP_WRMIFX3_0_CH1_CTRL1_PIX_BITS_16BITS); 204 205 chrom_h = DIV_ROUND_UP(pix_mp->width, info->hdiv); 206 c3_isp_write(cap->isp, 207 C3_ISP_WRMIFX3_REG(ISP_WRMIFX3_0_WIN_CHROM_H, cap->id), 208 ISP_WRMIFX3_0_WIN_CHROM_H_CHROM_HEND(chrom_h)); 209 210 chrom_v = DIV_ROUND_UP(pix_mp->height, info->vdiv); 211 c3_isp_write(cap->isp, 212 C3_ISP_WRMIFX3_REG(ISP_WRMIFX3_0_WIN_CHROM_V, cap->id), 213 ISP_WRMIFX3_0_WIN_CHROM_V_CHROM_VEND(chrom_v)); 214 } 215 216 static int c3_isp_cap_dummy_buff_create(struct c3_isp_capture *cap) 217 { 218 struct c3_isp_dummy_buffer *dummy_buff = &cap->dummy_buff; 219 struct v4l2_pix_format_mplane *pix_mp = &cap->format.pix_mp; 220 221 if (pix_mp->num_planes == 1) 222 dummy_buff->size = pix_mp->plane_fmt[C3_ISP_PLANE_Y].sizeimage; 223 else 224 dummy_buff->size = 225 max(pix_mp->plane_fmt[C3_ISP_PLANE_Y].sizeimage, 226 pix_mp->plane_fmt[C3_ISP_PLANE_UV].sizeimage); 227 228 /* The driver never access vaddr, no mapping is required */ 229 dummy_buff->vaddr = dma_alloc_attrs(cap->isp->dev, dummy_buff->size, 230 &dummy_buff->dma_addr, GFP_KERNEL, 231 DMA_ATTR_NO_KERNEL_MAPPING); 232 if (!dummy_buff->vaddr) 233 return -ENOMEM; 234 235 return 0; 236 } 237 238 static void c3_isp_cap_dummy_buff_destroy(struct c3_isp_capture *cap) 239 { 240 dma_free_attrs(cap->isp->dev, cap->dummy_buff.size, 241 cap->dummy_buff.vaddr, cap->dummy_buff.dma_addr, 242 DMA_ATTR_NO_KERNEL_MAPPING); 243 } 244 245 static void c3_isp_cap_cfg_buff(struct c3_isp_capture *cap) 246 { 247 cap->buff = list_first_entry_or_null(&cap->pending, 248 struct c3_isp_cap_buffer, list); 249 250 c3_isp_cap_wrmifx3_buff(cap); 251 252 if (cap->buff) 253 list_del(&cap->buff->list); 254 } 255 256 static void c3_isp_cap_start(struct c3_isp_capture *cap) 257 { 258 u32 mask; 259 u32 val; 260 261 scoped_guard(spinlock_irqsave, &cap->buff_lock) 262 c3_isp_cap_cfg_buff(cap); 263 264 c3_isp_cap_wrmifx3_format(cap); 265 266 if (cap->id == C3_ISP_CAP_DEV_0) { 267 mask = ISP_TOP_PATH_EN_WRMIF0_EN_MASK; 268 val = ISP_TOP_PATH_EN_WRMIF0_EN; 269 } else if (cap->id == C3_ISP_CAP_DEV_1) { 270 mask = ISP_TOP_PATH_EN_WRMIF1_EN_MASK; 271 val = ISP_TOP_PATH_EN_WRMIF1_EN; 272 } else { 273 mask = ISP_TOP_PATH_EN_WRMIF2_EN_MASK; 274 val = ISP_TOP_PATH_EN_WRMIF2_EN; 275 } 276 277 c3_isp_update_bits(cap->isp, ISP_TOP_PATH_EN, mask, val); 278 } 279 280 static void c3_isp_cap_stop(struct c3_isp_capture *cap) 281 { 282 u32 mask; 283 u32 val; 284 285 if (cap->id == C3_ISP_CAP_DEV_0) { 286 mask = ISP_TOP_PATH_EN_WRMIF0_EN_MASK; 287 val = ISP_TOP_PATH_EN_WRMIF0_DIS; 288 } else if (cap->id == C3_ISP_CAP_DEV_1) { 289 mask = ISP_TOP_PATH_EN_WRMIF1_EN_MASK; 290 val = ISP_TOP_PATH_EN_WRMIF1_DIS; 291 } else { 292 mask = ISP_TOP_PATH_EN_WRMIF2_EN_MASK; 293 val = ISP_TOP_PATH_EN_WRMIF2_DIS; 294 } 295 296 c3_isp_update_bits(cap->isp, ISP_TOP_PATH_EN, mask, val); 297 } 298 299 static void c3_isp_cap_done(struct c3_isp_capture *cap) 300 { 301 struct c3_isp_cap_buffer *buff = cap->buff; 302 303 guard(spinlock_irqsave)(&cap->buff_lock); 304 305 if (buff) { 306 buff->vb.sequence = cap->isp->frm_sequence; 307 buff->vb.vb2_buf.timestamp = ktime_get(); 308 buff->vb.field = V4L2_FIELD_NONE; 309 vb2_buffer_done(&buff->vb.vb2_buf, VB2_BUF_STATE_DONE); 310 } 311 312 c3_isp_cap_cfg_buff(cap); 313 } 314 315 /* V4L2 video operations */ 316 317 static const struct c3_isp_cap_format_info *c3_cap_find_fmt(u32 fourcc) 318 { 319 for (unsigned int i = 0; i < ARRAY_SIZE(cap_formats); i++) { 320 if (cap_formats[i].fourcc == fourcc) 321 return &cap_formats[i]; 322 } 323 324 return NULL; 325 } 326 327 static void c3_cap_try_fmt(struct v4l2_pix_format_mplane *pix_mp) 328 { 329 const struct c3_isp_cap_format_info *fmt; 330 const struct v4l2_format_info *info; 331 struct v4l2_plane_pix_format *plane; 332 333 fmt = c3_cap_find_fmt(pix_mp->pixelformat); 334 if (!fmt) 335 fmt = &cap_formats[0]; 336 337 pix_mp->width = clamp(pix_mp->width, C3_ISP_MIN_WIDTH, 338 C3_ISP_MAX_WIDTH); 339 pix_mp->height = clamp(pix_mp->height, C3_ISP_MIN_HEIGHT, 340 C3_ISP_MAX_HEIGHT); 341 pix_mp->pixelformat = fmt->fourcc; 342 pix_mp->field = V4L2_FIELD_NONE; 343 pix_mp->colorspace = V4L2_COLORSPACE_SRGB; 344 pix_mp->ycbcr_enc = V4L2_YCBCR_ENC_601; 345 pix_mp->quantization = V4L2_QUANTIZATION_LIM_RANGE; 346 347 info = v4l2_format_info(fmt->fourcc); 348 pix_mp->num_planes = info->mem_planes; 349 memset(pix_mp->plane_fmt, 0, sizeof(pix_mp->plane_fmt)); 350 351 for (unsigned int i = 0; i < info->comp_planes; i++) { 352 unsigned int hdiv = (i == 0) ? 1 : info->hdiv; 353 unsigned int vdiv = (i == 0) ? 1 : info->vdiv; 354 355 plane = &pix_mp->plane_fmt[i]; 356 357 plane->bytesperline = DIV_ROUND_UP(pix_mp->width, hdiv) * 358 info->bpp[i] / info->bpp_div[i]; 359 plane->bytesperline = ALIGN(plane->bytesperline, 360 C3_ISP_DMA_SIZE_ALIGN_BYTES); 361 plane->sizeimage = plane->bytesperline * 362 DIV_ROUND_UP(pix_mp->height, vdiv); 363 } 364 } 365 366 static void c3_isp_cap_return_buffers(struct c3_isp_capture *cap, 367 enum vb2_buffer_state state) 368 { 369 struct c3_isp_cap_buffer *buff; 370 371 guard(spinlock_irqsave)(&cap->buff_lock); 372 373 if (cap->buff) { 374 vb2_buffer_done(&cap->buff->vb.vb2_buf, state); 375 cap->buff = NULL; 376 } 377 378 while (!list_empty(&cap->pending)) { 379 buff = list_first_entry(&cap->pending, 380 struct c3_isp_cap_buffer, list); 381 list_del(&buff->list); 382 vb2_buffer_done(&buff->vb.vb2_buf, state); 383 } 384 } 385 386 static int c3_isp_cap_querycap(struct file *file, void *fh, 387 struct v4l2_capability *cap) 388 { 389 strscpy(cap->driver, C3_ISP_DRIVER_NAME, sizeof(cap->driver)); 390 strscpy(cap->card, "AML C3 ISP", sizeof(cap->card)); 391 392 return 0; 393 } 394 395 static int c3_isp_cap_enum_fmt(struct file *file, void *fh, 396 struct v4l2_fmtdesc *f) 397 { 398 const struct c3_isp_cap_format_info *fmt; 399 unsigned int index = 0; 400 unsigned int i; 401 402 if (!f->mbus_code) { 403 if (f->index >= ARRAY_SIZE(cap_formats)) 404 return -EINVAL; 405 406 fmt = &cap_formats[f->index]; 407 f->pixelformat = fmt->fourcc; 408 return 0; 409 } 410 411 for (i = 0; i < ARRAY_SIZE(cap_formats); i++) { 412 fmt = &cap_formats[i]; 413 if (f->mbus_code != fmt->mbus_code) 414 continue; 415 416 if (index++ == f->index) { 417 f->pixelformat = cap_formats[i].fourcc; 418 return 0; 419 } 420 } 421 422 return -EINVAL; 423 } 424 425 static int c3_isp_cap_g_fmt_mplane(struct file *file, void *fh, 426 struct v4l2_format *f) 427 { 428 struct c3_isp_capture *cap = video_drvdata(file); 429 430 f->fmt.pix_mp = cap->format.pix_mp; 431 432 return 0; 433 } 434 435 static int c3_isp_cap_s_fmt_mplane(struct file *file, void *fh, 436 struct v4l2_format *f) 437 { 438 struct c3_isp_capture *cap = video_drvdata(file); 439 440 c3_cap_try_fmt(&f->fmt.pix_mp); 441 442 cap->format.pix_mp = f->fmt.pix_mp; 443 cap->format.info = c3_cap_find_fmt(f->fmt.pix_mp.pixelformat); 444 445 return 0; 446 } 447 448 static int c3_isp_cap_try_fmt_mplane(struct file *file, void *fh, 449 struct v4l2_format *f) 450 { 451 c3_cap_try_fmt(&f->fmt.pix_mp); 452 453 return 0; 454 } 455 456 static int c3_isp_cap_enum_frmsize(struct file *file, void *fh, 457 struct v4l2_frmsizeenum *fsize) 458 { 459 const struct c3_isp_cap_format_info *fmt; 460 461 if (fsize->index) 462 return -EINVAL; 463 464 fmt = c3_cap_find_fmt(fsize->pixel_format); 465 if (!fmt) 466 return -EINVAL; 467 468 fsize->type = V4L2_FRMSIZE_TYPE_STEPWISE; 469 fsize->stepwise.min_width = C3_ISP_MIN_WIDTH; 470 fsize->stepwise.min_height = C3_ISP_MIN_HEIGHT; 471 fsize->stepwise.max_width = C3_ISP_MAX_WIDTH; 472 fsize->stepwise.max_height = C3_ISP_MAX_HEIGHT; 473 fsize->stepwise.step_width = 2; 474 fsize->stepwise.step_height = 2; 475 476 return 0; 477 } 478 479 static const struct v4l2_ioctl_ops isp_cap_v4l2_ioctl_ops = { 480 .vidioc_querycap = c3_isp_cap_querycap, 481 .vidioc_enum_fmt_vid_cap = c3_isp_cap_enum_fmt, 482 .vidioc_g_fmt_vid_cap_mplane = c3_isp_cap_g_fmt_mplane, 483 .vidioc_s_fmt_vid_cap_mplane = c3_isp_cap_s_fmt_mplane, 484 .vidioc_try_fmt_vid_cap_mplane = c3_isp_cap_try_fmt_mplane, 485 .vidioc_reqbufs = vb2_ioctl_reqbufs, 486 .vidioc_querybuf = vb2_ioctl_querybuf, 487 .vidioc_qbuf = vb2_ioctl_qbuf, 488 .vidioc_expbuf = vb2_ioctl_expbuf, 489 .vidioc_dqbuf = vb2_ioctl_dqbuf, 490 .vidioc_prepare_buf = vb2_ioctl_prepare_buf, 491 .vidioc_create_bufs = vb2_ioctl_create_bufs, 492 .vidioc_streamon = vb2_ioctl_streamon, 493 .vidioc_streamoff = vb2_ioctl_streamoff, 494 .vidioc_enum_framesizes = c3_isp_cap_enum_frmsize, 495 .vidioc_subscribe_event = v4l2_ctrl_subscribe_event, 496 .vidioc_unsubscribe_event = v4l2_event_unsubscribe, 497 }; 498 499 static const struct v4l2_file_operations isp_cap_v4l2_fops = { 500 .open = v4l2_fh_open, 501 .release = vb2_fop_release, 502 .poll = vb2_fop_poll, 503 .unlocked_ioctl = video_ioctl2, 504 .mmap = vb2_fop_mmap, 505 }; 506 507 static int c3_isp_cap_link_validate(struct media_link *link) 508 { 509 struct video_device *vdev = 510 media_entity_to_video_device(link->sink->entity); 511 struct v4l2_subdev *sd = 512 media_entity_to_v4l2_subdev(link->source->entity); 513 struct c3_isp_capture *cap = video_get_drvdata(vdev); 514 struct v4l2_subdev_format src_fmt = { 515 .which = V4L2_SUBDEV_FORMAT_ACTIVE, 516 .pad = link->source->index, 517 }; 518 int ret; 519 520 ret = v4l2_subdev_call_state_active(sd, pad, get_fmt, &src_fmt); 521 if (ret) 522 return ret; 523 524 if (src_fmt.format.width != cap->format.pix_mp.width || 525 src_fmt.format.height != cap->format.pix_mp.height || 526 src_fmt.format.code != cap->format.info->mbus_code) { 527 dev_err(cap->isp->dev, 528 "link %s: %u -> %s: %u not valid: 0x%04x/%ux%u not match 0x%04x/%ux%u\n", 529 link->source->entity->name, link->source->index, 530 link->sink->entity->name, link->sink->index, 531 src_fmt.format.code, src_fmt.format.width, 532 src_fmt.format.height, cap->format.info->mbus_code, 533 cap->format.pix_mp.width, cap->format.pix_mp.height); 534 535 return -EPIPE; 536 } 537 538 return 0; 539 } 540 541 static const struct media_entity_operations isp_cap_entity_ops = { 542 .link_validate = c3_isp_cap_link_validate, 543 }; 544 545 static int c3_isp_vb2_queue_setup(struct vb2_queue *q, 546 unsigned int *num_buffers, 547 unsigned int *num_planes, 548 unsigned int sizes[], 549 struct device *alloc_devs[]) 550 { 551 struct c3_isp_capture *cap = vb2_get_drv_priv(q); 552 const struct v4l2_pix_format_mplane *pix_mp = &cap->format.pix_mp; 553 unsigned int i; 554 555 if (*num_planes) { 556 if (*num_planes != pix_mp->num_planes) 557 return -EINVAL; 558 559 for (i = 0; i < pix_mp->num_planes; i++) 560 if (sizes[i] < pix_mp->plane_fmt[i].sizeimage) 561 return -EINVAL; 562 563 return 0; 564 } 565 566 *num_planes = pix_mp->num_planes; 567 for (i = 0; i < pix_mp->num_planes; i++) 568 sizes[i] = pix_mp->plane_fmt[i].sizeimage; 569 570 return 0; 571 } 572 573 static void c3_isp_vb2_buf_queue(struct vb2_buffer *vb) 574 { 575 struct vb2_v4l2_buffer *v4l2_buf = to_vb2_v4l2_buffer(vb); 576 struct c3_isp_cap_buffer *buf = 577 container_of(v4l2_buf, struct c3_isp_cap_buffer, vb); 578 struct c3_isp_capture *cap = vb2_get_drv_priv(vb->vb2_queue); 579 580 guard(spinlock_irqsave)(&cap->buff_lock); 581 582 list_add_tail(&buf->list, &cap->pending); 583 } 584 585 static int c3_isp_vb2_buf_prepare(struct vb2_buffer *vb) 586 { 587 struct c3_isp_capture *cap = vb2_get_drv_priv(vb->vb2_queue); 588 unsigned long size; 589 590 for (unsigned int i = 0; i < cap->format.pix_mp.num_planes; i++) { 591 size = cap->format.pix_mp.plane_fmt[i].sizeimage; 592 if (vb2_plane_size(vb, i) < size) { 593 dev_err(cap->isp->dev, 594 "User buffer too small (%ld < %lu)\n", 595 vb2_plane_size(vb, i), size); 596 return -EINVAL; 597 } 598 599 vb2_set_plane_payload(vb, i, size); 600 } 601 602 return 0; 603 } 604 605 static int c3_isp_vb2_buf_init(struct vb2_buffer *vb) 606 { 607 struct c3_isp_capture *cap = vb2_get_drv_priv(vb->vb2_queue); 608 struct vb2_v4l2_buffer *v4l2_buf = to_vb2_v4l2_buffer(vb); 609 struct c3_isp_cap_buffer *buf = 610 container_of(v4l2_buf, struct c3_isp_cap_buffer, vb); 611 612 for (unsigned int i = 0; i < cap->format.pix_mp.num_planes; i++) 613 buf->dma_addr[i] = vb2_dma_contig_plane_dma_addr(vb, i); 614 615 return 0; 616 } 617 618 static int c3_isp_vb2_start_streaming(struct vb2_queue *q, 619 unsigned int count) 620 { 621 struct c3_isp_capture *cap = vb2_get_drv_priv(q); 622 int ret; 623 624 ret = video_device_pipeline_start(&cap->vdev, &cap->isp->pipe); 625 if (ret) { 626 dev_err(cap->isp->dev, 627 "Failed to start cap%u pipeline: %d\n", cap->id, ret); 628 goto err_return_buffers; 629 } 630 631 ret = c3_isp_cap_dummy_buff_create(cap); 632 if (ret) 633 goto err_pipeline_stop; 634 635 ret = pm_runtime_resume_and_get(cap->isp->dev); 636 if (ret) 637 goto err_dummy_destroy; 638 639 c3_isp_cap_start(cap); 640 641 ret = v4l2_subdev_enable_streams(&cap->rsz->sd, C3_ISP_RSZ_PAD_SOURCE, 642 BIT(0)); 643 if (ret) 644 goto err_pm_put; 645 646 return 0; 647 648 err_pm_put: 649 pm_runtime_put(cap->isp->dev); 650 err_dummy_destroy: 651 c3_isp_cap_dummy_buff_destroy(cap); 652 err_pipeline_stop: 653 video_device_pipeline_stop(&cap->vdev); 654 err_return_buffers: 655 c3_isp_cap_return_buffers(cap, VB2_BUF_STATE_QUEUED); 656 return ret; 657 } 658 659 static void c3_isp_vb2_stop_streaming(struct vb2_queue *q) 660 { 661 struct c3_isp_capture *cap = vb2_get_drv_priv(q); 662 663 c3_isp_cap_stop(cap); 664 665 c3_isp_cap_return_buffers(cap, VB2_BUF_STATE_ERROR); 666 667 v4l2_subdev_disable_streams(&cap->rsz->sd, C3_ISP_RSZ_PAD_SOURCE, 668 BIT(0)); 669 670 pm_runtime_put(cap->isp->dev); 671 672 c3_isp_cap_dummy_buff_destroy(cap); 673 674 video_device_pipeline_stop(&cap->vdev); 675 } 676 677 static const struct vb2_ops isp_video_vb2_ops = { 678 .queue_setup = c3_isp_vb2_queue_setup, 679 .buf_queue = c3_isp_vb2_buf_queue, 680 .buf_prepare = c3_isp_vb2_buf_prepare, 681 .buf_init = c3_isp_vb2_buf_init, 682 .start_streaming = c3_isp_vb2_start_streaming, 683 .stop_streaming = c3_isp_vb2_stop_streaming, 684 }; 685 686 static int c3_isp_register_capture(struct c3_isp_capture *cap) 687 { 688 struct video_device *vdev = &cap->vdev; 689 struct vb2_queue *vb2_q = &cap->vb2_q; 690 int ret; 691 692 snprintf(vdev->name, sizeof(vdev->name), "c3-isp-cap%u", cap->id); 693 vdev->fops = &isp_cap_v4l2_fops; 694 vdev->ioctl_ops = &isp_cap_v4l2_ioctl_ops; 695 vdev->v4l2_dev = &cap->isp->v4l2_dev; 696 vdev->entity.ops = &isp_cap_entity_ops; 697 vdev->lock = &cap->lock; 698 vdev->minor = -1; 699 vdev->queue = vb2_q; 700 vdev->release = video_device_release_empty; 701 vdev->device_caps = V4L2_CAP_VIDEO_CAPTURE_MPLANE | 702 V4L2_CAP_STREAMING; 703 vdev->vfl_dir = VFL_DIR_RX; 704 video_set_drvdata(vdev, cap); 705 706 vb2_q->drv_priv = cap; 707 vb2_q->mem_ops = &vb2_dma_contig_memops; 708 vb2_q->ops = &isp_video_vb2_ops; 709 vb2_q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE; 710 vb2_q->io_modes = VB2_DMABUF | VB2_MMAP; 711 vb2_q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC; 712 vb2_q->buf_struct_size = sizeof(struct c3_isp_cap_buffer); 713 vb2_q->dev = cap->isp->dev; 714 vb2_q->lock = &cap->lock; 715 716 ret = vb2_queue_init(vb2_q); 717 if (ret) 718 goto err_destroy; 719 720 cap->pad.flags = MEDIA_PAD_FL_SINK; 721 ret = media_entity_pads_init(&vdev->entity, 1, &cap->pad); 722 if (ret) 723 goto err_queue_release; 724 725 ret = video_register_device(vdev, VFL_TYPE_VIDEO, -1); 726 if (ret) { 727 dev_err(cap->isp->dev, 728 "Failed to register %s: %d\n", vdev->name, ret); 729 goto err_entity_cleanup; 730 } 731 732 return 0; 733 734 err_entity_cleanup: 735 media_entity_cleanup(&vdev->entity); 736 err_queue_release: 737 vb2_queue_release(vb2_q); 738 err_destroy: 739 mutex_destroy(&cap->lock); 740 return ret; 741 } 742 743 int c3_isp_captures_register(struct c3_isp_device *isp) 744 { 745 int ret; 746 unsigned int i; 747 struct c3_isp_capture *cap; 748 749 for (i = C3_ISP_CAP_DEV_0; i < C3_ISP_NUM_CAP_DEVS; i++) { 750 cap = &isp->caps[i]; 751 memset(cap, 0, sizeof(*cap)); 752 753 cap->format.pix_mp.width = C3_ISP_DEFAULT_WIDTH; 754 cap->format.pix_mp.height = C3_ISP_DEFAULT_HEIGHT; 755 cap->format.pix_mp.field = V4L2_FIELD_NONE; 756 cap->format.pix_mp.pixelformat = V4L2_PIX_FMT_NV12M; 757 cap->format.pix_mp.colorspace = V4L2_COLORSPACE_SRGB; 758 cap->format.info = 759 c3_cap_find_fmt(cap->format.pix_mp.pixelformat); 760 761 c3_cap_try_fmt(&cap->format.pix_mp); 762 763 cap->id = i; 764 cap->rsz = &isp->resizers[i]; 765 cap->isp = isp; 766 INIT_LIST_HEAD(&cap->pending); 767 spin_lock_init(&cap->buff_lock); 768 mutex_init(&cap->lock); 769 770 ret = c3_isp_register_capture(cap); 771 if (ret) { 772 cap->isp = NULL; 773 mutex_destroy(&cap->lock); 774 c3_isp_captures_unregister(isp); 775 return ret; 776 } 777 } 778 779 return 0; 780 } 781 782 void c3_isp_captures_unregister(struct c3_isp_device *isp) 783 { 784 unsigned int i; 785 struct c3_isp_capture *cap; 786 787 for (i = C3_ISP_CAP_DEV_0; i < C3_ISP_NUM_CAP_DEVS; i++) { 788 cap = &isp->caps[i]; 789 790 if (!cap->isp) 791 continue; 792 vb2_queue_release(&cap->vb2_q); 793 media_entity_cleanup(&cap->vdev.entity); 794 video_unregister_device(&cap->vdev); 795 mutex_destroy(&cap->lock); 796 } 797 } 798 799 void c3_isp_captures_isr(struct c3_isp_device *isp) 800 { 801 c3_isp_cap_done(&isp->caps[C3_ISP_CAP_DEV_0]); 802 c3_isp_cap_done(&isp->caps[C3_ISP_CAP_DEV_1]); 803 c3_isp_cap_done(&isp->caps[C3_ISP_CAP_DEV_2]); 804 } 805