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/regmap.h> 21 #include <linux/mfd/syscon.h> 22 #include <linux/of.h> 23 #include <linux/of_graph.h> 24 #include <linux/platform_device.h> 25 #include <media/v4l2-async.h> 26 #include <media/v4l2-device.h> 27 #include <media/v4l2-subdev.h> 28 29 struct video_mux { 30 struct v4l2_subdev subdev; 31 struct media_pad *pads; 32 struct v4l2_mbus_framefmt *format_mbus; 33 struct regmap_field *field; 34 struct mutex lock; 35 int active; 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 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 = regmap_field_write(vmux->field, local->index); 75 if (ret < 0) 76 goto out; 77 vmux->active = local->index; 78 } else { 79 if (vmux->active != local->index) 80 goto out; 81 82 dev_dbg(sd->dev, "going inactive\n"); 83 vmux->active = -1; 84 } 85 86 out: 87 mutex_unlock(&vmux->lock); 88 return ret; 89 } 90 91 static const struct media_entity_operations video_mux_ops = { 92 .link_setup = video_mux_link_setup, 93 .link_validate = v4l2_subdev_link_validate, 94 }; 95 96 static int video_mux_s_stream(struct v4l2_subdev *sd, int enable) 97 { 98 struct video_mux *vmux = v4l2_subdev_to_video_mux(sd); 99 struct v4l2_subdev *upstream_sd; 100 struct media_pad *pad; 101 102 if (vmux->active == -1) { 103 dev_err(sd->dev, "Can not start streaming on inactive mux\n"); 104 return -EINVAL; 105 } 106 107 pad = media_entity_remote_pad(&sd->entity.pads[vmux->active]); 108 if (!pad) { 109 dev_err(sd->dev, "Failed to find remote source pad\n"); 110 return -ENOLINK; 111 } 112 113 if (!is_media_entity_v4l2_subdev(pad->entity)) { 114 dev_err(sd->dev, "Upstream entity is not a v4l2 subdev\n"); 115 return -ENODEV; 116 } 117 118 upstream_sd = media_entity_to_v4l2_subdev(pad->entity); 119 120 return v4l2_subdev_call(upstream_sd, video, s_stream, enable); 121 } 122 123 static const struct v4l2_subdev_video_ops video_mux_subdev_video_ops = { 124 .s_stream = video_mux_s_stream, 125 }; 126 127 static struct v4l2_mbus_framefmt * 128 __video_mux_get_pad_format(struct v4l2_subdev *sd, 129 struct v4l2_subdev_pad_config *cfg, 130 unsigned int pad, u32 which) 131 { 132 struct video_mux *vmux = v4l2_subdev_to_video_mux(sd); 133 134 switch (which) { 135 case V4L2_SUBDEV_FORMAT_TRY: 136 return v4l2_subdev_get_try_format(sd, cfg, pad); 137 case V4L2_SUBDEV_FORMAT_ACTIVE: 138 return &vmux->format_mbus[pad]; 139 default: 140 return NULL; 141 } 142 } 143 144 static int video_mux_get_format(struct v4l2_subdev *sd, 145 struct v4l2_subdev_pad_config *cfg, 146 struct v4l2_subdev_format *sdformat) 147 { 148 struct video_mux *vmux = v4l2_subdev_to_video_mux(sd); 149 150 mutex_lock(&vmux->lock); 151 152 sdformat->format = *__video_mux_get_pad_format(sd, cfg, sdformat->pad, 153 sdformat->which); 154 155 mutex_unlock(&vmux->lock); 156 157 return 0; 158 } 159 160 static int video_mux_set_format(struct v4l2_subdev *sd, 161 struct v4l2_subdev_pad_config *cfg, 162 struct v4l2_subdev_format *sdformat) 163 { 164 struct video_mux *vmux = v4l2_subdev_to_video_mux(sd); 165 struct v4l2_mbus_framefmt *mbusformat; 166 struct media_pad *pad = &vmux->pads[sdformat->pad]; 167 168 mbusformat = __video_mux_get_pad_format(sd, cfg, sdformat->pad, 169 sdformat->which); 170 if (!mbusformat) 171 return -EINVAL; 172 173 mutex_lock(&vmux->lock); 174 175 /* Source pad mirrors active sink pad, no limitations on sink pads */ 176 if ((pad->flags & MEDIA_PAD_FL_SOURCE) && vmux->active >= 0) 177 sdformat->format = vmux->format_mbus[vmux->active]; 178 179 *mbusformat = sdformat->format; 180 181 mutex_unlock(&vmux->lock); 182 183 return 0; 184 } 185 186 static const struct v4l2_subdev_pad_ops video_mux_pad_ops = { 187 .get_fmt = video_mux_get_format, 188 .set_fmt = video_mux_set_format, 189 }; 190 191 static const struct v4l2_subdev_ops video_mux_subdev_ops = { 192 .pad = &video_mux_pad_ops, 193 .video = &video_mux_subdev_video_ops, 194 }; 195 196 static int video_mux_probe_mmio_mux(struct video_mux *vmux) 197 { 198 struct device *dev = vmux->subdev.dev; 199 struct of_phandle_args args; 200 struct reg_field field; 201 struct regmap *regmap; 202 u32 reg, mask; 203 int ret; 204 205 ret = of_parse_phandle_with_args(dev->of_node, "mux-controls", 206 "#mux-control-cells", 0, &args); 207 if (ret) 208 return ret; 209 210 if (!of_device_is_compatible(args.np, "mmio-mux")) 211 return -EINVAL; 212 213 regmap = syscon_node_to_regmap(args.np->parent); 214 if (IS_ERR(regmap)) 215 return PTR_ERR(regmap); 216 217 ret = of_property_read_u32_index(args.np, "mux-reg-masks", 218 2 * args.args[0], ®); 219 if (!ret) 220 ret = of_property_read_u32_index(args.np, "mux-reg-masks", 221 2 * args.args[0] + 1, &mask); 222 if (ret < 0) 223 return ret; 224 225 field.reg = reg; 226 field.msb = fls(mask) - 1; 227 field.lsb = ffs(mask) - 1; 228 229 vmux->field = devm_regmap_field_alloc(dev, regmap, field); 230 if (IS_ERR(vmux->field)) 231 return PTR_ERR(vmux->field); 232 233 return 0; 234 } 235 236 static int video_mux_probe(struct platform_device *pdev) 237 { 238 struct device_node *np = pdev->dev.of_node; 239 struct device *dev = &pdev->dev; 240 struct device_node *ep; 241 struct video_mux *vmux; 242 unsigned int num_pads = 0; 243 int ret; 244 int i; 245 246 vmux = devm_kzalloc(dev, sizeof(*vmux), GFP_KERNEL); 247 if (!vmux) 248 return -ENOMEM; 249 250 platform_set_drvdata(pdev, vmux); 251 252 v4l2_subdev_init(&vmux->subdev, &video_mux_subdev_ops); 253 snprintf(vmux->subdev.name, sizeof(vmux->subdev.name), "%s", np->name); 254 vmux->subdev.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE; 255 vmux->subdev.dev = dev; 256 257 /* 258 * The largest numbered port is the output port. It determines 259 * total number of pads. 260 */ 261 for_each_endpoint_of_node(np, ep) { 262 struct of_endpoint endpoint; 263 264 of_graph_parse_endpoint(ep, &endpoint); 265 num_pads = max(num_pads, endpoint.port + 1); 266 } 267 268 if (num_pads < 2) { 269 dev_err(dev, "Not enough ports %d\n", num_pads); 270 return -EINVAL; 271 } 272 273 ret = video_mux_probe_mmio_mux(vmux); 274 if (ret) { 275 if (ret != -EPROBE_DEFER) 276 dev_err(dev, "Failed to get mux: %d\n", ret); 277 return ret; 278 } 279 280 mutex_init(&vmux->lock); 281 vmux->active = -1; 282 vmux->pads = devm_kcalloc(dev, num_pads, sizeof(*vmux->pads), 283 GFP_KERNEL); 284 vmux->format_mbus = devm_kcalloc(dev, num_pads, 285 sizeof(*vmux->format_mbus), 286 GFP_KERNEL); 287 288 for (i = 0; i < num_pads - 1; i++) 289 vmux->pads[i].flags = MEDIA_PAD_FL_SINK; 290 vmux->pads[num_pads - 1].flags = MEDIA_PAD_FL_SOURCE; 291 292 vmux->subdev.entity.function = MEDIA_ENT_F_VID_MUX; 293 ret = media_entity_pads_init(&vmux->subdev.entity, num_pads, 294 vmux->pads); 295 if (ret < 0) 296 return ret; 297 298 vmux->subdev.entity.ops = &video_mux_ops; 299 300 return v4l2_async_register_subdev(&vmux->subdev); 301 } 302 303 static int video_mux_remove(struct platform_device *pdev) 304 { 305 struct video_mux *vmux = platform_get_drvdata(pdev); 306 struct v4l2_subdev *sd = &vmux->subdev; 307 308 v4l2_async_unregister_subdev(sd); 309 media_entity_cleanup(&sd->entity); 310 311 return 0; 312 } 313 314 static const struct of_device_id video_mux_dt_ids[] = { 315 { .compatible = "video-mux", }, 316 { /* sentinel */ } 317 }; 318 MODULE_DEVICE_TABLE(of, video_mux_dt_ids); 319 320 static struct platform_driver video_mux_driver = { 321 .probe = video_mux_probe, 322 .remove = video_mux_remove, 323 .driver = { 324 .of_match_table = video_mux_dt_ids, 325 .name = "video-mux", 326 }, 327 }; 328 329 module_platform_driver(video_mux_driver); 330 331 MODULE_DESCRIPTION("video stream multiplexer"); 332 MODULE_AUTHOR("Sascha Hauer, Pengutronix"); 333 MODULE_AUTHOR("Philipp Zabel, Pengutronix"); 334 MODULE_LICENSE("GPL"); 335