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(src_sd: csi2, sink_sd: 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, pad_index: 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(dev: imxmd->md.dev, size: 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(new: &pad_vdev->list, head: 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 srcpad: 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(dev: imxmd->md.dev,
131 n: entity->num_pads, size: 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(list: to_pad_vdev_list(sd, pad_index: 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, srcpad: 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(n: 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(v4l2_dev: &imxmd->v4l2_dev);
183unlock:
184 mutex_unlock(lock: &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(hdl: vfd->ctrl_handler,
210 add: sd->ctrl_handler,
211 NULL, from_other_dev: 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(pad: spad);
224 if (!pad || !is_media_entity_v4l2_subdev(entity: pad->entity))
225 continue;
226
227 ret = imx_media_inherit_controls(imxmd, vfd, entity: 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(entity: 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_index: 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(hdl: 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 entity: &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: pad->index);
317 if (!pad_vdev_list)
318 continue;
319 list_for_each_entry(pad_vdev, pad_vdev_list, list)
320 v4l2_event_queue(vdev: pad_vdev->vdev->vfd, ev: 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, size: sizeof(*imxmd), GFP_KERNEL);
339 if (!imxmd)
340 return ERR_PTR(error: -ENOMEM);
341
342 dev_set_drvdata(dev, data: 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(buf: imxmd->md.bus_info, size: sizeof(imxmd->md.bus_info),
355 fmt: "platform:%s", dev_name(dev: imxmd->md.dev));
356
357 media_device_init(mdev: &imxmd->md);
358
359 ret = v4l2_device_register(dev, v4l2_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(list: &imxmd->vdev_list);
367
368 v4l2_async_nf_init(notifier: &imxmd->notifier, v4l2_dev: &imxmd->v4l2_dev);
369
370 return imxmd;
371
372cleanup:
373 media_device_cleanup(mdev: &imxmd->md);
374
375 return ERR_PTR(error: 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(head: &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(notifier: &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);
402

source code of linux/drivers/staging/media/imx/imx-media-dev-common.c