Commit | Line | Data |
---|---|---|
5cebaac6 MR |
1 | // SPDX-License-Identifier: GPL-2.0 |
2 | /* | |
3 | * video-i2c.c - Support for I2C transport video devices | |
4 | * | |
5 | * Copyright (C) 2018 Matt Ranostay <matt.ranostay@konsulko.com> | |
6 | * | |
7 | * Supported: | |
8 | * - Panasonic AMG88xx Grid-Eye Sensors | |
9 | */ | |
10 | ||
11 | #include <linux/delay.h> | |
12 | #include <linux/freezer.h> | |
13 | #include <linux/kthread.h> | |
14 | #include <linux/i2c.h> | |
15 | #include <linux/list.h> | |
16 | #include <linux/module.h> | |
17 | #include <linux/mutex.h> | |
18 | #include <linux/of_device.h> | |
19 | #include <linux/sched.h> | |
20 | #include <linux/slab.h> | |
21 | #include <linux/videodev2.h> | |
22 | #include <media/v4l2-common.h> | |
23 | #include <media/v4l2-device.h> | |
24 | #include <media/v4l2-event.h> | |
25 | #include <media/v4l2-fh.h> | |
26 | #include <media/v4l2-ioctl.h> | |
27 | #include <media/videobuf2-v4l2.h> | |
28 | #include <media/videobuf2-vmalloc.h> | |
29 | ||
30 | #define VIDEO_I2C_DRIVER "video-i2c" | |
31 | ||
32 | struct video_i2c_chip; | |
33 | ||
34 | struct video_i2c_buffer { | |
35 | struct vb2_v4l2_buffer vb; | |
36 | struct list_head list; | |
37 | }; | |
38 | ||
39 | struct video_i2c_data { | |
40 | struct i2c_client *client; | |
41 | const struct video_i2c_chip *chip; | |
42 | struct mutex lock; | |
43 | spinlock_t slock; | |
44 | unsigned int sequence; | |
45 | struct mutex queue_lock; | |
46 | ||
47 | struct v4l2_device v4l2_dev; | |
48 | struct video_device vdev; | |
49 | struct vb2_queue vb_vidq; | |
50 | ||
51 | struct task_struct *kthread_vid_cap; | |
52 | struct list_head vid_cap_active; | |
53 | }; | |
54 | ||
1b3d5f2a | 55 | static const struct v4l2_fmtdesc amg88xx_format = { |
5cebaac6 MR |
56 | .pixelformat = V4L2_PIX_FMT_Y12, |
57 | }; | |
58 | ||
1b3d5f2a | 59 | static const struct v4l2_frmsize_discrete amg88xx_size = { |
5cebaac6 MR |
60 | .width = 8, |
61 | .height = 8, | |
62 | }; | |
63 | ||
64 | struct video_i2c_chip { | |
65 | /* video dimensions */ | |
66 | const struct v4l2_fmtdesc *format; | |
67 | const struct v4l2_frmsize_discrete *size; | |
68 | ||
69 | /* max frames per second */ | |
70 | unsigned int max_fps; | |
71 | ||
72 | /* pixel buffer size */ | |
73 | unsigned int buffer_size; | |
74 | ||
75 | /* pixel size in bits */ | |
76 | unsigned int bpp; | |
77 | ||
78 | /* xfer function */ | |
79 | int (*xfer)(struct video_i2c_data *data, char *buf); | |
80 | }; | |
81 | ||
82 | static int amg88xx_xfer(struct video_i2c_data *data, char *buf) | |
83 | { | |
84 | struct i2c_client *client = data->client; | |
85 | struct i2c_msg msg[2]; | |
86 | u8 reg = 0x80; | |
87 | int ret; | |
88 | ||
89 | msg[0].addr = client->addr; | |
90 | msg[0].flags = 0; | |
91 | msg[0].len = 1; | |
92 | msg[0].buf = (char *)® | |
93 | ||
94 | msg[1].addr = client->addr; | |
95 | msg[1].flags = I2C_M_RD; | |
96 | msg[1].len = data->chip->buffer_size; | |
97 | msg[1].buf = (char *)buf; | |
98 | ||
99 | ret = i2c_transfer(client->adapter, msg, 2); | |
100 | ||
101 | return (ret == 2) ? 0 : -EIO; | |
102 | } | |
103 | ||
104 | #define AMG88XX 0 | |
105 | ||
106 | static const struct video_i2c_chip video_i2c_chip[] = { | |
107 | [AMG88XX] = { | |
108 | .size = &amg88xx_size, | |
109 | .format = &amg88xx_format, | |
110 | .max_fps = 10, | |
111 | .buffer_size = 128, | |
112 | .bpp = 16, | |
113 | .xfer = &amg88xx_xfer, | |
114 | }, | |
115 | }; | |
116 | ||
117 | static const struct v4l2_file_operations video_i2c_fops = { | |
118 | .owner = THIS_MODULE, | |
119 | .open = v4l2_fh_open, | |
120 | .release = vb2_fop_release, | |
121 | .poll = vb2_fop_poll, | |
122 | .read = vb2_fop_read, | |
123 | .mmap = vb2_fop_mmap, | |
124 | .unlocked_ioctl = video_ioctl2, | |
125 | }; | |
126 | ||
127 | static int queue_setup(struct vb2_queue *vq, | |
128 | unsigned int *nbuffers, unsigned int *nplanes, | |
129 | unsigned int sizes[], struct device *alloc_devs[]) | |
130 | { | |
131 | struct video_i2c_data *data = vb2_get_drv_priv(vq); | |
132 | unsigned int size = data->chip->buffer_size; | |
133 | ||
134 | if (vq->num_buffers + *nbuffers < 2) | |
135 | *nbuffers = 2; | |
136 | ||
137 | if (*nplanes) | |
138 | return sizes[0] < size ? -EINVAL : 0; | |
139 | ||
140 | *nplanes = 1; | |
141 | sizes[0] = size; | |
142 | ||
143 | return 0; | |
144 | } | |
145 | ||
146 | static int buffer_prepare(struct vb2_buffer *vb) | |
147 | { | |
148 | struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb); | |
149 | struct video_i2c_data *data = vb2_get_drv_priv(vb->vb2_queue); | |
150 | unsigned int size = data->chip->buffer_size; | |
151 | ||
152 | if (vb2_plane_size(vb, 0) < size) | |
153 | return -EINVAL; | |
154 | ||
155 | vbuf->field = V4L2_FIELD_NONE; | |
156 | vb2_set_plane_payload(vb, 0, size); | |
157 | ||
158 | return 0; | |
159 | } | |
160 | ||
161 | static void buffer_queue(struct vb2_buffer *vb) | |
162 | { | |
163 | struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb); | |
164 | struct video_i2c_data *data = vb2_get_drv_priv(vb->vb2_queue); | |
165 | struct video_i2c_buffer *buf = | |
166 | container_of(vbuf, struct video_i2c_buffer, vb); | |
167 | ||
168 | spin_lock(&data->slock); | |
169 | list_add_tail(&buf->list, &data->vid_cap_active); | |
170 | spin_unlock(&data->slock); | |
171 | } | |
172 | ||
173 | static int video_i2c_thread_vid_cap(void *priv) | |
174 | { | |
175 | struct video_i2c_data *data = priv; | |
176 | unsigned int delay = msecs_to_jiffies(1000 / data->chip->max_fps); | |
177 | ||
178 | set_freezable(); | |
179 | ||
180 | do { | |
181 | unsigned long start_jiffies = jiffies; | |
182 | struct video_i2c_buffer *vid_cap_buf = NULL; | |
183 | int schedule_delay; | |
184 | ||
185 | try_to_freeze(); | |
186 | ||
187 | spin_lock(&data->slock); | |
188 | ||
189 | if (!list_empty(&data->vid_cap_active)) { | |
190 | vid_cap_buf = list_last_entry(&data->vid_cap_active, | |
191 | struct video_i2c_buffer, list); | |
192 | list_del(&vid_cap_buf->list); | |
193 | } | |
194 | ||
195 | spin_unlock(&data->slock); | |
196 | ||
197 | if (vid_cap_buf) { | |
198 | struct vb2_buffer *vb2_buf = &vid_cap_buf->vb.vb2_buf; | |
199 | void *vbuf = vb2_plane_vaddr(vb2_buf, 0); | |
200 | int ret; | |
201 | ||
202 | ret = data->chip->xfer(data, vbuf); | |
203 | vb2_buf->timestamp = ktime_get_ns(); | |
204 | vid_cap_buf->vb.sequence = data->sequence++; | |
205 | vb2_buffer_done(vb2_buf, ret ? | |
206 | VB2_BUF_STATE_ERROR : VB2_BUF_STATE_DONE); | |
207 | } | |
208 | ||
209 | schedule_delay = delay - (jiffies - start_jiffies); | |
210 | ||
211 | if (time_after(jiffies, start_jiffies + delay)) | |
212 | schedule_delay = delay; | |
213 | ||
214 | schedule_timeout_interruptible(schedule_delay); | |
215 | } while (!kthread_should_stop()); | |
216 | ||
217 | return 0; | |
218 | } | |
219 | ||
220 | static void video_i2c_del_list(struct vb2_queue *vq, enum vb2_buffer_state state) | |
221 | { | |
222 | struct video_i2c_data *data = vb2_get_drv_priv(vq); | |
223 | struct video_i2c_buffer *buf, *tmp; | |
224 | ||
225 | spin_lock(&data->slock); | |
226 | ||
227 | list_for_each_entry_safe(buf, tmp, &data->vid_cap_active, list) { | |
228 | list_del(&buf->list); | |
229 | vb2_buffer_done(&buf->vb.vb2_buf, state); | |
230 | } | |
231 | ||
232 | spin_unlock(&data->slock); | |
233 | } | |
234 | ||
235 | static int start_streaming(struct vb2_queue *vq, unsigned int count) | |
236 | { | |
237 | struct video_i2c_data *data = vb2_get_drv_priv(vq); | |
238 | ||
239 | if (data->kthread_vid_cap) | |
240 | return 0; | |
241 | ||
242 | data->sequence = 0; | |
243 | data->kthread_vid_cap = kthread_run(video_i2c_thread_vid_cap, data, | |
244 | "%s-vid-cap", data->v4l2_dev.name); | |
245 | if (!IS_ERR(data->kthread_vid_cap)) | |
246 | return 0; | |
247 | ||
248 | video_i2c_del_list(vq, VB2_BUF_STATE_QUEUED); | |
249 | ||
250 | return PTR_ERR(data->kthread_vid_cap); | |
251 | } | |
252 | ||
253 | static void stop_streaming(struct vb2_queue *vq) | |
254 | { | |
255 | struct video_i2c_data *data = vb2_get_drv_priv(vq); | |
256 | ||
257 | if (data->kthread_vid_cap == NULL) | |
258 | return; | |
259 | ||
260 | kthread_stop(data->kthread_vid_cap); | |
261 | data->kthread_vid_cap = NULL; | |
262 | ||
263 | video_i2c_del_list(vq, VB2_BUF_STATE_ERROR); | |
264 | } | |
265 | ||
266 | static struct vb2_ops video_i2c_video_qops = { | |
267 | .queue_setup = queue_setup, | |
268 | .buf_prepare = buffer_prepare, | |
269 | .buf_queue = buffer_queue, | |
270 | .start_streaming = start_streaming, | |
271 | .stop_streaming = stop_streaming, | |
272 | .wait_prepare = vb2_ops_wait_prepare, | |
273 | .wait_finish = vb2_ops_wait_finish, | |
274 | }; | |
275 | ||
276 | static int video_i2c_querycap(struct file *file, void *priv, | |
277 | struct v4l2_capability *vcap) | |
278 | { | |
279 | struct video_i2c_data *data = video_drvdata(file); | |
280 | struct i2c_client *client = data->client; | |
281 | ||
282 | strlcpy(vcap->driver, data->v4l2_dev.name, sizeof(vcap->driver)); | |
283 | strlcpy(vcap->card, data->vdev.name, sizeof(vcap->card)); | |
284 | ||
285 | sprintf(vcap->bus_info, "I2C:%d-%d", client->adapter->nr, client->addr); | |
286 | ||
287 | return 0; | |
288 | } | |
289 | ||
290 | static int video_i2c_g_input(struct file *file, void *fh, unsigned int *inp) | |
291 | { | |
292 | *inp = 0; | |
293 | ||
294 | return 0; | |
295 | } | |
296 | ||
297 | static int video_i2c_s_input(struct file *file, void *fh, unsigned int inp) | |
298 | { | |
299 | return (inp > 0) ? -EINVAL : 0; | |
300 | } | |
301 | ||
302 | static int video_i2c_enum_input(struct file *file, void *fh, | |
303 | struct v4l2_input *vin) | |
304 | { | |
305 | if (vin->index > 0) | |
306 | return -EINVAL; | |
307 | ||
308 | strlcpy(vin->name, "Camera", sizeof(vin->name)); | |
309 | ||
310 | vin->type = V4L2_INPUT_TYPE_CAMERA; | |
311 | ||
312 | return 0; | |
313 | } | |
314 | ||
315 | static int video_i2c_enum_fmt_vid_cap(struct file *file, void *fh, | |
316 | struct v4l2_fmtdesc *fmt) | |
317 | { | |
318 | struct video_i2c_data *data = video_drvdata(file); | |
319 | enum v4l2_buf_type type = fmt->type; | |
320 | ||
321 | if (fmt->index > 0) | |
322 | return -EINVAL; | |
323 | ||
324 | *fmt = *data->chip->format; | |
325 | fmt->type = type; | |
326 | ||
327 | return 0; | |
328 | } | |
329 | ||
330 | static int video_i2c_enum_framesizes(struct file *file, void *fh, | |
331 | struct v4l2_frmsizeenum *fsize) | |
332 | { | |
333 | const struct video_i2c_data *data = video_drvdata(file); | |
334 | const struct v4l2_frmsize_discrete *size = data->chip->size; | |
335 | ||
336 | /* currently only one frame size is allowed */ | |
337 | if (fsize->index > 0) | |
338 | return -EINVAL; | |
339 | ||
340 | if (fsize->pixel_format != data->chip->format->pixelformat) | |
341 | return -EINVAL; | |
342 | ||
343 | fsize->type = V4L2_FRMSIZE_TYPE_DISCRETE; | |
344 | fsize->discrete.width = size->width; | |
345 | fsize->discrete.height = size->height; | |
346 | ||
347 | return 0; | |
348 | } | |
349 | ||
350 | static int video_i2c_enum_frameintervals(struct file *file, void *priv, | |
351 | struct v4l2_frmivalenum *fe) | |
352 | { | |
353 | const struct video_i2c_data *data = video_drvdata(file); | |
354 | const struct v4l2_frmsize_discrete *size = data->chip->size; | |
355 | ||
356 | if (fe->index > 0) | |
357 | return -EINVAL; | |
358 | ||
359 | if (fe->width != size->width || fe->height != size->height) | |
360 | return -EINVAL; | |
361 | ||
362 | fe->type = V4L2_FRMIVAL_TYPE_DISCRETE; | |
363 | fe->discrete.numerator = 1; | |
364 | fe->discrete.denominator = data->chip->max_fps; | |
365 | ||
366 | return 0; | |
367 | } | |
368 | ||
369 | static int video_i2c_try_fmt_vid_cap(struct file *file, void *fh, | |
370 | struct v4l2_format *fmt) | |
371 | { | |
372 | const struct video_i2c_data *data = video_drvdata(file); | |
373 | const struct v4l2_frmsize_discrete *size = data->chip->size; | |
374 | struct v4l2_pix_format *pix = &fmt->fmt.pix; | |
375 | unsigned int bpp = data->chip->bpp / 8; | |
376 | ||
377 | pix->width = size->width; | |
378 | pix->height = size->height; | |
379 | pix->pixelformat = data->chip->format->pixelformat; | |
380 | pix->field = V4L2_FIELD_NONE; | |
381 | pix->bytesperline = pix->width * bpp; | |
382 | pix->sizeimage = pix->bytesperline * pix->height; | |
383 | pix->colorspace = V4L2_COLORSPACE_RAW; | |
384 | ||
385 | return 0; | |
386 | } | |
387 | ||
388 | static int video_i2c_s_fmt_vid_cap(struct file *file, void *fh, | |
389 | struct v4l2_format *fmt) | |
390 | { | |
391 | struct video_i2c_data *data = video_drvdata(file); | |
392 | ||
393 | if (vb2_is_busy(&data->vb_vidq)) | |
394 | return -EBUSY; | |
395 | ||
396 | return video_i2c_try_fmt_vid_cap(file, fh, fmt); | |
397 | } | |
398 | ||
399 | static int video_i2c_g_parm(struct file *filp, void *priv, | |
400 | struct v4l2_streamparm *parm) | |
401 | { | |
402 | struct video_i2c_data *data = video_drvdata(filp); | |
403 | ||
404 | if (parm->type != V4L2_BUF_TYPE_VIDEO_CAPTURE) | |
405 | return -EINVAL; | |
406 | ||
407 | parm->parm.capture.readbuffers = 1; | |
408 | parm->parm.capture.capability = V4L2_CAP_TIMEPERFRAME; | |
409 | parm->parm.capture.timeperframe.numerator = 1; | |
410 | parm->parm.capture.timeperframe.denominator = data->chip->max_fps; | |
411 | ||
412 | return 0; | |
413 | } | |
414 | ||
415 | static const struct v4l2_ioctl_ops video_i2c_ioctl_ops = { | |
416 | .vidioc_querycap = video_i2c_querycap, | |
417 | .vidioc_g_input = video_i2c_g_input, | |
418 | .vidioc_s_input = video_i2c_s_input, | |
419 | .vidioc_enum_input = video_i2c_enum_input, | |
420 | .vidioc_enum_fmt_vid_cap = video_i2c_enum_fmt_vid_cap, | |
421 | .vidioc_enum_framesizes = video_i2c_enum_framesizes, | |
422 | .vidioc_enum_frameintervals = video_i2c_enum_frameintervals, | |
423 | .vidioc_g_fmt_vid_cap = video_i2c_try_fmt_vid_cap, | |
424 | .vidioc_s_fmt_vid_cap = video_i2c_s_fmt_vid_cap, | |
425 | .vidioc_g_parm = video_i2c_g_parm, | |
426 | .vidioc_s_parm = video_i2c_g_parm, | |
427 | .vidioc_try_fmt_vid_cap = video_i2c_try_fmt_vid_cap, | |
428 | .vidioc_reqbufs = vb2_ioctl_reqbufs, | |
429 | .vidioc_create_bufs = vb2_ioctl_create_bufs, | |
430 | .vidioc_prepare_buf = vb2_ioctl_prepare_buf, | |
431 | .vidioc_querybuf = vb2_ioctl_querybuf, | |
432 | .vidioc_qbuf = vb2_ioctl_qbuf, | |
433 | .vidioc_dqbuf = vb2_ioctl_dqbuf, | |
434 | .vidioc_streamon = vb2_ioctl_streamon, | |
435 | .vidioc_streamoff = vb2_ioctl_streamoff, | |
436 | }; | |
437 | ||
438 | static void video_i2c_release(struct video_device *vdev) | |
439 | { | |
440 | kfree(video_get_drvdata(vdev)); | |
441 | } | |
442 | ||
443 | static int video_i2c_probe(struct i2c_client *client, | |
444 | const struct i2c_device_id *id) | |
445 | { | |
446 | struct video_i2c_data *data; | |
447 | struct v4l2_device *v4l2_dev; | |
448 | struct vb2_queue *queue; | |
449 | int ret = -ENODEV; | |
450 | ||
451 | data = kzalloc(sizeof(*data), GFP_KERNEL); | |
452 | if (!data) | |
453 | return -ENOMEM; | |
454 | ||
455 | if (dev_fwnode(&client->dev)) | |
456 | data->chip = device_get_match_data(&client->dev); | |
457 | else if (id) | |
458 | data->chip = &video_i2c_chip[id->driver_data]; | |
459 | else | |
460 | goto error_free_device; | |
461 | ||
462 | data->client = client; | |
463 | v4l2_dev = &data->v4l2_dev; | |
464 | strlcpy(v4l2_dev->name, VIDEO_I2C_DRIVER, sizeof(v4l2_dev->name)); | |
465 | ||
466 | ret = v4l2_device_register(&client->dev, v4l2_dev); | |
467 | if (ret < 0) | |
468 | goto error_free_device; | |
469 | ||
470 | mutex_init(&data->lock); | |
471 | mutex_init(&data->queue_lock); | |
472 | ||
473 | queue = &data->vb_vidq; | |
474 | queue->type = V4L2_BUF_TYPE_VIDEO_CAPTURE; | |
475 | queue->io_modes = VB2_DMABUF | VB2_MMAP | VB2_USERPTR | VB2_READ; | |
476 | queue->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC; | |
477 | queue->drv_priv = data; | |
478 | queue->buf_struct_size = sizeof(struct video_i2c_buffer); | |
479 | queue->min_buffers_needed = 1; | |
480 | queue->ops = &video_i2c_video_qops; | |
481 | queue->mem_ops = &vb2_vmalloc_memops; | |
482 | ||
483 | ret = vb2_queue_init(queue); | |
484 | if (ret < 0) | |
485 | goto error_unregister_device; | |
486 | ||
487 | data->vdev.queue = queue; | |
488 | data->vdev.queue->lock = &data->queue_lock; | |
489 | ||
490 | snprintf(data->vdev.name, sizeof(data->vdev.name), | |
491 | "I2C %d-%d Transport Video", | |
492 | client->adapter->nr, client->addr); | |
493 | ||
494 | data->vdev.v4l2_dev = v4l2_dev; | |
495 | data->vdev.fops = &video_i2c_fops; | |
496 | data->vdev.lock = &data->lock; | |
497 | data->vdev.ioctl_ops = &video_i2c_ioctl_ops; | |
498 | data->vdev.release = video_i2c_release; | |
499 | data->vdev.device_caps = V4L2_CAP_VIDEO_CAPTURE | | |
500 | V4L2_CAP_READWRITE | V4L2_CAP_STREAMING; | |
501 | ||
502 | spin_lock_init(&data->slock); | |
503 | INIT_LIST_HEAD(&data->vid_cap_active); | |
504 | ||
505 | video_set_drvdata(&data->vdev, data); | |
506 | i2c_set_clientdata(client, data); | |
507 | ||
508 | ret = video_register_device(&data->vdev, VFL_TYPE_GRABBER, -1); | |
509 | if (ret < 0) | |
510 | goto error_unregister_device; | |
511 | ||
512 | return 0; | |
513 | ||
514 | error_unregister_device: | |
515 | v4l2_device_unregister(v4l2_dev); | |
516 | mutex_destroy(&data->lock); | |
517 | mutex_destroy(&data->queue_lock); | |
518 | ||
519 | error_free_device: | |
520 | kfree(data); | |
521 | ||
522 | return ret; | |
523 | } | |
524 | ||
525 | static int video_i2c_remove(struct i2c_client *client) | |
526 | { | |
527 | struct video_i2c_data *data = i2c_get_clientdata(client); | |
528 | ||
529 | video_unregister_device(&data->vdev); | |
530 | v4l2_device_unregister(&data->v4l2_dev); | |
531 | ||
532 | mutex_destroy(&data->lock); | |
533 | mutex_destroy(&data->queue_lock); | |
534 | ||
535 | return 0; | |
536 | } | |
537 | ||
538 | static const struct i2c_device_id video_i2c_id_table[] = { | |
539 | { "amg88xx", AMG88XX }, | |
540 | {} | |
541 | }; | |
542 | MODULE_DEVICE_TABLE(i2c, video_i2c_id_table); | |
543 | ||
544 | static const struct of_device_id video_i2c_of_match[] = { | |
545 | { .compatible = "panasonic,amg88xx", .data = &video_i2c_chip[AMG88XX] }, | |
546 | {} | |
547 | }; | |
548 | MODULE_DEVICE_TABLE(of, video_i2c_of_match); | |
549 | ||
550 | static struct i2c_driver video_i2c_driver = { | |
551 | .driver = { | |
552 | .name = VIDEO_I2C_DRIVER, | |
553 | .of_match_table = video_i2c_of_match, | |
554 | }, | |
555 | .probe = video_i2c_probe, | |
556 | .remove = video_i2c_remove, | |
557 | .id_table = video_i2c_id_table, | |
558 | }; | |
559 | ||
560 | module_i2c_driver(video_i2c_driver); | |
561 | ||
562 | MODULE_AUTHOR("Matt Ranostay <matt.ranostay@konsulko.com>"); | |
563 | MODULE_DESCRIPTION("I2C transport video support"); | |
564 | MODULE_LICENSE("GPL v2"); |