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 | |
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(vb: &buf->vb.vb2_buf, state); |
52 | list_del(entry: &buf->list); |
53 | } |
54 | spin_unlock_irqrestore(lock: &voutdev->qlock, flags); |
55 | } |
56 | |
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 | |
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(list: &buf->list); |
88 | |
89 | return 0; |
90 | } |
91 | |
92 | static int buffer_prepare(struct vb2_buffer *vb) |
93 | { |
94 | struct mgb4_vout_dev *voutdev = vb2_get_drv_priv(q: 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, plane_no: 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, plane_no: 0, size); |
107 | |
108 | return 0; |
109 | } |
110 | |
111 | static void buffer_queue(struct vb2_buffer *vb) |
112 | { |
113 | struct mgb4_vout_dev *vindev = vb2_get_drv_priv(q: 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(new: &buf->list, head: &vindev->buf_list); |
120 | spin_unlock_irqrestore(lock: &vindev->qlock, flags); |
121 | } |
122 | |
123 | static void stop_streaming(struct vb2_queue *vq) |
124 | { |
125 | struct mgb4_vout_dev *voutdev = vb2_get_drv_priv(q: vq); |
126 | struct mgb4_dev *mgbdev = voutdev->mgbdev; |
127 | int irq = xdma_get_user_irq(pdev: mgbdev->xdev, user_irq_index: voutdev->config->irq); |
128 | |
129 | xdma_disable_user_irq(pdev: mgbdev->xdev, irq_num: irq); |
130 | cancel_work_sync(work: &voutdev->dma_work); |
131 | mgb4_mask_reg(regs: &mgbdev->video, reg: voutdev->config->regs.config, mask: 0x2, val: 0x0); |
132 | return_all_buffers(voutdev, state: VB2_BUF_STATE_ERROR); |
133 | } |
134 | |
135 | static int start_streaming(struct vb2_queue *vq, unsigned int count) |
136 | { |
137 | struct mgb4_vout_dev *voutdev = vb2_get_drv_priv(q: 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(pdev: mgbdev->xdev, user_irq_index: config->irq); |
144 | int rv; |
145 | u32 addr; |
146 | |
147 | mgb4_mask_reg(regs: video, reg: config->regs.config, mask: 0x2, val: 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, state: 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(entry: voutdev->buf_list.next); |
159 | |
160 | rv = mgb4_dma_transfer(mgbdev, channel: config->dma_channel, write: true, paddr: addr, |
161 | sgt: vb2_dma_sg_plane_desc(vb: &buf->vb.vb2_buf, plane_no: 0)); |
162 | if (rv < 0) { |
163 | dev_warn(dev, "DMA transfer error\n" ); |
164 | vb2_buffer_done(vb: &buf->vb.vb2_buf, state: VB2_BUF_STATE_ERROR); |
165 | } else { |
166 | vb2_buffer_done(vb: &buf->vb.vb2_buf, state: VB2_BUF_STATE_DONE); |
167 | } |
168 | |
169 | xdma_enable_user_irq(pdev: mgbdev->xdev, irq_num: 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 | |
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 | |
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 | |
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 | |
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 | |
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(q: &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 | |
252 | static int vidioc_g_output(struct file *file, void *priv, unsigned int *i) |
253 | { |
254 | *i = 0; |
255 | return 0; |
256 | } |
257 | |
258 | static int vidioc_s_output(struct file *file, void *priv, unsigned int i) |
259 | { |
260 | return i ? -EINVAL : 0; |
261 | } |
262 | |
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 | |
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(filp: file); |
313 | if (rv) |
314 | goto out; |
315 | |
316 | if (!v4l2_fh_is_singular_file(filp: 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(lock: &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 | |
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(head: &voutdev->buf_list)) { |
351 | buf = list_first_entry(&voutdev->buf_list, |
352 | struct mgb4_frame_buffer, list); |
353 | list_del_init(entry: voutdev->buf_list.next); |
354 | } |
355 | spin_unlock_irqrestore(lock: &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(vb: &buf->vb.vb2_buf, state: VB2_BUF_STATE_ERROR); |
364 | return; |
365 | } |
366 | |
367 | rv = mgb4_dma_transfer(mgbdev: voutdev->mgbdev, channel: voutdev->config->dma_channel, |
368 | write: true, paddr: addr, |
369 | sgt: vb2_dma_sg_plane_desc(vb: &buf->vb.vb2_buf, plane_no: 0)); |
370 | if (rv < 0) { |
371 | dev_warn(dev, "DMA transfer error\n" ); |
372 | vb2_buffer_done(vb: &buf->vb.vb2_buf, state: VB2_BUF_STATE_ERROR); |
373 | } else { |
374 | vb2_buffer_done(vb: &buf->vb.vb2_buf, state: VB2_BUF_STATE_DONE); |
375 | } |
376 | } |
377 | |
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(work: &voutdev->dma_work); |
384 | |
385 | mgb4_write_reg(video, 0xB4, 1U << voutdev->config->irq); |
386 | |
387 | return IRQ_HANDLED; |
388 | } |
389 | |
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(client: ser, adap: voutdev->mgbdev->i2c_adap, info, addr_size: 8); |
401 | if (rv < 0) { |
402 | dev_err(dev, "failed to create serializer\n" ); |
403 | return rv; |
404 | } |
405 | rv = mgb4_i2c_configure(client: ser, values: 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(client: ser); |
415 | |
416 | return rv; |
417 | } |
418 | |
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, freq: 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 |
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(name: voutdev->vdev.name, |
444 | parent: 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(name: "registers" , mode: 0444, parent: voutdev->debugfs, |
468 | regset: &voutdev->regset); |
469 | } |
470 | #endif |
471 | |
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(size: 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(list: &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(pdev: mgbdev->xdev, user_irq_index: voutdev->config->irq); |
496 | rv = request_irq(irq, handler, flags: 0, name: "mgb4-vout" , dev: 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, v4l2_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(q: &voutdev->queue); |
531 | if (rv) { |
532 | dev_err(dev, "failed to initialize vb2 queue\n" ); |
533 | goto err_v4l2_dev; |
534 | } |
535 | |
536 | snprintf(buf: voutdev->vdev.name, size: sizeof(voutdev->vdev.name), fmt: "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(vdev: &voutdev->vdev, data: voutdev); |
548 | |
549 | rv = video_register_device(vdev: &voutdev->vdev, type: VFL_TYPE_VIDEO, nr: -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(dev: &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(vdev: &voutdev->vdev); |
572 | err_v4l2_dev: |
573 | v4l2_device_unregister(v4l2_dev: &voutdev->v4l2dev); |
574 | err_irq: |
575 | free_irq(irq, voutdev); |
576 | err_alloc: |
577 | kfree(objp: voutdev); |
578 | |
579 | return NULL; |
580 | } |
581 | |
582 | void mgb4_vout_free(struct mgb4_vout_dev *voutdev) |
583 | { |
584 | const struct attribute_group **groups; |
585 | int irq = xdma_get_user_irq(pdev: voutdev->mgbdev->xdev, user_irq_index: voutdev->config->irq); |
586 | |
587 | free_irq(irq, voutdev); |
588 | |
589 | #ifdef CONFIG_DEBUG_FS |
590 | debugfs_remove_recursive(dentry: 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(dev: &voutdev->vdev.dev, groups); |
596 | |
597 | mgb4_i2c_free(client: &voutdev->ser); |
598 | video_unregister_device(vdev: &voutdev->vdev); |
599 | v4l2_device_unregister(v4l2_dev: &voutdev->v4l2dev); |
600 | |
601 | kfree(objp: voutdev); |
602 | } |
603 | |