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