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 const struct v4l2_mbus_framefmt video_mux_format_mbus_default = { 38 .width = 1, 39 .height = 1, 40 .code = MEDIA_BUS_FMT_Y8_1X8, 41 .field = V4L2_FIELD_NONE, 42 }; 43 44 static inline struct video_mux *v4l2_subdev_to_video_mux(struct v4l2_subdev *sd) 45 { 46 return container_of(sd, struct video_mux, subdev); 47 } 48 49 static int video_mux_link_setup(struct media_entity *entity, 50 const struct media_pad *local, 51 const struct media_pad *remote, u32 flags) 52 { 53 struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity); 54 struct video_mux *vmux = v4l2_subdev_to_video_mux(sd); 55 u16 source_pad = entity->num_pads - 1; 56 int ret = 0; 57 58 /* 59 * The mux state is determined by the enabled sink pad link. 60 * Enabling or disabling the source pad link has no effect. 61 */ 62 if (local->flags & MEDIA_PAD_FL_SOURCE) 63 return 0; 64 65 dev_dbg(sd->dev, "link setup '%s':%d->'%s':%d[%d]", 66 remote->entity->name, remote->index, local->entity->name, 67 local->index, flags & MEDIA_LNK_FL_ENABLED); 68 69 mutex_lock(&vmux->lock); 70 71 if (flags & MEDIA_LNK_FL_ENABLED) { 72 if (vmux->active == local->index) 73 goto out; 74 75 if (vmux->active >= 0) { 76 ret = -EBUSY; 77 goto out; 78 } 79 80 dev_dbg(sd->dev, "setting %d active\n", local->index); 81 ret = mux_control_try_select(vmux->mux, local->index); 82 if (ret < 0) 83 goto out; 84 vmux->active = local->index; 85 86 /* Propagate the active format to the source */ 87 vmux->format_mbus[source_pad] = vmux->format_mbus[vmux->active]; 88 } else { 89 if (vmux->active != local->index) 90 goto out; 91 92 dev_dbg(sd->dev, "going inactive\n"); 93 mux_control_deselect(vmux->mux); 94 vmux->active = -1; 95 } 96 97 out: 98 mutex_unlock(&vmux->lock); 99 return ret; 100 } 101 102 static const struct media_entity_operations video_mux_ops = { 103 .link_setup = video_mux_link_setup, 104 .link_validate = v4l2_subdev_link_validate, 105 }; 106 107 static int video_mux_s_stream(struct v4l2_subdev *sd, int enable) 108 { 109 struct video_mux *vmux = v4l2_subdev_to_video_mux(sd); 110 struct v4l2_subdev *upstream_sd; 111 struct media_pad *pad; 112 113 if (vmux->active == -1) { 114 dev_err(sd->dev, "Can not start streaming on inactive mux\n"); 115 return -EINVAL; 116 } 117 118 pad = media_entity_remote_pad(&sd->entity.pads[vmux->active]); 119 if (!pad) { 120 dev_err(sd->dev, "Failed to find remote source pad\n"); 121 return -ENOLINK; 122 } 123 124 if (!is_media_entity_v4l2_subdev(pad->entity)) { 125 dev_err(sd->dev, "Upstream entity is not a v4l2 subdev\n"); 126 return -ENODEV; 127 } 128 129 upstream_sd = media_entity_to_v4l2_subdev(pad->entity); 130 131 return v4l2_subdev_call(upstream_sd, video, s_stream, enable); 132 } 133 134 static const struct v4l2_subdev_video_ops video_mux_subdev_video_ops = { 135 .s_stream = video_mux_s_stream, 136 }; 137 138 static struct v4l2_mbus_framefmt * 139 __video_mux_get_pad_format(struct v4l2_subdev *sd, 140 struct v4l2_subdev_pad_config *cfg, 141 unsigned int pad, u32 which) 142 { 143 struct video_mux *vmux = v4l2_subdev_to_video_mux(sd); 144 145 switch (which) { 146 case V4L2_SUBDEV_FORMAT_TRY: 147 return v4l2_subdev_get_try_format(sd, cfg, pad); 148 case V4L2_SUBDEV_FORMAT_ACTIVE: 149 return &vmux->format_mbus[pad]; 150 default: 151 return NULL; 152 } 153 } 154 155 static int video_mux_get_format(struct v4l2_subdev *sd, 156 struct v4l2_subdev_pad_config *cfg, 157 struct v4l2_subdev_format *sdformat) 158 { 159 struct video_mux *vmux = v4l2_subdev_to_video_mux(sd); 160 161 mutex_lock(&vmux->lock); 162 163 sdformat->format = *__video_mux_get_pad_format(sd, cfg, sdformat->pad, 164 sdformat->which); 165 166 mutex_unlock(&vmux->lock); 167 168 return 0; 169 } 170 171 static int video_mux_set_format(struct v4l2_subdev *sd, 172 struct v4l2_subdev_pad_config *cfg, 173 struct v4l2_subdev_format *sdformat) 174 { 175 struct video_mux *vmux = v4l2_subdev_to_video_mux(sd); 176 struct v4l2_mbus_framefmt *mbusformat, *source_mbusformat; 177 struct media_pad *pad = &vmux->pads[sdformat->pad]; 178 u16 source_pad = sd->entity.num_pads - 1; 179 180 mbusformat = __video_mux_get_pad_format(sd, cfg, sdformat->pad, 181 sdformat->which); 182 if (!mbusformat) 183 return -EINVAL; 184 185 source_mbusformat = __video_mux_get_pad_format(sd, cfg, source_pad, 186 sdformat->which); 187 if (!source_mbusformat) 188 return -EINVAL; 189 190 /* No size limitations except V4L2 compliance requirements */ 191 v4l_bound_align_image(&sdformat->format.width, 1, 65536, 0, 192 &sdformat->format.height, 1, 65536, 0, 0); 193 194 /* All formats except LVDS and vendor specific formats are acceptable */ 195 switch (sdformat->format.code) { 196 case MEDIA_BUS_FMT_RGB444_1X12: 197 case MEDIA_BUS_FMT_RGB444_2X8_PADHI_BE: 198 case MEDIA_BUS_FMT_RGB444_2X8_PADHI_LE: 199 case MEDIA_BUS_FMT_RGB555_2X8_PADHI_BE: 200 case MEDIA_BUS_FMT_RGB555_2X8_PADHI_LE: 201 case MEDIA_BUS_FMT_RGB565_1X16: 202 case MEDIA_BUS_FMT_BGR565_2X8_BE: 203 case MEDIA_BUS_FMT_BGR565_2X8_LE: 204 case MEDIA_BUS_FMT_RGB565_2X8_BE: 205 case MEDIA_BUS_FMT_RGB565_2X8_LE: 206 case MEDIA_BUS_FMT_RGB666_1X18: 207 case MEDIA_BUS_FMT_RBG888_1X24: 208 case MEDIA_BUS_FMT_RGB666_1X24_CPADHI: 209 case MEDIA_BUS_FMT_BGR888_1X24: 210 case MEDIA_BUS_FMT_GBR888_1X24: 211 case MEDIA_BUS_FMT_RGB888_1X24: 212 case MEDIA_BUS_FMT_RGB888_2X12_BE: 213 case MEDIA_BUS_FMT_RGB888_2X12_LE: 214 case MEDIA_BUS_FMT_ARGB8888_1X32: 215 case MEDIA_BUS_FMT_RGB888_1X32_PADHI: 216 case MEDIA_BUS_FMT_RGB101010_1X30: 217 case MEDIA_BUS_FMT_RGB121212_1X36: 218 case MEDIA_BUS_FMT_RGB161616_1X48: 219 case MEDIA_BUS_FMT_Y8_1X8: 220 case MEDIA_BUS_FMT_UV8_1X8: 221 case MEDIA_BUS_FMT_UYVY8_1_5X8: 222 case MEDIA_BUS_FMT_VYUY8_1_5X8: 223 case MEDIA_BUS_FMT_YUYV8_1_5X8: 224 case MEDIA_BUS_FMT_YVYU8_1_5X8: 225 case MEDIA_BUS_FMT_UYVY8_2X8: 226 case MEDIA_BUS_FMT_VYUY8_2X8: 227 case MEDIA_BUS_FMT_YUYV8_2X8: 228 case MEDIA_BUS_FMT_YVYU8_2X8: 229 case MEDIA_BUS_FMT_Y10_1X10: 230 case MEDIA_BUS_FMT_UYVY10_2X10: 231 case MEDIA_BUS_FMT_VYUY10_2X10: 232 case MEDIA_BUS_FMT_YUYV10_2X10: 233 case MEDIA_BUS_FMT_YVYU10_2X10: 234 case MEDIA_BUS_FMT_Y12_1X12: 235 case MEDIA_BUS_FMT_UYVY12_2X12: 236 case MEDIA_BUS_FMT_VYUY12_2X12: 237 case MEDIA_BUS_FMT_YUYV12_2X12: 238 case MEDIA_BUS_FMT_YVYU12_2X12: 239 case MEDIA_BUS_FMT_UYVY8_1X16: 240 case MEDIA_BUS_FMT_VYUY8_1X16: 241 case MEDIA_BUS_FMT_YUYV8_1X16: 242 case MEDIA_BUS_FMT_YVYU8_1X16: 243 case MEDIA_BUS_FMT_YDYUYDYV8_1X16: 244 case MEDIA_BUS_FMT_UYVY10_1X20: 245 case MEDIA_BUS_FMT_VYUY10_1X20: 246 case MEDIA_BUS_FMT_YUYV10_1X20: 247 case MEDIA_BUS_FMT_YVYU10_1X20: 248 case MEDIA_BUS_FMT_VUY8_1X24: 249 case MEDIA_BUS_FMT_YUV8_1X24: 250 case MEDIA_BUS_FMT_UYYVYY8_0_5X24: 251 case MEDIA_BUS_FMT_UYVY12_1X24: 252 case MEDIA_BUS_FMT_VYUY12_1X24: 253 case MEDIA_BUS_FMT_YUYV12_1X24: 254 case MEDIA_BUS_FMT_YVYU12_1X24: 255 case MEDIA_BUS_FMT_YUV10_1X30: 256 case MEDIA_BUS_FMT_UYYVYY10_0_5X30: 257 case MEDIA_BUS_FMT_AYUV8_1X32: 258 case MEDIA_BUS_FMT_UYYVYY12_0_5X36: 259 case MEDIA_BUS_FMT_YUV12_1X36: 260 case MEDIA_BUS_FMT_YUV16_1X48: 261 case MEDIA_BUS_FMT_UYYVYY16_0_5X48: 262 case MEDIA_BUS_FMT_JPEG_1X8: 263 case MEDIA_BUS_FMT_AHSV8888_1X32: 264 break; 265 default: 266 sdformat->format.code = MEDIA_BUS_FMT_Y8_1X8; 267 break; 268 } 269 if (sdformat->format.field == V4L2_FIELD_ANY) 270 sdformat->format.field = V4L2_FIELD_NONE; 271 272 mutex_lock(&vmux->lock); 273 274 /* Source pad mirrors active sink pad, no limitations on sink pads */ 275 if ((pad->flags & MEDIA_PAD_FL_SOURCE) && vmux->active >= 0) 276 sdformat->format = vmux->format_mbus[vmux->active]; 277 278 *mbusformat = sdformat->format; 279 280 /* Propagate the format from an active sink to source */ 281 if ((pad->flags & MEDIA_PAD_FL_SINK) && (pad->index == vmux->active)) 282 *source_mbusformat = sdformat->format; 283 284 mutex_unlock(&vmux->lock); 285 286 return 0; 287 } 288 289 static int video_mux_init_cfg(struct v4l2_subdev *sd, 290 struct v4l2_subdev_pad_config *cfg) 291 { 292 struct video_mux *vmux = v4l2_subdev_to_video_mux(sd); 293 struct v4l2_mbus_framefmt *mbusformat; 294 unsigned int i; 295 296 mutex_lock(&vmux->lock); 297 298 for (i = 0; i < sd->entity.num_pads; i++) { 299 mbusformat = v4l2_subdev_get_try_format(sd, cfg, i); 300 *mbusformat = video_mux_format_mbus_default; 301 } 302 303 mutex_unlock(&vmux->lock); 304 305 return 0; 306 } 307 308 static const struct v4l2_subdev_pad_ops video_mux_pad_ops = { 309 .init_cfg = video_mux_init_cfg, 310 .get_fmt = video_mux_get_format, 311 .set_fmt = video_mux_set_format, 312 }; 313 314 static const struct v4l2_subdev_ops video_mux_subdev_ops = { 315 .pad = &video_mux_pad_ops, 316 .video = &video_mux_subdev_video_ops, 317 }; 318 319 static int video_mux_probe(struct platform_device *pdev) 320 { 321 struct device_node *np = pdev->dev.of_node; 322 struct device *dev = &pdev->dev; 323 struct device_node *ep; 324 struct video_mux *vmux; 325 unsigned int num_pads = 0; 326 unsigned int i; 327 int ret; 328 329 vmux = devm_kzalloc(dev, sizeof(*vmux), GFP_KERNEL); 330 if (!vmux) 331 return -ENOMEM; 332 333 platform_set_drvdata(pdev, vmux); 334 335 v4l2_subdev_init(&vmux->subdev, &video_mux_subdev_ops); 336 snprintf(vmux->subdev.name, sizeof(vmux->subdev.name), "%pOFn", np); 337 vmux->subdev.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE; 338 vmux->subdev.dev = dev; 339 340 /* 341 * The largest numbered port is the output port. It determines 342 * total number of pads. 343 */ 344 for_each_endpoint_of_node(np, ep) { 345 struct of_endpoint endpoint; 346 347 of_graph_parse_endpoint(ep, &endpoint); 348 num_pads = max(num_pads, endpoint.port + 1); 349 } 350 351 if (num_pads < 2) { 352 dev_err(dev, "Not enough ports %d\n", num_pads); 353 return -EINVAL; 354 } 355 356 vmux->mux = devm_mux_control_get(dev, NULL); 357 if (IS_ERR(vmux->mux)) { 358 ret = PTR_ERR(vmux->mux); 359 if (ret != -EPROBE_DEFER) 360 dev_err(dev, "Failed to get mux: %d\n", ret); 361 return ret; 362 } 363 364 mutex_init(&vmux->lock); 365 vmux->active = -1; 366 vmux->pads = devm_kcalloc(dev, num_pads, sizeof(*vmux->pads), 367 GFP_KERNEL); 368 vmux->format_mbus = devm_kcalloc(dev, num_pads, 369 sizeof(*vmux->format_mbus), 370 GFP_KERNEL); 371 372 for (i = 0; i < num_pads; i++) { 373 vmux->pads[i].flags = (i < num_pads - 1) ? MEDIA_PAD_FL_SINK 374 : MEDIA_PAD_FL_SOURCE; 375 vmux->format_mbus[i] = video_mux_format_mbus_default; 376 } 377 378 vmux->subdev.entity.function = MEDIA_ENT_F_VID_MUX; 379 ret = media_entity_pads_init(&vmux->subdev.entity, num_pads, 380 vmux->pads); 381 if (ret < 0) 382 return ret; 383 384 vmux->subdev.entity.ops = &video_mux_ops; 385 386 return v4l2_async_register_subdev(&vmux->subdev); 387 } 388 389 static int video_mux_remove(struct platform_device *pdev) 390 { 391 struct video_mux *vmux = platform_get_drvdata(pdev); 392 struct v4l2_subdev *sd = &vmux->subdev; 393 394 v4l2_async_unregister_subdev(sd); 395 media_entity_cleanup(&sd->entity); 396 397 return 0; 398 } 399 400 static const struct of_device_id video_mux_dt_ids[] = { 401 { .compatible = "video-mux", }, 402 { /* sentinel */ } 403 }; 404 MODULE_DEVICE_TABLE(of, video_mux_dt_ids); 405 406 static struct platform_driver video_mux_driver = { 407 .probe = video_mux_probe, 408 .remove = video_mux_remove, 409 .driver = { 410 .of_match_table = video_mux_dt_ids, 411 .name = "video-mux", 412 }, 413 }; 414 415 module_platform_driver(video_mux_driver); 416 417 MODULE_DESCRIPTION("video stream multiplexer"); 418 MODULE_AUTHOR("Sascha Hauer, Pengutronix"); 419 MODULE_AUTHOR("Philipp Zabel, Pengutronix"); 420 MODULE_LICENSE("GPL"); 421