media/platform/video-mux.c

0001 // SPDX-License-Identifier: GPL-2.0-or-later
0002 /*
0003  * video stream multiplexer controlled via mux control
0004  *
0005  * Copyright (C) 2013 Pengutronix, Sascha Hauer <kernel@pengutronix.de>
0006  * Copyright (C) 2016-2017 Pengutronix, Philipp Zabel <kernel@pengutronix.de>
0007  */
0008
0009 #include <linux/err.h>
0010 #include <linux/module.h>
0011 #include <linux/mutex.h>
0012 #include <linux/mux/consumer.h>
0013 #include <linux/of.h>
0014 #include <linux/of_graph.h>
0015 #include <linux/platform_device.h>
0016 #include <linux/slab.h>
0017 #include <media/v4l2-async.h>
0018 #include <media/v4l2-device.h>
0019 #include <media/v4l2-fwnode.h>
0020 #include <media/v4l2-mc.h>
0021 #include <media/v4l2-subdev.h>
0022
0023 struct video_mux {
0024     struct v4l2_subdev subdev;
0025     struct v4l2_async_notifier notifier;
0026     struct media_pad *pads;
0027     struct v4l2_mbus_framefmt *format_mbus;
0028     struct mux_control *mux;
0029     struct mutex lock;
0030     int active;
0031 };
0032
0033 static const struct v4l2_mbus_framefmt video_mux_format_mbus_default = {
0034     .width = 1,
0035     .height = 1,
0036     .code = MEDIA_BUS_FMT_Y8_1X8,
0037     .field = V4L2_FIELD_NONE,
0038 };
0039
0040 static inline struct video_mux *
0041 notifier_to_video_mux(struct v4l2_async_notifier *n)
0042 {
0043     return container_of(n, struct video_mux, notifier);
0044 }
0045
0046 static inline struct video_mux *v4l2_subdev_to_video_mux(struct v4l2_subdev *sd)
0047 {
0048     return container_of(sd, struct video_mux, subdev);
0049 }
0050
0051 static int video_mux_link_setup(struct media_entity *entity,
0052                 const struct media_pad *local,
0053                 const struct media_pad *remote, u32 flags)
0054 {
0055     struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
0056     struct video_mux *vmux = v4l2_subdev_to_video_mux(sd);
0057     u16 source_pad = entity->num_pads - 1;
0058     int ret = 0;
0059
0060     /*
0061      * The mux state is determined by the enabled sink pad link.
0062      * Enabling or disabling the source pad link has no effect.
0063      */
0064     if (local->flags & MEDIA_PAD_FL_SOURCE)
0065         return 0;
0066
0067     dev_dbg(sd->dev, "link setup '%s':%d->'%s':%d[%d]",
0068         remote->entity->name, remote->index, local->entity->name,
0069         local->index, flags & MEDIA_LNK_FL_ENABLED);
0070
0071     mutex_lock(&vmux->lock);
0072
0073     if (flags & MEDIA_LNK_FL_ENABLED) {
0074         if (vmux->active == local->index)
0075             goto out;
0076
0077         if (vmux->active >= 0) {
0078             ret = -EBUSY;
0079             goto out;
0080         }
0081
0082         dev_dbg(sd->dev, "setting %d active\n", local->index);
0083         ret = mux_control_try_select(vmux->mux, local->index);
0084         if (ret < 0)
0085             goto out;
0086         vmux->active = local->index;
0087
0088         /* Propagate the active format to the source */
0089         vmux->format_mbus[source_pad] = vmux->format_mbus[vmux->active];
0090     } else {
0091         if (vmux->active != local->index)
0092             goto out;
0093
0094         dev_dbg(sd->dev, "going inactive\n");
0095         mux_control_deselect(vmux->mux);
0096         vmux->active = -1;
0097     }
0098
0099 out:
0100     mutex_unlock(&vmux->lock);
0101     return ret;
0102 }
0103
0104 static const struct media_entity_operations video_mux_ops = {
0105     .link_setup = video_mux_link_setup,
0106     .link_validate = v4l2_subdev_link_validate,
0107     .get_fwnode_pad = v4l2_subdev_get_fwnode_pad_1_to_1,
0108 };
0109
0110 static int video_mux_s_stream(struct v4l2_subdev *sd, int enable)
0111 {
0112     struct video_mux *vmux = v4l2_subdev_to_video_mux(sd);
0113     struct v4l2_subdev *upstream_sd;
0114     struct media_pad *pad;
0115
0116     if (vmux->active == -1) {
0117         dev_err(sd->dev, "Can not start streaming on inactive mux\n");
0118         return -EINVAL;
0119     }
0120
0121     pad = media_pad_remote_pad_first(&sd->entity.pads[vmux->active]);
0122     if (!pad) {
0123         dev_err(sd->dev, "Failed to find remote source pad\n");
0124         return -ENOLINK;
0125     }
0126
0127     if (!is_media_entity_v4l2_subdev(pad->entity)) {
0128         dev_err(sd->dev, "Upstream entity is not a v4l2 subdev\n");
0129         return -ENODEV;
0130     }
0131
0132     upstream_sd = media_entity_to_v4l2_subdev(pad->entity);
0133
0134     return v4l2_subdev_call(upstream_sd, video, s_stream, enable);
0135 }
0136
0137 static const struct v4l2_subdev_video_ops video_mux_subdev_video_ops = {
0138     .s_stream = video_mux_s_stream,
0139 };
0140
0141 static struct v4l2_mbus_framefmt *
0142 __video_mux_get_pad_format(struct v4l2_subdev *sd,
0143                struct v4l2_subdev_state *sd_state,
0144                unsigned int pad, u32 which)
0145 {
0146     struct video_mux *vmux = v4l2_subdev_to_video_mux(sd);
0147
0148     switch (which) {
0149     case V4L2_SUBDEV_FORMAT_TRY:
0150         return v4l2_subdev_get_try_format(sd, sd_state, pad);
0151     case V4L2_SUBDEV_FORMAT_ACTIVE:
0152         return &vmux->format_mbus[pad];
0153     default:
0154         return NULL;
0155     }
0156 }
0157
0158 static int video_mux_get_format(struct v4l2_subdev *sd,
0159                 struct v4l2_subdev_state *sd_state,
0160                 struct v4l2_subdev_format *sdformat)
0161 {
0162     struct video_mux *vmux = v4l2_subdev_to_video_mux(sd);
0163
0164     mutex_lock(&vmux->lock);
0165
0166     sdformat->format = *__video_mux_get_pad_format(sd, sd_state,
0167                                sdformat->pad,
0168                                sdformat->which);
0169
0170     mutex_unlock(&vmux->lock);
0171
0172     return 0;
0173 }
0174
0175 static int video_mux_set_format(struct v4l2_subdev *sd,
0176                 struct v4l2_subdev_state *sd_state,
0177                 struct v4l2_subdev_format *sdformat)
0178 {
0179     struct video_mux *vmux = v4l2_subdev_to_video_mux(sd);
0180     struct v4l2_mbus_framefmt *mbusformat, *source_mbusformat;
0181     struct media_pad *pad = &vmux->pads[sdformat->pad];
0182     u16 source_pad = sd->entity.num_pads - 1;
0183
0184     mbusformat = __video_mux_get_pad_format(sd, sd_state, sdformat->pad,
0185                         sdformat->which);
0186     if (!mbusformat)
0187         return -EINVAL;
0188
0189     source_mbusformat = __video_mux_get_pad_format(sd, sd_state,
0190                                source_pad,
0191                                sdformat->which);
0192     if (!source_mbusformat)
0193         return -EINVAL;
0194
0195     /* No size limitations except V4L2 compliance requirements */
0196     v4l_bound_align_image(&sdformat->format.width, 1, 65536, 0,
0197                   &sdformat->format.height, 1, 65536, 0, 0);
0198
0199     /* All formats except LVDS and vendor specific formats are acceptable */
0200     switch (sdformat->format.code) {
0201     case MEDIA_BUS_FMT_RGB444_1X12:
0202     case MEDIA_BUS_FMT_RGB444_2X8_PADHI_BE:
0203     case MEDIA_BUS_FMT_RGB444_2X8_PADHI_LE:
0204     case MEDIA_BUS_FMT_RGB555_2X8_PADHI_BE:
0205     case MEDIA_BUS_FMT_RGB555_2X8_PADHI_LE:
0206     case MEDIA_BUS_FMT_RGB565_1X16:
0207     case MEDIA_BUS_FMT_BGR565_2X8_BE:
0208     case MEDIA_BUS_FMT_BGR565_2X8_LE:
0209     case MEDIA_BUS_FMT_RGB565_2X8_BE:
0210     case MEDIA_BUS_FMT_RGB565_2X8_LE:
0211     case MEDIA_BUS_FMT_RGB666_1X18:
0212     case MEDIA_BUS_FMT_RBG888_1X24:
0213     case MEDIA_BUS_FMT_RGB666_1X24_CPADHI:
0214     case MEDIA_BUS_FMT_BGR888_1X24:
0215     case MEDIA_BUS_FMT_GBR888_1X24:
0216     case MEDIA_BUS_FMT_RGB888_1X24:
0217     case MEDIA_BUS_FMT_RGB888_2X12_BE:
0218     case MEDIA_BUS_FMT_RGB888_2X12_LE:
0219     case MEDIA_BUS_FMT_ARGB8888_1X32:
0220     case MEDIA_BUS_FMT_RGB888_1X32_PADHI:
0221     case MEDIA_BUS_FMT_RGB101010_1X30:
0222     case MEDIA_BUS_FMT_RGB121212_1X36:
0223     case MEDIA_BUS_FMT_RGB161616_1X48:
0224     case MEDIA_BUS_FMT_Y8_1X8:
0225     case MEDIA_BUS_FMT_UV8_1X8:
0226     case MEDIA_BUS_FMT_UYVY8_1_5X8:
0227     case MEDIA_BUS_FMT_VYUY8_1_5X8:
0228     case MEDIA_BUS_FMT_YUYV8_1_5X8:
0229     case MEDIA_BUS_FMT_YVYU8_1_5X8:
0230     case MEDIA_BUS_FMT_UYVY8_2X8:
0231     case MEDIA_BUS_FMT_VYUY8_2X8:
0232     case MEDIA_BUS_FMT_YUYV8_2X8:
0233     case MEDIA_BUS_FMT_YVYU8_2X8:
0234     case MEDIA_BUS_FMT_Y10_1X10:
0235     case MEDIA_BUS_FMT_UYVY10_2X10:
0236     case MEDIA_BUS_FMT_VYUY10_2X10:
0237     case MEDIA_BUS_FMT_YUYV10_2X10:
0238     case MEDIA_BUS_FMT_YVYU10_2X10:
0239     case MEDIA_BUS_FMT_Y12_1X12:
0240     case MEDIA_BUS_FMT_UYVY12_2X12:
0241     case MEDIA_BUS_FMT_VYUY12_2X12:
0242     case MEDIA_BUS_FMT_YUYV12_2X12:
0243     case MEDIA_BUS_FMT_YVYU12_2X12:
0244     case MEDIA_BUS_FMT_UYVY8_1X16:
0245     case MEDIA_BUS_FMT_VYUY8_1X16:
0246     case MEDIA_BUS_FMT_YUYV8_1X16:
0247     case MEDIA_BUS_FMT_YVYU8_1X16:
0248     case MEDIA_BUS_FMT_YDYUYDYV8_1X16:
0249     case MEDIA_BUS_FMT_UYVY10_1X20:
0250     case MEDIA_BUS_FMT_VYUY10_1X20:
0251     case MEDIA_BUS_FMT_YUYV10_1X20:
0252     case MEDIA_BUS_FMT_YVYU10_1X20:
0253     case MEDIA_BUS_FMT_VUY8_1X24:
0254     case MEDIA_BUS_FMT_YUV8_1X24:
0255     case MEDIA_BUS_FMT_UYYVYY8_0_5X24:
0256     case MEDIA_BUS_FMT_UYVY12_1X24:
0257     case MEDIA_BUS_FMT_VYUY12_1X24:
0258     case MEDIA_BUS_FMT_YUYV12_1X24:
0259     case MEDIA_BUS_FMT_YVYU12_1X24:
0260     case MEDIA_BUS_FMT_YUV10_1X30:
0261     case MEDIA_BUS_FMT_UYYVYY10_0_5X30:
0262     case MEDIA_BUS_FMT_AYUV8_1X32:
0263     case MEDIA_BUS_FMT_UYYVYY12_0_5X36:
0264     case MEDIA_BUS_FMT_YUV12_1X36:
0265     case MEDIA_BUS_FMT_YUV16_1X48:
0266     case MEDIA_BUS_FMT_UYYVYY16_0_5X48:
0267     case MEDIA_BUS_FMT_JPEG_1X8:
0268     case MEDIA_BUS_FMT_AHSV8888_1X32:
0269     case MEDIA_BUS_FMT_SBGGR8_1X8:
0270     case MEDIA_BUS_FMT_SGBRG8_1X8:
0271     case MEDIA_BUS_FMT_SGRBG8_1X8:
0272     case MEDIA_BUS_FMT_SRGGB8_1X8:
0273     case MEDIA_BUS_FMT_SBGGR10_1X10:
0274     case MEDIA_BUS_FMT_SGBRG10_1X10:
0275     case MEDIA_BUS_FMT_SGRBG10_1X10:
0276     case MEDIA_BUS_FMT_SRGGB10_1X10:
0277     case MEDIA_BUS_FMT_SBGGR12_1X12:
0278     case MEDIA_BUS_FMT_SGBRG12_1X12:
0279     case MEDIA_BUS_FMT_SGRBG12_1X12:
0280     case MEDIA_BUS_FMT_SRGGB12_1X12:
0281     case MEDIA_BUS_FMT_SBGGR14_1X14:
0282     case MEDIA_BUS_FMT_SGBRG14_1X14:
0283     case MEDIA_BUS_FMT_SGRBG14_1X14:
0284     case MEDIA_BUS_FMT_SRGGB14_1X14:
0285     case MEDIA_BUS_FMT_SBGGR16_1X16:
0286     case MEDIA_BUS_FMT_SGBRG16_1X16:
0287     case MEDIA_BUS_FMT_SGRBG16_1X16:
0288     case MEDIA_BUS_FMT_SRGGB16_1X16:
0289         break;
0290     default:
0291         sdformat->format.code = MEDIA_BUS_FMT_Y8_1X8;
0292         break;
0293     }
0294     if (sdformat->format.field == V4L2_FIELD_ANY)
0295         sdformat->format.field = V4L2_FIELD_NONE;
0296
0297     mutex_lock(&vmux->lock);
0298
0299     /* Source pad mirrors active sink pad, no limitations on sink pads */
0300     if ((pad->flags & MEDIA_PAD_FL_SOURCE) && vmux->active >= 0)
0301         sdformat->format = vmux->format_mbus[vmux->active];
0302
0303     *mbusformat = sdformat->format;
0304
0305     /* Propagate the format from an active sink to source */
0306     if ((pad->flags & MEDIA_PAD_FL_SINK) && (pad->index == vmux->active))
0307         *source_mbusformat = sdformat->format;
0308
0309     mutex_unlock(&vmux->lock);
0310
0311     return 0;
0312 }
0313
0314 static int video_mux_init_cfg(struct v4l2_subdev *sd,
0315                   struct v4l2_subdev_state *sd_state)
0316 {
0317     struct video_mux *vmux = v4l2_subdev_to_video_mux(sd);
0318     struct v4l2_mbus_framefmt *mbusformat;
0319     unsigned int i;
0320
0321     mutex_lock(&vmux->lock);
0322
0323     for (i = 0; i < sd->entity.num_pads; i++) {
0324         mbusformat = v4l2_subdev_get_try_format(sd, sd_state, i);
0325         *mbusformat = video_mux_format_mbus_default;
0326     }
0327
0328     mutex_unlock(&vmux->lock);
0329
0330     return 0;
0331 }
0332
0333 static const struct v4l2_subdev_pad_ops video_mux_pad_ops = {
0334     .init_cfg = video_mux_init_cfg,
0335     .get_fmt = video_mux_get_format,
0336     .set_fmt = video_mux_set_format,
0337 };
0338
0339 static const struct v4l2_subdev_ops video_mux_subdev_ops = {
0340     .pad = &video_mux_pad_ops,
0341     .video = &video_mux_subdev_video_ops,
0342 };
0343
0344 static int video_mux_notify_bound(struct v4l2_async_notifier *notifier,
0345                   struct v4l2_subdev *sd,
0346                   struct v4l2_async_subdev *asd)
0347 {
0348     struct video_mux *vmux = notifier_to_video_mux(notifier);
0349
0350     return v4l2_create_fwnode_links(sd, &vmux->subdev);
0351 }
0352
0353 static const struct v4l2_async_notifier_operations video_mux_notify_ops = {
0354     .bound = video_mux_notify_bound,
0355 };
0356
0357 static int video_mux_async_register(struct video_mux *vmux,
0358                     unsigned int num_input_pads)
0359 {
0360     unsigned int i;
0361     int ret;
0362
0363     v4l2_async_nf_init(&vmux->notifier);
0364
0365     for (i = 0; i < num_input_pads; i++) {
0366         struct v4l2_async_subdev *asd;
0367         struct fwnode_handle *ep, *remote_ep;
0368
0369         ep = fwnode_graph_get_endpoint_by_id(
0370             dev_fwnode(vmux->subdev.dev), i, 0,
0371             FWNODE_GRAPH_ENDPOINT_NEXT);
0372         if (!ep)
0373             continue;
0374
0375         /* Skip dangling endpoints for backwards compatibility */
0376         remote_ep = fwnode_graph_get_remote_endpoint(ep);
0377         if (!remote_ep) {
0378             fwnode_handle_put(ep);
0379             continue;
0380         }
0381         fwnode_handle_put(remote_ep);
0382
0383         asd = v4l2_async_nf_add_fwnode_remote(&vmux->notifier, ep,
0384                               struct v4l2_async_subdev);
0385
0386         fwnode_handle_put(ep);
0387
0388         if (IS_ERR(asd)) {
0389             ret = PTR_ERR(asd);
0390             /* OK if asd already exists */
0391             if (ret != -EEXIST)
0392                 return ret;
0393         }
0394     }
0395
0396     vmux->notifier.ops = &video_mux_notify_ops;
0397
0398     ret = v4l2_async_subdev_nf_register(&vmux->subdev, &vmux->notifier);
0399     if (ret)
0400         return ret;
0401
0402     return v4l2_async_register_subdev(&vmux->subdev);
0403 }
0404
0405 static int video_mux_probe(struct platform_device *pdev)
0406 {
0407     struct device_node *np = pdev->dev.of_node;
0408     struct device *dev = &pdev->dev;
0409     struct device_node *ep;
0410     struct video_mux *vmux;
0411     unsigned int num_pads = 0;
0412     unsigned int i;
0413     int ret;
0414
0415     vmux = devm_kzalloc(dev, sizeof(*vmux), GFP_KERNEL);
0416     if (!vmux)
0417         return -ENOMEM;
0418
0419     platform_set_drvdata(pdev, vmux);
0420
0421     v4l2_subdev_init(&vmux->subdev, &video_mux_subdev_ops);
0422     snprintf(vmux->subdev.name, sizeof(vmux->subdev.name), "%pOFn", np);
0423     vmux->subdev.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
0424     vmux->subdev.dev = dev;
0425
0426     /*
0427      * The largest numbered port is the output port. It determines
0428      * total number of pads.
0429      */
0430     for_each_endpoint_of_node(np, ep) {
0431         struct of_endpoint endpoint;
0432
0433         of_graph_parse_endpoint(ep, &endpoint);
0434         num_pads = max(num_pads, endpoint.port + 1);
0435     }
0436
0437     if (num_pads < 2) {
0438         dev_err(dev, "Not enough ports %d\n", num_pads);
0439         return -EINVAL;
0440     }
0441
0442     vmux->mux = devm_mux_control_get(dev, NULL);
0443     if (IS_ERR(vmux->mux)) {
0444         ret = PTR_ERR(vmux->mux);
0445         return dev_err_probe(dev, ret, "Failed to get mux\n");
0446     }
0447
0448     mutex_init(&vmux->lock);
0449     vmux->active = -1;
0450     vmux->pads = devm_kcalloc(dev, num_pads, sizeof(*vmux->pads),
0451                   GFP_KERNEL);
0452     if (!vmux->pads)
0453         return -ENOMEM;
0454
0455     vmux->format_mbus = devm_kcalloc(dev, num_pads,
0456                      sizeof(*vmux->format_mbus),
0457                      GFP_KERNEL);
0458     if (!vmux->format_mbus)
0459         return -ENOMEM;
0460
0461     for (i = 0; i < num_pads; i++) {
0462         vmux->pads[i].flags = (i < num_pads - 1) ? MEDIA_PAD_FL_SINK
0463                              : MEDIA_PAD_FL_SOURCE;
0464         vmux->format_mbus[i] = video_mux_format_mbus_default;
0465     }
0466
0467     vmux->subdev.entity.function = MEDIA_ENT_F_VID_MUX;
0468     ret = media_entity_pads_init(&vmux->subdev.entity, num_pads,
0469                      vmux->pads);
0470     if (ret < 0)
0471         return ret;
0472
0473     vmux->subdev.entity.ops = &video_mux_ops;
0474
0475     ret = video_mux_async_register(vmux, num_pads - 1);
0476     if (ret) {
0477         v4l2_async_nf_unregister(&vmux->notifier);
0478         v4l2_async_nf_cleanup(&vmux->notifier);
0479     }
0480
0481     return ret;
0482 }
0483
0484 static int video_mux_remove(struct platform_device *pdev)
0485 {
0486     struct video_mux *vmux = platform_get_drvdata(pdev);
0487     struct v4l2_subdev *sd = &vmux->subdev;
0488
0489     v4l2_async_nf_unregister(&vmux->notifier);
0490     v4l2_async_nf_cleanup(&vmux->notifier);
0491     v4l2_async_unregister_subdev(sd);
0492     media_entity_cleanup(&sd->entity);
0493
0494     return 0;
0495 }
0496
0497 static const struct of_device_id video_mux_dt_ids[] = {
0498     { .compatible = "video-mux", },
0499     { /* sentinel */ }
0500 };
0501 MODULE_DEVICE_TABLE(of, video_mux_dt_ids);
0502
0503 static struct platform_driver video_mux_driver = {
0504     .probe      = video_mux_probe,
0505     .remove     = video_mux_remove,
0506     .driver     = {
0507         .of_match_table = video_mux_dt_ids,
0508         .name = "video-mux",
0509     },
0510 };
0511
0512 module_platform_driver(video_mux_driver);
0513
0514 MODULE_DESCRIPTION("video stream multiplexer");
0515 MODULE_AUTHOR("Sascha Hauer, Pengutronix");
0516 MODULE_AUTHOR("Philipp Zabel, Pengutronix");
0517 MODULE_LICENSE("GPL");