1 /*
2  * V4L2 Media Controller Driver for Freescale i.MX5/6 SOC
3  *
4  * Copyright (c) 2016 Mentor Graphics Inc.
5  *
6  * This program is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  */
11 #include <linux/delay.h>
12 #include <linux/fs.h>
13 #include <linux/module.h>
14 #include <linux/of_graph.h>
15 #include <linux/of_platform.h>
16 #include <linux/pinctrl/consumer.h>
17 #include <linux/platform_device.h>
18 #include <linux/sched.h>
19 #include <linux/slab.h>
20 #include <linux/spinlock.h>
21 #include <linux/timer.h>
22 #include <media/v4l2-ctrls.h>
23 #include <media/v4l2-event.h>
24 #include <media/v4l2-ioctl.h>
25 #include <media/v4l2-mc.h>
26 #include <video/imx-ipu-v3.h>
27 #include <media/imx.h>
28 #include "imx-media.h"
29 
notifier2dev(struct v4l2_async_notifier * n)30 static inline struct imx_media_dev *notifier2dev(struct v4l2_async_notifier *n)
31 {
32 	return container_of(n, struct imx_media_dev, subdev_notifier);
33 }
34 
35 /*
36  * Find an asd by fwnode or device name. This is called during
37  * driver load to form the async subdev list and bind them.
38  */
39 static struct v4l2_async_subdev *
find_async_subdev(struct imx_media_dev * imxmd,struct fwnode_handle * fwnode,const char * devname)40 find_async_subdev(struct imx_media_dev *imxmd,
41 		  struct fwnode_handle *fwnode,
42 		  const char *devname)
43 {
44 	struct imx_media_async_subdev *imxasd;
45 	struct v4l2_async_subdev *asd;
46 
47 	list_for_each_entry(imxasd, &imxmd->asd_list, list) {
48 		asd = &imxasd->asd;
49 		switch (asd->match_type) {
50 		case V4L2_ASYNC_MATCH_FWNODE:
51 			if (fwnode && asd->match.fwnode == fwnode)
52 				return asd;
53 			break;
54 		case V4L2_ASYNC_MATCH_DEVNAME:
55 			if (devname && !strcmp(asd->match.device_name,
56 					       devname))
57 				return asd;
58 			break;
59 		default:
60 			break;
61 		}
62 	}
63 
64 	return NULL;
65 }
66 
67 
68 /*
69  * Adds a subdev to the async subdev list. If fwnode is non-NULL, adds
70  * the async as a V4L2_ASYNC_MATCH_FWNODE match type, otherwise as
71  * a V4L2_ASYNC_MATCH_DEVNAME match type using the dev_name of the
72  * given platform_device. This is called during driver load when
73  * forming the async subdev list.
74  */
imx_media_add_async_subdev(struct imx_media_dev * imxmd,struct fwnode_handle * fwnode,struct platform_device * pdev)75 int imx_media_add_async_subdev(struct imx_media_dev *imxmd,
76 			       struct fwnode_handle *fwnode,
77 			       struct platform_device *pdev)
78 {
79 	struct device_node *np = to_of_node(fwnode);
80 	struct imx_media_async_subdev *imxasd;
81 	struct v4l2_async_subdev *asd;
82 	const char *devname = NULL;
83 	int ret = 0;
84 
85 	mutex_lock(&imxmd->mutex);
86 
87 	if (pdev)
88 		devname = dev_name(&pdev->dev);
89 
90 	/* return -EEXIST if this asd already added */
91 	if (find_async_subdev(imxmd, fwnode, devname)) {
92 		dev_dbg(imxmd->md.dev, "%s: already added %s\n",
93 			__func__, np ? np->name : devname);
94 		ret = -EEXIST;
95 		goto out;
96 	}
97 
98 	imxasd = devm_kzalloc(imxmd->md.dev, sizeof(*imxasd), GFP_KERNEL);
99 	if (!imxasd) {
100 		ret = -ENOMEM;
101 		goto out;
102 	}
103 	asd = &imxasd->asd;
104 
105 	if (fwnode) {
106 		asd->match_type = V4L2_ASYNC_MATCH_FWNODE;
107 		asd->match.fwnode = fwnode;
108 	} else {
109 		asd->match_type = V4L2_ASYNC_MATCH_DEVNAME;
110 		asd->match.device_name = devname;
111 		imxasd->pdev = pdev;
112 	}
113 
114 	list_add_tail(&imxasd->list, &imxmd->asd_list);
115 
116 	imxmd->subdev_notifier.num_subdevs++;
117 
118 	dev_dbg(imxmd->md.dev, "%s: added %s, match type %s\n",
119 		__func__, np ? np->name : devname, np ? "FWNODE" : "DEVNAME");
120 
121 out:
122 	mutex_unlock(&imxmd->mutex);
123 	return ret;
124 }
125 
126 /*
127  * get IPU from this CSI and add it to the list of IPUs
128  * the media driver will control.
129  */
imx_media_get_ipu(struct imx_media_dev * imxmd,struct v4l2_subdev * csi_sd)130 static int imx_media_get_ipu(struct imx_media_dev *imxmd,
131 			     struct v4l2_subdev *csi_sd)
132 {
133 	struct ipu_soc *ipu;
134 	int ipu_id;
135 
136 	ipu = dev_get_drvdata(csi_sd->dev->parent);
137 	if (!ipu) {
138 		v4l2_err(&imxmd->v4l2_dev,
139 			 "CSI %s has no parent IPU!\n", csi_sd->name);
140 		return -ENODEV;
141 	}
142 
143 	ipu_id = ipu_get_num(ipu);
144 	if (ipu_id > 1) {
145 		v4l2_err(&imxmd->v4l2_dev, "invalid IPU id %d!\n", ipu_id);
146 		return -ENODEV;
147 	}
148 
149 	if (!imxmd->ipu[ipu_id])
150 		imxmd->ipu[ipu_id] = ipu;
151 
152 	return 0;
153 }
154 
155 /* async subdev bound notifier */
imx_media_subdev_bound(struct v4l2_async_notifier * notifier,struct v4l2_subdev * sd,struct v4l2_async_subdev * asd)156 static int imx_media_subdev_bound(struct v4l2_async_notifier *notifier,
157 				  struct v4l2_subdev *sd,
158 				  struct v4l2_async_subdev *asd)
159 {
160 	struct imx_media_dev *imxmd = notifier2dev(notifier);
161 	int ret = 0;
162 
163 	mutex_lock(&imxmd->mutex);
164 
165 	if (sd->grp_id & IMX_MEDIA_GRP_ID_CSI) {
166 		ret = imx_media_get_ipu(imxmd, sd);
167 		if (ret)
168 			goto out;
169 	}
170 
171 	v4l2_info(&imxmd->v4l2_dev, "subdev %s bound\n", sd->name);
172 out:
173 	mutex_unlock(&imxmd->mutex);
174 	return ret;
175 }
176 
177 /*
178  * create the media links for all subdevs that registered async.
179  * Called after all async subdevs have bound.
180  */
imx_media_create_links(struct v4l2_async_notifier * notifier)181 static int imx_media_create_links(struct v4l2_async_notifier *notifier)
182 {
183 	struct imx_media_dev *imxmd = notifier2dev(notifier);
184 	struct v4l2_subdev *sd;
185 	int ret;
186 
187 	/*
188 	 * Only links are created between subdevices that are known
189 	 * to the async notifier. If there are other non-async subdevices,
190 	 * they were created internally by some subdevice (smiapp is one
191 	 * example). In those cases it is expected the subdevice is
192 	 * responsible for creating those internal links.
193 	 */
194 	list_for_each_entry(sd, &notifier->done, async_list) {
195 		switch (sd->grp_id) {
196 		case IMX_MEDIA_GRP_ID_VDIC:
197 		case IMX_MEDIA_GRP_ID_IC_PRP:
198 		case IMX_MEDIA_GRP_ID_IC_PRPENC:
199 		case IMX_MEDIA_GRP_ID_IC_PRPVF:
200 		case IMX_MEDIA_GRP_ID_CSI0:
201 		case IMX_MEDIA_GRP_ID_CSI1:
202 			ret = imx_media_create_internal_links(imxmd, sd);
203 			if (ret)
204 				return ret;
205 			/*
206 			 * the CSIs straddle between the external and the IPU
207 			 * internal entities, so create the external links
208 			 * to the CSI sink pads.
209 			 */
210 			if (sd->grp_id & IMX_MEDIA_GRP_ID_CSI)
211 				imx_media_create_csi_of_links(imxmd, sd);
212 			break;
213 		default:
214 			/* this is an external fwnode subdev */
215 			imx_media_create_of_links(imxmd, sd);
216 			break;
217 		}
218 	}
219 
220 	return 0;
221 }
222 
223 /*
224  * adds given video device to given imx-media source pad vdev list.
225  * Continues upstream from the pad entity's sink pads.
226  */
imx_media_add_vdev_to_pad(struct imx_media_dev * imxmd,struct imx_media_video_dev * vdev,struct media_pad * srcpad)227 static int imx_media_add_vdev_to_pad(struct imx_media_dev *imxmd,
228 				     struct imx_media_video_dev *vdev,
229 				     struct media_pad *srcpad)
230 {
231 	struct media_entity *entity = srcpad->entity;
232 	struct imx_media_pad_vdev *pad_vdev;
233 	struct list_head *pad_vdev_list;
234 	struct media_link *link;
235 	struct v4l2_subdev *sd;
236 	int i, ret;
237 
238 	/* skip this entity if not a v4l2_subdev */
239 	if (!is_media_entity_v4l2_subdev(entity))
240 		return 0;
241 
242 	sd = media_entity_to_v4l2_subdev(entity);
243 
244 	pad_vdev_list = to_pad_vdev_list(sd, srcpad->index);
245 	if (!pad_vdev_list) {
246 		v4l2_warn(&imxmd->v4l2_dev, "%s:%u has no vdev list!\n",
247 			  entity->name, srcpad->index);
248 		/*
249 		 * shouldn't happen, but no reason to fail driver load,
250 		 * just skip this entity.
251 		 */
252 		return 0;
253 	}
254 
255 	/* just return if we've been here before */
256 	list_for_each_entry(pad_vdev, pad_vdev_list, list) {
257 		if (pad_vdev->vdev == vdev)
258 			return 0;
259 	}
260 
261 	dev_dbg(imxmd->md.dev, "adding %s to pad %s:%u\n",
262 		vdev->vfd->entity.name, entity->name, srcpad->index);
263 
264 	pad_vdev = devm_kzalloc(imxmd->md.dev, sizeof(*pad_vdev), GFP_KERNEL);
265 	if (!pad_vdev)
266 		return -ENOMEM;
267 
268 	/* attach this vdev to this pad */
269 	pad_vdev->vdev = vdev;
270 	list_add_tail(&pad_vdev->list, pad_vdev_list);
271 
272 	/* move upstream from this entity's sink pads */
273 	for (i = 0; i < entity->num_pads; i++) {
274 		struct media_pad *pad = &entity->pads[i];
275 
276 		if (!(pad->flags & MEDIA_PAD_FL_SINK))
277 			continue;
278 
279 		list_for_each_entry(link, &entity->links, list) {
280 			if (link->sink != pad)
281 				continue;
282 			ret = imx_media_add_vdev_to_pad(imxmd, vdev,
283 							link->source);
284 			if (ret)
285 				return ret;
286 		}
287 	}
288 
289 	return 0;
290 }
291 
292 /*
293  * For every subdevice, allocate an array of list_head's, one list_head
294  * for each pad, to hold the list of video devices reachable from that
295  * pad.
296  */
imx_media_alloc_pad_vdev_lists(struct imx_media_dev * imxmd)297 static int imx_media_alloc_pad_vdev_lists(struct imx_media_dev *imxmd)
298 {
299 	struct list_head *vdev_lists;
300 	struct media_entity *entity;
301 	struct v4l2_subdev *sd;
302 	int i;
303 
304 	list_for_each_entry(sd, &imxmd->v4l2_dev.subdevs, list) {
305 		entity = &sd->entity;
306 		vdev_lists = devm_kcalloc(
307 			imxmd->md.dev,
308 			entity->num_pads, sizeof(*vdev_lists),
309 			GFP_KERNEL);
310 		if (!vdev_lists)
311 			return -ENOMEM;
312 
313 		/* attach to the subdev's host private pointer */
314 		sd->host_priv = vdev_lists;
315 
316 		for (i = 0; i < entity->num_pads; i++)
317 			INIT_LIST_HEAD(to_pad_vdev_list(sd, i));
318 	}
319 
320 	return 0;
321 }
322 
323 /* form the vdev lists in all imx-media source pads */
imx_media_create_pad_vdev_lists(struct imx_media_dev * imxmd)324 static int imx_media_create_pad_vdev_lists(struct imx_media_dev *imxmd)
325 {
326 	struct imx_media_video_dev *vdev;
327 	struct media_link *link;
328 	int ret;
329 
330 	ret = imx_media_alloc_pad_vdev_lists(imxmd);
331 	if (ret)
332 		return ret;
333 
334 	list_for_each_entry(vdev, &imxmd->vdev_list, list) {
335 		link = list_first_entry(&vdev->vfd->entity.links,
336 					struct media_link, list);
337 		ret = imx_media_add_vdev_to_pad(imxmd, vdev, link->source);
338 		if (ret)
339 			return ret;
340 	}
341 
342 	return 0;
343 }
344 
345 /* async subdev complete notifier */
imx_media_probe_complete(struct v4l2_async_notifier * notifier)346 static int imx_media_probe_complete(struct v4l2_async_notifier *notifier)
347 {
348 	struct imx_media_dev *imxmd = notifier2dev(notifier);
349 	int ret;
350 
351 	mutex_lock(&imxmd->mutex);
352 
353 	ret = imx_media_create_links(notifier);
354 	if (ret)
355 		goto unlock;
356 
357 	ret = imx_media_create_pad_vdev_lists(imxmd);
358 	if (ret)
359 		goto unlock;
360 
361 	ret = v4l2_device_register_subdev_nodes(&imxmd->v4l2_dev);
362 unlock:
363 	mutex_unlock(&imxmd->mutex);
364 	if (ret)
365 		return ret;
366 
367 	return media_device_register(&imxmd->md);
368 }
369 
370 static const struct v4l2_async_notifier_operations imx_media_subdev_ops = {
371 	.bound = imx_media_subdev_bound,
372 	.complete = imx_media_probe_complete,
373 };
374 
375 /*
376  * adds controls to a video device from an entity subdevice.
377  * Continues upstream from the entity's sink pads.
378  */
imx_media_inherit_controls(struct imx_media_dev * imxmd,struct video_device * vfd,struct media_entity * entity)379 static int imx_media_inherit_controls(struct imx_media_dev *imxmd,
380 				      struct video_device *vfd,
381 				      struct media_entity *entity)
382 {
383 	int i, ret = 0;
384 
385 	if (is_media_entity_v4l2_subdev(entity)) {
386 		struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
387 
388 		dev_dbg(imxmd->md.dev,
389 			"adding controls to %s from %s\n",
390 			vfd->entity.name, sd->entity.name);
391 
392 		ret = v4l2_ctrl_add_handler(vfd->ctrl_handler,
393 					    sd->ctrl_handler,
394 					    NULL);
395 		if (ret)
396 			return ret;
397 	}
398 
399 	/* move upstream */
400 	for (i = 0; i < entity->num_pads; i++) {
401 		struct media_pad *pad, *spad = &entity->pads[i];
402 
403 		if (!(spad->flags & MEDIA_PAD_FL_SINK))
404 			continue;
405 
406 		pad = media_entity_remote_pad(spad);
407 		if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
408 			continue;
409 
410 		ret = imx_media_inherit_controls(imxmd, vfd, pad->entity);
411 		if (ret)
412 			break;
413 	}
414 
415 	return ret;
416 }
417 
imx_media_link_notify(struct media_link * link,u32 flags,unsigned int notification)418 static int imx_media_link_notify(struct media_link *link, u32 flags,
419 				 unsigned int notification)
420 {
421 	struct media_entity *source = link->source->entity;
422 	struct imx_media_pad_vdev *pad_vdev;
423 	struct list_head *pad_vdev_list;
424 	struct imx_media_dev *imxmd;
425 	struct video_device *vfd;
426 	struct v4l2_subdev *sd;
427 	int pad_idx, ret;
428 
429 	ret = v4l2_pipeline_link_notify(link, flags, notification);
430 	if (ret)
431 		return ret;
432 
433 	/* don't bother if source is not a subdev */
434 	if (!is_media_entity_v4l2_subdev(source))
435 		return 0;
436 
437 	sd = media_entity_to_v4l2_subdev(source);
438 	pad_idx = link->source->index;
439 
440 	imxmd = dev_get_drvdata(sd->v4l2_dev->dev);
441 
442 	pad_vdev_list = to_pad_vdev_list(sd, pad_idx);
443 	if (!pad_vdev_list) {
444 		/* shouldn't happen, but no reason to fail link setup */
445 		return 0;
446 	}
447 
448 	/*
449 	 * Before disabling a link, reset controls for all video
450 	 * devices reachable from this link.
451 	 *
452 	 * After enabling a link, refresh controls for all video
453 	 * devices reachable from this link.
454 	 */
455 	if (notification == MEDIA_DEV_NOTIFY_PRE_LINK_CH &&
456 	    !(flags & MEDIA_LNK_FL_ENABLED)) {
457 		list_for_each_entry(pad_vdev, pad_vdev_list, list) {
458 			vfd = pad_vdev->vdev->vfd;
459 			dev_dbg(imxmd->md.dev,
460 				"reset controls for %s\n",
461 				vfd->entity.name);
462 			v4l2_ctrl_handler_free(vfd->ctrl_handler);
463 			v4l2_ctrl_handler_init(vfd->ctrl_handler, 0);
464 		}
465 	} else if (notification == MEDIA_DEV_NOTIFY_POST_LINK_CH &&
466 		   (link->flags & MEDIA_LNK_FL_ENABLED)) {
467 		list_for_each_entry(pad_vdev, pad_vdev_list, list) {
468 			vfd = pad_vdev->vdev->vfd;
469 			dev_dbg(imxmd->md.dev,
470 				"refresh controls for %s\n",
471 				vfd->entity.name);
472 			ret = imx_media_inherit_controls(imxmd, vfd,
473 							 &vfd->entity);
474 			if (ret)
475 				break;
476 		}
477 	}
478 
479 	return ret;
480 }
481 
482 static const struct media_device_ops imx_media_md_ops = {
483 	.link_notify = imx_media_link_notify,
484 };
485 
imx_media_probe(struct platform_device * pdev)486 static int imx_media_probe(struct platform_device *pdev)
487 {
488 	struct device *dev = &pdev->dev;
489 	struct device_node *node = dev->of_node;
490 	struct imx_media_async_subdev *imxasd;
491 	struct v4l2_async_subdev **subdevs;
492 	struct imx_media_dev *imxmd;
493 	int num_subdevs, i, ret;
494 
495 	imxmd = devm_kzalloc(dev, sizeof(*imxmd), GFP_KERNEL);
496 	if (!imxmd)
497 		return -ENOMEM;
498 
499 	dev_set_drvdata(dev, imxmd);
500 
501 	strlcpy(imxmd->md.model, "imx-media", sizeof(imxmd->md.model));
502 	imxmd->md.ops = &imx_media_md_ops;
503 	imxmd->md.dev = dev;
504 
505 	mutex_init(&imxmd->mutex);
506 
507 	imxmd->v4l2_dev.mdev = &imxmd->md;
508 	strlcpy(imxmd->v4l2_dev.name, "imx-media",
509 		sizeof(imxmd->v4l2_dev.name));
510 
511 	media_device_init(&imxmd->md);
512 
513 	ret = v4l2_device_register(dev, &imxmd->v4l2_dev);
514 	if (ret < 0) {
515 		v4l2_err(&imxmd->v4l2_dev,
516 			 "Failed to register v4l2_device: %d\n", ret);
517 		goto cleanup;
518 	}
519 
520 	dev_set_drvdata(imxmd->v4l2_dev.dev, imxmd);
521 
522 	INIT_LIST_HEAD(&imxmd->asd_list);
523 	INIT_LIST_HEAD(&imxmd->vdev_list);
524 
525 	ret = imx_media_add_of_subdevs(imxmd, node);
526 	if (ret) {
527 		v4l2_err(&imxmd->v4l2_dev,
528 			 "add_of_subdevs failed with %d\n", ret);
529 		goto unreg_dev;
530 	}
531 
532 	ret = imx_media_add_internal_subdevs(imxmd);
533 	if (ret) {
534 		v4l2_err(&imxmd->v4l2_dev,
535 			 "add_internal_subdevs failed with %d\n", ret);
536 		goto unreg_dev;
537 	}
538 
539 	num_subdevs = imxmd->subdev_notifier.num_subdevs;
540 
541 	/* no subdevs? just bail */
542 	if (num_subdevs == 0) {
543 		ret = -ENODEV;
544 		goto unreg_dev;
545 	}
546 
547 	subdevs = devm_kcalloc(imxmd->md.dev, num_subdevs, sizeof(*subdevs),
548 			       GFP_KERNEL);
549 	if (!subdevs) {
550 		ret = -ENOMEM;
551 		goto unreg_dev;
552 	}
553 
554 	i = 0;
555 	list_for_each_entry(imxasd, &imxmd->asd_list, list)
556 		subdevs[i++] = &imxasd->asd;
557 
558 	/* prepare the async subdev notifier and register it */
559 	imxmd->subdev_notifier.subdevs = subdevs;
560 	imxmd->subdev_notifier.ops = &imx_media_subdev_ops;
561 	ret = v4l2_async_notifier_register(&imxmd->v4l2_dev,
562 					   &imxmd->subdev_notifier);
563 	if (ret) {
564 		v4l2_err(&imxmd->v4l2_dev,
565 			 "v4l2_async_notifier_register failed with %d\n", ret);
566 		goto del_int;
567 	}
568 
569 	return 0;
570 
571 del_int:
572 	imx_media_remove_internal_subdevs(imxmd);
573 unreg_dev:
574 	v4l2_device_unregister(&imxmd->v4l2_dev);
575 cleanup:
576 	media_device_cleanup(&imxmd->md);
577 	return ret;
578 }
579 
imx_media_remove(struct platform_device * pdev)580 static int imx_media_remove(struct platform_device *pdev)
581 {
582 	struct imx_media_dev *imxmd =
583 		(struct imx_media_dev *)platform_get_drvdata(pdev);
584 
585 	v4l2_info(&imxmd->v4l2_dev, "Removing imx-media\n");
586 
587 	v4l2_async_notifier_unregister(&imxmd->subdev_notifier);
588 	imx_media_remove_internal_subdevs(imxmd);
589 	v4l2_device_unregister(&imxmd->v4l2_dev);
590 	media_device_unregister(&imxmd->md);
591 	media_device_cleanup(&imxmd->md);
592 
593 	return 0;
594 }
595 
596 static const struct of_device_id imx_media_dt_ids[] = {
597 	{ .compatible = "fsl,imx-capture-subsystem" },
598 	{ /* sentinel */ }
599 };
600 MODULE_DEVICE_TABLE(of, imx_media_dt_ids);
601 
602 static struct platform_driver imx_media_pdrv = {
603 	.probe		= imx_media_probe,
604 	.remove		= imx_media_remove,
605 	.driver		= {
606 		.name	= "imx-media",
607 		.of_match_table	= imx_media_dt_ids,
608 	},
609 };
610 
611 module_platform_driver(imx_media_pdrv);
612 
613 MODULE_DESCRIPTION("i.MX5/6 v4l2 media controller driver");
614 MODULE_AUTHOR("Steve Longerbeam <steve_longerbeam@mentor.com>");
615 MODULE_LICENSE("GPL");
616