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 "mgb4_core.h"
20 #include "mgb4_dma.h"
21 #include "mgb4_sysfs.h"
22 #include "mgb4_io.h"
23 #include "mgb4_cmt.h"
24 #include "mgb4_vout.h"
25
26 ATTRIBUTE_GROUPS(mgb4_fpdl3_out);
27 ATTRIBUTE_GROUPS(mgb4_gmsl_out);
28
29 static const struct mgb4_vout_config vout_cfg[] = {
30 {0, 0, 8, {0x78, 0x60, 0x64, 0x68, 0x74, 0x6C, 0x70, 0x7c}},
31 {1, 1, 9, {0x98, 0x80, 0x84, 0x88, 0x94, 0x8c, 0x90, 0x9c}}
32 };
33
34 static const struct i2c_board_info fpdl3_ser_info[] = {
35 {I2C_BOARD_INFO("serializer1", 0x14)},
36 {I2C_BOARD_INFO("serializer2", 0x16)},
37 };
38
39 static const struct mgb4_i2c_kv fpdl3_i2c[] = {
40 {0x05, 0xFF, 0x04}, {0x06, 0xFF, 0x01}, {0xC2, 0xFF, 0x80}
41 };
42
return_all_buffers(struct mgb4_vout_dev * voutdev,enum vb2_buffer_state state)43 static void return_all_buffers(struct mgb4_vout_dev *voutdev,
44 enum vb2_buffer_state state)
45 {
46 struct mgb4_frame_buffer *buf, *node;
47 unsigned long flags;
48
49 spin_lock_irqsave(&voutdev->qlock, flags);
50 list_for_each_entry_safe(buf, node, &voutdev->buf_list, list) {
51 vb2_buffer_done(&buf->vb.vb2_buf, state);
52 list_del(&buf->list);
53 }
54 spin_unlock_irqrestore(&voutdev->qlock, flags);
55 }
56
queue_setup(struct vb2_queue * q,unsigned int * nbuffers,unsigned int * nplanes,unsigned int sizes[],struct device * alloc_devs[])57 static int queue_setup(struct vb2_queue *q, unsigned int *nbuffers,
58 unsigned int *nplanes, unsigned int sizes[],
59 struct device *alloc_devs[])
60 {
61 struct mgb4_vout_dev *voutdev = vb2_get_drv_priv(q);
62 unsigned int size;
63
64 /*
65 * If I/O reconfiguration is in process, do not allow to start
66 * the queue. See video_source_store() in mgb4_sysfs_out.c for
67 * details.
68 */
69 if (test_bit(0, &voutdev->mgbdev->io_reconfig))
70 return -EBUSY;
71
72 size = (voutdev->width + voutdev->padding) * voutdev->height * 4;
73
74 if (*nplanes)
75 return sizes[0] < size ? -EINVAL : 0;
76 *nplanes = 1;
77 sizes[0] = size;
78
79 return 0;
80 }
81
buffer_init(struct vb2_buffer * vb)82 static int buffer_init(struct vb2_buffer *vb)
83 {
84 struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
85 struct mgb4_frame_buffer *buf = to_frame_buffer(vbuf);
86
87 INIT_LIST_HEAD(&buf->list);
88
89 return 0;
90 }
91
buffer_prepare(struct vb2_buffer * vb)92 static int buffer_prepare(struct vb2_buffer *vb)
93 {
94 struct mgb4_vout_dev *voutdev = vb2_get_drv_priv(vb->vb2_queue);
95 struct device *dev = &voutdev->mgbdev->pdev->dev;
96 unsigned int size;
97
98 size = (voutdev->width + voutdev->padding) * voutdev->height * 4;
99
100 if (vb2_plane_size(vb, 0) < size) {
101 dev_err(dev, "buffer too small (%lu < %u)\n",
102 vb2_plane_size(vb, 0), size);
103 return -EINVAL;
104 }
105
106 vb2_set_plane_payload(vb, 0, size);
107
108 return 0;
109 }
110
buffer_queue(struct vb2_buffer * vb)111 static void buffer_queue(struct vb2_buffer *vb)
112 {
113 struct mgb4_vout_dev *vindev = vb2_get_drv_priv(vb->vb2_queue);
114 struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
115 struct mgb4_frame_buffer *buf = to_frame_buffer(vbuf);
116 unsigned long flags;
117
118 spin_lock_irqsave(&vindev->qlock, flags);
119 list_add_tail(&buf->list, &vindev->buf_list);
120 spin_unlock_irqrestore(&vindev->qlock, flags);
121 }
122
stop_streaming(struct vb2_queue * vq)123 static void stop_streaming(struct vb2_queue *vq)
124 {
125 struct mgb4_vout_dev *voutdev = vb2_get_drv_priv(vq);
126 struct mgb4_dev *mgbdev = voutdev->mgbdev;
127 int irq = xdma_get_user_irq(mgbdev->xdev, voutdev->config->irq);
128
129 xdma_disable_user_irq(mgbdev->xdev, irq);
130 cancel_work_sync(&voutdev->dma_work);
131 mgb4_mask_reg(&mgbdev->video, voutdev->config->regs.config, 0x2, 0x0);
132 return_all_buffers(voutdev, VB2_BUF_STATE_ERROR);
133 }
134
start_streaming(struct vb2_queue * vq,unsigned int count)135 static int start_streaming(struct vb2_queue *vq, unsigned int count)
136 {
137 struct mgb4_vout_dev *voutdev = vb2_get_drv_priv(vq);
138 struct mgb4_dev *mgbdev = voutdev->mgbdev;
139 struct device *dev = &mgbdev->pdev->dev;
140 struct mgb4_frame_buffer *buf;
141 struct mgb4_regs *video = &mgbdev->video;
142 const struct mgb4_vout_config *config = voutdev->config;
143 int irq = xdma_get_user_irq(mgbdev->xdev, config->irq);
144 int rv;
145 u32 addr;
146
147 mgb4_mask_reg(video, config->regs.config, 0x2, 0x2);
148
149 addr = mgb4_read_reg(video, config->regs.address);
150 if (addr >= MGB4_ERR_QUEUE_FULL) {
151 dev_dbg(dev, "frame queue error (%d)\n", (int)addr);
152 return_all_buffers(voutdev, VB2_BUF_STATE_QUEUED);
153 return -EBUSY;
154 }
155
156 buf = list_first_entry(&voutdev->buf_list, struct mgb4_frame_buffer,
157 list);
158 list_del_init(voutdev->buf_list.next);
159
160 rv = mgb4_dma_transfer(mgbdev, config->dma_channel, true, addr,
161 vb2_dma_sg_plane_desc(&buf->vb.vb2_buf, 0));
162 if (rv < 0) {
163 dev_warn(dev, "DMA transfer error\n");
164 vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
165 } else {
166 vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_DONE);
167 }
168
169 xdma_enable_user_irq(mgbdev->xdev, irq);
170
171 return 0;
172 }
173
174 static const struct vb2_ops queue_ops = {
175 .queue_setup = queue_setup,
176 .buf_init = buffer_init,
177 .buf_prepare = buffer_prepare,
178 .buf_queue = buffer_queue,
179 .start_streaming = start_streaming,
180 .stop_streaming = stop_streaming,
181 .wait_prepare = vb2_ops_wait_prepare,
182 .wait_finish = vb2_ops_wait_finish
183 };
184
vidioc_querycap(struct file * file,void * priv,struct v4l2_capability * cap)185 static int vidioc_querycap(struct file *file, void *priv,
186 struct v4l2_capability *cap)
187 {
188 strscpy(cap->driver, KBUILD_MODNAME, sizeof(cap->driver));
189 strscpy(cap->card, "MGB4 PCIe Card", sizeof(cap->card));
190
191 return 0;
192 }
193
vidioc_enum_fmt(struct file * file,void * priv,struct v4l2_fmtdesc * f)194 static int vidioc_enum_fmt(struct file *file, void *priv,
195 struct v4l2_fmtdesc *f)
196 {
197 if (f->index != 0)
198 return -EINVAL;
199
200 f->pixelformat = V4L2_PIX_FMT_ABGR32;
201
202 return 0;
203 }
204
vidioc_g_fmt(struct file * file,void * priv,struct v4l2_format * f)205 static int vidioc_g_fmt(struct file *file, void *priv, struct v4l2_format *f)
206 {
207 struct mgb4_vout_dev *voutdev = video_drvdata(file);
208
209 f->fmt.pix.pixelformat = V4L2_PIX_FMT_ABGR32;
210 f->fmt.pix.width = voutdev->width;
211 f->fmt.pix.height = voutdev->height;
212 f->fmt.pix.field = V4L2_FIELD_NONE;
213 f->fmt.pix.colorspace = V4L2_COLORSPACE_RAW;
214 f->fmt.pix.bytesperline = (f->fmt.pix.width + voutdev->padding) * 4;
215 f->fmt.pix.sizeimage = f->fmt.pix.bytesperline * f->fmt.pix.height;
216
217 return 0;
218 }
219
vidioc_try_fmt(struct file * file,void * priv,struct v4l2_format * f)220 static int vidioc_try_fmt(struct file *file, void *priv, struct v4l2_format *f)
221 {
222 struct mgb4_vout_dev *voutdev = video_drvdata(file);
223
224 f->fmt.pix.pixelformat = V4L2_PIX_FMT_ABGR32;
225 f->fmt.pix.width = voutdev->width;
226 f->fmt.pix.height = voutdev->height;
227 f->fmt.pix.field = V4L2_FIELD_NONE;
228 f->fmt.pix.colorspace = V4L2_COLORSPACE_RAW;
229 f->fmt.pix.bytesperline = max(f->fmt.pix.width * 4,
230 ALIGN_DOWN(f->fmt.pix.bytesperline, 4));
231 f->fmt.pix.sizeimage = f->fmt.pix.bytesperline * f->fmt.pix.height;
232
233 return 0;
234 }
235
vidioc_s_fmt(struct file * file,void * priv,struct v4l2_format * f)236 static int vidioc_s_fmt(struct file *file, void *priv, struct v4l2_format *f)
237 {
238 struct mgb4_vout_dev *voutdev = video_drvdata(file);
239 struct mgb4_regs *video = &voutdev->mgbdev->video;
240
241 if (vb2_is_busy(&voutdev->queue))
242 return -EBUSY;
243
244 vidioc_try_fmt(file, priv, f);
245
246 voutdev->padding = (f->fmt.pix.bytesperline - (f->fmt.pix.width * 4)) / 4;
247 mgb4_write_reg(video, voutdev->config->regs.padding, voutdev->padding);
248
249 return 0;
250 }
251
vidioc_g_output(struct file * file,void * priv,unsigned int * i)252 static int vidioc_g_output(struct file *file, void *priv, unsigned int *i)
253 {
254 *i = 0;
255 return 0;
256 }
257
vidioc_s_output(struct file * file,void * priv,unsigned int i)258 static int vidioc_s_output(struct file *file, void *priv, unsigned int i)
259 {
260 return i ? -EINVAL : 0;
261 }
262
vidioc_enum_output(struct file * file,void * priv,struct v4l2_output * out)263 static int vidioc_enum_output(struct file *file, void *priv,
264 struct v4l2_output *out)
265 {
266 if (out->index != 0)
267 return -EINVAL;
268
269 out->type = V4L2_OUTPUT_TYPE_ANALOG;
270 strscpy(out->name, "MGB4", sizeof(out->name));
271
272 return 0;
273 }
274
275 static const struct v4l2_ioctl_ops video_ioctl_ops = {
276 .vidioc_querycap = vidioc_querycap,
277 .vidioc_enum_fmt_vid_out = vidioc_enum_fmt,
278 .vidioc_try_fmt_vid_out = vidioc_try_fmt,
279 .vidioc_s_fmt_vid_out = vidioc_s_fmt,
280 .vidioc_g_fmt_vid_out = vidioc_g_fmt,
281 .vidioc_enum_output = vidioc_enum_output,
282 .vidioc_g_output = vidioc_g_output,
283 .vidioc_s_output = vidioc_s_output,
284 .vidioc_reqbufs = vb2_ioctl_reqbufs,
285 .vidioc_create_bufs = vb2_ioctl_create_bufs,
286 .vidioc_prepare_buf = vb2_ioctl_prepare_buf,
287 .vidioc_querybuf = vb2_ioctl_querybuf,
288 .vidioc_qbuf = vb2_ioctl_qbuf,
289 .vidioc_dqbuf = vb2_ioctl_dqbuf,
290 .vidioc_expbuf = vb2_ioctl_expbuf,
291 .vidioc_streamon = vb2_ioctl_streamon,
292 .vidioc_streamoff = vb2_ioctl_streamoff,
293 };
294
fh_open(struct file * file)295 static int fh_open(struct file *file)
296 {
297 struct mgb4_vout_dev *voutdev = video_drvdata(file);
298 struct mgb4_regs *video = &voutdev->mgbdev->video;
299 struct device *dev = &voutdev->mgbdev->pdev->dev;
300 u32 config, resolution;
301 int rv;
302
303 /* Return EBUSY when the device is in loopback mode */
304 config = mgb4_read_reg(video, voutdev->config->regs.config);
305 if ((config & 0xc) >> 2 != voutdev->config->id + MGB4_VIN_DEVICES) {
306 dev_dbg(dev, "can not open - device in loopback mode");
307 return -EBUSY;
308 }
309
310 mutex_lock(&voutdev->lock);
311
312 rv = v4l2_fh_open(file);
313 if (rv)
314 goto out;
315
316 if (!v4l2_fh_is_singular_file(file))
317 goto out;
318
319 resolution = mgb4_read_reg(video, voutdev->config->regs.resolution);
320 voutdev->width = resolution >> 16;
321 voutdev->height = resolution & 0xFFFF;
322
323 out:
324 mutex_unlock(&voutdev->lock);
325 return rv;
326 }
327
328 static const struct v4l2_file_operations video_fops = {
329 .owner = THIS_MODULE,
330 .open = fh_open,
331 .release = vb2_fop_release,
332 .unlocked_ioctl = video_ioctl2,
333 .write = vb2_fop_write,
334 .mmap = vb2_fop_mmap,
335 .poll = vb2_fop_poll,
336 };
337
dma_transfer(struct work_struct * work)338 static void dma_transfer(struct work_struct *work)
339 {
340 struct mgb4_vout_dev *voutdev = container_of(work, struct mgb4_vout_dev,
341 dma_work);
342 struct device *dev = &voutdev->mgbdev->pdev->dev;
343 struct mgb4_regs *video = &voutdev->mgbdev->video;
344 struct mgb4_frame_buffer *buf = NULL;
345 unsigned long flags;
346 u32 addr;
347 int rv;
348
349 spin_lock_irqsave(&voutdev->qlock, flags);
350 if (!list_empty(&voutdev->buf_list)) {
351 buf = list_first_entry(&voutdev->buf_list,
352 struct mgb4_frame_buffer, list);
353 list_del_init(voutdev->buf_list.next);
354 }
355 spin_unlock_irqrestore(&voutdev->qlock, flags);
356
357 if (!buf)
358 return;
359
360 addr = mgb4_read_reg(video, voutdev->config->regs.address);
361 if (addr >= MGB4_ERR_QUEUE_FULL) {
362 dev_dbg(dev, "frame queue error (%d)\n", (int)addr);
363 vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
364 return;
365 }
366
367 rv = mgb4_dma_transfer(voutdev->mgbdev, voutdev->config->dma_channel,
368 true, addr,
369 vb2_dma_sg_plane_desc(&buf->vb.vb2_buf, 0));
370 if (rv < 0) {
371 dev_warn(dev, "DMA transfer error\n");
372 vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
373 } else {
374 vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_DONE);
375 }
376 }
377
handler(int irq,void * ctx)378 static irqreturn_t handler(int irq, void *ctx)
379 {
380 struct mgb4_vout_dev *voutdev = (struct mgb4_vout_dev *)ctx;
381 struct mgb4_regs *video = &voutdev->mgbdev->video;
382
383 schedule_work(&voutdev->dma_work);
384
385 mgb4_write_reg(video, 0xB4, 1U << voutdev->config->irq);
386
387 return IRQ_HANDLED;
388 }
389
ser_init(struct mgb4_vout_dev * voutdev,int id)390 static int ser_init(struct mgb4_vout_dev *voutdev, int id)
391 {
392 int rv;
393 const struct i2c_board_info *info = &fpdl3_ser_info[id];
394 struct mgb4_i2c_client *ser = &voutdev->ser;
395 struct device *dev = &voutdev->mgbdev->pdev->dev;
396
397 if (MGB4_IS_GMSL(voutdev->mgbdev))
398 return 0;
399
400 rv = mgb4_i2c_init(ser, voutdev->mgbdev->i2c_adap, info, 8);
401 if (rv < 0) {
402 dev_err(dev, "failed to create serializer\n");
403 return rv;
404 }
405 rv = mgb4_i2c_configure(ser, fpdl3_i2c, ARRAY_SIZE(fpdl3_i2c));
406 if (rv < 0) {
407 dev_err(dev, "failed to configure serializer\n");
408 goto err_i2c_dev;
409 }
410
411 return 0;
412
413 err_i2c_dev:
414 mgb4_i2c_free(ser);
415
416 return rv;
417 }
418
fpga_init(struct mgb4_vout_dev * voutdev)419 static void fpga_init(struct mgb4_vout_dev *voutdev)
420 {
421 struct mgb4_regs *video = &voutdev->mgbdev->video;
422 const struct mgb4_vout_regs *regs = &voutdev->config->regs;
423
424 mgb4_write_reg(video, regs->config, 0x00000011);
425 mgb4_write_reg(video, regs->resolution,
426 (MGB4_DEFAULT_WIDTH << 16) | MGB4_DEFAULT_HEIGHT);
427 mgb4_write_reg(video, regs->hsync, 0x00102020);
428 mgb4_write_reg(video, regs->vsync, 0x40020202);
429 mgb4_write_reg(video, regs->frame_period, MGB4_DEFAULT_PERIOD);
430 mgb4_write_reg(video, regs->padding, 0x00000000);
431
432 voutdev->freq = mgb4_cmt_set_vout_freq(voutdev, 70000 >> 1) << 1;
433
434 mgb4_write_reg(video, regs->config,
435 (voutdev->config->id + MGB4_VIN_DEVICES) << 2 | 1 << 4);
436 }
437
438 #ifdef CONFIG_DEBUG_FS
debugfs_init(struct mgb4_vout_dev * voutdev)439 static void debugfs_init(struct mgb4_vout_dev *voutdev)
440 {
441 struct mgb4_regs *video = &voutdev->mgbdev->video;
442
443 voutdev->debugfs = debugfs_create_dir(voutdev->vdev.name,
444 voutdev->mgbdev->debugfs);
445 if (!voutdev->debugfs)
446 return;
447
448 voutdev->regs[0].name = "CONFIG";
449 voutdev->regs[0].offset = voutdev->config->regs.config;
450 voutdev->regs[1].name = "STATUS";
451 voutdev->regs[1].offset = voutdev->config->regs.status;
452 voutdev->regs[2].name = "RESOLUTION";
453 voutdev->regs[2].offset = voutdev->config->regs.resolution;
454 voutdev->regs[3].name = "VIDEO_PARAMS_1";
455 voutdev->regs[3].offset = voutdev->config->regs.hsync;
456 voutdev->regs[4].name = "VIDEO_PARAMS_2";
457 voutdev->regs[4].offset = voutdev->config->regs.vsync;
458 voutdev->regs[5].name = "FRAME_PERIOD";
459 voutdev->regs[5].offset = voutdev->config->regs.frame_period;
460 voutdev->regs[6].name = "PADDING";
461 voutdev->regs[6].offset = voutdev->config->regs.padding;
462
463 voutdev->regset.base = video->membase;
464 voutdev->regset.regs = voutdev->regs;
465 voutdev->regset.nregs = ARRAY_SIZE(voutdev->regs);
466
467 debugfs_create_regset32("registers", 0444, voutdev->debugfs,
468 &voutdev->regset);
469 }
470 #endif
471
mgb4_vout_create(struct mgb4_dev * mgbdev,int id)472 struct mgb4_vout_dev *mgb4_vout_create(struct mgb4_dev *mgbdev, int id)
473 {
474 int rv, irq;
475 const struct attribute_group **groups;
476 struct mgb4_vout_dev *voutdev;
477 struct pci_dev *pdev = mgbdev->pdev;
478 struct device *dev = &pdev->dev;
479
480 voutdev = kzalloc(sizeof(*voutdev), GFP_KERNEL);
481 if (!voutdev)
482 return NULL;
483
484 voutdev->mgbdev = mgbdev;
485 voutdev->config = &vout_cfg[id];
486
487 /* Frame queue */
488 INIT_LIST_HEAD(&voutdev->buf_list);
489 spin_lock_init(&voutdev->qlock);
490
491 /* DMA transfer stuff */
492 INIT_WORK(&voutdev->dma_work, dma_transfer);
493
494 /* IRQ callback */
495 irq = xdma_get_user_irq(mgbdev->xdev, voutdev->config->irq);
496 rv = request_irq(irq, handler, 0, "mgb4-vout", voutdev);
497 if (rv) {
498 dev_err(dev, "failed to register irq handler\n");
499 goto err_alloc;
500 }
501
502 /* Set the FPGA registers default values */
503 fpga_init(voutdev);
504
505 /* Set the serializer default values */
506 rv = ser_init(voutdev, id);
507 if (rv)
508 goto err_irq;
509
510 /* V4L2 stuff init */
511 rv = v4l2_device_register(dev, &voutdev->v4l2dev);
512 if (rv) {
513 dev_err(dev, "failed to register v4l2 device\n");
514 goto err_irq;
515 }
516
517 mutex_init(&voutdev->lock);
518
519 voutdev->queue.type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
520 voutdev->queue.io_modes = VB2_MMAP | VB2_DMABUF | VB2_WRITE;
521 voutdev->queue.buf_struct_size = sizeof(struct mgb4_frame_buffer);
522 voutdev->queue.ops = &queue_ops;
523 voutdev->queue.mem_ops = &vb2_dma_sg_memops;
524 voutdev->queue.gfp_flags = GFP_DMA32;
525 voutdev->queue.timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
526 voutdev->queue.min_queued_buffers = 2;
527 voutdev->queue.drv_priv = voutdev;
528 voutdev->queue.lock = &voutdev->lock;
529 voutdev->queue.dev = dev;
530 rv = vb2_queue_init(&voutdev->queue);
531 if (rv) {
532 dev_err(dev, "failed to initialize vb2 queue\n");
533 goto err_v4l2_dev;
534 }
535
536 snprintf(voutdev->vdev.name, sizeof(voutdev->vdev.name), "mgb4-out%d",
537 id + 1);
538 voutdev->vdev.device_caps = V4L2_CAP_VIDEO_OUTPUT | V4L2_CAP_READWRITE
539 | V4L2_CAP_STREAMING;
540 voutdev->vdev.vfl_dir = VFL_DIR_TX;
541 voutdev->vdev.fops = &video_fops;
542 voutdev->vdev.ioctl_ops = &video_ioctl_ops;
543 voutdev->vdev.release = video_device_release_empty;
544 voutdev->vdev.v4l2_dev = &voutdev->v4l2dev;
545 voutdev->vdev.lock = &voutdev->lock;
546 voutdev->vdev.queue = &voutdev->queue;
547 video_set_drvdata(&voutdev->vdev, voutdev);
548
549 rv = video_register_device(&voutdev->vdev, VFL_TYPE_VIDEO, -1);
550 if (rv) {
551 dev_err(dev, "failed to register video device\n");
552 goto err_v4l2_dev;
553 }
554
555 /* Module sysfs attributes */
556 groups = MGB4_IS_GMSL(mgbdev)
557 ? mgb4_gmsl_out_groups : mgb4_fpdl3_out_groups;
558 rv = device_add_groups(&voutdev->vdev.dev, groups);
559 if (rv) {
560 dev_err(dev, "failed to create sysfs attributes\n");
561 goto err_video_dev;
562 }
563
564 #ifdef CONFIG_DEBUG_FS
565 debugfs_init(voutdev);
566 #endif
567
568 return voutdev;
569
570 err_video_dev:
571 video_unregister_device(&voutdev->vdev);
572 err_v4l2_dev:
573 v4l2_device_unregister(&voutdev->v4l2dev);
574 err_irq:
575 free_irq(irq, voutdev);
576 err_alloc:
577 kfree(voutdev);
578
579 return NULL;
580 }
581
mgb4_vout_free(struct mgb4_vout_dev * voutdev)582 void mgb4_vout_free(struct mgb4_vout_dev *voutdev)
583 {
584 const struct attribute_group **groups;
585 int irq = xdma_get_user_irq(voutdev->mgbdev->xdev, voutdev->config->irq);
586
587 free_irq(irq, voutdev);
588
589 #ifdef CONFIG_DEBUG_FS
590 debugfs_remove_recursive(voutdev->debugfs);
591 #endif
592
593 groups = MGB4_IS_GMSL(voutdev->mgbdev)
594 ? mgb4_gmsl_out_groups : mgb4_fpdl3_out_groups;
595 device_remove_groups(&voutdev->vdev.dev, groups);
596
597 mgb4_i2c_free(&voutdev->ser);
598 video_unregister_device(&voutdev->vdev);
599 v4l2_device_unregister(&voutdev->v4l2dev);
600
601 kfree(voutdev);
602 }
603