1 | // SPDX-License-Identifier: GPL-2.0-or-later |
2 | /* |
3 | * video stream multiplexer controlled via mux control |
4 | * |
5 | * Copyright (C) 2013 Pengutronix, Sascha Hauer <kernel@pengutronix.de> |
6 | * Copyright (C) 2016-2017 Pengutronix, Philipp Zabel <kernel@pengutronix.de> |
7 | */ |
8 | |
9 | #include <linux/err.h> |
10 | #include <linux/module.h> |
11 | #include <linux/mutex.h> |
12 | #include <linux/mux/consumer.h> |
13 | #include <linux/of.h> |
14 | #include <linux/of_graph.h> |
15 | #include <linux/platform_device.h> |
16 | #include <linux/slab.h> |
17 | #include <media/v4l2-async.h> |
18 | #include <media/v4l2-device.h> |
19 | #include <media/v4l2-fwnode.h> |
20 | #include <media/v4l2-mc.h> |
21 | #include <media/v4l2-subdev.h> |
22 | |
23 | struct video_mux { |
24 | struct v4l2_subdev subdev; |
25 | struct v4l2_async_notifier notifier; |
26 | struct media_pad *pads; |
27 | struct mux_control *mux; |
28 | struct mutex lock; |
29 | int active; |
30 | }; |
31 | |
32 | static const struct v4l2_mbus_framefmt video_mux_format_mbus_default = { |
33 | .width = 1, |
34 | .height = 1, |
35 | .code = MEDIA_BUS_FMT_Y8_1X8, |
36 | .field = V4L2_FIELD_NONE, |
37 | }; |
38 | |
39 | static inline struct video_mux * |
40 | notifier_to_video_mux(struct v4l2_async_notifier *n) |
41 | { |
42 | return container_of(n, struct video_mux, notifier); |
43 | } |
44 | |
45 | static inline struct video_mux *v4l2_subdev_to_video_mux(struct v4l2_subdev *sd) |
46 | { |
47 | return container_of(sd, struct video_mux, subdev); |
48 | } |
49 | |
50 | static int video_mux_link_setup(struct media_entity *entity, |
51 | const struct media_pad *local, |
52 | const struct media_pad *remote, u32 flags) |
53 | { |
54 | struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity); |
55 | struct video_mux *vmux = v4l2_subdev_to_video_mux(sd); |
56 | u16 source_pad = entity->num_pads - 1; |
57 | int ret = 0; |
58 | |
59 | /* |
60 | * The mux state is determined by the enabled sink pad link. |
61 | * Enabling or disabling the source pad link has no effect. |
62 | */ |
63 | if (local->flags & MEDIA_PAD_FL_SOURCE) |
64 | return 0; |
65 | |
66 | dev_dbg(sd->dev, "link setup '%s':%d->'%s':%d[%d]" , |
67 | remote->entity->name, remote->index, local->entity->name, |
68 | local->index, flags & MEDIA_LNK_FL_ENABLED); |
69 | |
70 | mutex_lock(&vmux->lock); |
71 | |
72 | if (flags & MEDIA_LNK_FL_ENABLED) { |
73 | struct v4l2_subdev_state *sd_state; |
74 | struct v4l2_mbus_framefmt *source_mbusformat; |
75 | |
76 | if (vmux->active == local->index) |
77 | goto out; |
78 | |
79 | if (vmux->active >= 0) { |
80 | ret = -EBUSY; |
81 | goto out; |
82 | } |
83 | |
84 | dev_dbg(sd->dev, "setting %d active\n" , local->index); |
85 | ret = mux_control_try_select(mux: vmux->mux, state: local->index); |
86 | if (ret < 0) |
87 | goto out; |
88 | vmux->active = local->index; |
89 | |
90 | /* Propagate the active format to the source */ |
91 | sd_state = v4l2_subdev_lock_and_get_active_state(sd); |
92 | source_mbusformat = v4l2_subdev_state_get_format(sd_state, |
93 | source_pad); |
94 | *source_mbusformat = *v4l2_subdev_state_get_format(sd_state, |
95 | vmux->active); |
96 | v4l2_subdev_unlock_state(state: sd_state); |
97 | } else { |
98 | if (vmux->active != local->index) |
99 | goto out; |
100 | |
101 | dev_dbg(sd->dev, "going inactive\n" ); |
102 | mux_control_deselect(mux: vmux->mux); |
103 | vmux->active = -1; |
104 | } |
105 | |
106 | out: |
107 | mutex_unlock(lock: &vmux->lock); |
108 | return ret; |
109 | } |
110 | |
111 | static const struct media_entity_operations video_mux_ops = { |
112 | .link_setup = video_mux_link_setup, |
113 | .link_validate = v4l2_subdev_link_validate, |
114 | .get_fwnode_pad = v4l2_subdev_get_fwnode_pad_1_to_1, |
115 | }; |
116 | |
117 | static int video_mux_s_stream(struct v4l2_subdev *sd, int enable) |
118 | { |
119 | struct video_mux *vmux = v4l2_subdev_to_video_mux(sd); |
120 | struct v4l2_subdev *upstream_sd; |
121 | struct media_pad *pad; |
122 | |
123 | if (vmux->active == -1) { |
124 | dev_err(sd->dev, "Can not start streaming on inactive mux\n" ); |
125 | return -EINVAL; |
126 | } |
127 | |
128 | pad = media_pad_remote_pad_first(pad: &sd->entity.pads[vmux->active]); |
129 | if (!pad) { |
130 | dev_err(sd->dev, "Failed to find remote source pad\n" ); |
131 | return -ENOLINK; |
132 | } |
133 | |
134 | if (!is_media_entity_v4l2_subdev(entity: pad->entity)) { |
135 | dev_err(sd->dev, "Upstream entity is not a v4l2 subdev\n" ); |
136 | return -ENODEV; |
137 | } |
138 | |
139 | upstream_sd = media_entity_to_v4l2_subdev(pad->entity); |
140 | |
141 | return v4l2_subdev_call(upstream_sd, video, s_stream, enable); |
142 | } |
143 | |
144 | static const struct v4l2_subdev_video_ops video_mux_subdev_video_ops = { |
145 | .s_stream = video_mux_s_stream, |
146 | }; |
147 | |
148 | static int video_mux_set_format(struct v4l2_subdev *sd, |
149 | struct v4l2_subdev_state *sd_state, |
150 | struct v4l2_subdev_format *sdformat) |
151 | { |
152 | struct video_mux *vmux = v4l2_subdev_to_video_mux(sd); |
153 | struct v4l2_mbus_framefmt *mbusformat, *source_mbusformat; |
154 | struct media_pad *pad = &vmux->pads[sdformat->pad]; |
155 | u16 source_pad = sd->entity.num_pads - 1; |
156 | |
157 | mbusformat = v4l2_subdev_state_get_format(sd_state, sdformat->pad); |
158 | if (!mbusformat) |
159 | return -EINVAL; |
160 | |
161 | source_mbusformat = v4l2_subdev_state_get_format(sd_state, source_pad); |
162 | if (!source_mbusformat) |
163 | return -EINVAL; |
164 | |
165 | /* No size limitations except V4L2 compliance requirements */ |
166 | v4l_bound_align_image(width: &sdformat->format.width, wmin: 1, wmax: 65536, walign: 0, |
167 | height: &sdformat->format.height, hmin: 1, hmax: 65536, halign: 0, salign: 0); |
168 | |
169 | /* All formats except LVDS and vendor specific formats are acceptable */ |
170 | switch (sdformat->format.code) { |
171 | case MEDIA_BUS_FMT_RGB444_1X12: |
172 | case MEDIA_BUS_FMT_RGB444_2X8_PADHI_BE: |
173 | case MEDIA_BUS_FMT_RGB444_2X8_PADHI_LE: |
174 | case MEDIA_BUS_FMT_RGB555_2X8_PADHI_BE: |
175 | case MEDIA_BUS_FMT_RGB555_2X8_PADHI_LE: |
176 | case MEDIA_BUS_FMT_RGB565_1X16: |
177 | case MEDIA_BUS_FMT_BGR565_2X8_BE: |
178 | case MEDIA_BUS_FMT_BGR565_2X8_LE: |
179 | case MEDIA_BUS_FMT_RGB565_2X8_BE: |
180 | case MEDIA_BUS_FMT_RGB565_2X8_LE: |
181 | case MEDIA_BUS_FMT_RGB666_1X18: |
182 | case MEDIA_BUS_FMT_RBG888_1X24: |
183 | case MEDIA_BUS_FMT_RGB666_1X24_CPADHI: |
184 | case MEDIA_BUS_FMT_BGR888_1X24: |
185 | case MEDIA_BUS_FMT_GBR888_1X24: |
186 | case MEDIA_BUS_FMT_RGB888_1X24: |
187 | case MEDIA_BUS_FMT_RGB888_2X12_BE: |
188 | case MEDIA_BUS_FMT_RGB888_2X12_LE: |
189 | case MEDIA_BUS_FMT_ARGB8888_1X32: |
190 | case MEDIA_BUS_FMT_RGB888_1X32_PADHI: |
191 | case MEDIA_BUS_FMT_RGB101010_1X30: |
192 | case MEDIA_BUS_FMT_RGB121212_1X36: |
193 | case MEDIA_BUS_FMT_RGB161616_1X48: |
194 | case MEDIA_BUS_FMT_Y8_1X8: |
195 | case MEDIA_BUS_FMT_UV8_1X8: |
196 | case MEDIA_BUS_FMT_UYVY8_1_5X8: |
197 | case MEDIA_BUS_FMT_VYUY8_1_5X8: |
198 | case MEDIA_BUS_FMT_YUYV8_1_5X8: |
199 | case MEDIA_BUS_FMT_YVYU8_1_5X8: |
200 | case MEDIA_BUS_FMT_UYVY8_2X8: |
201 | case MEDIA_BUS_FMT_VYUY8_2X8: |
202 | case MEDIA_BUS_FMT_YUYV8_2X8: |
203 | case MEDIA_BUS_FMT_YVYU8_2X8: |
204 | case MEDIA_BUS_FMT_Y10_1X10: |
205 | case MEDIA_BUS_FMT_UYVY10_2X10: |
206 | case MEDIA_BUS_FMT_VYUY10_2X10: |
207 | case MEDIA_BUS_FMT_YUYV10_2X10: |
208 | case MEDIA_BUS_FMT_YVYU10_2X10: |
209 | case MEDIA_BUS_FMT_Y12_1X12: |
210 | case MEDIA_BUS_FMT_UYVY12_2X12: |
211 | case MEDIA_BUS_FMT_VYUY12_2X12: |
212 | case MEDIA_BUS_FMT_YUYV12_2X12: |
213 | case MEDIA_BUS_FMT_YVYU12_2X12: |
214 | case MEDIA_BUS_FMT_UYVY8_1X16: |
215 | case MEDIA_BUS_FMT_VYUY8_1X16: |
216 | case MEDIA_BUS_FMT_YUYV8_1X16: |
217 | case MEDIA_BUS_FMT_YVYU8_1X16: |
218 | case MEDIA_BUS_FMT_YDYUYDYV8_1X16: |
219 | case MEDIA_BUS_FMT_UYVY10_1X20: |
220 | case MEDIA_BUS_FMT_VYUY10_1X20: |
221 | case MEDIA_BUS_FMT_YUYV10_1X20: |
222 | case MEDIA_BUS_FMT_YVYU10_1X20: |
223 | case MEDIA_BUS_FMT_VUY8_1X24: |
224 | case MEDIA_BUS_FMT_YUV8_1X24: |
225 | case MEDIA_BUS_FMT_UYYVYY8_0_5X24: |
226 | case MEDIA_BUS_FMT_UYVY12_1X24: |
227 | case MEDIA_BUS_FMT_VYUY12_1X24: |
228 | case MEDIA_BUS_FMT_YUYV12_1X24: |
229 | case MEDIA_BUS_FMT_YVYU12_1X24: |
230 | case MEDIA_BUS_FMT_YUV10_1X30: |
231 | case MEDIA_BUS_FMT_UYYVYY10_0_5X30: |
232 | case MEDIA_BUS_FMT_AYUV8_1X32: |
233 | case MEDIA_BUS_FMT_UYYVYY12_0_5X36: |
234 | case MEDIA_BUS_FMT_YUV12_1X36: |
235 | case MEDIA_BUS_FMT_YUV16_1X48: |
236 | case MEDIA_BUS_FMT_UYYVYY16_0_5X48: |
237 | case MEDIA_BUS_FMT_JPEG_1X8: |
238 | case MEDIA_BUS_FMT_AHSV8888_1X32: |
239 | case MEDIA_BUS_FMT_SBGGR8_1X8: |
240 | case MEDIA_BUS_FMT_SGBRG8_1X8: |
241 | case MEDIA_BUS_FMT_SGRBG8_1X8: |
242 | case MEDIA_BUS_FMT_SRGGB8_1X8: |
243 | case MEDIA_BUS_FMT_SBGGR10_1X10: |
244 | case MEDIA_BUS_FMT_SGBRG10_1X10: |
245 | case MEDIA_BUS_FMT_SGRBG10_1X10: |
246 | case MEDIA_BUS_FMT_SRGGB10_1X10: |
247 | case MEDIA_BUS_FMT_SBGGR12_1X12: |
248 | case MEDIA_BUS_FMT_SGBRG12_1X12: |
249 | case MEDIA_BUS_FMT_SGRBG12_1X12: |
250 | case MEDIA_BUS_FMT_SRGGB12_1X12: |
251 | case MEDIA_BUS_FMT_SBGGR14_1X14: |
252 | case MEDIA_BUS_FMT_SGBRG14_1X14: |
253 | case MEDIA_BUS_FMT_SGRBG14_1X14: |
254 | case MEDIA_BUS_FMT_SRGGB14_1X14: |
255 | case MEDIA_BUS_FMT_SBGGR16_1X16: |
256 | case MEDIA_BUS_FMT_SGBRG16_1X16: |
257 | case MEDIA_BUS_FMT_SGRBG16_1X16: |
258 | case MEDIA_BUS_FMT_SRGGB16_1X16: |
259 | break; |
260 | default: |
261 | sdformat->format.code = MEDIA_BUS_FMT_Y8_1X8; |
262 | break; |
263 | } |
264 | if (sdformat->format.field == V4L2_FIELD_ANY) |
265 | sdformat->format.field = V4L2_FIELD_NONE; |
266 | |
267 | mutex_lock(&vmux->lock); |
268 | |
269 | /* Source pad mirrors active sink pad, no limitations on sink pads */ |
270 | if ((pad->flags & MEDIA_PAD_FL_SOURCE) && vmux->active >= 0) |
271 | sdformat->format = *v4l2_subdev_state_get_format(sd_state, |
272 | vmux->active); |
273 | |
274 | *mbusformat = sdformat->format; |
275 | |
276 | /* Propagate the format from an active sink to source */ |
277 | if ((pad->flags & MEDIA_PAD_FL_SINK) && (pad->index == vmux->active)) |
278 | *source_mbusformat = sdformat->format; |
279 | |
280 | mutex_unlock(lock: &vmux->lock); |
281 | |
282 | return 0; |
283 | } |
284 | |
285 | static int video_mux_init_state(struct v4l2_subdev *sd, |
286 | struct v4l2_subdev_state *sd_state) |
287 | { |
288 | struct video_mux *vmux = v4l2_subdev_to_video_mux(sd); |
289 | struct v4l2_mbus_framefmt *mbusformat; |
290 | unsigned int i; |
291 | |
292 | mutex_lock(&vmux->lock); |
293 | |
294 | for (i = 0; i < sd->entity.num_pads; i++) { |
295 | mbusformat = v4l2_subdev_state_get_format(sd_state, i); |
296 | *mbusformat = video_mux_format_mbus_default; |
297 | } |
298 | |
299 | mutex_unlock(lock: &vmux->lock); |
300 | |
301 | return 0; |
302 | } |
303 | |
304 | static const struct v4l2_subdev_pad_ops video_mux_pad_ops = { |
305 | .get_fmt = v4l2_subdev_get_fmt, |
306 | .set_fmt = video_mux_set_format, |
307 | }; |
308 | |
309 | static const struct v4l2_subdev_ops video_mux_subdev_ops = { |
310 | .pad = &video_mux_pad_ops, |
311 | .video = &video_mux_subdev_video_ops, |
312 | }; |
313 | |
314 | static const struct v4l2_subdev_internal_ops video_mux_internal_ops = { |
315 | .init_state = video_mux_init_state, |
316 | }; |
317 | |
318 | static int video_mux_notify_bound(struct v4l2_async_notifier *notifier, |
319 | struct v4l2_subdev *sd, |
320 | struct v4l2_async_connection *asd) |
321 | { |
322 | struct video_mux *vmux = notifier_to_video_mux(n: notifier); |
323 | |
324 | return v4l2_create_fwnode_links(src_sd: sd, sink_sd: &vmux->subdev); |
325 | } |
326 | |
327 | static const struct v4l2_async_notifier_operations video_mux_notify_ops = { |
328 | .bound = video_mux_notify_bound, |
329 | }; |
330 | |
331 | static int video_mux_async_register(struct video_mux *vmux, |
332 | unsigned int num_input_pads) |
333 | { |
334 | unsigned int i; |
335 | int ret; |
336 | |
337 | v4l2_async_subdev_nf_init(notifier: &vmux->notifier, sd: &vmux->subdev); |
338 | |
339 | for (i = 0; i < num_input_pads; i++) { |
340 | struct v4l2_async_connection *asd; |
341 | struct fwnode_handle *ep, *remote_ep; |
342 | |
343 | ep = fwnode_graph_get_endpoint_by_id( |
344 | dev_fwnode(vmux->subdev.dev), port: i, endpoint: 0, |
345 | FWNODE_GRAPH_ENDPOINT_NEXT); |
346 | if (!ep) |
347 | continue; |
348 | |
349 | /* Skip dangling endpoints for backwards compatibility */ |
350 | remote_ep = fwnode_graph_get_remote_endpoint(fwnode: ep); |
351 | if (!remote_ep) { |
352 | fwnode_handle_put(fwnode: ep); |
353 | continue; |
354 | } |
355 | fwnode_handle_put(fwnode: remote_ep); |
356 | |
357 | asd = v4l2_async_nf_add_fwnode_remote(&vmux->notifier, ep, |
358 | struct v4l2_async_connection); |
359 | |
360 | fwnode_handle_put(fwnode: ep); |
361 | |
362 | if (IS_ERR(ptr: asd)) { |
363 | ret = PTR_ERR(ptr: asd); |
364 | /* OK if asd already exists */ |
365 | if (ret != -EEXIST) |
366 | goto err_nf_cleanup; |
367 | } |
368 | } |
369 | |
370 | vmux->notifier.ops = &video_mux_notify_ops; |
371 | |
372 | ret = v4l2_async_nf_register(notifier: &vmux->notifier); |
373 | if (ret) |
374 | goto err_nf_cleanup; |
375 | |
376 | ret = v4l2_async_register_subdev(sd: &vmux->subdev); |
377 | if (ret) |
378 | goto err_nf_unregister; |
379 | |
380 | return 0; |
381 | |
382 | err_nf_unregister: |
383 | v4l2_async_nf_unregister(notifier: &vmux->notifier); |
384 | err_nf_cleanup: |
385 | v4l2_async_nf_cleanup(notifier: &vmux->notifier); |
386 | return ret; |
387 | } |
388 | |
389 | static int video_mux_probe(struct platform_device *pdev) |
390 | { |
391 | struct device_node *np = pdev->dev.of_node; |
392 | struct device *dev = &pdev->dev; |
393 | struct device_node *ep; |
394 | struct video_mux *vmux; |
395 | unsigned int num_pads = 0; |
396 | unsigned int i; |
397 | int ret; |
398 | |
399 | vmux = devm_kzalloc(dev, size: sizeof(*vmux), GFP_KERNEL); |
400 | if (!vmux) |
401 | return -ENOMEM; |
402 | |
403 | platform_set_drvdata(pdev, data: vmux); |
404 | |
405 | v4l2_subdev_init(sd: &vmux->subdev, ops: &video_mux_subdev_ops); |
406 | vmux->subdev.internal_ops = &video_mux_internal_ops; |
407 | snprintf(buf: vmux->subdev.name, size: sizeof(vmux->subdev.name), fmt: "%pOFn" , np); |
408 | vmux->subdev.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE; |
409 | vmux->subdev.dev = dev; |
410 | |
411 | /* |
412 | * The largest numbered port is the output port. It determines |
413 | * total number of pads. |
414 | */ |
415 | for_each_endpoint_of_node(np, ep) { |
416 | struct of_endpoint endpoint; |
417 | |
418 | of_graph_parse_endpoint(node: ep, endpoint: &endpoint); |
419 | num_pads = max(num_pads, endpoint.port + 1); |
420 | } |
421 | |
422 | if (num_pads < 2) { |
423 | dev_err(dev, "Not enough ports %d\n" , num_pads); |
424 | return -EINVAL; |
425 | } |
426 | |
427 | vmux->mux = devm_mux_control_get(dev, NULL); |
428 | if (IS_ERR(ptr: vmux->mux)) { |
429 | ret = PTR_ERR(ptr: vmux->mux); |
430 | return dev_err_probe(dev, err: ret, fmt: "Failed to get mux\n" ); |
431 | } |
432 | |
433 | mutex_init(&vmux->lock); |
434 | vmux->active = -1; |
435 | vmux->pads = devm_kcalloc(dev, n: num_pads, size: sizeof(*vmux->pads), |
436 | GFP_KERNEL); |
437 | if (!vmux->pads) |
438 | return -ENOMEM; |
439 | |
440 | for (i = 0; i < num_pads; i++) |
441 | vmux->pads[i].flags = (i < num_pads - 1) ? MEDIA_PAD_FL_SINK |
442 | : MEDIA_PAD_FL_SOURCE; |
443 | |
444 | vmux->subdev.entity.function = MEDIA_ENT_F_VID_MUX; |
445 | ret = media_entity_pads_init(entity: &vmux->subdev.entity, num_pads, |
446 | pads: vmux->pads); |
447 | if (ret < 0) |
448 | return ret; |
449 | |
450 | vmux->subdev.entity.ops = &video_mux_ops; |
451 | |
452 | ret = v4l2_subdev_init_finalize(&vmux->subdev); |
453 | if (ret < 0) |
454 | goto err_entity_cleanup; |
455 | |
456 | ret = video_mux_async_register(vmux, num_input_pads: num_pads - 1); |
457 | if (ret) |
458 | goto err_subdev_cleanup; |
459 | |
460 | return 0; |
461 | |
462 | err_subdev_cleanup: |
463 | v4l2_subdev_cleanup(sd: &vmux->subdev); |
464 | err_entity_cleanup: |
465 | media_entity_cleanup(entity: &vmux->subdev.entity); |
466 | return ret; |
467 | } |
468 | |
469 | static void video_mux_remove(struct platform_device *pdev) |
470 | { |
471 | struct video_mux *vmux = platform_get_drvdata(pdev); |
472 | struct v4l2_subdev *sd = &vmux->subdev; |
473 | |
474 | v4l2_async_nf_unregister(notifier: &vmux->notifier); |
475 | v4l2_async_nf_cleanup(notifier: &vmux->notifier); |
476 | v4l2_async_unregister_subdev(sd); |
477 | v4l2_subdev_cleanup(sd); |
478 | media_entity_cleanup(entity: &sd->entity); |
479 | } |
480 | |
481 | static const struct of_device_id video_mux_dt_ids[] = { |
482 | { .compatible = "video-mux" , }, |
483 | { /* sentinel */ } |
484 | }; |
485 | MODULE_DEVICE_TABLE(of, video_mux_dt_ids); |
486 | |
487 | static struct platform_driver video_mux_driver = { |
488 | .probe = video_mux_probe, |
489 | .remove_new = video_mux_remove, |
490 | .driver = { |
491 | .of_match_table = video_mux_dt_ids, |
492 | .name = "video-mux" , |
493 | }, |
494 | }; |
495 | |
496 | module_platform_driver(video_mux_driver); |
497 | |
498 | MODULE_DESCRIPTION("video stream multiplexer" ); |
499 | MODULE_AUTHOR("Sascha Hauer, Pengutronix" ); |
500 | MODULE_AUTHOR("Philipp Zabel, Pengutronix" ); |
501 | MODULE_LICENSE("GPL" ); |
502 | |