1 // SPDX-License-Identifier: GPL-2.0-or-later 2 /* 3 * video stream multiplexer controlled via mux control 4 * 5 * Copyright (C) 2013 Pengutronix, Sascha Hauer <kernel@pengutronix.de> 6 * Copyright (C) 2016-2017 Pengutronix, Philipp Zabel <kernel@pengutronix.de> 7 */ 8 9 #include <linux/err.h> 10 #include <linux/module.h> 11 #include <linux/mutex.h> 12 #include <linux/mux/consumer.h> 13 #include <linux/of.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> 22 23 struct video_mux { 24 struct v4l2_subdev subdev; 25 struct v4l2_async_notifier notifier; 26 struct media_pad *pads; 27 struct mux_control *mux; 28 struct mutex lock; 29 int active; 30 }; 31 32 static const struct v4l2_mbus_framefmt video_mux_format_mbus_default = { 33 .width = 1, 34 .height = 1, 35 .code = MEDIA_BUS_FMT_Y8_1X8, 36 .field = V4L2_FIELD_NONE, 37 }; 38 39 static inline struct video_mux * 40 notifier_to_video_mux(struct v4l2_async_notifier *n) 41 { 42 return container_of(n, struct video_mux, notifier); 43 } 44 45 static inline struct video_mux *v4l2_subdev_to_video_mux(struct v4l2_subdev *sd) 46 { 47 return container_of(sd, struct video_mux, subdev); 48 } 49 50 static int video_mux_link_setup(struct media_entity *entity, 51 const struct media_pad *local, 52 const struct media_pad *remote, u32 flags) 53 { 54 struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity); 55 struct video_mux *vmux = v4l2_subdev_to_video_mux(sd); 56 u16 source_pad = entity->num_pads - 1; 57 int ret = 0; 58 59 /* 60 * The mux state is determined by the enabled sink pad link. 61 * Enabling or disabling the source pad link has no effect. 62 */ 63 if (local->flags & MEDIA_PAD_FL_SOURCE) 64 return 0; 65 66 dev_dbg(sd->dev, "link setup '%s':%d->'%s':%d[%d]", 67 remote->entity->name, remote->index, local->entity->name, 68 local->index, flags & MEDIA_LNK_FL_ENABLED); 69 70 mutex_lock(&vmux->lock); 71 72 if (flags & MEDIA_LNK_FL_ENABLED) { 73 struct v4l2_subdev_state *sd_state; 74 struct v4l2_mbus_framefmt *source_mbusformat; 75 76 if (vmux->active == local->index) 77 goto out; 78 79 if (vmux->active >= 0) { 80 ret = -EBUSY; 81 goto out; 82 } 83 84 dev_dbg(sd->dev, "setting %d active\n", local->index); 85 ret = mux_control_try_select(vmux->mux, local->index); 86 if (ret < 0) 87 goto out; 88 vmux->active = local->index; 89 90 /* Propagate the active format to the source */ 91 sd_state = v4l2_subdev_lock_and_get_active_state(sd); 92 source_mbusformat = v4l2_subdev_state_get_format(sd_state, 93 source_pad); 94 *source_mbusformat = *v4l2_subdev_state_get_format(sd_state, 95 vmux->active); 96 v4l2_subdev_unlock_state(sd_state); 97 } else { 98 if (vmux->active != local->index) 99 goto out; 100 101 dev_dbg(sd->dev, "going inactive\n"); 102 mux_control_deselect(vmux->mux); 103 vmux->active = -1; 104 } 105 106 out: 107 mutex_unlock(&vmux->lock); 108 return ret; 109 } 110 111 static const struct media_entity_operations video_mux_ops = { 112 .link_setup = video_mux_link_setup, 113 .link_validate = v4l2_subdev_link_validate, 114 .get_fwnode_pad = v4l2_subdev_get_fwnode_pad_1_to_1, 115 }; 116 117 static int video_mux_s_stream(struct v4l2_subdev *sd, int enable) 118 { 119 struct video_mux *vmux = v4l2_subdev_to_video_mux(sd); 120 struct v4l2_subdev *upstream_sd; 121 struct media_pad *pad; 122 123 if (vmux->active == -1) { 124 dev_err(sd->dev, "Can not start streaming on inactive mux\n"); 125 return -EINVAL; 126 } 127 128 pad = media_pad_remote_pad_first(&sd->entity.pads[vmux->active]); 129 if (!pad) { 130 dev_err(sd->dev, "Failed to find remote source pad\n"); 131 return -ENOLINK; 132 } 133 134 if (!is_media_entity_v4l2_subdev(pad->entity)) { 135 dev_err(sd->dev, "Upstream entity is not a v4l2 subdev\n"); 136 return -ENODEV; 137 } 138 139 upstream_sd = media_entity_to_v4l2_subdev(pad->entity); 140 141 return v4l2_subdev_call(upstream_sd, video, s_stream, enable); 142 } 143 144 static const struct v4l2_subdev_video_ops video_mux_subdev_video_ops = { 145 .s_stream = video_mux_s_stream, 146 }; 147 148 static int video_mux_set_format(struct v4l2_subdev *sd, 149 struct v4l2_subdev_state *sd_state, 150 struct v4l2_subdev_format *sdformat) 151 { 152 struct video_mux *vmux = v4l2_subdev_to_video_mux(sd); 153 struct v4l2_mbus_framefmt *mbusformat, *source_mbusformat; 154 struct media_pad *pad = &vmux->pads[sdformat->pad]; 155 u16 source_pad = sd->entity.num_pads - 1; 156 157 mbusformat = v4l2_subdev_state_get_format(sd_state, sdformat->pad); 158 if (!mbusformat) 159 return -EINVAL; 160 161 source_mbusformat = v4l2_subdev_state_get_format(sd_state, source_pad); 162 if (!source_mbusformat) 163 return -EINVAL; 164 165 /* No size limitations except V4L2 compliance requirements */ 166 v4l_bound_align_image(&sdformat->format.width, 1, 65536, 0, 167 &sdformat->format.height, 1, 65536, 0, 0); 168 169 /* All formats except LVDS and vendor specific formats are acceptable */ 170 switch (sdformat->format.code) { 171 case MEDIA_BUS_FMT_RGB444_1X12: 172 case MEDIA_BUS_FMT_RGB444_2X8_PADHI_BE: 173 case MEDIA_BUS_FMT_RGB444_2X8_PADHI_LE: 174 case MEDIA_BUS_FMT_RGB555_2X8_PADHI_BE: 175 case MEDIA_BUS_FMT_RGB555_2X8_PADHI_LE: 176 case MEDIA_BUS_FMT_RGB565_1X16: 177 case MEDIA_BUS_FMT_BGR565_2X8_BE: 178 case MEDIA_BUS_FMT_BGR565_2X8_LE: 179 case MEDIA_BUS_FMT_RGB565_2X8_BE: 180 case MEDIA_BUS_FMT_RGB565_2X8_LE: 181 case MEDIA_BUS_FMT_RGB666_1X18: 182 case MEDIA_BUS_FMT_RBG888_1X24: 183 case MEDIA_BUS_FMT_RGB666_1X24_CPADHI: 184 case MEDIA_BUS_FMT_BGR888_1X24: 185 case MEDIA_BUS_FMT_GBR888_1X24: 186 case MEDIA_BUS_FMT_RGB888_1X24: 187 case MEDIA_BUS_FMT_RGB888_2X12_BE: 188 case MEDIA_BUS_FMT_RGB888_2X12_LE: 189 case MEDIA_BUS_FMT_ARGB8888_1X32: 190 case MEDIA_BUS_FMT_RGB888_1X32_PADHI: 191 case MEDIA_BUS_FMT_RGB101010_1X30: 192 case MEDIA_BUS_FMT_RGB121212_1X36: 193 case MEDIA_BUS_FMT_RGB161616_1X48: 194 case MEDIA_BUS_FMT_Y8_1X8: 195 case MEDIA_BUS_FMT_UV8_1X8: 196 case MEDIA_BUS_FMT_UYVY8_1_5X8: 197 case MEDIA_BUS_FMT_VYUY8_1_5X8: 198 case MEDIA_BUS_FMT_YUYV8_1_5X8: 199 case MEDIA_BUS_FMT_YVYU8_1_5X8: 200 case MEDIA_BUS_FMT_UYVY8_2X8: 201 case MEDIA_BUS_FMT_VYUY8_2X8: 202 case MEDIA_BUS_FMT_YUYV8_2X8: 203 case MEDIA_BUS_FMT_YVYU8_2X8: 204 case MEDIA_BUS_FMT_Y10_1X10: 205 case MEDIA_BUS_FMT_UYVY10_2X10: 206 case MEDIA_BUS_FMT_VYUY10_2X10: 207 case MEDIA_BUS_FMT_YUYV10_2X10: 208 case MEDIA_BUS_FMT_YVYU10_2X10: 209 case MEDIA_BUS_FMT_Y12_1X12: 210 case MEDIA_BUS_FMT_UYVY12_2X12: 211 case MEDIA_BUS_FMT_VYUY12_2X12: 212 case MEDIA_BUS_FMT_YUYV12_2X12: 213 case MEDIA_BUS_FMT_YVYU12_2X12: 214 case MEDIA_BUS_FMT_UYVY8_1X16: 215 case MEDIA_BUS_FMT_VYUY8_1X16: 216 case MEDIA_BUS_FMT_YUYV8_1X16: 217 case MEDIA_BUS_FMT_YVYU8_1X16: 218 case MEDIA_BUS_FMT_YDYUYDYV8_1X16: 219 case MEDIA_BUS_FMT_UYVY10_1X20: 220 case MEDIA_BUS_FMT_VYUY10_1X20: 221 case MEDIA_BUS_FMT_YUYV10_1X20: 222 case MEDIA_BUS_FMT_YVYU10_1X20: 223 case MEDIA_BUS_FMT_VUY8_1X24: 224 case MEDIA_BUS_FMT_YUV8_1X24: 225 case MEDIA_BUS_FMT_UYYVYY8_0_5X24: 226 case MEDIA_BUS_FMT_UYVY12_1X24: 227 case MEDIA_BUS_FMT_VYUY12_1X24: 228 case MEDIA_BUS_FMT_YUYV12_1X24: 229 case MEDIA_BUS_FMT_YVYU12_1X24: 230 case MEDIA_BUS_FMT_YUV10_1X30: 231 case MEDIA_BUS_FMT_UYYVYY10_0_5X30: 232 case MEDIA_BUS_FMT_AYUV8_1X32: 233 case MEDIA_BUS_FMT_UYYVYY12_0_5X36: 234 case MEDIA_BUS_FMT_YUV12_1X36: 235 case MEDIA_BUS_FMT_YUV16_1X48: 236 case MEDIA_BUS_FMT_UYYVYY16_0_5X48: 237 case MEDIA_BUS_FMT_JPEG_1X8: 238 case MEDIA_BUS_FMT_AHSV8888_1X32: 239 case MEDIA_BUS_FMT_SBGGR8_1X8: 240 case MEDIA_BUS_FMT_SGBRG8_1X8: 241 case MEDIA_BUS_FMT_SGRBG8_1X8: 242 case MEDIA_BUS_FMT_SRGGB8_1X8: 243 case MEDIA_BUS_FMT_SBGGR10_1X10: 244 case MEDIA_BUS_FMT_SGBRG10_1X10: 245 case MEDIA_BUS_FMT_SGRBG10_1X10: 246 case MEDIA_BUS_FMT_SRGGB10_1X10: 247 case MEDIA_BUS_FMT_SBGGR12_1X12: 248 case MEDIA_BUS_FMT_SGBRG12_1X12: 249 case MEDIA_BUS_FMT_SGRBG12_1X12: 250 case MEDIA_BUS_FMT_SRGGB12_1X12: 251 case MEDIA_BUS_FMT_SBGGR14_1X14: 252 case MEDIA_BUS_FMT_SGBRG14_1X14: 253 case MEDIA_BUS_FMT_SGRBG14_1X14: 254 case MEDIA_BUS_FMT_SRGGB14_1X14: 255 case MEDIA_BUS_FMT_SBGGR16_1X16: 256 case MEDIA_BUS_FMT_SGBRG16_1X16: 257 case MEDIA_BUS_FMT_SGRBG16_1X16: 258 case MEDIA_BUS_FMT_SRGGB16_1X16: 259 break; 260 default: 261 sdformat->format.code = MEDIA_BUS_FMT_Y8_1X8; 262 break; 263 } 264 if (sdformat->format.field == V4L2_FIELD_ANY) 265 sdformat->format.field = V4L2_FIELD_NONE; 266 267 mutex_lock(&vmux->lock); 268 269 /* Source pad mirrors active sink pad, no limitations on sink pads */ 270 if ((pad->flags & MEDIA_PAD_FL_SOURCE) && vmux->active >= 0) 271 sdformat->format = *v4l2_subdev_state_get_format(sd_state, 272 vmux->active); 273 274 *mbusformat = sdformat->format; 275 276 /* Propagate the format from an active sink to source */ 277 if ((pad->flags & MEDIA_PAD_FL_SINK) && (pad->index == vmux->active)) 278 *source_mbusformat = sdformat->format; 279 280 mutex_unlock(&vmux->lock); 281 282 return 0; 283 } 284 285 static int video_mux_init_state(struct v4l2_subdev *sd, 286 struct v4l2_subdev_state *sd_state) 287 { 288 struct video_mux *vmux = v4l2_subdev_to_video_mux(sd); 289 struct v4l2_mbus_framefmt *mbusformat; 290 unsigned int i; 291 292 mutex_lock(&vmux->lock); 293 294 for (i = 0; i < sd->entity.num_pads; i++) { 295 mbusformat = v4l2_subdev_state_get_format(sd_state, i); 296 *mbusformat = video_mux_format_mbus_default; 297 } 298 299 mutex_unlock(&vmux->lock); 300 301 return 0; 302 } 303 304 static const struct v4l2_subdev_pad_ops video_mux_pad_ops = { 305 .get_fmt = v4l2_subdev_get_fmt, 306 .set_fmt = video_mux_set_format, 307 }; 308 309 static const struct v4l2_subdev_ops video_mux_subdev_ops = { 310 .pad = &video_mux_pad_ops, 311 .video = &video_mux_subdev_video_ops, 312 }; 313 314 static const struct v4l2_subdev_internal_ops video_mux_internal_ops = { 315 .init_state = video_mux_init_state, 316 }; 317 318 static int video_mux_notify_bound(struct v4l2_async_notifier *notifier, 319 struct v4l2_subdev *sd, 320 struct v4l2_async_connection *asd) 321 { 322 struct video_mux *vmux = notifier_to_video_mux(notifier); 323 324 return v4l2_create_fwnode_links(sd, &vmux->subdev); 325 } 326 327 static const struct v4l2_async_notifier_operations video_mux_notify_ops = { 328 .bound = video_mux_notify_bound, 329 }; 330 331 static int video_mux_async_register(struct video_mux *vmux, 332 unsigned int num_input_pads) 333 { 334 unsigned int i; 335 int ret; 336 337 v4l2_async_subdev_nf_init(&vmux->notifier, &vmux->subdev); 338 339 for (i = 0; i < num_input_pads; i++) { 340 struct v4l2_async_connection *asd; 341 struct fwnode_handle *ep, *remote_ep; 342 343 ep = fwnode_graph_get_endpoint_by_id( 344 dev_fwnode(vmux->subdev.dev), i, 0, 345 FWNODE_GRAPH_ENDPOINT_NEXT); 346 if (!ep) 347 continue; 348 349 /* Skip dangling endpoints for backwards compatibility */ 350 remote_ep = fwnode_graph_get_remote_endpoint(ep); 351 if (!remote_ep) { 352 fwnode_handle_put(ep); 353 continue; 354 } 355 fwnode_handle_put(remote_ep); 356 357 asd = v4l2_async_nf_add_fwnode_remote(&vmux->notifier, ep, 358 struct v4l2_async_connection); 359 360 fwnode_handle_put(ep); 361 362 if (IS_ERR(asd)) { 363 ret = PTR_ERR(asd); 364 /* OK if asd already exists */ 365 if (ret != -EEXIST) 366 goto err_nf_cleanup; 367 } 368 } 369 370 vmux->notifier.ops = &video_mux_notify_ops; 371 372 ret = v4l2_async_nf_register(&vmux->notifier); 373 if (ret) 374 goto err_nf_cleanup; 375 376 ret = v4l2_async_register_subdev(&vmux->subdev); 377 if (ret) 378 goto err_nf_unregister; 379 380 return 0; 381 382 err_nf_unregister: 383 v4l2_async_nf_unregister(&vmux->notifier); 384 err_nf_cleanup: 385 v4l2_async_nf_cleanup(&vmux->notifier); 386 return ret; 387 } 388 389 static int video_mux_probe(struct platform_device *pdev) 390 { 391 struct device_node *np = pdev->dev.of_node; 392 struct device *dev = &pdev->dev; 393 struct device_node *ep; 394 struct video_mux *vmux; 395 unsigned int num_pads = 0; 396 unsigned int i; 397 int ret; 398 399 vmux = devm_kzalloc(dev, sizeof(*vmux), GFP_KERNEL); 400 if (!vmux) 401 return -ENOMEM; 402 403 platform_set_drvdata(pdev, vmux); 404 405 v4l2_subdev_init(&vmux->subdev, &video_mux_subdev_ops); 406 vmux->subdev.internal_ops = &video_mux_internal_ops; 407 snprintf(vmux->subdev.name, sizeof(vmux->subdev.name), "%pOFn", np); 408 vmux->subdev.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE; 409 vmux->subdev.dev = dev; 410 411 /* 412 * The largest numbered port is the output port. It determines 413 * total number of pads. 414 */ 415 for_each_endpoint_of_node(np, ep) { 416 struct of_endpoint endpoint; 417 418 of_graph_parse_endpoint(ep, &endpoint); 419 num_pads = max(num_pads, endpoint.port + 1); 420 } 421 422 if (num_pads < 2) { 423 dev_err(dev, "Not enough ports %d\n", num_pads); 424 return -EINVAL; 425 } 426 427 vmux->mux = devm_mux_control_get(dev, NULL); 428 if (IS_ERR(vmux->mux)) { 429 ret = PTR_ERR(vmux->mux); 430 return dev_err_probe(dev, ret, "Failed to get mux\n"); 431 } 432 433 mutex_init(&vmux->lock); 434 vmux->active = -1; 435 vmux->pads = devm_kcalloc(dev, num_pads, sizeof(*vmux->pads), 436 GFP_KERNEL); 437 if (!vmux->pads) 438 return -ENOMEM; 439 440 for (i = 0; i < num_pads; i++) 441 vmux->pads[i].flags = (i < num_pads - 1) ? MEDIA_PAD_FL_SINK 442 : MEDIA_PAD_FL_SOURCE; 443 444 vmux->subdev.entity.function = MEDIA_ENT_F_VID_MUX; 445 ret = media_entity_pads_init(&vmux->subdev.entity, num_pads, 446 vmux->pads); 447 if (ret < 0) 448 return ret; 449 450 vmux->subdev.entity.ops = &video_mux_ops; 451 452 ret = v4l2_subdev_init_finalize(&vmux->subdev); 453 if (ret < 0) 454 goto err_entity_cleanup; 455 456 ret = video_mux_async_register(vmux, num_pads - 1); 457 if (ret) 458 goto err_subdev_cleanup; 459 460 return 0; 461 462 err_subdev_cleanup: 463 v4l2_subdev_cleanup(&vmux->subdev); 464 err_entity_cleanup: 465 media_entity_cleanup(&vmux->subdev.entity); 466 return ret; 467 } 468 469 static void video_mux_remove(struct platform_device *pdev) 470 { 471 struct video_mux *vmux = platform_get_drvdata(pdev); 472 struct v4l2_subdev *sd = &vmux->subdev; 473 474 v4l2_async_nf_unregister(&vmux->notifier); 475 v4l2_async_nf_cleanup(&vmux->notifier); 476 v4l2_async_unregister_subdev(sd); 477 v4l2_subdev_cleanup(sd); 478 media_entity_cleanup(&sd->entity); 479 } 480 481 static const struct of_device_id video_mux_dt_ids[] = { 482 { .compatible = "video-mux", }, 483 { /* sentinel */ } 484 }; 485 MODULE_DEVICE_TABLE(of, video_mux_dt_ids); 486 487 static struct platform_driver video_mux_driver = { 488 .probe = video_mux_probe, 489 .remove_new = video_mux_remove, 490 .driver = { 491 .of_match_table = video_mux_dt_ids, 492 .name = "video-mux", 493 }, 494 }; 495 496 module_platform_driver(video_mux_driver); 497 498 MODULE_DESCRIPTION("video stream multiplexer"); 499 MODULE_AUTHOR("Sascha Hauer, Pengutronix"); 500 MODULE_AUTHOR("Philipp Zabel, Pengutronix"); 501 MODULE_LICENSE("GPL"); 502