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 | |
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 | */ |
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(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 | */ |
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, 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 | */ |
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(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 */ |
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, srcpad: link->source); |
161 | if (ret) |
162 | return ret; |
163 | } |
164 | |
165 | return 0; |
166 | } |
167 | |
168 | /* async subdev complete notifier */ |
169 | int 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); |
183 | unlock: |
184 | mutex_unlock(lock: &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 | */ |
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(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 | |
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(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 | |
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: 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 | |
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 | |
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, 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 | |
372 | cleanup: |
373 | media_device_cleanup(mdev: &imxmd->md); |
374 | |
375 | return ERR_PTR(error: ret); |
376 | } |
377 | EXPORT_SYMBOL_GPL(imx_media_dev_init); |
378 | |
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(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 | } |
401 | EXPORT_SYMBOL_GPL(imx_media_dev_notifier_register); |
402 | |