1 // SPDX-License-Identifier: GPL-2.0-or-later
3 * video stream multiplexer controlled via mux control
5 * Copyright (C) 2013 Pengutronix, Sascha Hauer <kernel@pengutronix.de>
6 * Copyright (C) 2016-2017 Pengutronix, Philipp Zabel <kernel@pengutronix.de>
10 #include <linux/module.h>
11 #include <linux/mutex.h>
12 #include <linux/mux/consumer.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>
23 struct video_mux_asd {
24 struct v4l2_async_subdev base;
28 static inline struct video_mux_asd *to_video_mux_asd(struct v4l2_async_subdev *asd)
30 return container_of(asd, struct video_mux_asd, base);
33 struct video_mux_pad_cfg {
34 unsigned int num_lanes;
36 struct v4l2_subdev *source;
40 struct v4l2_subdev subdev;
41 struct v4l2_async_notifier notifier;
42 struct media_pad *pads;
43 struct v4l2_mbus_framefmt *format_mbus;
44 struct video_mux_pad_cfg *cfg;
45 struct mux_control *mux;
50 static const struct v4l2_mbus_framefmt video_mux_format_mbus_default = {
53 .code = MEDIA_BUS_FMT_Y8_1X8,
54 .field = V4L2_FIELD_NONE,
57 static inline struct video_mux *
58 notifier_to_video_mux(struct v4l2_async_notifier *n)
60 return container_of(n, struct video_mux, notifier);
63 static inline struct video_mux *v4l2_subdev_to_video_mux(struct v4l2_subdev *sd)
65 return container_of(sd, struct video_mux, subdev);
68 static int video_mux_link_setup(struct media_entity *entity,
69 const struct media_pad *local,
70 const struct media_pad *remote, u32 flags)
72 struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
73 struct video_mux *vmux = v4l2_subdev_to_video_mux(sd);
74 u16 source_pad = entity->num_pads - 1;
78 * The mux state is determined by the enabled sink pad link.
79 * Enabling or disabling the source pad link has no effect.
81 if (local->flags & MEDIA_PAD_FL_SOURCE)
84 dev_dbg(sd->dev, "link setup '%s':%d->'%s':%d[%d]",
85 remote->entity->name, remote->index, local->entity->name,
86 local->index, flags & MEDIA_LNK_FL_ENABLED);
88 mutex_lock(&vmux->lock);
90 if (flags & MEDIA_LNK_FL_ENABLED) {
91 if (vmux->active == local->index)
94 if (vmux->active >= 0) {
99 dev_dbg(sd->dev, "setting %d active\n", local->index);
100 ret = mux_control_try_select(vmux->mux, local->index);
103 vmux->active = local->index;
105 /* Propagate the active format to the source */
106 vmux->format_mbus[source_pad] = vmux->format_mbus[vmux->active];
108 if (vmux->active != local->index)
111 dev_dbg(sd->dev, "going inactive\n");
112 mux_control_deselect(vmux->mux);
117 mutex_unlock(&vmux->lock);
121 static const struct media_entity_operations video_mux_ops = {
122 .link_setup = video_mux_link_setup,
123 .link_validate = v4l2_subdev_link_validate,
124 .get_fwnode_pad = v4l2_subdev_get_fwnode_pad_1_to_1,
127 static int video_mux_s_stream(struct v4l2_subdev *sd, int enable)
129 struct video_mux *vmux = v4l2_subdev_to_video_mux(sd);
130 struct v4l2_subdev *upstream_sd;
131 struct media_pad *pad;
133 if (vmux->active == -1) {
134 dev_err(sd->dev, "Can not start streaming on inactive mux\n");
138 pad = media_entity_remote_pad(&sd->entity.pads[vmux->active]);
140 dev_err(sd->dev, "Failed to find remote source pad\n");
144 if (!is_media_entity_v4l2_subdev(pad->entity)) {
145 dev_err(sd->dev, "Upstream entity is not a v4l2 subdev\n");
149 upstream_sd = media_entity_to_v4l2_subdev(pad->entity);
151 return v4l2_subdev_call(upstream_sd, video, s_stream, enable);
154 static const struct v4l2_subdev_video_ops video_mux_subdev_video_ops = {
155 .s_stream = video_mux_s_stream,
158 static struct v4l2_mbus_framefmt *
159 __video_mux_get_pad_format(struct v4l2_subdev *sd,
160 struct v4l2_subdev_state *sd_state,
161 unsigned int pad, u32 which)
163 struct video_mux *vmux = v4l2_subdev_to_video_mux(sd);
166 case V4L2_SUBDEV_FORMAT_TRY:
167 return v4l2_subdev_get_try_format(sd, sd_state, pad);
168 case V4L2_SUBDEV_FORMAT_ACTIVE:
169 return &vmux->format_mbus[pad];
175 static int video_mux_get_format(struct v4l2_subdev *sd,
176 struct v4l2_subdev_state *sd_state,
177 struct v4l2_subdev_format *sdformat)
179 struct video_mux *vmux = v4l2_subdev_to_video_mux(sd);
181 mutex_lock(&vmux->lock);
183 sdformat->format = *__video_mux_get_pad_format(sd, sd_state,
187 mutex_unlock(&vmux->lock);
192 static int video_mux_set_format(struct v4l2_subdev *sd,
193 struct v4l2_subdev_state *sd_state,
194 struct v4l2_subdev_format *sdformat)
196 struct video_mux *vmux = v4l2_subdev_to_video_mux(sd);
197 struct v4l2_mbus_framefmt *mbusformat, *source_mbusformat;
198 struct media_pad *pad = &vmux->pads[sdformat->pad];
199 u16 source_pad = sd->entity.num_pads - 1;
201 mbusformat = __video_mux_get_pad_format(sd, sd_state, sdformat->pad,
206 source_mbusformat = __video_mux_get_pad_format(sd, sd_state,
209 if (!source_mbusformat)
212 /* No size limitations except V4L2 compliance requirements */
213 v4l_bound_align_image(&sdformat->format.width, 1, 65536, 0,
214 &sdformat->format.height, 1, 65536, 0, 0);
216 /* All formats except LVDS and vendor specific formats are acceptable */
217 switch (sdformat->format.code) {
218 case MEDIA_BUS_FMT_RGB444_1X12:
219 case MEDIA_BUS_FMT_RGB444_2X8_PADHI_BE:
220 case MEDIA_BUS_FMT_RGB444_2X8_PADHI_LE:
221 case MEDIA_BUS_FMT_RGB555_2X8_PADHI_BE:
222 case MEDIA_BUS_FMT_RGB555_2X8_PADHI_LE:
223 case MEDIA_BUS_FMT_RGB565_1X16:
224 case MEDIA_BUS_FMT_BGR565_2X8_BE:
225 case MEDIA_BUS_FMT_BGR565_2X8_LE:
226 case MEDIA_BUS_FMT_RGB565_2X8_BE:
227 case MEDIA_BUS_FMT_RGB565_2X8_LE:
228 case MEDIA_BUS_FMT_RGB666_1X18:
229 case MEDIA_BUS_FMT_RBG888_1X24:
230 case MEDIA_BUS_FMT_RGB666_1X24_CPADHI:
231 case MEDIA_BUS_FMT_BGR888_1X24:
232 case MEDIA_BUS_FMT_GBR888_1X24:
233 case MEDIA_BUS_FMT_RGB888_1X24:
234 case MEDIA_BUS_FMT_RGB888_2X12_BE:
235 case MEDIA_BUS_FMT_RGB888_2X12_LE:
236 case MEDIA_BUS_FMT_ARGB8888_1X32:
237 case MEDIA_BUS_FMT_RGB888_1X32_PADHI:
238 case MEDIA_BUS_FMT_RGB101010_1X30:
239 case MEDIA_BUS_FMT_RGB121212_1X36:
240 case MEDIA_BUS_FMT_RGB161616_1X48:
241 case MEDIA_BUS_FMT_Y8_1X8:
242 case MEDIA_BUS_FMT_UV8_1X8:
243 case MEDIA_BUS_FMT_UYVY8_1_5X8:
244 case MEDIA_BUS_FMT_VYUY8_1_5X8:
245 case MEDIA_BUS_FMT_YUYV8_1_5X8:
246 case MEDIA_BUS_FMT_YVYU8_1_5X8:
247 case MEDIA_BUS_FMT_UYVY8_2X8:
248 case MEDIA_BUS_FMT_VYUY8_2X8:
249 case MEDIA_BUS_FMT_YUYV8_2X8:
250 case MEDIA_BUS_FMT_YVYU8_2X8:
251 case MEDIA_BUS_FMT_Y10_1X10:
252 case MEDIA_BUS_FMT_UYVY10_2X10:
253 case MEDIA_BUS_FMT_VYUY10_2X10:
254 case MEDIA_BUS_FMT_YUYV10_2X10:
255 case MEDIA_BUS_FMT_YVYU10_2X10:
256 case MEDIA_BUS_FMT_Y12_1X12:
257 case MEDIA_BUS_FMT_UYVY12_2X12:
258 case MEDIA_BUS_FMT_VYUY12_2X12:
259 case MEDIA_BUS_FMT_YUYV12_2X12:
260 case MEDIA_BUS_FMT_YVYU12_2X12:
261 case MEDIA_BUS_FMT_UYVY8_1X16:
262 case MEDIA_BUS_FMT_VYUY8_1X16:
263 case MEDIA_BUS_FMT_YUYV8_1X16:
264 case MEDIA_BUS_FMT_YVYU8_1X16:
265 case MEDIA_BUS_FMT_YDYUYDYV8_1X16:
266 case MEDIA_BUS_FMT_UYVY10_1X20:
267 case MEDIA_BUS_FMT_VYUY10_1X20:
268 case MEDIA_BUS_FMT_YUYV10_1X20:
269 case MEDIA_BUS_FMT_YVYU10_1X20:
270 case MEDIA_BUS_FMT_VUY8_1X24:
271 case MEDIA_BUS_FMT_YUV8_1X24:
272 case MEDIA_BUS_FMT_UYYVYY8_0_5X24:
273 case MEDIA_BUS_FMT_UYVY12_1X24:
274 case MEDIA_BUS_FMT_VYUY12_1X24:
275 case MEDIA_BUS_FMT_YUYV12_1X24:
276 case MEDIA_BUS_FMT_YVYU12_1X24:
277 case MEDIA_BUS_FMT_YUV10_1X30:
278 case MEDIA_BUS_FMT_UYYVYY10_0_5X30:
279 case MEDIA_BUS_FMT_AYUV8_1X32:
280 case MEDIA_BUS_FMT_UYYVYY12_0_5X36:
281 case MEDIA_BUS_FMT_YUV12_1X36:
282 case MEDIA_BUS_FMT_YUV16_1X48:
283 case MEDIA_BUS_FMT_UYYVYY16_0_5X48:
284 case MEDIA_BUS_FMT_JPEG_1X8:
285 case MEDIA_BUS_FMT_AHSV8888_1X32:
286 case MEDIA_BUS_FMT_SBGGR8_1X8:
287 case MEDIA_BUS_FMT_SGBRG8_1X8:
288 case MEDIA_BUS_FMT_SGRBG8_1X8:
289 case MEDIA_BUS_FMT_SRGGB8_1X8:
290 case MEDIA_BUS_FMT_SBGGR10_1X10:
291 case MEDIA_BUS_FMT_SGBRG10_1X10:
292 case MEDIA_BUS_FMT_SGRBG10_1X10:
293 case MEDIA_BUS_FMT_SRGGB10_1X10:
294 case MEDIA_BUS_FMT_SBGGR12_1X12:
295 case MEDIA_BUS_FMT_SGBRG12_1X12:
296 case MEDIA_BUS_FMT_SGRBG12_1X12:
297 case MEDIA_BUS_FMT_SRGGB12_1X12:
298 case MEDIA_BUS_FMT_SBGGR14_1X14:
299 case MEDIA_BUS_FMT_SGBRG14_1X14:
300 case MEDIA_BUS_FMT_SGRBG14_1X14:
301 case MEDIA_BUS_FMT_SRGGB14_1X14:
302 case MEDIA_BUS_FMT_SBGGR16_1X16:
303 case MEDIA_BUS_FMT_SGBRG16_1X16:
304 case MEDIA_BUS_FMT_SGRBG16_1X16:
305 case MEDIA_BUS_FMT_SRGGB16_1X16:
308 sdformat->format.code = MEDIA_BUS_FMT_Y8_1X8;
311 if (sdformat->format.field == V4L2_FIELD_ANY)
312 sdformat->format.field = V4L2_FIELD_NONE;
314 mutex_lock(&vmux->lock);
316 /* Source pad mirrors active sink pad, no limitations on sink pads */
317 if ((pad->flags & MEDIA_PAD_FL_SOURCE) && vmux->active >= 0)
318 sdformat->format = vmux->format_mbus[vmux->active];
320 *mbusformat = sdformat->format;
322 /* Propagate the format from an active sink to source */
323 if ((pad->flags & MEDIA_PAD_FL_SINK) && (pad->index == vmux->active))
324 *source_mbusformat = sdformat->format;
326 mutex_unlock(&vmux->lock);
331 static int video_mux_init_cfg(struct v4l2_subdev *sd,
332 struct v4l2_subdev_state *sd_state)
334 struct video_mux *vmux = v4l2_subdev_to_video_mux(sd);
335 struct v4l2_mbus_framefmt *mbusformat;
338 mutex_lock(&vmux->lock);
340 for (i = 0; i < sd->entity.num_pads; i++) {
341 mbusformat = v4l2_subdev_get_try_format(sd, sd_state, i);
342 *mbusformat = video_mux_format_mbus_default;
345 mutex_unlock(&vmux->lock);
350 static int video_mux_get_mbus_config(struct v4l2_subdev *sd,
352 struct v4l2_mbus_config *cfg)
354 struct video_mux *vmux = v4l2_subdev_to_video_mux(sd);
355 const u32 mask = V4L2_MBUS_CSI2_LANE_MASK;
358 ret = v4l2_subdev_call(vmux->cfg[vmux->active].source, pad, get_mbus_config,
361 if (ret != -ENOIOCTLCMD)
364 cfg->type = V4L2_MBUS_CSI2_DPHY;
365 cfg->flags = (vmux->cfg[vmux->active].num_lanes << __ffs(mask)) & mask;
367 /* Support for non-continuous CSI-2 clock is missing in pdate mode */
368 if (vmux->cfg[vmux->active].non_continuous)
369 cfg->flags |= V4L2_MBUS_CSI2_NONCONTINUOUS_CLOCK;
371 cfg->flags |= V4L2_MBUS_CSI2_CONTINUOUS_CLOCK;
376 static const struct v4l2_subdev_pad_ops video_mux_pad_ops = {
377 .init_cfg = video_mux_init_cfg,
378 .get_fmt = video_mux_get_format,
379 .set_fmt = video_mux_set_format,
380 .get_mbus_config = video_mux_get_mbus_config,
383 static const struct v4l2_subdev_ops video_mux_subdev_ops = {
384 .pad = &video_mux_pad_ops,
385 .video = &video_mux_subdev_video_ops,
388 static int video_mux_notify_bound(struct v4l2_async_notifier *notifier,
389 struct v4l2_subdev *sd,
390 struct v4l2_async_subdev *asd)
392 struct video_mux *vmux = notifier_to_video_mux(notifier);
393 unsigned int port = to_video_mux_asd(asd)->port;
395 vmux->cfg[port].source = sd;
397 return v4l2_create_fwnode_links(sd, &vmux->subdev);
400 static const struct v4l2_async_notifier_operations video_mux_notify_ops = {
401 .bound = video_mux_notify_bound,
404 static int video_mux_async_register(struct video_mux *vmux,
405 unsigned int num_input_pads)
410 v4l2_async_notifier_init(&vmux->notifier);
412 for (i = 0; i < num_input_pads; i++) {
413 struct video_mux_asd *asd;
414 struct fwnode_handle *ep, *remote_ep;
416 ep = fwnode_graph_get_endpoint_by_id(
417 dev_fwnode(vmux->subdev.dev), i, 0,
418 FWNODE_GRAPH_ENDPOINT_NEXT);
422 /* Skip dangling endpoints for backwards compatibility */
423 remote_ep = fwnode_graph_get_remote_endpoint(ep);
425 fwnode_handle_put(ep);
428 fwnode_handle_put(remote_ep);
430 asd = v4l2_async_notifier_add_fwnode_remote_subdev(
431 &vmux->notifier, ep, struct video_mux_asd);
434 fwnode_handle_put(ep);
438 /* OK if asd already exists */
444 vmux->notifier.ops = &video_mux_notify_ops;
446 ret = v4l2_async_subdev_notifier_register(&vmux->subdev,
451 return v4l2_async_register_subdev(&vmux->subdev);
454 static int video_mux_probe(struct platform_device *pdev)
456 struct device_node *np = pdev->dev.of_node;
457 struct device *dev = &pdev->dev;
458 struct v4l2_fwnode_endpoint fwnode_ep = {
459 .bus_type = V4L2_MBUS_CSI2_DPHY
461 struct device_node *ep;
462 struct video_mux *vmux;
463 unsigned int num_pads = 0;
467 vmux = devm_kzalloc(dev, sizeof(*vmux), GFP_KERNEL);
471 platform_set_drvdata(pdev, vmux);
473 v4l2_subdev_init(&vmux->subdev, &video_mux_subdev_ops);
474 snprintf(vmux->subdev.name, sizeof(vmux->subdev.name), "%pOFn", np);
475 vmux->subdev.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
476 vmux->subdev.dev = dev;
479 * The largest numbered port is the output port. It determines
480 * total number of pads.
482 for_each_endpoint_of_node(np, ep) {
483 struct of_endpoint endpoint;
485 of_graph_parse_endpoint(ep, &endpoint);
486 num_pads = max(num_pads, endpoint.port + 1);
490 dev_err(dev, "Not enough ports %d\n", num_pads);
494 vmux->mux = devm_mux_control_get(dev, NULL);
495 if (IS_ERR(vmux->mux)) {
496 ret = PTR_ERR(vmux->mux);
497 if (ret != -EPROBE_DEFER)
498 dev_err(dev, "Failed to get mux: %d\n", ret);
502 mutex_init(&vmux->lock);
504 vmux->pads = devm_kcalloc(dev, num_pads, sizeof(*vmux->pads),
509 vmux->format_mbus = devm_kcalloc(dev, num_pads,
510 sizeof(*vmux->format_mbus),
512 if (!vmux->format_mbus)
515 vmux->cfg = devm_kcalloc(dev, num_pads, sizeof(*vmux->cfg), GFP_KERNEL);
519 for (i = 0; i < num_pads; i++) {
520 vmux->pads[i].flags = (i < num_pads - 1) ? MEDIA_PAD_FL_SINK
521 : MEDIA_PAD_FL_SOURCE;
522 vmux->format_mbus[i] = video_mux_format_mbus_default;
524 ep = of_graph_get_endpoint_by_regs(pdev->dev.of_node, i, 0);
526 ret = v4l2_fwnode_endpoint_parse(of_fwnode_handle(ep), &fwnode_ep);
528 /* Get number of data lanes */
529 vmux->cfg[i].num_lanes = fwnode_ep.bus.mipi_csi2.num_data_lanes;
530 vmux->cfg[i].non_continuous = fwnode_ep.bus.mipi_csi2.flags &
531 V4L2_MBUS_CSI2_NONCONTINUOUS_CLOCK;
537 vmux->subdev.entity.function = MEDIA_ENT_F_VID_MUX;
538 ret = media_entity_pads_init(&vmux->subdev.entity, num_pads,
543 vmux->subdev.entity.ops = &video_mux_ops;
545 ret = video_mux_async_register(vmux, num_pads - 1);
547 v4l2_async_notifier_unregister(&vmux->notifier);
548 v4l2_async_notifier_cleanup(&vmux->notifier);
554 static int video_mux_remove(struct platform_device *pdev)
556 struct video_mux *vmux = platform_get_drvdata(pdev);
557 struct v4l2_subdev *sd = &vmux->subdev;
559 v4l2_async_notifier_unregister(&vmux->notifier);
560 v4l2_async_notifier_cleanup(&vmux->notifier);
561 v4l2_async_unregister_subdev(sd);
562 media_entity_cleanup(&sd->entity);
567 static const struct of_device_id video_mux_dt_ids[] = {
568 { .compatible = "video-mux", },
571 MODULE_DEVICE_TABLE(of, video_mux_dt_ids);
573 static struct platform_driver video_mux_driver = {
574 .probe = video_mux_probe,
575 .remove = video_mux_remove,
577 .of_match_table = video_mux_dt_ids,
582 module_platform_driver(video_mux_driver);
584 MODULE_DESCRIPTION("video stream multiplexer");
585 MODULE_AUTHOR("Sascha Hauer, Pengutronix");
586 MODULE_AUTHOR("Philipp Zabel, Pengutronix");
587 MODULE_LICENSE("GPL");