1
2
3
4
5
6
7
8
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
21struct fract {
22 u8 numerator;
23 u8 denominator;
24};
25
26
27
28
29
30
31
32
33
34
35struct 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
44static 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
81static 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
130static 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
165static 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
210
211
212static 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
235
236
237
238
239
240
241
242
243
244static 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
269static 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
285static 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
318
319
320static 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
348static 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
378static 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
398static 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
408static 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
430static 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
467error:
468 media_pipeline_stop(&vdev->entity);
469
470 video->ops->flush_buffers(video, VB2_BUF_STATE_QUEUED);
471
472 return ret;
473}
474
475static 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
504static 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
517
518
519static 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
532static 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
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
567static 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
576static 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;
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
656static 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
663static 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
680static 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
692static int video_g_input(struct file *file, void *fh, unsigned int *input)
693{
694 *input = 0;
695
696 return 0;
697}
698
699static int video_s_input(struct file *file, void *fh, unsigned int input)
700{
701 return input == 0 ? 0 : -EINVAL;
702}
703
704static const struct v4l2_ioctl_ops msm_vid_ioctl_ops = {
705 .vidioc_querycap = video_querycap,
706 .vidioc_enum_fmt_vid_cap = 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
726
727
728static 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
759error_pm_use:
760 v4l2_fh_release(file);
761
762error_alloc:
763 mutex_unlock(&video->lock);
764
765 return ret;
766}
767
768static 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
781static 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
793
794
795static 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
810
811
812
813
814
815
816static 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
839
840
841
842
843
844
845
846
847
848
849int 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
936error_video_register:
937 media_entity_cleanup(&vdev->entity);
938 mutex_destroy(&video->lock);
939error_media_init:
940 vb2_queue_release(&video->vb2_q);
941error_vb2_init:
942 mutex_destroy(&video->q_lock);
943
944 return ret;
945}
946
947void 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
953void 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
