1 | // SPDX-License-Identifier: GPL-2.0 |
2 | /* |
3 | * video.c - V4L2 component for Mostcore |
4 | * |
5 | * Copyright (C) 2015, Microchip Technology Germany II GmbH & Co. KG |
6 | */ |
7 | |
8 | #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt |
9 | |
10 | #include <linux/module.h> |
11 | #include <linux/slab.h> |
12 | #include <linux/init.h> |
13 | #include <linux/device.h> |
14 | #include <linux/suspend.h> |
15 | #include <linux/videodev2.h> |
16 | #include <linux/mutex.h> |
17 | #include <media/v4l2-common.h> |
18 | #include <media/v4l2-ioctl.h> |
19 | #include <media/v4l2-event.h> |
20 | #include <media/v4l2-device.h> |
21 | #include <media/v4l2-ctrls.h> |
22 | #include <media/v4l2-fh.h> |
23 | #include <linux/most.h> |
24 | |
25 | #define V4L2_CMP_MAX_INPUT 1 |
26 | |
27 | static struct most_component comp; |
28 | |
29 | struct most_video_dev { |
30 | struct most_interface *iface; |
31 | int ch_idx; |
32 | struct list_head list; |
33 | bool mute; |
34 | |
35 | struct list_head pending_mbos; |
36 | spinlock_t list_lock; |
37 | |
38 | struct v4l2_device v4l2_dev; |
39 | atomic_t access_ref; |
40 | struct video_device *vdev; |
41 | unsigned int ctrl_input; |
42 | |
43 | struct mutex lock; |
44 | |
45 | wait_queue_head_t wait_data; |
46 | }; |
47 | |
48 | struct comp_fh { |
49 | /* must be the first field of this struct! */ |
50 | struct v4l2_fh fh; |
51 | struct most_video_dev *mdev; |
52 | u32 offs; |
53 | }; |
54 | |
55 | static LIST_HEAD(video_devices); |
56 | static DEFINE_SPINLOCK(list_lock); |
57 | |
58 | static inline bool data_ready(struct most_video_dev *mdev) |
59 | { |
60 | return !list_empty(head: &mdev->pending_mbos); |
61 | } |
62 | |
63 | static inline struct mbo *get_top_mbo(struct most_video_dev *mdev) |
64 | { |
65 | return list_first_entry(&mdev->pending_mbos, struct mbo, list); |
66 | } |
67 | |
68 | static int comp_vdev_open(struct file *filp) |
69 | { |
70 | int ret; |
71 | struct video_device *vdev = video_devdata(file: filp); |
72 | struct most_video_dev *mdev = video_drvdata(file: filp); |
73 | struct comp_fh *fh; |
74 | |
75 | switch (vdev->vfl_type) { |
76 | case VFL_TYPE_VIDEO: |
77 | break; |
78 | default: |
79 | return -EINVAL; |
80 | } |
81 | |
82 | fh = kzalloc(size: sizeof(*fh), GFP_KERNEL); |
83 | if (!fh) |
84 | return -ENOMEM; |
85 | |
86 | if (!atomic_inc_and_test(v: &mdev->access_ref)) { |
87 | v4l2_err(&mdev->v4l2_dev, "too many clients\n" ); |
88 | ret = -EBUSY; |
89 | goto err_dec; |
90 | } |
91 | |
92 | fh->mdev = mdev; |
93 | v4l2_fh_init(fh: &fh->fh, vdev); |
94 | filp->private_data = fh; |
95 | |
96 | v4l2_fh_add(fh: &fh->fh); |
97 | |
98 | ret = most_start_channel(iface: mdev->iface, channel_idx: mdev->ch_idx, comp: &comp); |
99 | if (ret) { |
100 | v4l2_err(&mdev->v4l2_dev, "most_start_channel() failed\n" ); |
101 | goto err_rm; |
102 | } |
103 | |
104 | return 0; |
105 | |
106 | err_rm: |
107 | v4l2_fh_del(fh: &fh->fh); |
108 | v4l2_fh_exit(fh: &fh->fh); |
109 | |
110 | err_dec: |
111 | atomic_dec(v: &mdev->access_ref); |
112 | kfree(objp: fh); |
113 | return ret; |
114 | } |
115 | |
116 | static int comp_vdev_close(struct file *filp) |
117 | { |
118 | struct comp_fh *fh = filp->private_data; |
119 | struct most_video_dev *mdev = fh->mdev; |
120 | struct mbo *mbo, *tmp; |
121 | |
122 | /* |
123 | * We need to put MBOs back before we call most_stop_channel() |
124 | * to deallocate MBOs. |
125 | * From the other hand mostcore still calling rx_completion() |
126 | * to deliver MBOs until most_stop_channel() is called. |
127 | * Use mute to work around this issue. |
128 | * This must be implemented in core. |
129 | */ |
130 | |
131 | spin_lock_irq(lock: &mdev->list_lock); |
132 | mdev->mute = true; |
133 | list_for_each_entry_safe(mbo, tmp, &mdev->pending_mbos, list) { |
134 | list_del(entry: &mbo->list); |
135 | spin_unlock_irq(lock: &mdev->list_lock); |
136 | most_put_mbo(mbo); |
137 | spin_lock_irq(lock: &mdev->list_lock); |
138 | } |
139 | spin_unlock_irq(lock: &mdev->list_lock); |
140 | most_stop_channel(iface: mdev->iface, channel_idx: mdev->ch_idx, comp: &comp); |
141 | mdev->mute = false; |
142 | |
143 | v4l2_fh_del(fh: &fh->fh); |
144 | v4l2_fh_exit(fh: &fh->fh); |
145 | |
146 | atomic_dec(v: &mdev->access_ref); |
147 | kfree(objp: fh); |
148 | return 0; |
149 | } |
150 | |
151 | static ssize_t comp_vdev_read(struct file *filp, char __user *buf, |
152 | size_t count, loff_t *pos) |
153 | { |
154 | struct comp_fh *fh = filp->private_data; |
155 | struct most_video_dev *mdev = fh->mdev; |
156 | int ret = 0; |
157 | |
158 | if (*pos) |
159 | return -ESPIPE; |
160 | |
161 | if (!mdev) |
162 | return -ENODEV; |
163 | |
164 | /* wait for the first buffer */ |
165 | if (!(filp->f_flags & O_NONBLOCK)) { |
166 | if (wait_event_interruptible(mdev->wait_data, data_ready(mdev))) |
167 | return -ERESTARTSYS; |
168 | } |
169 | |
170 | if (!data_ready(mdev)) |
171 | return -EAGAIN; |
172 | |
173 | while (count > 0 && data_ready(mdev)) { |
174 | struct mbo *const mbo = get_top_mbo(mdev); |
175 | int const rem = mbo->processed_length - fh->offs; |
176 | int const cnt = rem < count ? rem : count; |
177 | |
178 | if (copy_to_user(to: buf, from: mbo->virt_address + fh->offs, n: cnt)) { |
179 | v4l2_err(&mdev->v4l2_dev, "read: copy_to_user failed\n" ); |
180 | if (!ret) |
181 | ret = -EFAULT; |
182 | return ret; |
183 | } |
184 | |
185 | fh->offs += cnt; |
186 | count -= cnt; |
187 | buf += cnt; |
188 | ret += cnt; |
189 | |
190 | if (cnt >= rem) { |
191 | fh->offs = 0; |
192 | spin_lock_irq(lock: &mdev->list_lock); |
193 | list_del(entry: &mbo->list); |
194 | spin_unlock_irq(lock: &mdev->list_lock); |
195 | most_put_mbo(mbo); |
196 | } |
197 | } |
198 | return ret; |
199 | } |
200 | |
201 | static __poll_t comp_vdev_poll(struct file *filp, poll_table *wait) |
202 | { |
203 | struct comp_fh *fh = filp->private_data; |
204 | struct most_video_dev *mdev = fh->mdev; |
205 | __poll_t mask = 0; |
206 | |
207 | /* only wait if no data is available */ |
208 | if (!data_ready(mdev)) |
209 | poll_wait(filp, wait_address: &mdev->wait_data, p: wait); |
210 | if (data_ready(mdev)) |
211 | mask |= EPOLLIN | EPOLLRDNORM; |
212 | |
213 | return mask; |
214 | } |
215 | |
216 | static void comp_set_format_struct(struct v4l2_format *f) |
217 | { |
218 | f->fmt.pix.width = 8; |
219 | f->fmt.pix.height = 8; |
220 | f->fmt.pix.pixelformat = V4L2_PIX_FMT_MPEG; |
221 | f->fmt.pix.bytesperline = 0; |
222 | f->fmt.pix.sizeimage = 188 * 2; |
223 | f->fmt.pix.colorspace = V4L2_COLORSPACE_REC709; |
224 | f->fmt.pix.field = V4L2_FIELD_NONE; |
225 | f->fmt.pix.priv = 0; |
226 | } |
227 | |
228 | static int comp_set_format(struct most_video_dev *mdev, unsigned int cmd, |
229 | struct v4l2_format *format) |
230 | { |
231 | if (format->fmt.pix.pixelformat != V4L2_PIX_FMT_MPEG) |
232 | return -EINVAL; |
233 | |
234 | if (cmd == VIDIOC_TRY_FMT) |
235 | return 0; |
236 | |
237 | comp_set_format_struct(f: format); |
238 | |
239 | return 0; |
240 | } |
241 | |
242 | static int vidioc_querycap(struct file *file, void *priv, |
243 | struct v4l2_capability *cap) |
244 | { |
245 | struct comp_fh *fh = priv; |
246 | struct most_video_dev *mdev = fh->mdev; |
247 | |
248 | strscpy(cap->driver, "v4l2_component" , sizeof(cap->driver)); |
249 | strscpy(cap->card, "MOST" , sizeof(cap->card)); |
250 | snprintf(buf: cap->bus_info, size: sizeof(cap->bus_info), |
251 | fmt: "%s" , mdev->iface->description); |
252 | return 0; |
253 | } |
254 | |
255 | static int vidioc_enum_fmt_vid_cap(struct file *file, void *priv, |
256 | struct v4l2_fmtdesc *f) |
257 | { |
258 | if (f->index) |
259 | return -EINVAL; |
260 | |
261 | strscpy(f->description, "MPEG" , sizeof(f->description)); |
262 | f->type = V4L2_BUF_TYPE_VIDEO_CAPTURE; |
263 | f->flags = V4L2_FMT_FLAG_COMPRESSED; |
264 | f->pixelformat = V4L2_PIX_FMT_MPEG; |
265 | |
266 | return 0; |
267 | } |
268 | |
269 | static int vidioc_g_fmt_vid_cap(struct file *file, void *priv, |
270 | struct v4l2_format *f) |
271 | { |
272 | comp_set_format_struct(f); |
273 | return 0; |
274 | } |
275 | |
276 | static int vidioc_try_fmt_vid_cap(struct file *file, void *priv, |
277 | struct v4l2_format *f) |
278 | { |
279 | struct comp_fh *fh = priv; |
280 | struct most_video_dev *mdev = fh->mdev; |
281 | |
282 | return comp_set_format(mdev, VIDIOC_TRY_FMT, format: f); |
283 | } |
284 | |
285 | static int vidioc_s_fmt_vid_cap(struct file *file, void *priv, |
286 | struct v4l2_format *f) |
287 | { |
288 | struct comp_fh *fh = priv; |
289 | struct most_video_dev *mdev = fh->mdev; |
290 | |
291 | return comp_set_format(mdev, VIDIOC_S_FMT, format: f); |
292 | } |
293 | |
294 | static int vidioc_g_std(struct file *file, void *priv, v4l2_std_id *norm) |
295 | { |
296 | *norm = V4L2_STD_UNKNOWN; |
297 | return 0; |
298 | } |
299 | |
300 | static int vidioc_enum_input(struct file *file, void *priv, |
301 | struct v4l2_input *input) |
302 | { |
303 | struct comp_fh *fh = priv; |
304 | struct most_video_dev *mdev = fh->mdev; |
305 | |
306 | if (input->index >= V4L2_CMP_MAX_INPUT) |
307 | return -EINVAL; |
308 | |
309 | strscpy(input->name, "MOST Video" , sizeof(input->name)); |
310 | input->type |= V4L2_INPUT_TYPE_CAMERA; |
311 | input->audioset = 0; |
312 | |
313 | input->std = mdev->vdev->tvnorms; |
314 | |
315 | return 0; |
316 | } |
317 | |
318 | static int vidioc_g_input(struct file *file, void *priv, unsigned int *i) |
319 | { |
320 | struct comp_fh *fh = priv; |
321 | struct most_video_dev *mdev = fh->mdev; |
322 | *i = mdev->ctrl_input; |
323 | return 0; |
324 | } |
325 | |
326 | static int vidioc_s_input(struct file *file, void *priv, unsigned int index) |
327 | { |
328 | struct comp_fh *fh = priv; |
329 | struct most_video_dev *mdev = fh->mdev; |
330 | |
331 | if (index >= V4L2_CMP_MAX_INPUT) |
332 | return -EINVAL; |
333 | mdev->ctrl_input = index; |
334 | return 0; |
335 | } |
336 | |
337 | static const struct v4l2_file_operations comp_fops = { |
338 | .owner = THIS_MODULE, |
339 | .open = comp_vdev_open, |
340 | .release = comp_vdev_close, |
341 | .read = comp_vdev_read, |
342 | .poll = comp_vdev_poll, |
343 | .unlocked_ioctl = video_ioctl2, |
344 | }; |
345 | |
346 | static const struct v4l2_ioctl_ops video_ioctl_ops = { |
347 | .vidioc_querycap = vidioc_querycap, |
348 | .vidioc_enum_fmt_vid_cap = vidioc_enum_fmt_vid_cap, |
349 | .vidioc_g_fmt_vid_cap = vidioc_g_fmt_vid_cap, |
350 | .vidioc_try_fmt_vid_cap = vidioc_try_fmt_vid_cap, |
351 | .vidioc_s_fmt_vid_cap = vidioc_s_fmt_vid_cap, |
352 | .vidioc_g_std = vidioc_g_std, |
353 | .vidioc_enum_input = vidioc_enum_input, |
354 | .vidioc_g_input = vidioc_g_input, |
355 | .vidioc_s_input = vidioc_s_input, |
356 | }; |
357 | |
358 | static const struct video_device comp_videodev_template = { |
359 | .fops = &comp_fops, |
360 | .release = video_device_release, |
361 | .ioctl_ops = &video_ioctl_ops, |
362 | .tvnorms = V4L2_STD_UNKNOWN, |
363 | .device_caps = V4L2_CAP_READWRITE | V4L2_CAP_VIDEO_CAPTURE, |
364 | }; |
365 | |
366 | /**************************************************************************/ |
367 | |
368 | static struct most_video_dev *get_comp_dev(struct most_interface *iface, int channel_idx) |
369 | { |
370 | struct most_video_dev *mdev; |
371 | unsigned long flags; |
372 | |
373 | spin_lock_irqsave(&list_lock, flags); |
374 | list_for_each_entry(mdev, &video_devices, list) { |
375 | if (mdev->iface == iface && mdev->ch_idx == channel_idx) { |
376 | spin_unlock_irqrestore(lock: &list_lock, flags); |
377 | return mdev; |
378 | } |
379 | } |
380 | spin_unlock_irqrestore(lock: &list_lock, flags); |
381 | return NULL; |
382 | } |
383 | |
384 | static int comp_rx_data(struct mbo *mbo) |
385 | { |
386 | unsigned long flags; |
387 | struct most_video_dev *mdev = |
388 | get_comp_dev(iface: mbo->ifp, channel_idx: mbo->hdm_channel_id); |
389 | |
390 | if (!mdev) |
391 | return -EIO; |
392 | |
393 | spin_lock_irqsave(&mdev->list_lock, flags); |
394 | if (unlikely(mdev->mute)) { |
395 | spin_unlock_irqrestore(lock: &mdev->list_lock, flags); |
396 | return -EIO; |
397 | } |
398 | |
399 | list_add_tail(new: &mbo->list, head: &mdev->pending_mbos); |
400 | spin_unlock_irqrestore(lock: &mdev->list_lock, flags); |
401 | wake_up_interruptible(&mdev->wait_data); |
402 | return 0; |
403 | } |
404 | |
405 | static int comp_register_videodev(struct most_video_dev *mdev) |
406 | { |
407 | int ret; |
408 | |
409 | init_waitqueue_head(&mdev->wait_data); |
410 | |
411 | /* allocate and fill v4l2 video struct */ |
412 | mdev->vdev = video_device_alloc(); |
413 | if (!mdev->vdev) |
414 | return -ENOMEM; |
415 | |
416 | /* Fill the video capture device struct */ |
417 | *mdev->vdev = comp_videodev_template; |
418 | mdev->vdev->v4l2_dev = &mdev->v4l2_dev; |
419 | mdev->vdev->lock = &mdev->lock; |
420 | snprintf(buf: mdev->vdev->name, size: sizeof(mdev->vdev->name), fmt: "MOST: %s" , |
421 | mdev->v4l2_dev.name); |
422 | |
423 | /* Register the v4l2 device */ |
424 | video_set_drvdata(vdev: mdev->vdev, data: mdev); |
425 | ret = video_register_device(vdev: mdev->vdev, type: VFL_TYPE_VIDEO, nr: -1); |
426 | if (ret) { |
427 | v4l2_err(&mdev->v4l2_dev, "video_register_device failed (%d)\n" , |
428 | ret); |
429 | video_device_release(vdev: mdev->vdev); |
430 | } |
431 | |
432 | return ret; |
433 | } |
434 | |
435 | static void comp_unregister_videodev(struct most_video_dev *mdev) |
436 | { |
437 | video_unregister_device(vdev: mdev->vdev); |
438 | } |
439 | |
440 | static void comp_v4l2_dev_release(struct v4l2_device *v4l2_dev) |
441 | { |
442 | struct most_video_dev *mdev = |
443 | container_of(v4l2_dev, struct most_video_dev, v4l2_dev); |
444 | |
445 | v4l2_device_unregister(v4l2_dev); |
446 | kfree(objp: mdev); |
447 | } |
448 | |
449 | static int comp_probe_channel(struct most_interface *iface, int channel_idx, |
450 | struct most_channel_config *ccfg, char *name, |
451 | char *args) |
452 | { |
453 | int ret; |
454 | struct most_video_dev *mdev = get_comp_dev(iface, channel_idx); |
455 | |
456 | if (mdev) { |
457 | pr_err("channel already linked\n" ); |
458 | return -EEXIST; |
459 | } |
460 | |
461 | if (ccfg->direction != MOST_CH_RX) { |
462 | pr_err("wrong direction, expect rx\n" ); |
463 | return -EINVAL; |
464 | } |
465 | |
466 | if (ccfg->data_type != MOST_CH_SYNC && |
467 | ccfg->data_type != MOST_CH_ISOC) { |
468 | pr_err("wrong channel type, expect sync or isoc\n" ); |
469 | return -EINVAL; |
470 | } |
471 | |
472 | mdev = kzalloc(size: sizeof(*mdev), GFP_KERNEL); |
473 | if (!mdev) |
474 | return -ENOMEM; |
475 | |
476 | mutex_init(&mdev->lock); |
477 | atomic_set(v: &mdev->access_ref, i: -1); |
478 | spin_lock_init(&mdev->list_lock); |
479 | INIT_LIST_HEAD(list: &mdev->pending_mbos); |
480 | mdev->iface = iface; |
481 | mdev->ch_idx = channel_idx; |
482 | mdev->v4l2_dev.release = comp_v4l2_dev_release; |
483 | |
484 | /* Create the v4l2_device */ |
485 | strscpy(mdev->v4l2_dev.name, name, sizeof(mdev->v4l2_dev.name)); |
486 | ret = v4l2_device_register(NULL, v4l2_dev: &mdev->v4l2_dev); |
487 | if (ret) { |
488 | pr_err("v4l2_device_register() failed\n" ); |
489 | kfree(objp: mdev); |
490 | return ret; |
491 | } |
492 | |
493 | ret = comp_register_videodev(mdev); |
494 | if (ret) |
495 | goto err_unreg; |
496 | |
497 | spin_lock_irq(lock: &list_lock); |
498 | list_add(new: &mdev->list, head: &video_devices); |
499 | spin_unlock_irq(lock: &list_lock); |
500 | return 0; |
501 | |
502 | err_unreg: |
503 | v4l2_device_disconnect(v4l2_dev: &mdev->v4l2_dev); |
504 | v4l2_device_put(v4l2_dev: &mdev->v4l2_dev); |
505 | return ret; |
506 | } |
507 | |
508 | static int comp_disconnect_channel(struct most_interface *iface, |
509 | int channel_idx) |
510 | { |
511 | struct most_video_dev *mdev = get_comp_dev(iface, channel_idx); |
512 | |
513 | if (!mdev) { |
514 | pr_err("no such channel is linked\n" ); |
515 | return -ENOENT; |
516 | } |
517 | |
518 | spin_lock_irq(lock: &list_lock); |
519 | list_del(entry: &mdev->list); |
520 | spin_unlock_irq(lock: &list_lock); |
521 | |
522 | comp_unregister_videodev(mdev); |
523 | v4l2_device_disconnect(v4l2_dev: &mdev->v4l2_dev); |
524 | v4l2_device_put(v4l2_dev: &mdev->v4l2_dev); |
525 | return 0; |
526 | } |
527 | |
528 | static struct most_component comp = { |
529 | .mod = THIS_MODULE, |
530 | .name = "video" , |
531 | .probe_channel = comp_probe_channel, |
532 | .disconnect_channel = comp_disconnect_channel, |
533 | .rx_completion = comp_rx_data, |
534 | }; |
535 | |
536 | static int __init comp_init(void) |
537 | { |
538 | int err; |
539 | |
540 | err = most_register_component(comp: &comp); |
541 | if (err) |
542 | return err; |
543 | err = most_register_configfs_subsys(comp: &comp); |
544 | if (err) { |
545 | most_deregister_component(comp: &comp); |
546 | return err; |
547 | } |
548 | return 0; |
549 | } |
550 | |
551 | static void __exit comp_exit(void) |
552 | { |
553 | struct most_video_dev *mdev, *tmp; |
554 | |
555 | /* |
556 | * As the mostcore currently doesn't call disconnect_channel() |
557 | * for linked channels while we call most_deregister_component() |
558 | * we simulate this call here. |
559 | * This must be fixed in core. |
560 | */ |
561 | spin_lock_irq(lock: &list_lock); |
562 | list_for_each_entry_safe(mdev, tmp, &video_devices, list) { |
563 | list_del(entry: &mdev->list); |
564 | spin_unlock_irq(lock: &list_lock); |
565 | |
566 | comp_unregister_videodev(mdev); |
567 | v4l2_device_disconnect(v4l2_dev: &mdev->v4l2_dev); |
568 | v4l2_device_put(v4l2_dev: &mdev->v4l2_dev); |
569 | spin_lock_irq(lock: &list_lock); |
570 | } |
571 | spin_unlock_irq(lock: &list_lock); |
572 | |
573 | most_deregister_configfs_subsys(comp: &comp); |
574 | most_deregister_component(comp: &comp); |
575 | BUG_ON(!list_empty(&video_devices)); |
576 | } |
577 | |
578 | module_init(comp_init); |
579 | module_exit(comp_exit); |
580 | |
581 | MODULE_DESCRIPTION("V4L2 Component Module for Mostcore" ); |
582 | MODULE_AUTHOR("Andrey Shvetsov <andrey.shvetsov@k2l.de>" ); |
583 | MODULE_LICENSE("GPL" ); |
584 | |