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