1 /* 2 * video stream multiplexer controlled via mux control 3 * 4 * Copyright (C) 2013 Pengutronix, Sascha Hauer <kernel@pengutronix.de> 5 * Copyright (C) 2016-2017 Pengutronix, Philipp Zabel <kernel@pengutronix.de> 6 * 7 * This program is free software; you can redistribute it and/or 8 * modify it under the terms of the GNU General Public License 9 * as published by the Free Software Foundation; either version 2 10 * of the License, or (at your option) any later version. 11 * This program is distributed in the hope that it will be useful, 12 * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 * GNU General Public License for more details. 15 */ 16 17 #include <linux/err.h> 18 #include <linux/module.h> 19 #include <linux/mutex.h> 20 #include <linux/mux/consumer.h> 21 #include <linux/of.h> 22 #include <linux/of_graph.h> 23 #include <linux/platform_device.h> 24 #include <media/v4l2-async.h> 25 #include <media/v4l2-device.h> 26 #include <media/v4l2-subdev.h> 27 28 struct video_mux { 29 struct v4l2_subdev subdev; 30 struct media_pad *pads; 31 struct v4l2_mbus_framefmt *format_mbus; 32 struct mux_control *mux; 33 struct mutex lock; 34 int active; 35 }; 36 37 static inline struct video_mux *v4l2_subdev_to_video_mux(struct v4l2_subdev *sd) 38 { 39 return container_of(sd, struct video_mux, subdev); 40 } 41 42 static int video_mux_link_setup(struct media_entity *entity, 43 const struct media_pad *local, 44 const struct media_pad *remote, u32 flags) 45 { 46 struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity); 47 struct video_mux *vmux = v4l2_subdev_to_video_mux(sd); 48 int ret = 0; 49 50 /* 51 * The mux state is determined by the enabled sink pad link. 52 * Enabling or disabling the source pad link has no effect. 53 */ 54 if (local->flags & MEDIA_PAD_FL_SOURCE) 55 return 0; 56 57 dev_dbg(sd->dev, "link setup '%s':%d->'%s':%d[%d]", 58 remote->entity->name, remote->index, local->entity->name, 59 local->index, flags & MEDIA_LNK_FL_ENABLED); 60 61 mutex_lock(&vmux->lock); 62 63 if (flags & MEDIA_LNK_FL_ENABLED) { 64 if (vmux->active == local->index) 65 goto out; 66 67 if (vmux->active >= 0) { 68 ret = -EBUSY; 69 goto out; 70 } 71 72 dev_dbg(sd->dev, "setting %d active\n", local->index); 73 ret = mux_control_try_select(vmux->mux, local->index); 74 if (ret < 0) 75 goto out; 76 vmux->active = local->index; 77 } else { 78 if (vmux->active != local->index) 79 goto out; 80 81 dev_dbg(sd->dev, "going inactive\n"); 82 mux_control_deselect(vmux->mux); 83 vmux->active = -1; 84 } 85 86 out: 87 mutex_unlock(&vmux->lock); 88 return ret; 89 } 90 91 static const struct media_entity_operations video_mux_ops = { 92 .link_setup = video_mux_link_setup, 93 .link_validate = v4l2_subdev_link_validate, 94 }; 95 96 static int video_mux_s_stream(struct v4l2_subdev *sd, int enable) 97 { 98 struct video_mux *vmux = v4l2_subdev_to_video_mux(sd); 99 struct v4l2_subdev *upstream_sd; 100 struct media_pad *pad; 101 102 if (vmux->active == -1) { 103 dev_err(sd->dev, "Can not start streaming on inactive mux\n"); 104 return -EINVAL; 105 } 106 107 pad = media_entity_remote_pad(&sd->entity.pads[vmux->active]); 108 if (!pad) { 109 dev_err(sd->dev, "Failed to find remote source pad\n"); 110 return -ENOLINK; 111 } 112 113 if (!is_media_entity_v4l2_subdev(pad->entity)) { 114 dev_err(sd->dev, "Upstream entity is not a v4l2 subdev\n"); 115 return -ENODEV; 116 } 117 118 upstream_sd = media_entity_to_v4l2_subdev(pad->entity); 119 120 return v4l2_subdev_call(upstream_sd, video, s_stream, enable); 121 } 122 123 static const struct v4l2_subdev_video_ops video_mux_subdev_video_ops = { 124 .s_stream = video_mux_s_stream, 125 }; 126 127 static struct v4l2_mbus_framefmt * 128 __video_mux_get_pad_format(struct v4l2_subdev *sd, 129 struct v4l2_subdev_pad_config *cfg, 130 unsigned int pad, u32 which) 131 { 132 struct video_mux *vmux = v4l2_subdev_to_video_mux(sd); 133 134 switch (which) { 135 case V4L2_SUBDEV_FORMAT_TRY: 136 return v4l2_subdev_get_try_format(sd, cfg, pad); 137 case V4L2_SUBDEV_FORMAT_ACTIVE: 138 return &vmux->format_mbus[pad]; 139 default: 140 return NULL; 141 } 142 } 143 144 static int video_mux_get_format(struct v4l2_subdev *sd, 145 struct v4l2_subdev_pad_config *cfg, 146 struct v4l2_subdev_format *sdformat) 147 { 148 struct video_mux *vmux = v4l2_subdev_to_video_mux(sd); 149 150 mutex_lock(&vmux->lock); 151 152 sdformat->format = *__video_mux_get_pad_format(sd, cfg, sdformat->pad, 153 sdformat->which); 154 155 mutex_unlock(&vmux->lock); 156 157 return 0; 158 } 159 160 static int video_mux_set_format(struct v4l2_subdev *sd, 161 struct v4l2_subdev_pad_config *cfg, 162 struct v4l2_subdev_format *sdformat) 163 { 164 struct video_mux *vmux = v4l2_subdev_to_video_mux(sd); 165 struct v4l2_mbus_framefmt *mbusformat; 166 struct media_pad *pad = &vmux->pads[sdformat->pad]; 167 168 mbusformat = __video_mux_get_pad_format(sd, cfg, sdformat->pad, 169 sdformat->which); 170 if (!mbusformat) 171 return -EINVAL; 172 173 mutex_lock(&vmux->lock); 174 175 /* Source pad mirrors active sink pad, no limitations on sink pads */ 176 if ((pad->flags & MEDIA_PAD_FL_SOURCE) && vmux->active >= 0) 177 sdformat->format = vmux->format_mbus[vmux->active]; 178 179 *mbusformat = sdformat->format; 180 181 mutex_unlock(&vmux->lock); 182 183 return 0; 184 } 185 186 static const struct v4l2_subdev_pad_ops video_mux_pad_ops = { 187 .get_fmt = video_mux_get_format, 188 .set_fmt = video_mux_set_format, 189 }; 190 191 static const struct v4l2_subdev_ops video_mux_subdev_ops = { 192 .pad = &video_mux_pad_ops, 193 .video = &video_mux_subdev_video_ops, 194 }; 195 196 static int video_mux_probe(struct platform_device *pdev) 197 { 198 struct device_node *np = pdev->dev.of_node; 199 struct device *dev = &pdev->dev; 200 struct device_node *ep; 201 struct video_mux *vmux; 202 unsigned int num_pads = 0; 203 int ret; 204 int i; 205 206 vmux = devm_kzalloc(dev, sizeof(*vmux), GFP_KERNEL); 207 if (!vmux) 208 return -ENOMEM; 209 210 platform_set_drvdata(pdev, vmux); 211 212 v4l2_subdev_init(&vmux->subdev, &video_mux_subdev_ops); 213 snprintf(vmux->subdev.name, sizeof(vmux->subdev.name), "%s", np->name); 214 vmux->subdev.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE; 215 vmux->subdev.dev = dev; 216 217 /* 218 * The largest numbered port is the output port. It determines 219 * total number of pads. 220 */ 221 for_each_endpoint_of_node(np, ep) { 222 struct of_endpoint endpoint; 223 224 of_graph_parse_endpoint(ep, &endpoint); 225 num_pads = max(num_pads, endpoint.port + 1); 226 } 227 228 if (num_pads < 2) { 229 dev_err(dev, "Not enough ports %d\n", num_pads); 230 return -EINVAL; 231 } 232 233 vmux->mux = devm_mux_control_get(dev, NULL); 234 if (IS_ERR(vmux->mux)) { 235 ret = PTR_ERR(vmux->mux); 236 if (ret != -EPROBE_DEFER) 237 dev_err(dev, "Failed to get mux: %d\n", ret); 238 return ret; 239 } 240 241 mutex_init(&vmux->lock); 242 vmux->active = -1; 243 vmux->pads = devm_kcalloc(dev, num_pads, sizeof(*vmux->pads), 244 GFP_KERNEL); 245 vmux->format_mbus = devm_kcalloc(dev, num_pads, 246 sizeof(*vmux->format_mbus), 247 GFP_KERNEL); 248 249 for (i = 0; i < num_pads - 1; i++) 250 vmux->pads[i].flags = MEDIA_PAD_FL_SINK; 251 vmux->pads[num_pads - 1].flags = MEDIA_PAD_FL_SOURCE; 252 253 vmux->subdev.entity.function = MEDIA_ENT_F_VID_MUX; 254 ret = media_entity_pads_init(&vmux->subdev.entity, num_pads, 255 vmux->pads); 256 if (ret < 0) 257 return ret; 258 259 vmux->subdev.entity.ops = &video_mux_ops; 260 261 return v4l2_async_register_subdev(&vmux->subdev); 262 } 263 264 static int video_mux_remove(struct platform_device *pdev) 265 { 266 struct video_mux *vmux = platform_get_drvdata(pdev); 267 struct v4l2_subdev *sd = &vmux->subdev; 268 269 v4l2_async_unregister_subdev(sd); 270 media_entity_cleanup(&sd->entity); 271 272 return 0; 273 } 274 275 static const struct of_device_id video_mux_dt_ids[] = { 276 { .compatible = "video-mux", }, 277 { /* sentinel */ } 278 }; 279 MODULE_DEVICE_TABLE(of, video_mux_dt_ids); 280 281 static struct platform_driver video_mux_driver = { 282 .probe = video_mux_probe, 283 .remove = video_mux_remove, 284 .driver = { 285 .of_match_table = video_mux_dt_ids, 286 .name = "video-mux", 287 }, 288 }; 289 290 module_platform_driver(video_mux_driver); 291 292 MODULE_DESCRIPTION("video stream multiplexer"); 293 MODULE_AUTHOR("Sascha Hauer, Pengutronix"); 294 MODULE_AUTHOR("Philipp Zabel, Pengutronix"); 295 MODULE_LICENSE("GPL"); 296