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