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