mali-c55-capture.c

  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
 25static 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
205void 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
211static 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
216static 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
230static bool
231mali_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
244bool 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
256static const struct mali_c55_format_info *
257mali_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
274static const char * const capture_device_names[] = {
275	"mali-c55 fr",
276	"mali-c55 ds",
277};
278
279static 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
324static const struct media_entity_operations mali_c55_media_ops = {
325	.link_validate = mali_c55_link_validate,
326};
327
328static 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
351static 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
373static 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
387void 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 */
457void 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
483static 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
491static 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
512static 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
532static 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
583static 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
622err_disable_rsz:
623	v4l2_subdev_disable_streams(&rsz->sd, MALI_C55_RSZ_SOURCE_PAD, BIT(0));
624err_disable_cap_dev:
625	mali_c55_cap_dev_stream_disable(cap_dev);
626	video_device_pipeline_stop(&cap_dev->vdev);
627err_pm_put:
628	pm_runtime_put_autosuspend(mali_c55->dev);
629err_return_buffers:
630	mali_c55_cap_dev_return_buffers(cap_dev, VB2_BUF_STATE_QUEUED);
631
632	return ret;
633}
634
635static 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
658static 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
666static 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
675static 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
730static 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
738static 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
747static 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
760static 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
770static 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
797static 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
806static 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
825static 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
912err_release_vb2q:
913	vb2_queue_release(vb2q);
914err_cleanup_media_entity:
915	media_entity_cleanup(&cap_dev->vdev.entity);
916err_destroy_mutex:
917	mutex_destroy(&cap_dev->lock);
918
919	return ret;
920}
921
922int 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
941static 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
954void 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}