1 // SPDX-License-Identifier: GPL-2.0 2 /* 3 * camss-video.c 4 * 5 * Qualcomm MSM Camera Subsystem - V4L2 device node 6 * 7 * Copyright (c) 2013-2015, The Linux Foundation. All rights reserved. 8 * Copyright (C) 2015-2018 Linaro Ltd. 9 */ 10 #include <linux/slab.h> 11 #include <media/media-entity.h> 12 #include <media/v4l2-dev.h> 13 #include <media/v4l2-device.h> 14 #include <media/v4l2-ioctl.h> 15 #include <media/v4l2-mc.h> 16 #include <media/videobuf2-dma-sg.h> 17 18 #include "camss-video.h" 19 #include "camss.h" 20 21 struct fract { 22 u8 numerator; 23 u8 denominator; 24 }; 25 26 /* 27 * struct camss_format_info - ISP media bus format information 28 * @code: V4L2 media bus format code 29 * @pixelformat: V4L2 pixel format FCC identifier 30 * @planes: Number of planes 31 * @hsub: Horizontal subsampling (for each plane) 32 * @vsub: Vertical subsampling (for each plane) 33 * @bpp: Bits per pixel when stored in memory (for each plane) 34 */ 35 struct camss_format_info { 36 u32 code; 37 u32 pixelformat; 38 u8 planes; 39 struct fract hsub[3]; 40 struct fract vsub[3]; 41 unsigned int bpp[3]; 42 }; 43 44 static const struct camss_format_info formats_rdi_8x16[] = { 45 { MEDIA_BUS_FMT_UYVY8_2X8, V4L2_PIX_FMT_UYVY, 1, 46 { { 1, 1 } }, { { 1, 1 } }, { 16 } }, 47 { MEDIA_BUS_FMT_VYUY8_2X8, V4L2_PIX_FMT_VYUY, 1, 48 { { 1, 1 } }, { { 1, 1 } }, { 16 } }, 49 { MEDIA_BUS_FMT_YUYV8_2X8, V4L2_PIX_FMT_YUYV, 1, 50 { { 1, 1 } }, { { 1, 1 } }, { 16 } }, 51 { MEDIA_BUS_FMT_YVYU8_2X8, V4L2_PIX_FMT_YVYU, 1, 52 { { 1, 1 } }, { { 1, 1 } }, { 16 } }, 53 { MEDIA_BUS_FMT_SBGGR8_1X8, V4L2_PIX_FMT_SBGGR8, 1, 54 { { 1, 1 } }, { { 1, 1 } }, { 8 } }, 55 { MEDIA_BUS_FMT_SGBRG8_1X8, V4L2_PIX_FMT_SGBRG8, 1, 56 { { 1, 1 } }, { { 1, 1 } }, { 8 } }, 57 { MEDIA_BUS_FMT_SGRBG8_1X8, V4L2_PIX_FMT_SGRBG8, 1, 58 { { 1, 1 } }, { { 1, 1 } }, { 8 } }, 59 { MEDIA_BUS_FMT_SRGGB8_1X8, V4L2_PIX_FMT_SRGGB8, 1, 60 { { 1, 1 } }, { { 1, 1 } }, { 8 } }, 61 { MEDIA_BUS_FMT_SBGGR10_1X10, V4L2_PIX_FMT_SBGGR10P, 1, 62 { { 1, 1 } }, { { 1, 1 } }, { 10 } }, 63 { MEDIA_BUS_FMT_SGBRG10_1X10, V4L2_PIX_FMT_SGBRG10P, 1, 64 { { 1, 1 } }, { { 1, 1 } }, { 10 } }, 65 { MEDIA_BUS_FMT_SGRBG10_1X10, V4L2_PIX_FMT_SGRBG10P, 1, 66 { { 1, 1 } }, { { 1, 1 } }, { 10 } }, 67 { MEDIA_BUS_FMT_SRGGB10_1X10, V4L2_PIX_FMT_SRGGB10P, 1, 68 { { 1, 1 } }, { { 1, 1 } }, { 10 } }, 69 { MEDIA_BUS_FMT_SBGGR12_1X12, V4L2_PIX_FMT_SBGGR12P, 1, 70 { { 1, 1 } }, { { 1, 1 } }, { 12 } }, 71 { MEDIA_BUS_FMT_SGBRG12_1X12, V4L2_PIX_FMT_SGBRG12P, 1, 72 { { 1, 1 } }, { { 1, 1 } }, { 12 } }, 73 { MEDIA_BUS_FMT_SGRBG12_1X12, V4L2_PIX_FMT_SGRBG12P, 1, 74 { { 1, 1 } }, { { 1, 1 } }, { 12 } }, 75 { MEDIA_BUS_FMT_SRGGB12_1X12, V4L2_PIX_FMT_SRGGB12P, 1, 76 { { 1, 1 } }, { { 1, 1 } }, { 12 } }, 77 { MEDIA_BUS_FMT_Y10_1X10, V4L2_PIX_FMT_Y10P, 1, 78 { { 1, 1 } }, { { 1, 1 } }, { 10 } }, 79 }; 80 81 static const struct camss_format_info formats_rdi_8x96[] = { 82 { MEDIA_BUS_FMT_UYVY8_2X8, V4L2_PIX_FMT_UYVY, 1, 83 { { 1, 1 } }, { { 1, 1 } }, { 16 } }, 84 { MEDIA_BUS_FMT_VYUY8_2X8, V4L2_PIX_FMT_VYUY, 1, 85 { { 1, 1 } }, { { 1, 1 } }, { 16 } }, 86 { MEDIA_BUS_FMT_YUYV8_2X8, V4L2_PIX_FMT_YUYV, 1, 87 { { 1, 1 } }, { { 1, 1 } }, { 16 } }, 88 { MEDIA_BUS_FMT_YVYU8_2X8, V4L2_PIX_FMT_YVYU, 1, 89 { { 1, 1 } }, { { 1, 1 } }, { 16 } }, 90 { MEDIA_BUS_FMT_SBGGR8_1X8, V4L2_PIX_FMT_SBGGR8, 1, 91 { { 1, 1 } }, { { 1, 1 } }, { 8 } }, 92 { MEDIA_BUS_FMT_SGBRG8_1X8, V4L2_PIX_FMT_SGBRG8, 1, 93 { { 1, 1 } }, { { 1, 1 } }, { 8 } }, 94 { MEDIA_BUS_FMT_SGRBG8_1X8, V4L2_PIX_FMT_SGRBG8, 1, 95 { { 1, 1 } }, { { 1, 1 } }, { 8 } }, 96 { MEDIA_BUS_FMT_SRGGB8_1X8, V4L2_PIX_FMT_SRGGB8, 1, 97 { { 1, 1 } }, { { 1, 1 } }, { 8 } }, 98 { MEDIA_BUS_FMT_SBGGR10_1X10, V4L2_PIX_FMT_SBGGR10P, 1, 99 { { 1, 1 } }, { { 1, 1 } }, { 10 } }, 100 { MEDIA_BUS_FMT_SGBRG10_1X10, V4L2_PIX_FMT_SGBRG10P, 1, 101 { { 1, 1 } }, { { 1, 1 } }, { 10 } }, 102 { MEDIA_BUS_FMT_SGRBG10_1X10, V4L2_PIX_FMT_SGRBG10P, 1, 103 { { 1, 1 } }, { { 1, 1 } }, { 10 } }, 104 { MEDIA_BUS_FMT_SRGGB10_1X10, V4L2_PIX_FMT_SRGGB10P, 1, 105 { { 1, 1 } }, { { 1, 1 } }, { 10 } }, 106 { MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE, V4L2_PIX_FMT_SBGGR10, 1, 107 { { 1, 1 } }, { { 1, 1 } }, { 16 } }, 108 { MEDIA_BUS_FMT_SBGGR12_1X12, V4L2_PIX_FMT_SBGGR12P, 1, 109 { { 1, 1 } }, { { 1, 1 } }, { 12 } }, 110 { MEDIA_BUS_FMT_SGBRG12_1X12, V4L2_PIX_FMT_SGBRG12P, 1, 111 { { 1, 1 } }, { { 1, 1 } }, { 12 } }, 112 { MEDIA_BUS_FMT_SGRBG12_1X12, V4L2_PIX_FMT_SGRBG12P, 1, 113 { { 1, 1 } }, { { 1, 1 } }, { 12 } }, 114 { MEDIA_BUS_FMT_SRGGB12_1X12, V4L2_PIX_FMT_SRGGB12P, 1, 115 { { 1, 1 } }, { { 1, 1 } }, { 12 } }, 116 { MEDIA_BUS_FMT_SBGGR14_1X14, V4L2_PIX_FMT_SBGGR14P, 1, 117 { { 1, 1 } }, { { 1, 1 } }, { 14 } }, 118 { MEDIA_BUS_FMT_SGBRG14_1X14, V4L2_PIX_FMT_SGBRG14P, 1, 119 { { 1, 1 } }, { { 1, 1 } }, { 14 } }, 120 { MEDIA_BUS_FMT_SGRBG14_1X14, V4L2_PIX_FMT_SGRBG14P, 1, 121 { { 1, 1 } }, { { 1, 1 } }, { 14 } }, 122 { MEDIA_BUS_FMT_SRGGB14_1X14, V4L2_PIX_FMT_SRGGB14P, 1, 123 { { 1, 1 } }, { { 1, 1 } }, { 14 } }, 124 { MEDIA_BUS_FMT_Y10_1X10, V4L2_PIX_FMT_Y10P, 1, 125 { { 1, 1 } }, { { 1, 1 } }, { 10 } }, 126 { MEDIA_BUS_FMT_Y10_2X8_PADHI_LE, V4L2_PIX_FMT_Y10, 1, 127 { { 1, 1 } }, { { 1, 1 } }, { 16 } }, 128 }; 129 130 static const struct camss_format_info formats_pix_8x16[] = { 131 { MEDIA_BUS_FMT_YUYV8_1_5X8, V4L2_PIX_FMT_NV12, 1, 132 { { 1, 1 } }, { { 2, 3 } }, { 8 } }, 133 { MEDIA_BUS_FMT_YVYU8_1_5X8, V4L2_PIX_FMT_NV12, 1, 134 { { 1, 1 } }, { { 2, 3 } }, { 8 } }, 135 { MEDIA_BUS_FMT_UYVY8_1_5X8, V4L2_PIX_FMT_NV12, 1, 136 { { 1, 1 } }, { { 2, 3 } }, { 8 } }, 137 { MEDIA_BUS_FMT_VYUY8_1_5X8, V4L2_PIX_FMT_NV12, 1, 138 { { 1, 1 } }, { { 2, 3 } }, { 8 } }, 139 { MEDIA_BUS_FMT_YUYV8_1_5X8, V4L2_PIX_FMT_NV21, 1, 140 { { 1, 1 } }, { { 2, 3 } }, { 8 } }, 141 { MEDIA_BUS_FMT_YVYU8_1_5X8, V4L2_PIX_FMT_NV21, 1, 142 { { 1, 1 } }, { { 2, 3 } }, { 8 } }, 143 { MEDIA_BUS_FMT_UYVY8_1_5X8, V4L2_PIX_FMT_NV21, 1, 144 { { 1, 1 } }, { { 2, 3 } }, { 8 } }, 145 { MEDIA_BUS_FMT_VYUY8_1_5X8, V4L2_PIX_FMT_NV21, 1, 146 { { 1, 1 } }, { { 2, 3 } }, { 8 } }, 147 { MEDIA_BUS_FMT_YUYV8_2X8, V4L2_PIX_FMT_NV16, 1, 148 { { 1, 1 } }, { { 1, 2 } }, { 8 } }, 149 { MEDIA_BUS_FMT_YVYU8_2X8, V4L2_PIX_FMT_NV16, 1, 150 { { 1, 1 } }, { { 1, 2 } }, { 8 } }, 151 { MEDIA_BUS_FMT_UYVY8_2X8, V4L2_PIX_FMT_NV16, 1, 152 { { 1, 1 } }, { { 1, 2 } }, { 8 } }, 153 { MEDIA_BUS_FMT_VYUY8_2X8, V4L2_PIX_FMT_NV16, 1, 154 { { 1, 1 } }, { { 1, 2 } }, { 8 } }, 155 { MEDIA_BUS_FMT_YUYV8_2X8, V4L2_PIX_FMT_NV61, 1, 156 { { 1, 1 } }, { { 1, 2 } }, { 8 } }, 157 { MEDIA_BUS_FMT_YVYU8_2X8, V4L2_PIX_FMT_NV61, 1, 158 { { 1, 1 } }, { { 1, 2 } }, { 8 } }, 159 { MEDIA_BUS_FMT_UYVY8_2X8, V4L2_PIX_FMT_NV61, 1, 160 { { 1, 1 } }, { { 1, 2 } }, { 8 } }, 161 { MEDIA_BUS_FMT_VYUY8_2X8, V4L2_PIX_FMT_NV61, 1, 162 { { 1, 1 } }, { { 1, 2 } }, { 8 } }, 163 }; 164 165 static const struct camss_format_info formats_pix_8x96[] = { 166 { MEDIA_BUS_FMT_YUYV8_1_5X8, V4L2_PIX_FMT_NV12, 1, 167 { { 1, 1 } }, { { 2, 3 } }, { 8 } }, 168 { MEDIA_BUS_FMT_YVYU8_1_5X8, V4L2_PIX_FMT_NV12, 1, 169 { { 1, 1 } }, { { 2, 3 } }, { 8 } }, 170 { MEDIA_BUS_FMT_UYVY8_1_5X8, V4L2_PIX_FMT_NV12, 1, 171 { { 1, 1 } }, { { 2, 3 } }, { 8 } }, 172 { MEDIA_BUS_FMT_VYUY8_1_5X8, V4L2_PIX_FMT_NV12, 1, 173 { { 1, 1 } }, { { 2, 3 } }, { 8 } }, 174 { MEDIA_BUS_FMT_YUYV8_1_5X8, V4L2_PIX_FMT_NV21, 1, 175 { { 1, 1 } }, { { 2, 3 } }, { 8 } }, 176 { MEDIA_BUS_FMT_YVYU8_1_5X8, V4L2_PIX_FMT_NV21, 1, 177 { { 1, 1 } }, { { 2, 3 } }, { 8 } }, 178 { MEDIA_BUS_FMT_UYVY8_1_5X8, V4L2_PIX_FMT_NV21, 1, 179 { { 1, 1 } }, { { 2, 3 } }, { 8 } }, 180 { MEDIA_BUS_FMT_VYUY8_1_5X8, V4L2_PIX_FMT_NV21, 1, 181 { { 1, 1 } }, { { 2, 3 } }, { 8 } }, 182 { MEDIA_BUS_FMT_YUYV8_2X8, V4L2_PIX_FMT_NV16, 1, 183 { { 1, 1 } }, { { 1, 2 } }, { 8 } }, 184 { MEDIA_BUS_FMT_YVYU8_2X8, V4L2_PIX_FMT_NV16, 1, 185 { { 1, 1 } }, { { 1, 2 } }, { 8 } }, 186 { MEDIA_BUS_FMT_UYVY8_2X8, V4L2_PIX_FMT_NV16, 1, 187 { { 1, 1 } }, { { 1, 2 } }, { 8 } }, 188 { MEDIA_BUS_FMT_VYUY8_2X8, V4L2_PIX_FMT_NV16, 1, 189 { { 1, 1 } }, { { 1, 2 } }, { 8 } }, 190 { MEDIA_BUS_FMT_YUYV8_2X8, V4L2_PIX_FMT_NV61, 1, 191 { { 1, 1 } }, { { 1, 2 } }, { 8 } }, 192 { MEDIA_BUS_FMT_YVYU8_2X8, V4L2_PIX_FMT_NV61, 1, 193 { { 1, 1 } }, { { 1, 2 } }, { 8 } }, 194 { MEDIA_BUS_FMT_UYVY8_2X8, V4L2_PIX_FMT_NV61, 1, 195 { { 1, 1 } }, { { 1, 2 } }, { 8 } }, 196 { MEDIA_BUS_FMT_VYUY8_2X8, V4L2_PIX_FMT_NV61, 1, 197 { { 1, 1 } }, { { 1, 2 } }, { 8 } }, 198 { MEDIA_BUS_FMT_UYVY8_2X8, V4L2_PIX_FMT_UYVY, 1, 199 { { 1, 1 } }, { { 1, 1 } }, { 16 } }, 200 { MEDIA_BUS_FMT_VYUY8_2X8, V4L2_PIX_FMT_VYUY, 1, 201 { { 1, 1 } }, { { 1, 1 } }, { 16 } }, 202 { MEDIA_BUS_FMT_YUYV8_2X8, V4L2_PIX_FMT_YUYV, 1, 203 { { 1, 1 } }, { { 1, 1 } }, { 16 } }, 204 { MEDIA_BUS_FMT_YVYU8_2X8, V4L2_PIX_FMT_YVYU, 1, 205 { { 1, 1 } }, { { 1, 1 } }, { 16 } }, 206 }; 207 208 /* ----------------------------------------------------------------------------- 209 * Helper functions 210 */ 211 212 static int video_find_format(u32 code, u32 pixelformat, 213 const struct camss_format_info *formats, 214 unsigned int nformats) 215 { 216 int i; 217 218 for (i = 0; i < nformats; i++) { 219 if (formats[i].code == code && 220 formats[i].pixelformat == pixelformat) 221 return i; 222 } 223 224 for (i = 0; i < nformats; i++) 225 if (formats[i].code == code) 226 return i; 227 228 WARN_ON(1); 229 230 return -EINVAL; 231 } 232 233 /* 234 * video_mbus_to_pix_mp - Convert v4l2_mbus_framefmt to v4l2_pix_format_mplane 235 * @mbus: v4l2_mbus_framefmt format (input) 236 * @pix: v4l2_pix_format_mplane format (output) 237 * @f: a pointer to formats array element to be used for the conversion 238 * @alignment: bytesperline alignment value 239 * 240 * Fill the output pix structure with information from the input mbus format. 241 * 242 * Return 0 on success or a negative error code otherwise 243 */ 244 static int video_mbus_to_pix_mp(const struct v4l2_mbus_framefmt *mbus, 245 struct v4l2_pix_format_mplane *pix, 246 const struct camss_format_info *f, 247 unsigned int alignment) 248 { 249 unsigned int i; 250 u32 bytesperline; 251 252 memset(pix, 0, sizeof(*pix)); 253 v4l2_fill_pix_format_mplane(pix, mbus); 254 pix->pixelformat = f->pixelformat; 255 pix->num_planes = f->planes; 256 for (i = 0; i < pix->num_planes; i++) { 257 bytesperline = pix->width / f->hsub[i].numerator * 258 f->hsub[i].denominator * f->bpp[i] / 8; 259 bytesperline = ALIGN(bytesperline, alignment); 260 pix->plane_fmt[i].bytesperline = bytesperline; 261 pix->plane_fmt[i].sizeimage = pix->height / 262 f->vsub[i].numerator * f->vsub[i].denominator * 263 bytesperline; 264 } 265 266 return 0; 267 } 268 269 static struct v4l2_subdev *video_remote_subdev(struct camss_video *video, 270 u32 *pad) 271 { 272 struct media_pad *remote; 273 274 remote = media_entity_remote_pad(&video->pad); 275 276 if (!remote || !is_media_entity_v4l2_subdev(remote->entity)) 277 return NULL; 278 279 if (pad) 280 *pad = remote->index; 281 282 return media_entity_to_v4l2_subdev(remote->entity); 283 } 284 285 static int video_get_subdev_format(struct camss_video *video, 286 struct v4l2_format *format) 287 { 288 struct v4l2_subdev_format fmt; 289 struct v4l2_subdev *subdev; 290 u32 pad; 291 int ret; 292 293 subdev = video_remote_subdev(video, &pad); 294 if (subdev == NULL) 295 return -EPIPE; 296 297 fmt.pad = pad; 298 fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE; 299 300 ret = v4l2_subdev_call(subdev, pad, get_fmt, NULL, &fmt); 301 if (ret) 302 return ret; 303 304 ret = video_find_format(fmt.format.code, 305 format->fmt.pix_mp.pixelformat, 306 video->formats, video->nformats); 307 if (ret < 0) 308 return ret; 309 310 format->type = video->type; 311 312 return video_mbus_to_pix_mp(&fmt.format, &format->fmt.pix_mp, 313 &video->formats[ret], video->bpl_alignment); 314 } 315 316 /* ----------------------------------------------------------------------------- 317 * Video queue operations 318 */ 319 320 static int video_queue_setup(struct vb2_queue *q, 321 unsigned int *num_buffers, unsigned int *num_planes, 322 unsigned int sizes[], struct device *alloc_devs[]) 323 { 324 struct camss_video *video = vb2_get_drv_priv(q); 325 const struct v4l2_pix_format_mplane *format = 326 &video->active_fmt.fmt.pix_mp; 327 unsigned int i; 328 329 if (*num_planes) { 330 if (*num_planes != format->num_planes) 331 return -EINVAL; 332 333 for (i = 0; i < *num_planes; i++) 334 if (sizes[i] < format->plane_fmt[i].sizeimage) 335 return -EINVAL; 336 337 return 0; 338 } 339 340 *num_planes = format->num_planes; 341 342 for (i = 0; i < *num_planes; i++) 343 sizes[i] = format->plane_fmt[i].sizeimage; 344 345 return 0; 346 } 347 348 static int video_buf_init(struct vb2_buffer *vb) 349 { 350 struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb); 351 struct camss_video *video = vb2_get_drv_priv(vb->vb2_queue); 352 struct camss_buffer *buffer = container_of(vbuf, struct camss_buffer, 353 vb); 354 const struct v4l2_pix_format_mplane *format = 355 &video->active_fmt.fmt.pix_mp; 356 struct sg_table *sgt; 357 unsigned int i; 358 359 for (i = 0; i < format->num_planes; i++) { 360 sgt = vb2_dma_sg_plane_desc(vb, i); 361 if (!sgt) 362 return -EFAULT; 363 364 buffer->addr[i] = sg_dma_address(sgt->sgl); 365 } 366 367 if (format->pixelformat == V4L2_PIX_FMT_NV12 || 368 format->pixelformat == V4L2_PIX_FMT_NV21 || 369 format->pixelformat == V4L2_PIX_FMT_NV16 || 370 format->pixelformat == V4L2_PIX_FMT_NV61) 371 buffer->addr[1] = buffer->addr[0] + 372 format->plane_fmt[0].bytesperline * 373 format->height; 374 375 return 0; 376 } 377 378 static int video_buf_prepare(struct vb2_buffer *vb) 379 { 380 struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb); 381 struct camss_video *video = vb2_get_drv_priv(vb->vb2_queue); 382 const struct v4l2_pix_format_mplane *format = 383 &video->active_fmt.fmt.pix_mp; 384 unsigned int i; 385 386 for (i = 0; i < format->num_planes; i++) { 387 if (format->plane_fmt[i].sizeimage > vb2_plane_size(vb, i)) 388 return -EINVAL; 389 390 vb2_set_plane_payload(vb, i, format->plane_fmt[i].sizeimage); 391 } 392 393 vbuf->field = V4L2_FIELD_NONE; 394 395 return 0; 396 } 397 398 static void video_buf_queue(struct vb2_buffer *vb) 399 { 400 struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb); 401 struct camss_video *video = vb2_get_drv_priv(vb->vb2_queue); 402 struct camss_buffer *buffer = container_of(vbuf, struct camss_buffer, 403 vb); 404 405 video->ops->queue_buffer(video, buffer); 406 } 407 408 static int video_check_format(struct camss_video *video) 409 { 410 struct v4l2_pix_format_mplane *pix = &video->active_fmt.fmt.pix_mp; 411 struct v4l2_format format; 412 struct v4l2_pix_format_mplane *sd_pix = &format.fmt.pix_mp; 413 int ret; 414 415 sd_pix->pixelformat = pix->pixelformat; 416 ret = video_get_subdev_format(video, &format); 417 if (ret < 0) 418 return ret; 419 420 if (pix->pixelformat != sd_pix->pixelformat || 421 pix->height != sd_pix->height || 422 pix->width != sd_pix->width || 423 pix->num_planes != sd_pix->num_planes || 424 pix->field != format.fmt.pix_mp.field) 425 return -EPIPE; 426 427 return 0; 428 } 429 430 static int video_start_streaming(struct vb2_queue *q, unsigned int count) 431 { 432 struct camss_video *video = vb2_get_drv_priv(q); 433 struct video_device *vdev = &video->vdev; 434 struct media_entity *entity; 435 struct media_pad *pad; 436 struct v4l2_subdev *subdev; 437 int ret; 438 439 ret = media_pipeline_start(&vdev->entity, &video->pipe); 440 if (ret < 0) 441 return ret; 442 443 ret = video_check_format(video); 444 if (ret < 0) 445 goto error; 446 447 entity = &vdev->entity; 448 while (1) { 449 pad = &entity->pads[0]; 450 if (!(pad->flags & MEDIA_PAD_FL_SINK)) 451 break; 452 453 pad = media_entity_remote_pad(pad); 454 if (!pad || !is_media_entity_v4l2_subdev(pad->entity)) 455 break; 456 457 entity = pad->entity; 458 subdev = media_entity_to_v4l2_subdev(entity); 459 460 ret = v4l2_subdev_call(subdev, video, s_stream, 1); 461 if (ret < 0 && ret != -ENOIOCTLCMD) 462 goto error; 463 } 464 465 return 0; 466 467 error: 468 media_pipeline_stop(&vdev->entity); 469 470 video->ops->flush_buffers(video, VB2_BUF_STATE_QUEUED); 471 472 return ret; 473 } 474 475 static void video_stop_streaming(struct vb2_queue *q) 476 { 477 struct camss_video *video = vb2_get_drv_priv(q); 478 struct video_device *vdev = &video->vdev; 479 struct media_entity *entity; 480 struct media_pad *pad; 481 struct v4l2_subdev *subdev; 482 483 entity = &vdev->entity; 484 while (1) { 485 pad = &entity->pads[0]; 486 if (!(pad->flags & MEDIA_PAD_FL_SINK)) 487 break; 488 489 pad = media_entity_remote_pad(pad); 490 if (!pad || !is_media_entity_v4l2_subdev(pad->entity)) 491 break; 492 493 entity = pad->entity; 494 subdev = media_entity_to_v4l2_subdev(entity); 495 496 v4l2_subdev_call(subdev, video, s_stream, 0); 497 } 498 499 media_pipeline_stop(&vdev->entity); 500 501 video->ops->flush_buffers(video, VB2_BUF_STATE_ERROR); 502 } 503 504 static const struct vb2_ops msm_video_vb2_q_ops = { 505 .queue_setup = video_queue_setup, 506 .wait_prepare = vb2_ops_wait_prepare, 507 .wait_finish = vb2_ops_wait_finish, 508 .buf_init = video_buf_init, 509 .buf_prepare = video_buf_prepare, 510 .buf_queue = video_buf_queue, 511 .start_streaming = video_start_streaming, 512 .stop_streaming = video_stop_streaming, 513 }; 514 515 /* ----------------------------------------------------------------------------- 516 * V4L2 ioctls 517 */ 518 519 static int video_querycap(struct file *file, void *fh, 520 struct v4l2_capability *cap) 521 { 522 struct camss_video *video = video_drvdata(file); 523 524 strscpy(cap->driver, "qcom-camss", sizeof(cap->driver)); 525 strscpy(cap->card, "Qualcomm Camera Subsystem", sizeof(cap->card)); 526 snprintf(cap->bus_info, sizeof(cap->bus_info), "platform:%s", 527 dev_name(video->camss->dev)); 528 529 return 0; 530 } 531 532 static int video_enum_fmt(struct file *file, void *fh, struct v4l2_fmtdesc *f) 533 { 534 struct camss_video *video = video_drvdata(file); 535 int i, j, k; 536 537 if (f->type != video->type) 538 return -EINVAL; 539 540 if (f->index >= video->nformats) 541 return -EINVAL; 542 543 /* find index "i" of "k"th unique pixelformat in formats array */ 544 k = -1; 545 for (i = 0; i < video->nformats; i++) { 546 for (j = 0; j < i; j++) { 547 if (video->formats[i].pixelformat == 548 video->formats[j].pixelformat) 549 break; 550 } 551 552 if (j == i) 553 k++; 554 555 if (k == f->index) 556 break; 557 } 558 559 if (k < f->index) 560 return -EINVAL; 561 562 f->pixelformat = video->formats[i].pixelformat; 563 564 return 0; 565 } 566 567 static int video_g_fmt(struct file *file, void *fh, struct v4l2_format *f) 568 { 569 struct camss_video *video = video_drvdata(file); 570 571 *f = video->active_fmt; 572 573 return 0; 574 } 575 576 static int __video_try_fmt(struct camss_video *video, struct v4l2_format *f) 577 { 578 struct v4l2_pix_format_mplane *pix_mp; 579 const struct camss_format_info *fi; 580 struct v4l2_plane_pix_format *p; 581 u32 bytesperline[3] = { 0 }; 582 u32 sizeimage[3] = { 0 }; 583 u32 width, height; 584 u32 bpl, lines; 585 int i, j; 586 587 pix_mp = &f->fmt.pix_mp; 588 589 if (video->line_based) 590 for (i = 0; i < pix_mp->num_planes && i < 3; i++) { 591 p = &pix_mp->plane_fmt[i]; 592 bytesperline[i] = clamp_t(u32, p->bytesperline, 593 1, 65528); 594 sizeimage[i] = clamp_t(u32, p->sizeimage, 595 bytesperline[i], 596 bytesperline[i] * 4096); 597 } 598 599 for (j = 0; j < video->nformats; j++) 600 if (pix_mp->pixelformat == video->formats[j].pixelformat) 601 break; 602 603 if (j == video->nformats) 604 j = 0; /* default format */ 605 606 fi = &video->formats[j]; 607 width = pix_mp->width; 608 height = pix_mp->height; 609 610 memset(pix_mp, 0, sizeof(*pix_mp)); 611 612 pix_mp->pixelformat = fi->pixelformat; 613 pix_mp->width = clamp_t(u32, width, 1, 8191); 614 pix_mp->height = clamp_t(u32, height, 1, 8191); 615 pix_mp->num_planes = fi->planes; 616 for (i = 0; i < pix_mp->num_planes; i++) { 617 bpl = pix_mp->width / fi->hsub[i].numerator * 618 fi->hsub[i].denominator * fi->bpp[i] / 8; 619 bpl = ALIGN(bpl, video->bpl_alignment); 620 pix_mp->plane_fmt[i].bytesperline = bpl; 621 pix_mp->plane_fmt[i].sizeimage = pix_mp->height / 622 fi->vsub[i].numerator * fi->vsub[i].denominator * bpl; 623 } 624 625 pix_mp->field = V4L2_FIELD_NONE; 626 pix_mp->colorspace = V4L2_COLORSPACE_SRGB; 627 pix_mp->flags = 0; 628 pix_mp->ycbcr_enc = V4L2_MAP_YCBCR_ENC_DEFAULT(pix_mp->colorspace); 629 pix_mp->quantization = V4L2_MAP_QUANTIZATION_DEFAULT(true, 630 pix_mp->colorspace, pix_mp->ycbcr_enc); 631 pix_mp->xfer_func = V4L2_MAP_XFER_FUNC_DEFAULT(pix_mp->colorspace); 632 633 if (video->line_based) 634 for (i = 0; i < pix_mp->num_planes; i++) { 635 p = &pix_mp->plane_fmt[i]; 636 p->bytesperline = clamp_t(u32, p->bytesperline, 637 1, 65528); 638 p->sizeimage = clamp_t(u32, p->sizeimage, 639 p->bytesperline, 640 p->bytesperline * 4096); 641 lines = p->sizeimage / p->bytesperline; 642 643 if (p->bytesperline < bytesperline[i]) 644 p->bytesperline = ALIGN(bytesperline[i], 8); 645 646 if (p->sizeimage < p->bytesperline * lines) 647 p->sizeimage = p->bytesperline * lines; 648 649 if (p->sizeimage < sizeimage[i]) 650 p->sizeimage = sizeimage[i]; 651 } 652 653 return 0; 654 } 655 656 static int video_try_fmt(struct file *file, void *fh, struct v4l2_format *f) 657 { 658 struct camss_video *video = video_drvdata(file); 659 660 return __video_try_fmt(video, f); 661 } 662 663 static int video_s_fmt(struct file *file, void *fh, struct v4l2_format *f) 664 { 665 struct camss_video *video = video_drvdata(file); 666 int ret; 667 668 if (vb2_is_busy(&video->vb2_q)) 669 return -EBUSY; 670 671 ret = __video_try_fmt(video, f); 672 if (ret < 0) 673 return ret; 674 675 video->active_fmt = *f; 676 677 return 0; 678 } 679 680 static int video_enum_input(struct file *file, void *fh, 681 struct v4l2_input *input) 682 { 683 if (input->index > 0) 684 return -EINVAL; 685 686 strscpy(input->name, "camera", sizeof(input->name)); 687 input->type = V4L2_INPUT_TYPE_CAMERA; 688 689 return 0; 690 } 691 692 static int video_g_input(struct file *file, void *fh, unsigned int *input) 693 { 694 *input = 0; 695 696 return 0; 697 } 698 699 static int video_s_input(struct file *file, void *fh, unsigned int input) 700 { 701 return input == 0 ? 0 : -EINVAL; 702 } 703 704 static const struct v4l2_ioctl_ops msm_vid_ioctl_ops = { 705 .vidioc_querycap = video_querycap, 706 .vidioc_enum_fmt_vid_cap_mplane = video_enum_fmt, 707 .vidioc_g_fmt_vid_cap_mplane = video_g_fmt, 708 .vidioc_s_fmt_vid_cap_mplane = video_s_fmt, 709 .vidioc_try_fmt_vid_cap_mplane = video_try_fmt, 710 .vidioc_reqbufs = vb2_ioctl_reqbufs, 711 .vidioc_querybuf = vb2_ioctl_querybuf, 712 .vidioc_qbuf = vb2_ioctl_qbuf, 713 .vidioc_expbuf = vb2_ioctl_expbuf, 714 .vidioc_dqbuf = vb2_ioctl_dqbuf, 715 .vidioc_create_bufs = vb2_ioctl_create_bufs, 716 .vidioc_prepare_buf = vb2_ioctl_prepare_buf, 717 .vidioc_streamon = vb2_ioctl_streamon, 718 .vidioc_streamoff = vb2_ioctl_streamoff, 719 .vidioc_enum_input = video_enum_input, 720 .vidioc_g_input = video_g_input, 721 .vidioc_s_input = video_s_input, 722 }; 723 724 /* ----------------------------------------------------------------------------- 725 * V4L2 file operations 726 */ 727 728 static int video_open(struct file *file) 729 { 730 struct video_device *vdev = video_devdata(file); 731 struct camss_video *video = video_drvdata(file); 732 struct v4l2_fh *vfh; 733 int ret; 734 735 mutex_lock(&video->lock); 736 737 vfh = kzalloc(sizeof(*vfh), GFP_KERNEL); 738 if (vfh == NULL) { 739 ret = -ENOMEM; 740 goto error_alloc; 741 } 742 743 v4l2_fh_init(vfh, vdev); 744 v4l2_fh_add(vfh); 745 746 file->private_data = vfh; 747 748 ret = v4l2_pipeline_pm_use(&vdev->entity, 1); 749 if (ret < 0) { 750 dev_err(video->camss->dev, "Failed to power up pipeline: %d\n", 751 ret); 752 goto error_pm_use; 753 } 754 755 mutex_unlock(&video->lock); 756 757 return 0; 758 759 error_pm_use: 760 v4l2_fh_release(file); 761 762 error_alloc: 763 mutex_unlock(&video->lock); 764 765 return ret; 766 } 767 768 static int video_release(struct file *file) 769 { 770 struct video_device *vdev = video_devdata(file); 771 772 vb2_fop_release(file); 773 774 v4l2_pipeline_pm_use(&vdev->entity, 0); 775 776 file->private_data = NULL; 777 778 return 0; 779 } 780 781 static const struct v4l2_file_operations msm_vid_fops = { 782 .owner = THIS_MODULE, 783 .unlocked_ioctl = video_ioctl2, 784 .open = video_open, 785 .release = video_release, 786 .poll = vb2_fop_poll, 787 .mmap = vb2_fop_mmap, 788 .read = vb2_fop_read, 789 }; 790 791 /* ----------------------------------------------------------------------------- 792 * CAMSS video core 793 */ 794 795 static void msm_video_release(struct video_device *vdev) 796 { 797 struct camss_video *video = video_get_drvdata(vdev); 798 799 media_entity_cleanup(&vdev->entity); 800 801 mutex_destroy(&video->q_lock); 802 mutex_destroy(&video->lock); 803 804 if (atomic_dec_and_test(&video->camss->ref_count)) 805 camss_delete(video->camss); 806 } 807 808 /* 809 * msm_video_init_format - Helper function to initialize format 810 * @video: struct camss_video 811 * 812 * Initialize pad format with default value. 813 * 814 * Return 0 on success or a negative error code otherwise 815 */ 816 static int msm_video_init_format(struct camss_video *video) 817 { 818 int ret; 819 struct v4l2_format format = { 820 .type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE, 821 .fmt.pix_mp = { 822 .width = 1920, 823 .height = 1080, 824 .pixelformat = video->formats[0].pixelformat, 825 }, 826 }; 827 828 ret = __video_try_fmt(video, &format); 829 if (ret < 0) 830 return ret; 831 832 video->active_fmt = format; 833 834 return 0; 835 } 836 837 /* 838 * msm_video_register - Register a video device node 839 * @video: struct camss_video 840 * @v4l2_dev: V4L2 device 841 * @name: name to be used for the video device node 842 * 843 * Initialize and register a video device node to a V4L2 device. Also 844 * initialize the vb2 queue. 845 * 846 * Return 0 on success or a negative error code otherwise 847 */ 848 849 int msm_video_register(struct camss_video *video, struct v4l2_device *v4l2_dev, 850 const char *name, int is_pix) 851 { 852 struct media_pad *pad = &video->pad; 853 struct video_device *vdev; 854 struct vb2_queue *q; 855 int ret; 856 857 vdev = &video->vdev; 858 859 mutex_init(&video->q_lock); 860 861 q = &video->vb2_q; 862 q->drv_priv = video; 863 q->mem_ops = &vb2_dma_sg_memops; 864 q->ops = &msm_video_vb2_q_ops; 865 q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE; 866 q->io_modes = VB2_DMABUF | VB2_MMAP | VB2_READ; 867 q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC; 868 q->buf_struct_size = sizeof(struct camss_buffer); 869 q->dev = video->camss->dev; 870 q->lock = &video->q_lock; 871 ret = vb2_queue_init(q); 872 if (ret < 0) { 873 dev_err(v4l2_dev->dev, "Failed to init vb2 queue: %d\n", ret); 874 goto error_vb2_init; 875 } 876 877 pad->flags = MEDIA_PAD_FL_SINK; 878 ret = media_entity_pads_init(&vdev->entity, 1, pad); 879 if (ret < 0) { 880 dev_err(v4l2_dev->dev, "Failed to init video entity: %d\n", 881 ret); 882 goto error_media_init; 883 } 884 885 mutex_init(&video->lock); 886 887 if (video->camss->version == CAMSS_8x16) { 888 if (is_pix) { 889 video->formats = formats_pix_8x16; 890 video->nformats = ARRAY_SIZE(formats_pix_8x16); 891 } else { 892 video->formats = formats_rdi_8x16; 893 video->nformats = ARRAY_SIZE(formats_rdi_8x16); 894 } 895 } else if (video->camss->version == CAMSS_8x96) { 896 if (is_pix) { 897 video->formats = formats_pix_8x96; 898 video->nformats = ARRAY_SIZE(formats_pix_8x96); 899 } else { 900 video->formats = formats_rdi_8x96; 901 video->nformats = ARRAY_SIZE(formats_rdi_8x96); 902 } 903 } else { 904 goto error_video_register; 905 } 906 907 ret = msm_video_init_format(video); 908 if (ret < 0) { 909 dev_err(v4l2_dev->dev, "Failed to init format: %d\n", ret); 910 goto error_video_register; 911 } 912 913 vdev->fops = &msm_vid_fops; 914 vdev->device_caps = V4L2_CAP_VIDEO_CAPTURE_MPLANE | V4L2_CAP_STREAMING | 915 V4L2_CAP_READWRITE; 916 vdev->ioctl_ops = &msm_vid_ioctl_ops; 917 vdev->release = msm_video_release; 918 vdev->v4l2_dev = v4l2_dev; 919 vdev->vfl_dir = VFL_DIR_RX; 920 vdev->queue = &video->vb2_q; 921 vdev->lock = &video->lock; 922 strscpy(vdev->name, name, sizeof(vdev->name)); 923 924 ret = video_register_device(vdev, VFL_TYPE_GRABBER, -1); 925 if (ret < 0) { 926 dev_err(v4l2_dev->dev, "Failed to register video device: %d\n", 927 ret); 928 goto error_video_register; 929 } 930 931 video_set_drvdata(vdev, video); 932 atomic_inc(&video->camss->ref_count); 933 934 return 0; 935 936 error_video_register: 937 media_entity_cleanup(&vdev->entity); 938 mutex_destroy(&video->lock); 939 error_media_init: 940 vb2_queue_release(&video->vb2_q); 941 error_vb2_init: 942 mutex_destroy(&video->q_lock); 943 944 return ret; 945 } 946 947 void msm_video_stop_streaming(struct camss_video *video) 948 { 949 if (vb2_is_streaming(&video->vb2_q)) 950 vb2_queue_release(&video->vb2_q); 951 } 952 953 void msm_video_unregister(struct camss_video *video) 954 { 955 atomic_inc(&video->camss->ref_count); 956 video_unregister_device(&video->vdev); 957 atomic_dec(&video->camss->ref_count); 958 } 959