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 u16 source_pad = entity->num_pads - 1; 49 int ret = 0; 50 51 /* 52 * The mux state is determined by the enabled sink pad link. 53 * Enabling or disabling the source pad link has no effect. 54 */ 55 if (local->flags & MEDIA_PAD_FL_SOURCE) 56 return 0; 57 58 dev_dbg(sd->dev, "link setup '%s':%d->'%s':%d[%d]", 59 remote->entity->name, remote->index, local->entity->name, 60 local->index, flags & MEDIA_LNK_FL_ENABLED); 61 62 mutex_lock(&vmux->lock); 63 64 if (flags & MEDIA_LNK_FL_ENABLED) { 65 if (vmux->active == local->index) 66 goto out; 67 68 if (vmux->active >= 0) { 69 ret = -EBUSY; 70 goto out; 71 } 72 73 dev_dbg(sd->dev, "setting %d active\n", local->index); 74 ret = mux_control_try_select(vmux->mux, local->index); 75 if (ret < 0) 76 goto out; 77 vmux->active = local->index; 78 79 /* Propagate the active format to the source */ 80 vmux->format_mbus[source_pad] = vmux->format_mbus[vmux->active]; 81 } else { 82 if (vmux->active != local->index) 83 goto out; 84 85 dev_dbg(sd->dev, "going inactive\n"); 86 mux_control_deselect(vmux->mux); 87 vmux->active = -1; 88 } 89 90 out: 91 mutex_unlock(&vmux->lock); 92 return ret; 93 } 94 95 static const struct media_entity_operations video_mux_ops = { 96 .link_setup = video_mux_link_setup, 97 .link_validate = v4l2_subdev_link_validate, 98 }; 99 100 static int video_mux_s_stream(struct v4l2_subdev *sd, int enable) 101 { 102 struct video_mux *vmux = v4l2_subdev_to_video_mux(sd); 103 struct v4l2_subdev *upstream_sd; 104 struct media_pad *pad; 105 106 if (vmux->active == -1) { 107 dev_err(sd->dev, "Can not start streaming on inactive mux\n"); 108 return -EINVAL; 109 } 110 111 pad = media_entity_remote_pad(&sd->entity.pads[vmux->active]); 112 if (!pad) { 113 dev_err(sd->dev, "Failed to find remote source pad\n"); 114 return -ENOLINK; 115 } 116 117 if (!is_media_entity_v4l2_subdev(pad->entity)) { 118 dev_err(sd->dev, "Upstream entity is not a v4l2 subdev\n"); 119 return -ENODEV; 120 } 121 122 upstream_sd = media_entity_to_v4l2_subdev(pad->entity); 123 124 return v4l2_subdev_call(upstream_sd, video, s_stream, enable); 125 } 126 127 static const struct v4l2_subdev_video_ops video_mux_subdev_video_ops = { 128 .s_stream = video_mux_s_stream, 129 }; 130 131 static struct v4l2_mbus_framefmt * 132 __video_mux_get_pad_format(struct v4l2_subdev *sd, 133 struct v4l2_subdev_pad_config *cfg, 134 unsigned int pad, u32 which) 135 { 136 struct video_mux *vmux = v4l2_subdev_to_video_mux(sd); 137 138 switch (which) { 139 case V4L2_SUBDEV_FORMAT_TRY: 140 return v4l2_subdev_get_try_format(sd, cfg, pad); 141 case V4L2_SUBDEV_FORMAT_ACTIVE: 142 return &vmux->format_mbus[pad]; 143 default: 144 return NULL; 145 } 146 } 147 148 static int video_mux_get_format(struct v4l2_subdev *sd, 149 struct v4l2_subdev_pad_config *cfg, 150 struct v4l2_subdev_format *sdformat) 151 { 152 struct video_mux *vmux = v4l2_subdev_to_video_mux(sd); 153 154 mutex_lock(&vmux->lock); 155 156 sdformat->format = *__video_mux_get_pad_format(sd, cfg, sdformat->pad, 157 sdformat->which); 158 159 mutex_unlock(&vmux->lock); 160 161 return 0; 162 } 163 164 static int video_mux_set_format(struct v4l2_subdev *sd, 165 struct v4l2_subdev_pad_config *cfg, 166 struct v4l2_subdev_format *sdformat) 167 { 168 struct video_mux *vmux = v4l2_subdev_to_video_mux(sd); 169 struct v4l2_mbus_framefmt *mbusformat, *source_mbusformat; 170 struct media_pad *pad = &vmux->pads[sdformat->pad]; 171 u16 source_pad = sd->entity.num_pads - 1; 172 173 mbusformat = __video_mux_get_pad_format(sd, cfg, sdformat->pad, 174 sdformat->which); 175 if (!mbusformat) 176 return -EINVAL; 177 178 source_mbusformat = __video_mux_get_pad_format(sd, cfg, source_pad, 179 sdformat->which); 180 if (!source_mbusformat) 181 return -EINVAL; 182 183 mutex_lock(&vmux->lock); 184 185 /* Source pad mirrors active sink pad, no limitations on sink pads */ 186 if ((pad->flags & MEDIA_PAD_FL_SOURCE) && vmux->active >= 0) 187 sdformat->format = vmux->format_mbus[vmux->active]; 188 189 *mbusformat = sdformat->format; 190 191 /* Propagate the format from an active sink to source */ 192 if ((pad->flags & MEDIA_PAD_FL_SINK) && (pad->index == vmux->active)) 193 *source_mbusformat = sdformat->format; 194 195 mutex_unlock(&vmux->lock); 196 197 return 0; 198 } 199 200 static const struct v4l2_subdev_pad_ops video_mux_pad_ops = { 201 .get_fmt = video_mux_get_format, 202 .set_fmt = video_mux_set_format, 203 }; 204 205 static const struct v4l2_subdev_ops video_mux_subdev_ops = { 206 .pad = &video_mux_pad_ops, 207 .video = &video_mux_subdev_video_ops, 208 }; 209 210 static int video_mux_probe(struct platform_device *pdev) 211 { 212 struct device_node *np = pdev->dev.of_node; 213 struct device *dev = &pdev->dev; 214 struct device_node *ep; 215 struct video_mux *vmux; 216 unsigned int num_pads = 0; 217 int ret; 218 int i; 219 220 vmux = devm_kzalloc(dev, sizeof(*vmux), GFP_KERNEL); 221 if (!vmux) 222 return -ENOMEM; 223 224 platform_set_drvdata(pdev, vmux); 225 226 v4l2_subdev_init(&vmux->subdev, &video_mux_subdev_ops); 227 snprintf(vmux->subdev.name, sizeof(vmux->subdev.name), "%s", np->name); 228 vmux->subdev.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE; 229 vmux->subdev.dev = dev; 230 231 /* 232 * The largest numbered port is the output port. It determines 233 * total number of pads. 234 */ 235 for_each_endpoint_of_node(np, ep) { 236 struct of_endpoint endpoint; 237 238 of_graph_parse_endpoint(ep, &endpoint); 239 num_pads = max(num_pads, endpoint.port + 1); 240 } 241 242 if (num_pads < 2) { 243 dev_err(dev, "Not enough ports %d\n", num_pads); 244 return -EINVAL; 245 } 246 247 vmux->mux = devm_mux_control_get(dev, NULL); 248 if (IS_ERR(vmux->mux)) { 249 ret = PTR_ERR(vmux->mux); 250 if (ret != -EPROBE_DEFER) 251 dev_err(dev, "Failed to get mux: %d\n", ret); 252 return ret; 253 } 254 255 mutex_init(&vmux->lock); 256 vmux->active = -1; 257 vmux->pads = devm_kcalloc(dev, num_pads, sizeof(*vmux->pads), 258 GFP_KERNEL); 259 vmux->format_mbus = devm_kcalloc(dev, num_pads, 260 sizeof(*vmux->format_mbus), 261 GFP_KERNEL); 262 263 for (i = 0; i < num_pads - 1; i++) 264 vmux->pads[i].flags = MEDIA_PAD_FL_SINK; 265 vmux->pads[num_pads - 1].flags = MEDIA_PAD_FL_SOURCE; 266 267 vmux->subdev.entity.function = MEDIA_ENT_F_VID_MUX; 268 ret = media_entity_pads_init(&vmux->subdev.entity, num_pads, 269 vmux->pads); 270 if (ret < 0) 271 return ret; 272 273 vmux->subdev.entity.ops = &video_mux_ops; 274 275 return v4l2_async_register_subdev(&vmux->subdev); 276 } 277 278 static int video_mux_remove(struct platform_device *pdev) 279 { 280 struct video_mux *vmux = platform_get_drvdata(pdev); 281 struct v4l2_subdev *sd = &vmux->subdev; 282 283 v4l2_async_unregister_subdev(sd); 284 media_entity_cleanup(&sd->entity); 285 286 return 0; 287 } 288 289 static const struct of_device_id video_mux_dt_ids[] = { 290 { .compatible = "video-mux", }, 291 { /* sentinel */ } 292 }; 293 MODULE_DEVICE_TABLE(of, video_mux_dt_ids); 294 295 static struct platform_driver video_mux_driver = { 296 .probe = video_mux_probe, 297 .remove = video_mux_remove, 298 .driver = { 299 .of_match_table = video_mux_dt_ids, 300 .name = "video-mux", 301 }, 302 }; 303 304 module_platform_driver(video_mux_driver); 305 306 MODULE_DESCRIPTION("video stream multiplexer"); 307 MODULE_AUTHOR("Sascha Hauer, Pengutronix"); 308 MODULE_AUTHOR("Philipp Zabel, Pengutronix"); 309 MODULE_LICENSE("GPL"); 310