1 /*
2  * V4L2 asynchronous subdevice registration API
3  *
4  * Copyright (C) 2012-2013, Guennadi Liakhovetski <g.liakhovetski@gmx.de>
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 version 2 as
8  * published by the Free Software Foundation.
9  */
10 
11 #include <linux/device.h>
12 #include <linux/err.h>
13 #include <linux/i2c.h>
14 #include <linux/list.h>
15 #include <linux/mm.h>
16 #include <linux/module.h>
17 #include <linux/mutex.h>
18 #include <linux/of.h>
19 #include <linux/platform_device.h>
20 #include <linux/slab.h>
21 #include <linux/types.h>
22 
23 #include <media/v4l2-async.h>
24 #include <media/v4l2-device.h>
25 #include <media/v4l2-fwnode.h>
26 #include <media/v4l2-subdev.h>
27 
v4l2_async_notifier_call_bound(struct v4l2_async_notifier * n,struct v4l2_subdev * subdev,struct v4l2_async_subdev * asd)28 static int v4l2_async_notifier_call_bound(struct v4l2_async_notifier *n,
29 					  struct v4l2_subdev *subdev,
30 					  struct v4l2_async_subdev *asd)
31 {
32 	if (!n->ops || !n->ops->bound)
33 		return 0;
34 
35 	return n->ops->bound(n, subdev, asd);
36 }
37 
v4l2_async_notifier_call_unbind(struct v4l2_async_notifier * n,struct v4l2_subdev * subdev,struct v4l2_async_subdev * asd)38 static void v4l2_async_notifier_call_unbind(struct v4l2_async_notifier *n,
39 					    struct v4l2_subdev *subdev,
40 					    struct v4l2_async_subdev *asd)
41 {
42 	if (!n->ops || !n->ops->unbind)
43 		return;
44 
45 	n->ops->unbind(n, subdev, asd);
46 }
47 
v4l2_async_notifier_call_complete(struct v4l2_async_notifier * n)48 static int v4l2_async_notifier_call_complete(struct v4l2_async_notifier *n)
49 {
50 	if (!n->ops || !n->ops->complete)
51 		return 0;
52 
53 	return n->ops->complete(n);
54 }
55 
match_i2c(struct v4l2_subdev * sd,struct v4l2_async_subdev * asd)56 static bool match_i2c(struct v4l2_subdev *sd, struct v4l2_async_subdev *asd)
57 {
58 #if IS_ENABLED(CONFIG_I2C)
59 	struct i2c_client *client = i2c_verify_client(sd->dev);
60 	return client &&
61 		asd->match.i2c.adapter_id == client->adapter->nr &&
62 		asd->match.i2c.address == client->addr;
63 #else
64 	return false;
65 #endif
66 }
67 
match_devname(struct v4l2_subdev * sd,struct v4l2_async_subdev * asd)68 static bool match_devname(struct v4l2_subdev *sd,
69 			  struct v4l2_async_subdev *asd)
70 {
71 	return !strcmp(asd->match.device_name, dev_name(sd->dev));
72 }
73 
match_fwnode(struct v4l2_subdev * sd,struct v4l2_async_subdev * asd)74 static bool match_fwnode(struct v4l2_subdev *sd, struct v4l2_async_subdev *asd)
75 {
76 	return sd->fwnode == asd->match.fwnode;
77 }
78 
match_custom(struct v4l2_subdev * sd,struct v4l2_async_subdev * asd)79 static bool match_custom(struct v4l2_subdev *sd, struct v4l2_async_subdev *asd)
80 {
81 	if (!asd->match.custom.match)
82 		/* Match always */
83 		return true;
84 
85 	return asd->match.custom.match(sd->dev, asd);
86 }
87 
88 static LIST_HEAD(subdev_list);
89 static LIST_HEAD(notifier_list);
90 static DEFINE_MUTEX(list_lock);
91 
v4l2_async_find_match(struct v4l2_async_notifier * notifier,struct v4l2_subdev * sd)92 static struct v4l2_async_subdev *v4l2_async_find_match(
93 	struct v4l2_async_notifier *notifier, struct v4l2_subdev *sd)
94 {
95 	bool (*match)(struct v4l2_subdev *, struct v4l2_async_subdev *);
96 	struct v4l2_async_subdev *asd;
97 
98 	list_for_each_entry(asd, &notifier->waiting, list) {
99 		/* bus_type has been verified valid before */
100 		switch (asd->match_type) {
101 		case V4L2_ASYNC_MATCH_CUSTOM:
102 			match = match_custom;
103 			break;
104 		case V4L2_ASYNC_MATCH_DEVNAME:
105 			match = match_devname;
106 			break;
107 		case V4L2_ASYNC_MATCH_I2C:
108 			match = match_i2c;
109 			break;
110 		case V4L2_ASYNC_MATCH_FWNODE:
111 			match = match_fwnode;
112 			break;
113 		default:
114 			/* Cannot happen, unless someone breaks us */
115 			WARN_ON(true);
116 			return NULL;
117 		}
118 
119 		/* match cannot be NULL here */
120 		if (match(sd, asd))
121 			return asd;
122 	}
123 
124 	return NULL;
125 }
126 
127 /* Find the sub-device notifier registered by a sub-device driver. */
v4l2_async_find_subdev_notifier(struct v4l2_subdev * sd)128 static struct v4l2_async_notifier *v4l2_async_find_subdev_notifier(
129 	struct v4l2_subdev *sd)
130 {
131 	struct v4l2_async_notifier *n;
132 
133 	list_for_each_entry(n, &notifier_list, list)
134 		if (n->sd == sd)
135 			return n;
136 
137 	return NULL;
138 }
139 
140 /* Get v4l2_device related to the notifier if one can be found. */
v4l2_async_notifier_find_v4l2_dev(struct v4l2_async_notifier * notifier)141 static struct v4l2_device *v4l2_async_notifier_find_v4l2_dev(
142 	struct v4l2_async_notifier *notifier)
143 {
144 	while (notifier->parent)
145 		notifier = notifier->parent;
146 
147 	return notifier->v4l2_dev;
148 }
149 
150 /*
151  * Return true if all child sub-device notifiers are complete, false otherwise.
152  */
v4l2_async_notifier_can_complete(struct v4l2_async_notifier * notifier)153 static bool v4l2_async_notifier_can_complete(
154 	struct v4l2_async_notifier *notifier)
155 {
156 	struct v4l2_subdev *sd;
157 
158 	if (!list_empty(&notifier->waiting))
159 		return false;
160 
161 	list_for_each_entry(sd, &notifier->done, async_list) {
162 		struct v4l2_async_notifier *subdev_notifier =
163 			v4l2_async_find_subdev_notifier(sd);
164 
165 		if (subdev_notifier &&
166 		    !v4l2_async_notifier_can_complete(subdev_notifier))
167 			return false;
168 	}
169 
170 	return true;
171 }
172 
173 /*
174  * Complete the master notifier if possible. This is done when all async
175  * sub-devices have been bound; v4l2_device is also available then.
176  */
v4l2_async_notifier_try_complete(struct v4l2_async_notifier * notifier)177 static int v4l2_async_notifier_try_complete(
178 	struct v4l2_async_notifier *notifier)
179 {
180 	/* Quick check whether there are still more sub-devices here. */
181 	if (!list_empty(&notifier->waiting))
182 		return 0;
183 
184 	/* Check the entire notifier tree; find the root notifier first. */
185 	while (notifier->parent)
186 		notifier = notifier->parent;
187 
188 	/* This is root if it has v4l2_dev. */
189 	if (!notifier->v4l2_dev)
190 		return 0;
191 
192 	/* Is everything ready? */
193 	if (!v4l2_async_notifier_can_complete(notifier))
194 		return 0;
195 
196 	return v4l2_async_notifier_call_complete(notifier);
197 }
198 
199 static int v4l2_async_notifier_try_all_subdevs(
200 	struct v4l2_async_notifier *notifier);
201 
v4l2_async_match_notify(struct v4l2_async_notifier * notifier,struct v4l2_device * v4l2_dev,struct v4l2_subdev * sd,struct v4l2_async_subdev * asd)202 static int v4l2_async_match_notify(struct v4l2_async_notifier *notifier,
203 				   struct v4l2_device *v4l2_dev,
204 				   struct v4l2_subdev *sd,
205 				   struct v4l2_async_subdev *asd)
206 {
207 	struct v4l2_async_notifier *subdev_notifier;
208 	int ret;
209 
210 	ret = v4l2_device_register_subdev(v4l2_dev, sd);
211 	if (ret < 0)
212 		return ret;
213 
214 	ret = v4l2_async_notifier_call_bound(notifier, sd, asd);
215 	if (ret < 0) {
216 		v4l2_device_unregister_subdev(sd);
217 		return ret;
218 	}
219 
220 	/* Remove from the waiting list */
221 	list_del(&asd->list);
222 	sd->asd = asd;
223 	sd->notifier = notifier;
224 
225 	/* Move from the global subdevice list to notifier's done */
226 	list_move(&sd->async_list, &notifier->done);
227 
228 	/*
229 	 * See if the sub-device has a notifier. If not, return here.
230 	 */
231 	subdev_notifier = v4l2_async_find_subdev_notifier(sd);
232 	if (!subdev_notifier || subdev_notifier->parent)
233 		return 0;
234 
235 	/*
236 	 * Proceed with checking for the sub-device notifier's async
237 	 * sub-devices, and return the result. The error will be handled by the
238 	 * caller.
239 	 */
240 	subdev_notifier->parent = notifier;
241 
242 	return v4l2_async_notifier_try_all_subdevs(subdev_notifier);
243 }
244 
245 /* Test all async sub-devices in a notifier for a match. */
v4l2_async_notifier_try_all_subdevs(struct v4l2_async_notifier * notifier)246 static int v4l2_async_notifier_try_all_subdevs(
247 	struct v4l2_async_notifier *notifier)
248 {
249 	struct v4l2_device *v4l2_dev =
250 		v4l2_async_notifier_find_v4l2_dev(notifier);
251 	struct v4l2_subdev *sd;
252 
253 	if (!v4l2_dev)
254 		return 0;
255 
256 again:
257 	list_for_each_entry(sd, &subdev_list, async_list) {
258 		struct v4l2_async_subdev *asd;
259 		int ret;
260 
261 		asd = v4l2_async_find_match(notifier, sd);
262 		if (!asd)
263 			continue;
264 
265 		ret = v4l2_async_match_notify(notifier, v4l2_dev, sd, asd);
266 		if (ret < 0)
267 			return ret;
268 
269 		/*
270 		 * v4l2_async_match_notify() may lead to registering a
271 		 * new notifier and thus changing the async subdevs
272 		 * list. In order to proceed safely from here, restart
273 		 * parsing the list from the beginning.
274 		 */
275 		goto again;
276 	}
277 
278 	return 0;
279 }
280 
v4l2_async_cleanup(struct v4l2_subdev * sd)281 static void v4l2_async_cleanup(struct v4l2_subdev *sd)
282 {
283 	v4l2_device_unregister_subdev(sd);
284 	/* Subdevice driver will reprobe and put the subdev back onto the list */
285 	list_del_init(&sd->async_list);
286 	sd->asd = NULL;
287 }
288 
289 /* Unbind all sub-devices in the notifier tree. */
v4l2_async_notifier_unbind_all_subdevs(struct v4l2_async_notifier * notifier)290 static void v4l2_async_notifier_unbind_all_subdevs(
291 	struct v4l2_async_notifier *notifier)
292 {
293 	struct v4l2_subdev *sd, *tmp;
294 
295 	list_for_each_entry_safe(sd, tmp, &notifier->done, async_list) {
296 		struct v4l2_async_notifier *subdev_notifier =
297 			v4l2_async_find_subdev_notifier(sd);
298 
299 		if (subdev_notifier)
300 			v4l2_async_notifier_unbind_all_subdevs(subdev_notifier);
301 
302 		v4l2_async_notifier_call_unbind(notifier, sd, sd->asd);
303 		v4l2_async_cleanup(sd);
304 
305 		list_move(&sd->async_list, &subdev_list);
306 	}
307 
308 	notifier->parent = NULL;
309 }
310 
311 /* See if an fwnode can be found in a notifier's lists. */
__v4l2_async_notifier_fwnode_has_async_subdev(struct v4l2_async_notifier * notifier,struct fwnode_handle * fwnode)312 static bool __v4l2_async_notifier_fwnode_has_async_subdev(
313 	struct v4l2_async_notifier *notifier, struct fwnode_handle *fwnode)
314 {
315 	struct v4l2_async_subdev *asd;
316 	struct v4l2_subdev *sd;
317 
318 	list_for_each_entry(asd, &notifier->waiting, list) {
319 		if (asd->match_type != V4L2_ASYNC_MATCH_FWNODE)
320 			continue;
321 
322 		if (asd->match.fwnode == fwnode)
323 			return true;
324 	}
325 
326 	list_for_each_entry(sd, &notifier->done, async_list) {
327 		if (WARN_ON(!sd->asd))
328 			continue;
329 
330 		if (sd->asd->match_type != V4L2_ASYNC_MATCH_FWNODE)
331 			continue;
332 
333 		if (sd->asd->match.fwnode == fwnode)
334 			return true;
335 	}
336 
337 	return false;
338 }
339 
340 /*
341  * Find out whether an async sub-device was set up for an fwnode already or
342  * whether it exists in a given notifier before @this_index.
343  */
v4l2_async_notifier_fwnode_has_async_subdev(struct v4l2_async_notifier * notifier,struct fwnode_handle * fwnode,unsigned int this_index)344 static bool v4l2_async_notifier_fwnode_has_async_subdev(
345 	struct v4l2_async_notifier *notifier, struct fwnode_handle *fwnode,
346 	unsigned int this_index)
347 {
348 	unsigned int j;
349 
350 	lockdep_assert_held(&list_lock);
351 
352 	/* Check that an fwnode is not being added more than once. */
353 	for (j = 0; j < this_index; j++) {
354 		struct v4l2_async_subdev *asd = notifier->subdevs[this_index];
355 		struct v4l2_async_subdev *other_asd = notifier->subdevs[j];
356 
357 		if (other_asd->match_type == V4L2_ASYNC_MATCH_FWNODE &&
358 		    asd->match.fwnode ==
359 		    other_asd->match.fwnode)
360 			return true;
361 	}
362 
363 	/* Check than an fwnode did not exist in other notifiers. */
364 	list_for_each_entry(notifier, &notifier_list, list)
365 		if (__v4l2_async_notifier_fwnode_has_async_subdev(
366 			    notifier, fwnode))
367 			return true;
368 
369 	return false;
370 }
371 
__v4l2_async_notifier_register(struct v4l2_async_notifier * notifier)372 static int __v4l2_async_notifier_register(struct v4l2_async_notifier *notifier)
373 {
374 	struct device *dev =
375 		notifier->v4l2_dev ? notifier->v4l2_dev->dev : NULL;
376 	struct v4l2_async_subdev *asd;
377 	int ret;
378 	int i;
379 
380 	if (notifier->num_subdevs > V4L2_MAX_SUBDEVS)
381 		return -EINVAL;
382 
383 	INIT_LIST_HEAD(&notifier->waiting);
384 	INIT_LIST_HEAD(&notifier->done);
385 
386 	mutex_lock(&list_lock);
387 
388 	for (i = 0; i < notifier->num_subdevs; i++) {
389 		asd = notifier->subdevs[i];
390 
391 		switch (asd->match_type) {
392 		case V4L2_ASYNC_MATCH_CUSTOM:
393 		case V4L2_ASYNC_MATCH_DEVNAME:
394 		case V4L2_ASYNC_MATCH_I2C:
395 			break;
396 		case V4L2_ASYNC_MATCH_FWNODE:
397 			if (v4l2_async_notifier_fwnode_has_async_subdev(
398 				    notifier, asd->match.fwnode, i)) {
399 				dev_err(dev,
400 					"fwnode has already been registered or in notifier's subdev list\n");
401 				ret = -EEXIST;
402 				goto err_unlock;
403 			}
404 			break;
405 		default:
406 			dev_err(dev, "Invalid match type %u on %p\n",
407 				asd->match_type, asd);
408 			ret = -EINVAL;
409 			goto err_unlock;
410 		}
411 		list_add_tail(&asd->list, &notifier->waiting);
412 	}
413 
414 	ret = v4l2_async_notifier_try_all_subdevs(notifier);
415 	if (ret < 0)
416 		goto err_unbind;
417 
418 	ret = v4l2_async_notifier_try_complete(notifier);
419 	if (ret < 0)
420 		goto err_unbind;
421 
422 	/* Keep also completed notifiers on the list */
423 	list_add(&notifier->list, &notifier_list);
424 
425 	mutex_unlock(&list_lock);
426 
427 	return 0;
428 
429 err_unbind:
430 	/*
431 	 * On failure, unbind all sub-devices registered through this notifier.
432 	 */
433 	v4l2_async_notifier_unbind_all_subdevs(notifier);
434 
435 err_unlock:
436 	mutex_unlock(&list_lock);
437 
438 	return ret;
439 }
440 
v4l2_async_notifier_register(struct v4l2_device * v4l2_dev,struct v4l2_async_notifier * notifier)441 int v4l2_async_notifier_register(struct v4l2_device *v4l2_dev,
442 				 struct v4l2_async_notifier *notifier)
443 {
444 	int ret;
445 
446 	if (WARN_ON(!v4l2_dev || notifier->sd))
447 		return -EINVAL;
448 
449 	notifier->v4l2_dev = v4l2_dev;
450 
451 	ret = __v4l2_async_notifier_register(notifier);
452 	if (ret)
453 		notifier->v4l2_dev = NULL;
454 
455 	return ret;
456 }
457 EXPORT_SYMBOL(v4l2_async_notifier_register);
458 
v4l2_async_subdev_notifier_register(struct v4l2_subdev * sd,struct v4l2_async_notifier * notifier)459 int v4l2_async_subdev_notifier_register(struct v4l2_subdev *sd,
460 					struct v4l2_async_notifier *notifier)
461 {
462 	int ret;
463 
464 	if (WARN_ON(!sd || notifier->v4l2_dev))
465 		return -EINVAL;
466 
467 	notifier->sd = sd;
468 
469 	ret = __v4l2_async_notifier_register(notifier);
470 	if (ret)
471 		notifier->sd = NULL;
472 
473 	return ret;
474 }
475 EXPORT_SYMBOL(v4l2_async_subdev_notifier_register);
476 
__v4l2_async_notifier_unregister(struct v4l2_async_notifier * notifier)477 static void __v4l2_async_notifier_unregister(
478 	struct v4l2_async_notifier *notifier)
479 {
480 	if (!notifier || (!notifier->v4l2_dev && !notifier->sd))
481 		return;
482 
483 	v4l2_async_notifier_unbind_all_subdevs(notifier);
484 
485 	notifier->sd = NULL;
486 	notifier->v4l2_dev = NULL;
487 
488 	list_del(&notifier->list);
489 }
490 
v4l2_async_notifier_unregister(struct v4l2_async_notifier * notifier)491 void v4l2_async_notifier_unregister(struct v4l2_async_notifier *notifier)
492 {
493 	mutex_lock(&list_lock);
494 
495 	__v4l2_async_notifier_unregister(notifier);
496 
497 	mutex_unlock(&list_lock);
498 }
499 EXPORT_SYMBOL(v4l2_async_notifier_unregister);
500 
v4l2_async_notifier_cleanup(struct v4l2_async_notifier * notifier)501 void v4l2_async_notifier_cleanup(struct v4l2_async_notifier *notifier)
502 {
503 	unsigned int i;
504 
505 	if (!notifier || !notifier->max_subdevs)
506 		return;
507 
508 	for (i = 0; i < notifier->num_subdevs; i++) {
509 		struct v4l2_async_subdev *asd = notifier->subdevs[i];
510 
511 		switch (asd->match_type) {
512 		case V4L2_ASYNC_MATCH_FWNODE:
513 			fwnode_handle_put(asd->match.fwnode);
514 			break;
515 		default:
516 			WARN_ON_ONCE(true);
517 			break;
518 		}
519 
520 		kfree(asd);
521 	}
522 
523 	notifier->max_subdevs = 0;
524 	notifier->num_subdevs = 0;
525 
526 	kvfree(notifier->subdevs);
527 	notifier->subdevs = NULL;
528 }
529 EXPORT_SYMBOL_GPL(v4l2_async_notifier_cleanup);
530 
v4l2_async_register_subdev(struct v4l2_subdev * sd)531 int v4l2_async_register_subdev(struct v4l2_subdev *sd)
532 {
533 	struct v4l2_async_notifier *subdev_notifier;
534 	struct v4l2_async_notifier *notifier;
535 	int ret;
536 
537 	/*
538 	 * No reference taken. The reference is held by the device
539 	 * (struct v4l2_subdev.dev), and async sub-device does not
540 	 * exist independently of the device at any point of time.
541 	 */
542 	if (!sd->fwnode && sd->dev)
543 		sd->fwnode = dev_fwnode(sd->dev);
544 
545 	mutex_lock(&list_lock);
546 
547 	INIT_LIST_HEAD(&sd->async_list);
548 
549 	list_for_each_entry(notifier, &notifier_list, list) {
550 		struct v4l2_device *v4l2_dev =
551 			v4l2_async_notifier_find_v4l2_dev(notifier);
552 		struct v4l2_async_subdev *asd;
553 
554 		if (!v4l2_dev)
555 			continue;
556 
557 		asd = v4l2_async_find_match(notifier, sd);
558 		if (!asd)
559 			continue;
560 
561 		ret = v4l2_async_match_notify(notifier, v4l2_dev, sd, asd);
562 		if (ret)
563 			goto err_unbind;
564 
565 		ret = v4l2_async_notifier_try_complete(notifier);
566 		if (ret)
567 			goto err_unbind;
568 
569 		goto out_unlock;
570 	}
571 
572 	/* None matched, wait for hot-plugging */
573 	list_add(&sd->async_list, &subdev_list);
574 
575 out_unlock:
576 	mutex_unlock(&list_lock);
577 
578 	return 0;
579 
580 err_unbind:
581 	/*
582 	 * Complete failed. Unbind the sub-devices bound through registering
583 	 * this async sub-device.
584 	 */
585 	subdev_notifier = v4l2_async_find_subdev_notifier(sd);
586 	if (subdev_notifier)
587 		v4l2_async_notifier_unbind_all_subdevs(subdev_notifier);
588 
589 	if (sd->asd)
590 		v4l2_async_notifier_call_unbind(notifier, sd, sd->asd);
591 	v4l2_async_cleanup(sd);
592 
593 	mutex_unlock(&list_lock);
594 
595 	return ret;
596 }
597 EXPORT_SYMBOL(v4l2_async_register_subdev);
598 
v4l2_async_unregister_subdev(struct v4l2_subdev * sd)599 void v4l2_async_unregister_subdev(struct v4l2_subdev *sd)
600 {
601 	mutex_lock(&list_lock);
602 
603 	__v4l2_async_notifier_unregister(sd->subdev_notifier);
604 	v4l2_async_notifier_cleanup(sd->subdev_notifier);
605 	kfree(sd->subdev_notifier);
606 	sd->subdev_notifier = NULL;
607 
608 	if (sd->asd) {
609 		struct v4l2_async_notifier *notifier = sd->notifier;
610 
611 		list_add(&sd->asd->list, &notifier->waiting);
612 
613 		v4l2_async_notifier_call_unbind(notifier, sd, sd->asd);
614 	}
615 
616 	v4l2_async_cleanup(sd);
617 
618 	mutex_unlock(&list_lock);
619 }
620 EXPORT_SYMBOL(v4l2_async_unregister_subdev);
621