imx-media-dev-common.c

  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
 15static 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 */
 24static 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 */
 51static 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 */
121static 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 */
147static 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 */
169int 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);
183unlock:
184	mutex_unlock(&imxmd->mutex);
185	if (ret)
186		return ret;
187
188	return media_device_register(&imxmd->md);
189}
190EXPORT_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 */
196static 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
235static 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
302static 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
324static const struct v4l2_async_notifier_operations imx_media_notifier_ops = {
325	.complete = imx_media_probe_complete,
326};
327
328static const struct media_device_ops imx_media_md_ops = {
329	.link_notify = imx_media_link_notify,
330};
331
332struct 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
372cleanup:
373	media_device_cleanup(&imxmd->md);
374
375	return ERR_PTR(ret);
376}
377EXPORT_SYMBOL_GPL(imx_media_dev_init);
378
379int 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}
401EXPORT_SYMBOL_GPL(imx_media_dev_notifier_register);