mgb4_vout.c

  1// SPDX-License-Identifier: GPL-2.0-only
  2/*
  3 * Copyright (C) 2021-2023 Digiteq Automotive
  4 *     author: Martin Tuma <martin.tuma@digiteqautomotive.com>
  5 *
  6 * This is the v4l2 output device module. It initializes the signal serializers
  7 * and creates the v4l2 video devices.
  8 *
  9 * When the device is in loopback mode (a direct, in HW, in->out frame passing
 10 * mode) we disable the v4l2 output by returning EBUSY in the open() syscall.
 11 */
 12
 13#include <linux/pci.h>
 14#include <linux/align.h>
 15#include <linux/dma/amd_xdma.h>
 16#include <media/v4l2-ioctl.h>
 17#include <media/videobuf2-v4l2.h>
 18#include <media/videobuf2-dma-sg.h>
 19#include <media/v4l2-dv-timings.h>
 20#include "mgb4_core.h"
 21#include "mgb4_dma.h"
 22#include "mgb4_sysfs.h"
 23#include "mgb4_io.h"
 24#include "mgb4_cmt.h"
 25#include "mgb4_vout.h"
 26
 27ATTRIBUTE_GROUPS(mgb4_fpdl3_out);
 28ATTRIBUTE_GROUPS(mgb4_gmsl_out);
 29
 30static const struct mgb4_vout_config vout_cfg[] = {
 31	{0, 0, 8, {0x78, 0x60, 0x64, 0x68, 0x74, 0x6C, 0x70, 0x7C, 0xE0}},
 32	{1, 1, 9, {0x98, 0x80, 0x84, 0x88, 0x94, 0x8C, 0x90, 0x9C, 0xE4}}
 33};
 34
 35static const struct i2c_board_info fpdl3_ser_info[] = {
 36	{I2C_BOARD_INFO("serializer1", 0x14)},
 37	{I2C_BOARD_INFO("serializer2", 0x16)},
 38};
 39
 40static const struct mgb4_i2c_kv fpdl3_i2c[] = {
 41	{0x05, 0xFF, 0x04}, {0x06, 0xFF, 0x01}, {0xC2, 0xFF, 0x80}
 42};
 43
 44static const struct v4l2_dv_timings_cap video_timings_cap = {
 45	.type = V4L2_DV_BT_656_1120,
 46	.bt = {
 47		.min_width = 240,
 48		.max_width = 4096,
 49		.min_height = 240,
 50		.max_height = 4096,
 51		.min_pixelclock = 1843200, /* 320 x 240 x 24Hz */
 52		.max_pixelclock = 530841600, /* 4096 x 2160 x 60Hz */
 53		.standards = V4L2_DV_BT_STD_CEA861 | V4L2_DV_BT_STD_DMT |
 54			V4L2_DV_BT_STD_CVT | V4L2_DV_BT_STD_GTF,
 55		.capabilities = V4L2_DV_BT_CAP_PROGRESSIVE |
 56			V4L2_DV_BT_CAP_CUSTOM,
 57	},
 58};
 59
 60static void get_timings(struct mgb4_vout_dev *voutdev,
 61			struct v4l2_dv_timings *timings)
 62{
 63	struct mgb4_regs *video = &voutdev->mgbdev->video;
 64	const struct mgb4_vout_regs *regs = &voutdev->config->regs;
 65
 66	u32 hsync = mgb4_read_reg(video, regs->hsync);
 67	u32 vsync = mgb4_read_reg(video, regs->vsync);
 68	u32 resolution = mgb4_read_reg(video, regs->resolution);
 69
 70	memset(timings, 0, sizeof(*timings));
 71	timings->type = V4L2_DV_BT_656_1120;
 72	timings->bt.width = resolution >> 16;
 73	timings->bt.height = resolution & 0xFFFF;
 74	if (hsync & (1U << 31))
 75		timings->bt.polarities |= V4L2_DV_HSYNC_POS_POL;
 76	if (vsync & (1U << 31))
 77		timings->bt.polarities |= V4L2_DV_VSYNC_POS_POL;
 78	timings->bt.pixelclock = voutdev->freq * 1000;
 79	timings->bt.hsync = (hsync & 0x00FF0000) >> 16;
 80	timings->bt.vsync = (vsync & 0x00FF0000) >> 16;
 81	timings->bt.hbackporch = (hsync & 0x0000FF00) >> 8;
 82	timings->bt.hfrontporch = hsync & 0x000000FF;
 83	timings->bt.vbackporch = (vsync & 0x0000FF00) >> 8;
 84	timings->bt.vfrontporch = vsync & 0x000000FF;
 85}
 86
 87static void return_all_buffers(struct mgb4_vout_dev *voutdev,
 88			       enum vb2_buffer_state state)
 89{
 90	struct mgb4_frame_buffer *buf, *node;
 91	unsigned long flags;
 92
 93	spin_lock_irqsave(&voutdev->qlock, flags);
 94	list_for_each_entry_safe(buf, node, &voutdev->buf_list, list) {
 95		vb2_buffer_done(&buf->vb.vb2_buf, state);
 96		list_del(&buf->list);
 97	}
 98	spin_unlock_irqrestore(&voutdev->qlock, flags);
 99}
100
101static int queue_setup(struct vb2_queue *q, unsigned int *nbuffers,
102		       unsigned int *nplanes, unsigned int sizes[],
103		       struct device *alloc_devs[])
104{
105	struct mgb4_vout_dev *voutdev = vb2_get_drv_priv(q);
106	struct mgb4_regs *video = &voutdev->mgbdev->video;
107	u32 config = mgb4_read_reg(video, voutdev->config->regs.config);
108	u32 pixelsize = (config & (1U << 16)) ? 2 : 4;
109	unsigned int size = (voutdev->width + voutdev->padding) * voutdev->height
110			    * pixelsize;
111
112	/*
113	 * If I/O reconfiguration is in process, do not allow to start
114	 * the queue. See video_source_store() in mgb4_sysfs_out.c for
115	 * details.
116	 */
117	if (test_bit(0, &voutdev->mgbdev->io_reconfig))
118		return -EBUSY;
119
120	if (*nplanes)
121		return sizes[0] < size ? -EINVAL : 0;
122	*nplanes = 1;
123	sizes[0] = size;
124
125	return 0;
126}
127
128static int buffer_init(struct vb2_buffer *vb)
129{
130	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
131	struct mgb4_frame_buffer *buf = to_frame_buffer(vbuf);
132
133	INIT_LIST_HEAD(&buf->list);
134
135	return 0;
136}
137
138static int buffer_prepare(struct vb2_buffer *vb)
139{
140	struct mgb4_vout_dev *voutdev = vb2_get_drv_priv(vb->vb2_queue);
141	struct device *dev = &voutdev->mgbdev->pdev->dev;
142	struct mgb4_regs *video = &voutdev->mgbdev->video;
143	u32 config = mgb4_read_reg(video, voutdev->config->regs.config);
144	u32 pixelsize = (config & (1U << 16)) ? 2 : 4;
145	unsigned int size = (voutdev->width + voutdev->padding) * voutdev->height
146			    * pixelsize;
147
148	if (vb2_plane_size(vb, 0) < size) {
149		dev_err(dev, "buffer too small (%lu < %u)\n",
150			vb2_plane_size(vb, 0), size);
151		return -EINVAL;
152	}
153
154	vb2_set_plane_payload(vb, 0, size);
155
156	return 0;
157}
158
159static void buffer_queue(struct vb2_buffer *vb)
160{
161	struct mgb4_vout_dev *vindev = vb2_get_drv_priv(vb->vb2_queue);
162	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
163	struct mgb4_frame_buffer *buf = to_frame_buffer(vbuf);
164	unsigned long flags;
165
166	spin_lock_irqsave(&vindev->qlock, flags);
167	list_add_tail(&buf->list, &vindev->buf_list);
168	spin_unlock_irqrestore(&vindev->qlock, flags);
169}
170
171static void stop_streaming(struct vb2_queue *vq)
172{
173	struct mgb4_vout_dev *voutdev = vb2_get_drv_priv(vq);
174	struct mgb4_dev *mgbdev = voutdev->mgbdev;
175	int irq = xdma_get_user_irq(mgbdev->xdev, voutdev->config->irq);
176
177	xdma_disable_user_irq(mgbdev->xdev, irq);
178	cancel_work_sync(&voutdev->dma_work);
179
180	mgb4_mask_reg(&mgbdev->video, voutdev->config->regs.config, 0x2, 0x0);
181	mgb4_write_reg(&mgbdev->video, voutdev->config->regs.padding, 0);
182
183	return_all_buffers(voutdev, VB2_BUF_STATE_ERROR);
184}
185
186static int start_streaming(struct vb2_queue *vq, unsigned int count)
187{
188	struct mgb4_vout_dev *voutdev = vb2_get_drv_priv(vq);
189	struct mgb4_dev *mgbdev = voutdev->mgbdev;
190	struct device *dev = &mgbdev->pdev->dev;
191	struct mgb4_frame_buffer *buf;
192	struct mgb4_regs *video = &mgbdev->video;
193	const struct mgb4_vout_config *config = voutdev->config;
194	int irq = xdma_get_user_irq(mgbdev->xdev, config->irq);
195	int rv;
196	u32 addr;
197
198	mgb4_write_reg(video, config->regs.padding, voutdev->padding);
199	mgb4_mask_reg(video, config->regs.config, 0x2, 0x2);
200
201	addr = mgb4_read_reg(video, config->regs.address);
202	if (addr >= MGB4_ERR_QUEUE_FULL) {
203		dev_dbg(dev, "frame queue error (%d)\n", (int)addr);
204		return_all_buffers(voutdev, VB2_BUF_STATE_QUEUED);
205		return -EBUSY;
206	}
207
208	buf = list_first_entry(&voutdev->buf_list, struct mgb4_frame_buffer,
209			       list);
210	list_del_init(voutdev->buf_list.next);
211
212	rv = mgb4_dma_transfer(mgbdev, config->dma_channel, true, addr,
213			       vb2_dma_sg_plane_desc(&buf->vb.vb2_buf, 0));
214	if (rv < 0) {
215		dev_warn(dev, "DMA transfer error\n");
216		vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
217	} else {
218		vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_DONE);
219	}
220
221	xdma_enable_user_irq(mgbdev->xdev, irq);
222
223	return 0;
224}
225
226static const struct vb2_ops queue_ops = {
227	.queue_setup = queue_setup,
228	.buf_init = buffer_init,
229	.buf_prepare = buffer_prepare,
230	.buf_queue = buffer_queue,
231	.start_streaming = start_streaming,
232	.stop_streaming = stop_streaming,
233};
234
235static int vidioc_querycap(struct file *file, void *priv,
236			   struct v4l2_capability *cap)
237{
238	strscpy(cap->driver, KBUILD_MODNAME, sizeof(cap->driver));
239	strscpy(cap->card, "MGB4 PCIe Card", sizeof(cap->card));
240
241	return 0;
242}
243
244static int vidioc_enum_fmt(struct file *file, void *priv,
245			   struct v4l2_fmtdesc *f)
246{
247	struct mgb4_vin_dev *voutdev = video_drvdata(file);
248	struct mgb4_regs *video = &voutdev->mgbdev->video;
249
250	if (f->index == 0) {
251		f->pixelformat = V4L2_PIX_FMT_ABGR32;
252		return 0;
253	} else if (f->index == 1 && has_yuv(video)) {
254		f->pixelformat = V4L2_PIX_FMT_YUYV;
255		return 0;
256	} else {
257		return -EINVAL;
258	}
259}
260
261static int vidioc_g_fmt(struct file *file, void *priv, struct v4l2_format *f)
262{
263	struct mgb4_vout_dev *voutdev = video_drvdata(file);
264	struct mgb4_regs *video = &voutdev->mgbdev->video;
265	u32 config = mgb4_read_reg(video, voutdev->config->regs.config);
266
267	f->fmt.pix.width = voutdev->width;
268	f->fmt.pix.height = voutdev->height;
269	f->fmt.pix.field = V4L2_FIELD_NONE;
270
271	if (config & (1U << 16)) {
272		f->fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;
273		if (config & (1U << 20)) {
274			f->fmt.pix.colorspace = V4L2_COLORSPACE_REC709;
275		} else {
276			if (config & (1U << 19))
277				f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
278			else
279				f->fmt.pix.colorspace = V4L2_COLORSPACE_SRGB;
280		}
281		f->fmt.pix.bytesperline = (f->fmt.pix.width + voutdev->padding) * 2;
282	} else {
283		f->fmt.pix.pixelformat = V4L2_PIX_FMT_ABGR32;
284		f->fmt.pix.colorspace = V4L2_COLORSPACE_RAW;
285		f->fmt.pix.bytesperline = (f->fmt.pix.width + voutdev->padding) * 4;
286	}
287
288	f->fmt.pix.sizeimage = f->fmt.pix.bytesperline * f->fmt.pix.height;
289
290	return 0;
291}
292
293static int vidioc_try_fmt(struct file *file, void *priv, struct v4l2_format *f)
294{
295	struct mgb4_vout_dev *voutdev = video_drvdata(file);
296	struct mgb4_regs *video = &voutdev->mgbdev->video;
297	u32 pixelsize;
298
299	f->fmt.pix.width = voutdev->width;
300	f->fmt.pix.height = voutdev->height;
301	f->fmt.pix.field = V4L2_FIELD_NONE;
302
303	if (has_yuv(video) && f->fmt.pix.pixelformat == V4L2_PIX_FMT_YUYV) {
304		pixelsize = 2;
305		if (!(f->fmt.pix.colorspace == V4L2_COLORSPACE_REC709 ||
306		      f->fmt.pix.colorspace == V4L2_COLORSPACE_SMPTE170M))
307			f->fmt.pix.colorspace = V4L2_COLORSPACE_SRGB;
308	} else {
309		pixelsize = 4;
310		f->fmt.pix.pixelformat = V4L2_PIX_FMT_ABGR32;
311		f->fmt.pix.colorspace = V4L2_COLORSPACE_RAW;
312	}
313
314	if (f->fmt.pix.bytesperline > f->fmt.pix.width * pixelsize &&
315	    f->fmt.pix.bytesperline < f->fmt.pix.width * pixelsize * 2)
316		f->fmt.pix.bytesperline = ALIGN(f->fmt.pix.bytesperline,
317						pixelsize);
318	else
319		f->fmt.pix.bytesperline = f->fmt.pix.width * pixelsize;
320	f->fmt.pix.sizeimage = f->fmt.pix.bytesperline * f->fmt.pix.height;
321
322	return 0;
323}
324
325static int vidioc_s_fmt(struct file *file, void *priv, struct v4l2_format *f)
326{
327	struct mgb4_vout_dev *voutdev = video_drvdata(file);
328	struct mgb4_regs *video = &voutdev->mgbdev->video;
329	u32 config, pixelsize;
330	int ret;
331
332	if (vb2_is_busy(&voutdev->queue))
333		return -EBUSY;
334
335	ret = vidioc_try_fmt(file, priv, f);
336	if (ret < 0)
337		return ret;
338
339	config = mgb4_read_reg(video, voutdev->config->regs.config);
340	if (f->fmt.pix.pixelformat == V4L2_PIX_FMT_YUYV) {
341		pixelsize = 2;
342		config |= 1U << 16;
343
344		if (f->fmt.pix.colorspace == V4L2_COLORSPACE_REC709) {
345			config |= 1U << 20;
346			config |= 1U << 19;
347		} else if (f->fmt.pix.colorspace == V4L2_COLORSPACE_SMPTE170M) {
348			config &= ~(1U << 20);
349			config |= 1U << 19;
350		} else {
351			config &= ~(1U << 20);
352			config &= ~(1U << 19);
353		}
354	} else {
355		pixelsize = 4;
356		config &= ~(1U << 16);
357	}
358	mgb4_write_reg(video, voutdev->config->regs.config, config);
359
360	voutdev->padding = (f->fmt.pix.bytesperline - (f->fmt.pix.width
361			    * pixelsize)) / pixelsize;
362
363	return 0;
364}
365
366static int vidioc_g_output(struct file *file, void *priv, unsigned int *i)
367{
368	*i = 0;
369	return 0;
370}
371
372static int vidioc_s_output(struct file *file, void *priv, unsigned int i)
373{
374	return i ? -EINVAL : 0;
375}
376
377static int vidioc_enum_output(struct file *file, void *priv,
378			      struct v4l2_output *out)
379{
380	if (out->index != 0)
381		return -EINVAL;
382
383	out->type = V4L2_OUTPUT_TYPE_ANALOG;
384	out->capabilities = V4L2_OUT_CAP_DV_TIMINGS;
385	strscpy(out->name, "MGB4", sizeof(out->name));
386
387	return 0;
388}
389
390static int vidioc_enum_frameintervals(struct file *file, void *priv,
391				      struct v4l2_frmivalenum *ival)
392{
393	struct mgb4_vout_dev *voutdev = video_drvdata(file);
394	struct mgb4_regs *video = &voutdev->mgbdev->video;
395	struct v4l2_dv_timings timings;
396
397	if (ival->index != 0)
398		return -EINVAL;
399	if (!(ival->pixel_format == V4L2_PIX_FMT_ABGR32 ||
400	      ((has_yuv(video) && ival->pixel_format == V4L2_PIX_FMT_YUYV))))
401		return -EINVAL;
402	if (ival->width != voutdev->width || ival->height != voutdev->height)
403		return -EINVAL;
404
405	get_timings(voutdev, &timings);
406
407	ival->type = V4L2_FRMIVAL_TYPE_STEPWISE;
408	ival->stepwise.max.denominator = MGB4_HW_FREQ;
409	ival->stepwise.max.numerator = 0xFFFFFFFF;
410	ival->stepwise.min.denominator = timings.bt.pixelclock;
411	ival->stepwise.min.numerator = pixel_size(&timings);
412	ival->stepwise.step.denominator = MGB4_HW_FREQ;
413	ival->stepwise.step.numerator = 1;
414
415	return 0;
416}
417
418static int vidioc_g_parm(struct file *file, void *priv,
419			 struct v4l2_streamparm *parm)
420{
421	struct mgb4_vout_dev *voutdev = video_drvdata(file);
422	struct mgb4_regs *video = &voutdev->mgbdev->video;
423	struct v4l2_fract *tpf = &parm->parm.output.timeperframe;
424	struct v4l2_dv_timings timings;
425	u32 timer;
426
427	parm->parm.output.writebuffers = 2;
428
429	if (has_timeperframe(video)) {
430		timer = mgb4_read_reg(video, voutdev->config->regs.timer);
431		if (timer < 0xFFFF) {
432			get_timings(voutdev, &timings);
433			tpf->numerator = pixel_size(&timings);
434			tpf->denominator = timings.bt.pixelclock;
435		} else {
436			tpf->numerator = timer;
437			tpf->denominator = MGB4_HW_FREQ;
438		}
439
440		parm->parm.output.capability = V4L2_CAP_TIMEPERFRAME;
441	}
442
443	return 0;
444}
445
446static int vidioc_s_parm(struct file *file, void *priv,
447			 struct v4l2_streamparm *parm)
448{
449	struct mgb4_vout_dev *voutdev = video_drvdata(file);
450	struct mgb4_regs *video = &voutdev->mgbdev->video;
451	struct v4l2_fract *tpf = &parm->parm.output.timeperframe;
452	struct v4l2_dv_timings timings;
453	u32 timer, period;
454
455	if (has_timeperframe(video)) {
456		timer = tpf->denominator ?
457			MGB4_PERIOD(tpf->numerator, tpf->denominator) : 0;
458		if (timer) {
459			get_timings(voutdev, &timings);
460			period = MGB4_PERIOD(pixel_size(&timings),
461					     timings.bt.pixelclock);
462			if (timer < period)
463				timer = 0;
464		}
465
466		mgb4_write_reg(video, voutdev->config->regs.timer, timer);
467	}
468
469	return vidioc_g_parm(file, priv, parm);
470}
471
472static int vidioc_g_dv_timings(struct file *file, void *fh,
473			       struct v4l2_dv_timings *timings)
474{
475	struct mgb4_vout_dev *voutdev = video_drvdata(file);
476
477	get_timings(voutdev, timings);
478
479	return 0;
480}
481
482static int vidioc_s_dv_timings(struct file *file, void *fh,
483			       struct v4l2_dv_timings *timings)
484{
485	struct mgb4_vout_dev *voutdev = video_drvdata(file);
486
487	get_timings(voutdev, timings);
488
489	return 0;
490}
491
492static int vidioc_enum_dv_timings(struct file *file, void *fh,
493				  struct v4l2_enum_dv_timings *timings)
494{
495	struct mgb4_vout_dev *voutdev = video_drvdata(file);
496
497	if (timings->index != 0)
498		return -EINVAL;
499
500	get_timings(voutdev, &timings->timings);
501
502	return 0;
503}
504
505static int vidioc_dv_timings_cap(struct file *file, void *fh,
506				 struct v4l2_dv_timings_cap *cap)
507{
508	*cap = video_timings_cap;
509
510	return 0;
511}
512
513static const struct v4l2_ioctl_ops video_ioctl_ops = {
514	.vidioc_querycap = vidioc_querycap,
515	.vidioc_enum_fmt_vid_out = vidioc_enum_fmt,
516	.vidioc_try_fmt_vid_out = vidioc_try_fmt,
517	.vidioc_s_fmt_vid_out = vidioc_s_fmt,
518	.vidioc_g_fmt_vid_out = vidioc_g_fmt,
519	.vidioc_enum_output = vidioc_enum_output,
520	.vidioc_enum_frameintervals = vidioc_enum_frameintervals,
521	.vidioc_g_output = vidioc_g_output,
522	.vidioc_s_output = vidioc_s_output,
523	.vidioc_g_parm = vidioc_g_parm,
524	.vidioc_s_parm = vidioc_s_parm,
525	.vidioc_dv_timings_cap = vidioc_dv_timings_cap,
526	.vidioc_enum_dv_timings = vidioc_enum_dv_timings,
527	.vidioc_g_dv_timings = vidioc_g_dv_timings,
528	.vidioc_s_dv_timings = vidioc_s_dv_timings,
529	.vidioc_reqbufs = vb2_ioctl_reqbufs,
530	.vidioc_create_bufs = vb2_ioctl_create_bufs,
531	.vidioc_prepare_buf = vb2_ioctl_prepare_buf,
532	.vidioc_querybuf = vb2_ioctl_querybuf,
533	.vidioc_qbuf = vb2_ioctl_qbuf,
534	.vidioc_dqbuf = vb2_ioctl_dqbuf,
535	.vidioc_expbuf = vb2_ioctl_expbuf,
536	.vidioc_streamon = vb2_ioctl_streamon,
537	.vidioc_streamoff = vb2_ioctl_streamoff,
538};
539
540static int fh_open(struct file *file)
541{
542	struct mgb4_vout_dev *voutdev = video_drvdata(file);
543	struct mgb4_regs *video = &voutdev->mgbdev->video;
544	struct device *dev = &voutdev->mgbdev->pdev->dev;
545	u32 config, resolution;
546	int rv;
547
548	/* Return EBUSY when the device is in loopback mode */
549	config = mgb4_read_reg(video, voutdev->config->regs.config);
550	if ((config & 0xc) >> 2 != voutdev->config->id + MGB4_VIN_DEVICES) {
551		dev_dbg(dev, "can not open - device in loopback mode");
552		return -EBUSY;
553	}
554
555	mutex_lock(&voutdev->lock);
556
557	rv = v4l2_fh_open(file);
558	if (rv)
559		goto out;
560
561	if (!v4l2_fh_is_singular_file(file))
562		goto out;
563
564	resolution = mgb4_read_reg(video, voutdev->config->regs.resolution);
565	voutdev->width = resolution >> 16;
566	voutdev->height = resolution & 0xFFFF;
567
568out:
569	mutex_unlock(&voutdev->lock);
570	return rv;
571}
572
573static const struct v4l2_file_operations video_fops = {
574	.owner = THIS_MODULE,
575	.open = fh_open,
576	.release = vb2_fop_release,
577	.unlocked_ioctl = video_ioctl2,
578	.write = vb2_fop_write,
579	.mmap = vb2_fop_mmap,
580	.poll = vb2_fop_poll,
581};
582
583static void dma_transfer(struct work_struct *work)
584{
585	struct mgb4_vout_dev *voutdev = container_of(work, struct mgb4_vout_dev,
586						     dma_work);
587	struct device *dev = &voutdev->mgbdev->pdev->dev;
588	struct mgb4_regs *video = &voutdev->mgbdev->video;
589	struct mgb4_frame_buffer *buf = NULL;
590	unsigned long flags;
591	u32 addr;
592	int rv;
593
594	spin_lock_irqsave(&voutdev->qlock, flags);
595	if (!list_empty(&voutdev->buf_list)) {
596		buf = list_first_entry(&voutdev->buf_list,
597				       struct mgb4_frame_buffer, list);
598		list_del_init(voutdev->buf_list.next);
599	}
600	spin_unlock_irqrestore(&voutdev->qlock, flags);
601
602	if (!buf)
603		return;
604
605	addr = mgb4_read_reg(video, voutdev->config->regs.address);
606	if (addr >= MGB4_ERR_QUEUE_FULL) {
607		dev_dbg(dev, "frame queue error (%d)\n", (int)addr);
608		vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
609		return;
610	}
611
612	rv = mgb4_dma_transfer(voutdev->mgbdev, voutdev->config->dma_channel,
613			       true, addr,
614			       vb2_dma_sg_plane_desc(&buf->vb.vb2_buf, 0));
615	if (rv < 0) {
616		dev_warn(dev, "DMA transfer error\n");
617		vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
618	} else {
619		vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_DONE);
620	}
621}
622
623static irqreturn_t handler(int irq, void *ctx)
624{
625	struct mgb4_vout_dev *voutdev = (struct mgb4_vout_dev *)ctx;
626	struct mgb4_regs *video = &voutdev->mgbdev->video;
627
628	schedule_work(&voutdev->dma_work);
629
630	mgb4_write_reg(video, 0xB4, 1U << voutdev->config->irq);
631
632	return IRQ_HANDLED;
633}
634
635static int ser_init(struct mgb4_vout_dev *voutdev, int id)
636{
637	int rv;
638	const struct i2c_board_info *info = &fpdl3_ser_info[id];
639	struct mgb4_i2c_client *ser = &voutdev->ser;
640	struct device *dev = &voutdev->mgbdev->pdev->dev;
641
642	if (MGB4_IS_GMSL(voutdev->mgbdev))
643		return 0;
644
645	rv = mgb4_i2c_init(ser, voutdev->mgbdev->i2c_adap, info, 8);
646	if (rv < 0) {
647		dev_err(dev, "failed to create serializer\n");
648		return rv;
649	}
650	rv = mgb4_i2c_configure(ser, fpdl3_i2c, ARRAY_SIZE(fpdl3_i2c));
651	if (rv < 0) {
652		dev_err(dev, "failed to configure serializer\n");
653		goto err_i2c_dev;
654	}
655
656	return 0;
657
658err_i2c_dev:
659	mgb4_i2c_free(ser);
660
661	return rv;
662}
663
664static void fpga_init(struct mgb4_vout_dev *voutdev)
665{
666	struct mgb4_regs *video = &voutdev->mgbdev->video;
667	const struct mgb4_vout_regs *regs = &voutdev->config->regs;
668
669	mgb4_write_reg(video, regs->config, 0x00000011);
670	mgb4_write_reg(video, regs->resolution, (1280 << 16) | 640);
671	mgb4_write_reg(video, regs->hsync, 0x00283232);
672	mgb4_write_reg(video, regs->vsync, 0x40141F1E);
673	mgb4_write_reg(video, regs->frame_limit, MGB4_HW_FREQ / 60);
674	mgb4_write_reg(video, regs->padding, 0x00000000);
675
676	voutdev->freq = mgb4_cmt_set_vout_freq(voutdev, 61150 >> 1) << 1;
677
678	mgb4_write_reg(video, regs->config,
679		       (voutdev->config->id + MGB4_VIN_DEVICES) << 2 | 1 << 4);
680}
681
682static void create_debugfs(struct mgb4_vout_dev *voutdev)
683{
684#ifdef CONFIG_DEBUG_FS
685	struct mgb4_regs *video = &voutdev->mgbdev->video;
686	struct dentry *entry;
687
688	if (IS_ERR_OR_NULL(voutdev->mgbdev->debugfs))
689		return;
690	entry = debugfs_create_dir(voutdev->vdev.name, voutdev->mgbdev->debugfs);
691	if (IS_ERR(entry))
692		return;
693
694	voutdev->regs[0].name = "CONFIG";
695	voutdev->regs[0].offset = voutdev->config->regs.config;
696	voutdev->regs[1].name = "STATUS";
697	voutdev->regs[1].offset = voutdev->config->regs.status;
698	voutdev->regs[2].name = "RESOLUTION";
699	voutdev->regs[2].offset = voutdev->config->regs.resolution;
700	voutdev->regs[3].name = "VIDEO_PARAMS_1";
701	voutdev->regs[3].offset = voutdev->config->regs.hsync;
702	voutdev->regs[4].name = "VIDEO_PARAMS_2";
703	voutdev->regs[4].offset = voutdev->config->regs.vsync;
704	voutdev->regs[5].name = "FRAME_LIMIT";
705	voutdev->regs[5].offset = voutdev->config->regs.frame_limit;
706	voutdev->regs[6].name = "PADDING_PIXELS";
707	voutdev->regs[6].offset = voutdev->config->regs.padding;
708	if (has_timeperframe(video)) {
709		voutdev->regs[7].name = "TIMER";
710		voutdev->regs[7].offset = voutdev->config->regs.timer;
711		voutdev->regset.nregs = 8;
712	} else {
713		voutdev->regset.nregs = 7;
714	}
715
716	voutdev->regset.base = video->membase;
717	voutdev->regset.regs = voutdev->regs;
718
719	debugfs_create_regset32("registers", 0444, entry, &voutdev->regset);
720#endif
721}
722
723struct mgb4_vout_dev *mgb4_vout_create(struct mgb4_dev *mgbdev, int id)
724{
725	int rv, irq;
726	const struct attribute_group **groups;
727	struct mgb4_vout_dev *voutdev;
728	struct pci_dev *pdev = mgbdev->pdev;
729	struct device *dev = &pdev->dev;
730
731	voutdev = kzalloc(sizeof(*voutdev), GFP_KERNEL);
732	if (!voutdev)
733		return NULL;
734
735	voutdev->mgbdev = mgbdev;
736	voutdev->config = &vout_cfg[id];
737
738	/* Frame queue */
739	INIT_LIST_HEAD(&voutdev->buf_list);
740	spin_lock_init(&voutdev->qlock);
741
742	/* DMA transfer stuff */
743	INIT_WORK(&voutdev->dma_work, dma_transfer);
744
745	/* IRQ callback */
746	irq = xdma_get_user_irq(mgbdev->xdev, voutdev->config->irq);
747	rv = request_irq(irq, handler, 0, "mgb4-vout", voutdev);
748	if (rv) {
749		dev_err(dev, "failed to register irq handler\n");
750		goto err_alloc;
751	}
752
753	/* Set the FPGA registers default values */
754	fpga_init(voutdev);
755
756	/* Set the serializer default values */
757	rv = ser_init(voutdev, id);
758	if (rv)
759		goto err_irq;
760
761	/* V4L2 stuff init  */
762	rv = v4l2_device_register(dev, &voutdev->v4l2dev);
763	if (rv) {
764		dev_err(dev, "failed to register v4l2 device\n");
765		goto err_irq;
766	}
767
768	mutex_init(&voutdev->lock);
769
770	voutdev->queue.type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
771	voutdev->queue.io_modes = VB2_MMAP | VB2_DMABUF | VB2_WRITE;
772	voutdev->queue.buf_struct_size = sizeof(struct mgb4_frame_buffer);
773	voutdev->queue.ops = &queue_ops;
774	voutdev->queue.mem_ops = &vb2_dma_sg_memops;
775	voutdev->queue.gfp_flags = GFP_DMA32;
776	voutdev->queue.timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
777	voutdev->queue.min_queued_buffers = 2;
778	voutdev->queue.drv_priv = voutdev;
779	voutdev->queue.lock = &voutdev->lock;
780	voutdev->queue.dev = dev;
781	rv = vb2_queue_init(&voutdev->queue);
782	if (rv) {
783		dev_err(dev, "failed to initialize vb2 queue\n");
784		goto err_v4l2_dev;
785	}
786
787	snprintf(voutdev->vdev.name, sizeof(voutdev->vdev.name), "mgb4-out%d",
788		 id + 1);
789	voutdev->vdev.device_caps = V4L2_CAP_VIDEO_OUTPUT | V4L2_CAP_READWRITE
790	  | V4L2_CAP_STREAMING;
791	voutdev->vdev.vfl_dir = VFL_DIR_TX;
792	voutdev->vdev.fops = &video_fops;
793	voutdev->vdev.ioctl_ops = &video_ioctl_ops;
794	voutdev->vdev.release = video_device_release_empty;
795	voutdev->vdev.v4l2_dev = &voutdev->v4l2dev;
796	voutdev->vdev.lock = &voutdev->lock;
797	voutdev->vdev.queue = &voutdev->queue;
798	video_set_drvdata(&voutdev->vdev, voutdev);
799
800	rv = video_register_device(&voutdev->vdev, VFL_TYPE_VIDEO, -1);
801	if (rv) {
802		dev_err(dev, "failed to register video device\n");
803		goto err_v4l2_dev;
804	}
805
806	/* Module sysfs attributes */
807	groups = MGB4_IS_GMSL(mgbdev)
808	  ? mgb4_gmsl_out_groups : mgb4_fpdl3_out_groups;
809	rv = device_add_groups(&voutdev->vdev.dev, groups);
810	if (rv) {
811		dev_err(dev, "failed to create sysfs attributes\n");
812		goto err_video_dev;
813	}
814
815	create_debugfs(voutdev);
816
817	return voutdev;
818
819err_video_dev:
820	video_unregister_device(&voutdev->vdev);
821err_v4l2_dev:
822	v4l2_device_unregister(&voutdev->v4l2dev);
823err_irq:
824	free_irq(irq, voutdev);
825err_alloc:
826	kfree(voutdev);
827
828	return NULL;
829}
830
831void mgb4_vout_free(struct mgb4_vout_dev *voutdev)
832{
833	const struct attribute_group **groups;
834	int irq = xdma_get_user_irq(voutdev->mgbdev->xdev, voutdev->config->irq);
835
836	free_irq(irq, voutdev);
837
838	groups = MGB4_IS_GMSL(voutdev->mgbdev)
839	  ? mgb4_gmsl_out_groups : mgb4_fpdl3_out_groups;
840	device_remove_groups(&voutdev->vdev.dev, groups);
841
842	mgb4_i2c_free(&voutdev->ser);
843	video_unregister_device(&voutdev->vdev);
844	v4l2_device_unregister(&voutdev->v4l2dev);
845
846	kfree(voutdev);
847}