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