1 // SPDX-License-Identifier: GPL-2.0
2 /*
3  * V4L2 Media Controller Driver for Freescale common i.MX5/6/7 SOC
4  *
5  * Copyright (c) 2019 Linaro Ltd
6  * Copyright (c) 2016 Mentor Graphics Inc.
7  */
8 
9 #include <linux/of_graph.h>
10 #include <linux/of_platform.h>
11 #include <media/v4l2-ctrls.h>
12 #include <media/v4l2-event.h>
13 #include <media/v4l2-ioctl.h>
14 #include <media/v4l2-mc.h>
15 #include "imx-media.h"
16 
notifier2dev(struct v4l2_async_notifier * n)17 static inline struct imx_media_dev *notifier2dev(struct v4l2_async_notifier *n)
18 {
19 	return container_of(n, struct imx_media_dev, notifier);
20 }
21 
22 /* async subdev bound notifier */
imx_media_subdev_bound(struct v4l2_async_notifier * notifier,struct v4l2_subdev * sd,struct v4l2_async_subdev * asd)23 static int imx_media_subdev_bound(struct v4l2_async_notifier *notifier,
24 				  struct v4l2_subdev *sd,
25 				  struct v4l2_async_subdev *asd)
26 {
27 	struct imx_media_dev *imxmd = notifier2dev(notifier);
28 
29 	dev_dbg(imxmd->md.dev, "subdev %s bound\n", sd->name);
30 
31 	return 0;
32 }
33 
34 /*
35  * Create the missing media links from the CSI-2 receiver.
36  * Called after all async subdevs have bound.
37  */
imx_media_create_csi2_links(struct imx_media_dev * imxmd)38 static void imx_media_create_csi2_links(struct imx_media_dev *imxmd)
39 {
40 	struct v4l2_subdev *sd, *csi2 = NULL;
41 
42 	list_for_each_entry(sd, &imxmd->v4l2_dev.subdevs, list) {
43 		if (sd->grp_id == IMX_MEDIA_GRP_ID_CSI2) {
44 			csi2 = sd;
45 			break;
46 		}
47 	}
48 	if (!csi2)
49 		return;
50 
51 	list_for_each_entry(sd, &imxmd->v4l2_dev.subdevs, list) {
52 		/* skip if not a CSI or a CSI mux */
53 		if (!(sd->grp_id & IMX_MEDIA_GRP_ID_IPU_CSI) &&
54 		    !(sd->grp_id & IMX_MEDIA_GRP_ID_CSI) &&
55 		    !(sd->grp_id & IMX_MEDIA_GRP_ID_CSI_MUX))
56 			continue;
57 
58 		v4l2_create_fwnode_links(csi2, sd);
59 	}
60 }
61 
62 /*
63  * adds given video device to given imx-media source pad vdev list.
64  * Continues upstream from the pad entity's sink pads.
65  */
imx_media_add_vdev_to_pad(struct imx_media_dev * imxmd,struct imx_media_video_dev * vdev,struct media_pad * srcpad)66 static int imx_media_add_vdev_to_pad(struct imx_media_dev *imxmd,
67 				     struct imx_media_video_dev *vdev,
68 				     struct media_pad *srcpad)
69 {
70 	struct media_entity *entity = srcpad->entity;
71 	struct imx_media_pad_vdev *pad_vdev;
72 	struct list_head *pad_vdev_list;
73 	struct media_link *link;
74 	struct v4l2_subdev *sd;
75 	int i, ret;
76 
77 	/* skip this entity if not a v4l2_subdev */
78 	if (!is_media_entity_v4l2_subdev(entity))
79 		return 0;
80 
81 	sd = media_entity_to_v4l2_subdev(entity);
82 
83 	pad_vdev_list = to_pad_vdev_list(sd, srcpad->index);
84 	if (!pad_vdev_list) {
85 		v4l2_warn(&imxmd->v4l2_dev, "%s:%u has no vdev list!\n",
86 			  entity->name, srcpad->index);
87 		/*
88 		 * shouldn't happen, but no reason to fail driver load,
89 		 * just skip this entity.
90 		 */
91 		return 0;
92 	}
93 
94 	/* just return if we've been here before */
95 	list_for_each_entry(pad_vdev, pad_vdev_list, list) {
96 		if (pad_vdev->vdev == vdev)
97 			return 0;
98 	}
99 
100 	dev_dbg(imxmd->md.dev, "adding %s to pad %s:%u\n",
101 		vdev->vfd->entity.name, entity->name, srcpad->index);
102 
103 	pad_vdev = devm_kzalloc(imxmd->md.dev, sizeof(*pad_vdev), GFP_KERNEL);
104 	if (!pad_vdev)
105 		return -ENOMEM;
106 
107 	/* attach this vdev to this pad */
108 	pad_vdev->vdev = vdev;
109 	list_add_tail(&pad_vdev->list, pad_vdev_list);
110 
111 	/* move upstream from this entity's sink pads */
112 	for (i = 0; i < entity->num_pads; i++) {
113 		struct media_pad *pad = &entity->pads[i];
114 
115 		if (!(pad->flags & MEDIA_PAD_FL_SINK))
116 			continue;
117 
118 		list_for_each_entry(link, &entity->links, list) {
119 			if (link->sink != pad)
120 				continue;
121 			ret = imx_media_add_vdev_to_pad(imxmd, vdev,
122 							link->source);
123 			if (ret)
124 				return ret;
125 		}
126 	}
127 
128 	return 0;
129 }
130 
131 /*
132  * For every subdevice, allocate an array of list_head's, one list_head
133  * for each pad, to hold the list of video devices reachable from that
134  * pad.
135  */
imx_media_alloc_pad_vdev_lists(struct imx_media_dev * imxmd)136 static int imx_media_alloc_pad_vdev_lists(struct imx_media_dev *imxmd)
137 {
138 	struct list_head *vdev_lists;
139 	struct media_entity *entity;
140 	struct v4l2_subdev *sd;
141 	int i;
142 
143 	list_for_each_entry(sd, &imxmd->v4l2_dev.subdevs, list) {
144 		entity = &sd->entity;
145 		vdev_lists = devm_kcalloc(imxmd->md.dev,
146 					  entity->num_pads, sizeof(*vdev_lists),
147 					  GFP_KERNEL);
148 		if (!vdev_lists)
149 			return -ENOMEM;
150 
151 		/* attach to the subdev's host private pointer */
152 		sd->host_priv = vdev_lists;
153 
154 		for (i = 0; i < entity->num_pads; i++)
155 			INIT_LIST_HEAD(to_pad_vdev_list(sd, i));
156 	}
157 
158 	return 0;
159 }
160 
161 /* form the vdev lists in all imx-media source pads */
imx_media_create_pad_vdev_lists(struct imx_media_dev * imxmd)162 static int imx_media_create_pad_vdev_lists(struct imx_media_dev *imxmd)
163 {
164 	struct imx_media_video_dev *vdev;
165 	struct media_link *link;
166 	int ret;
167 
168 	ret = imx_media_alloc_pad_vdev_lists(imxmd);
169 	if (ret)
170 		return ret;
171 
172 	list_for_each_entry(vdev, &imxmd->vdev_list, list) {
173 		link = list_first_entry(&vdev->vfd->entity.links,
174 					struct media_link, list);
175 		ret = imx_media_add_vdev_to_pad(imxmd, vdev, link->source);
176 		if (ret)
177 			return ret;
178 	}
179 
180 	return 0;
181 }
182 
183 /* async subdev complete notifier */
imx_media_probe_complete(struct v4l2_async_notifier * notifier)184 int imx_media_probe_complete(struct v4l2_async_notifier *notifier)
185 {
186 	struct imx_media_dev *imxmd = notifier2dev(notifier);
187 	int ret;
188 
189 	mutex_lock(&imxmd->mutex);
190 
191 	imx_media_create_csi2_links(imxmd);
192 
193 	ret = imx_media_create_pad_vdev_lists(imxmd);
194 	if (ret)
195 		goto unlock;
196 
197 	ret = v4l2_device_register_subdev_nodes(&imxmd->v4l2_dev);
198 unlock:
199 	mutex_unlock(&imxmd->mutex);
200 	if (ret)
201 		return ret;
202 
203 	return media_device_register(&imxmd->md);
204 }
205 EXPORT_SYMBOL_GPL(imx_media_probe_complete);
206 
207 /*
208  * adds controls to a video device from an entity subdevice.
209  * Continues upstream from the entity's sink pads.
210  */
imx_media_inherit_controls(struct imx_media_dev * imxmd,struct video_device * vfd,struct media_entity * entity)211 static int imx_media_inherit_controls(struct imx_media_dev *imxmd,
212 				      struct video_device *vfd,
213 				      struct media_entity *entity)
214 {
215 	int i, ret = 0;
216 
217 	if (is_media_entity_v4l2_subdev(entity)) {
218 		struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
219 
220 		dev_dbg(imxmd->md.dev,
221 			"adding controls to %s from %s\n",
222 			vfd->entity.name, sd->entity.name);
223 
224 		ret = v4l2_ctrl_add_handler(vfd->ctrl_handler,
225 					    sd->ctrl_handler,
226 					    NULL, true);
227 		if (ret)
228 			return ret;
229 	}
230 
231 	/* move upstream */
232 	for (i = 0; i < entity->num_pads; i++) {
233 		struct media_pad *pad, *spad = &entity->pads[i];
234 
235 		if (!(spad->flags & MEDIA_PAD_FL_SINK))
236 			continue;
237 
238 		pad = media_pad_remote_pad_first(spad);
239 		if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
240 			continue;
241 
242 		ret = imx_media_inherit_controls(imxmd, vfd, pad->entity);
243 		if (ret)
244 			break;
245 	}
246 
247 	return ret;
248 }
249 
imx_media_link_notify(struct media_link * link,u32 flags,unsigned int notification)250 static int imx_media_link_notify(struct media_link *link, u32 flags,
251 				 unsigned int notification)
252 {
253 	struct imx_media_dev *imxmd = container_of(link->graph_obj.mdev,
254 						   struct imx_media_dev, md);
255 	struct media_entity *source = link->source->entity;
256 	struct imx_media_pad_vdev *pad_vdev;
257 	struct list_head *pad_vdev_list;
258 	struct video_device *vfd;
259 	struct v4l2_subdev *sd;
260 	int pad_idx, ret;
261 
262 	ret = v4l2_pipeline_link_notify(link, flags, notification);
263 	if (ret)
264 		return ret;
265 
266 	/* don't bother if source is not a subdev */
267 	if (!is_media_entity_v4l2_subdev(source))
268 		return 0;
269 
270 	sd = media_entity_to_v4l2_subdev(source);
271 	pad_idx = link->source->index;
272 
273 	pad_vdev_list = to_pad_vdev_list(sd, pad_idx);
274 	if (!pad_vdev_list) {
275 		/* nothing to do if source sd has no pad vdev list */
276 		return 0;
277 	}
278 
279 	/*
280 	 * Before disabling a link, reset controls for all video
281 	 * devices reachable from this link.
282 	 *
283 	 * After enabling a link, refresh controls for all video
284 	 * devices reachable from this link.
285 	 */
286 	if (notification == MEDIA_DEV_NOTIFY_PRE_LINK_CH &&
287 	    !(flags & MEDIA_LNK_FL_ENABLED)) {
288 		list_for_each_entry(pad_vdev, pad_vdev_list, list) {
289 			vfd = pad_vdev->vdev->vfd;
290 			if (!vfd->ctrl_handler)
291 				continue;
292 			dev_dbg(imxmd->md.dev,
293 				"reset controls for %s\n",
294 				vfd->entity.name);
295 			v4l2_ctrl_handler_free(vfd->ctrl_handler);
296 			v4l2_ctrl_handler_init(vfd->ctrl_handler, 0);
297 		}
298 	} else if (notification == MEDIA_DEV_NOTIFY_POST_LINK_CH &&
299 		   (link->flags & MEDIA_LNK_FL_ENABLED)) {
300 		list_for_each_entry(pad_vdev, pad_vdev_list, list) {
301 			vfd = pad_vdev->vdev->vfd;
302 			if (!vfd->ctrl_handler)
303 				continue;
304 			dev_dbg(imxmd->md.dev,
305 				"refresh controls for %s\n",
306 				vfd->entity.name);
307 			ret = imx_media_inherit_controls(imxmd, vfd,
308 							 &vfd->entity);
309 			if (ret)
310 				break;
311 		}
312 	}
313 
314 	return ret;
315 }
316 
imx_media_notify(struct v4l2_subdev * sd,unsigned int notification,void * arg)317 static void imx_media_notify(struct v4l2_subdev *sd, unsigned int notification,
318 			     void *arg)
319 {
320 	struct media_entity *entity = &sd->entity;
321 	int i;
322 
323 	if (notification != V4L2_DEVICE_NOTIFY_EVENT)
324 		return;
325 
326 	for (i = 0; i < entity->num_pads; i++) {
327 		struct media_pad *pad = &entity->pads[i];
328 		struct imx_media_pad_vdev *pad_vdev;
329 		struct list_head *pad_vdev_list;
330 
331 		pad_vdev_list = to_pad_vdev_list(sd, pad->index);
332 		if (!pad_vdev_list)
333 			continue;
334 		list_for_each_entry(pad_vdev, pad_vdev_list, list)
335 			v4l2_event_queue(pad_vdev->vdev->vfd, arg);
336 	}
337 }
338 
339 static const struct v4l2_async_notifier_operations imx_media_notifier_ops = {
340 	.bound = imx_media_subdev_bound,
341 	.complete = imx_media_probe_complete,
342 };
343 
344 static const struct media_device_ops imx_media_md_ops = {
345 	.link_notify = imx_media_link_notify,
346 };
347 
imx_media_dev_init(struct device * dev,const struct media_device_ops * ops)348 struct imx_media_dev *imx_media_dev_init(struct device *dev,
349 					 const struct media_device_ops *ops)
350 {
351 	struct imx_media_dev *imxmd;
352 	int ret;
353 
354 	imxmd = devm_kzalloc(dev, sizeof(*imxmd), GFP_KERNEL);
355 	if (!imxmd)
356 		return ERR_PTR(-ENOMEM);
357 
358 	dev_set_drvdata(dev, imxmd);
359 
360 	strscpy(imxmd->md.model, "imx-media", sizeof(imxmd->md.model));
361 	imxmd->md.ops = ops ? ops : &imx_media_md_ops;
362 	imxmd->md.dev = dev;
363 
364 	mutex_init(&imxmd->mutex);
365 
366 	imxmd->v4l2_dev.mdev = &imxmd->md;
367 	imxmd->v4l2_dev.notify = imx_media_notify;
368 	strscpy(imxmd->v4l2_dev.name, "imx-media",
369 		sizeof(imxmd->v4l2_dev.name));
370 	snprintf(imxmd->md.bus_info, sizeof(imxmd->md.bus_info),
371 		 "platform:%s", dev_name(imxmd->md.dev));
372 
373 	media_device_init(&imxmd->md);
374 
375 	ret = v4l2_device_register(dev, &imxmd->v4l2_dev);
376 	if (ret < 0) {
377 		v4l2_err(&imxmd->v4l2_dev,
378 			 "Failed to register v4l2_device: %d\n", ret);
379 		goto cleanup;
380 	}
381 
382 	INIT_LIST_HEAD(&imxmd->vdev_list);
383 
384 	v4l2_async_nf_init(&imxmd->notifier);
385 
386 	return imxmd;
387 
388 cleanup:
389 	media_device_cleanup(&imxmd->md);
390 
391 	return ERR_PTR(ret);
392 }
393 EXPORT_SYMBOL_GPL(imx_media_dev_init);
394 
imx_media_dev_notifier_register(struct imx_media_dev * imxmd,const struct v4l2_async_notifier_operations * ops)395 int imx_media_dev_notifier_register(struct imx_media_dev *imxmd,
396 			    const struct v4l2_async_notifier_operations *ops)
397 {
398 	int ret;
399 
400 	/* no subdevs? just bail */
401 	if (list_empty(&imxmd->notifier.asd_list)) {
402 		v4l2_err(&imxmd->v4l2_dev, "no subdevs\n");
403 		return -ENODEV;
404 	}
405 
406 	/* prepare the async subdev notifier and register it */
407 	imxmd->notifier.ops = ops ? ops : &imx_media_notifier_ops;
408 	ret = v4l2_async_nf_register(&imxmd->v4l2_dev, &imxmd->notifier);
409 	if (ret) {
410 		v4l2_err(&imxmd->v4l2_dev,
411 			 "v4l2_async_nf_register failed with %d\n", ret);
412 		return ret;
413 	}
414 
415 	return 0;
416 }
417 EXPORT_SYMBOL_GPL(imx_media_dev_notifier_register);
418