1 // SPDX-License-Identifier: GPL-2.0 2 /* 3 * V4L2 Media Controller Driver for Freescale common i.MX5/6/7 SOC 4 * 5 * Copyright (c) 2019 Linaro Ltd 6 * Copyright (c) 2016 Mentor Graphics Inc. 7 */ 8 9 #include <linux/of_graph.h> 10 #include <linux/of_platform.h> 11 #include <media/v4l2-ctrls.h> 12 #include <media/v4l2-event.h> 13 #include <media/v4l2-ioctl.h> 14 #include <media/v4l2-mc.h> 15 #include "imx-media.h" 16 17 static inline struct imx_media_dev *notifier2dev(struct v4l2_async_notifier *n) 18 { 19 return container_of(n, struct imx_media_dev, notifier); 20 } 21 22 /* async subdev bound notifier */ 23 static int imx_media_subdev_bound(struct v4l2_async_notifier *notifier, 24 struct v4l2_subdev *sd, 25 struct v4l2_async_subdev *asd) 26 { 27 struct imx_media_dev *imxmd = notifier2dev(notifier); 28 29 dev_dbg(imxmd->md.dev, "subdev %s bound\n", sd->name); 30 31 return 0; 32 } 33 34 /* 35 * Create the missing media links from the CSI-2 receiver. 36 * Called after all async subdevs have bound. 37 */ 38 static void imx_media_create_csi2_links(struct imx_media_dev *imxmd) 39 { 40 struct v4l2_subdev *sd, *csi2 = NULL; 41 42 list_for_each_entry(sd, &imxmd->v4l2_dev.subdevs, list) { 43 if (sd->grp_id == IMX_MEDIA_GRP_ID_CSI2) { 44 csi2 = sd; 45 break; 46 } 47 } 48 if (!csi2) 49 return; 50 51 list_for_each_entry(sd, &imxmd->v4l2_dev.subdevs, list) { 52 /* skip if not a CSI or a CSI mux */ 53 if (!(sd->grp_id & IMX_MEDIA_GRP_ID_IPU_CSI) && 54 !(sd->grp_id & IMX_MEDIA_GRP_ID_CSI) && 55 !(sd->grp_id & IMX_MEDIA_GRP_ID_CSI_MUX)) 56 continue; 57 58 v4l2_create_fwnode_links(csi2, sd); 59 } 60 } 61 62 /* 63 * adds given video device to given imx-media source pad vdev list. 64 * Continues upstream from the pad entity's sink pads. 65 */ 66 static int imx_media_add_vdev_to_pad(struct imx_media_dev *imxmd, 67 struct imx_media_video_dev *vdev, 68 struct media_pad *srcpad) 69 { 70 struct media_entity *entity = srcpad->entity; 71 struct imx_media_pad_vdev *pad_vdev; 72 struct list_head *pad_vdev_list; 73 struct media_link *link; 74 struct v4l2_subdev *sd; 75 int i, ret; 76 77 /* skip this entity if not a v4l2_subdev */ 78 if (!is_media_entity_v4l2_subdev(entity)) 79 return 0; 80 81 sd = media_entity_to_v4l2_subdev(entity); 82 83 pad_vdev_list = to_pad_vdev_list(sd, srcpad->index); 84 if (!pad_vdev_list) { 85 v4l2_warn(&imxmd->v4l2_dev, "%s:%u has no vdev list!\n", 86 entity->name, srcpad->index); 87 /* 88 * shouldn't happen, but no reason to fail driver load, 89 * just skip this entity. 90 */ 91 return 0; 92 } 93 94 /* just return if we've been here before */ 95 list_for_each_entry(pad_vdev, pad_vdev_list, list) { 96 if (pad_vdev->vdev == vdev) 97 return 0; 98 } 99 100 dev_dbg(imxmd->md.dev, "adding %s to pad %s:%u\n", 101 vdev->vfd->entity.name, entity->name, srcpad->index); 102 103 pad_vdev = devm_kzalloc(imxmd->md.dev, sizeof(*pad_vdev), GFP_KERNEL); 104 if (!pad_vdev) 105 return -ENOMEM; 106 107 /* attach this vdev to this pad */ 108 pad_vdev->vdev = vdev; 109 list_add_tail(&pad_vdev->list, pad_vdev_list); 110 111 /* move upstream from this entity's sink pads */ 112 for (i = 0; i < entity->num_pads; i++) { 113 struct media_pad *pad = &entity->pads[i]; 114 115 if (!(pad->flags & MEDIA_PAD_FL_SINK)) 116 continue; 117 118 list_for_each_entry(link, &entity->links, list) { 119 if (link->sink != pad) 120 continue; 121 ret = imx_media_add_vdev_to_pad(imxmd, vdev, 122 link->source); 123 if (ret) 124 return ret; 125 } 126 } 127 128 return 0; 129 } 130 131 /* 132 * For every subdevice, allocate an array of list_head's, one list_head 133 * for each pad, to hold the list of video devices reachable from that 134 * pad. 135 */ 136 static int imx_media_alloc_pad_vdev_lists(struct imx_media_dev *imxmd) 137 { 138 struct list_head *vdev_lists; 139 struct media_entity *entity; 140 struct v4l2_subdev *sd; 141 int i; 142 143 list_for_each_entry(sd, &imxmd->v4l2_dev.subdevs, list) { 144 entity = &sd->entity; 145 vdev_lists = devm_kcalloc(imxmd->md.dev, 146 entity->num_pads, sizeof(*vdev_lists), 147 GFP_KERNEL); 148 if (!vdev_lists) 149 return -ENOMEM; 150 151 /* attach to the subdev's host private pointer */ 152 sd->host_priv = vdev_lists; 153 154 for (i = 0; i < entity->num_pads; i++) 155 INIT_LIST_HEAD(to_pad_vdev_list(sd, i)); 156 } 157 158 return 0; 159 } 160 161 /* form the vdev lists in all imx-media source pads */ 162 static int imx_media_create_pad_vdev_lists(struct imx_media_dev *imxmd) 163 { 164 struct imx_media_video_dev *vdev; 165 struct media_link *link; 166 int ret; 167 168 ret = imx_media_alloc_pad_vdev_lists(imxmd); 169 if (ret) 170 return ret; 171 172 list_for_each_entry(vdev, &imxmd->vdev_list, list) { 173 link = list_first_entry(&vdev->vfd->entity.links, 174 struct media_link, list); 175 ret = imx_media_add_vdev_to_pad(imxmd, vdev, link->source); 176 if (ret) 177 return ret; 178 } 179 180 return 0; 181 } 182 183 /* async subdev complete notifier */ 184 int imx_media_probe_complete(struct v4l2_async_notifier *notifier) 185 { 186 struct imx_media_dev *imxmd = notifier2dev(notifier); 187 int ret; 188 189 mutex_lock(&imxmd->mutex); 190 191 imx_media_create_csi2_links(imxmd); 192 193 ret = imx_media_create_pad_vdev_lists(imxmd); 194 if (ret) 195 goto unlock; 196 197 ret = v4l2_device_register_subdev_nodes(&imxmd->v4l2_dev); 198 unlock: 199 mutex_unlock(&imxmd->mutex); 200 if (ret) 201 return ret; 202 203 return media_device_register(&imxmd->md); 204 } 205 EXPORT_SYMBOL_GPL(imx_media_probe_complete); 206 207 /* 208 * adds controls to a video device from an entity subdevice. 209 * Continues upstream from the entity's sink pads. 210 */ 211 static int imx_media_inherit_controls(struct imx_media_dev *imxmd, 212 struct video_device *vfd, 213 struct media_entity *entity) 214 { 215 int i, ret = 0; 216 217 if (is_media_entity_v4l2_subdev(entity)) { 218 struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity); 219 220 dev_dbg(imxmd->md.dev, 221 "adding controls to %s from %s\n", 222 vfd->entity.name, sd->entity.name); 223 224 ret = v4l2_ctrl_add_handler(vfd->ctrl_handler, 225 sd->ctrl_handler, 226 NULL, true); 227 if (ret) 228 return ret; 229 } 230 231 /* move upstream */ 232 for (i = 0; i < entity->num_pads; i++) { 233 struct media_pad *pad, *spad = &entity->pads[i]; 234 235 if (!(spad->flags & MEDIA_PAD_FL_SINK)) 236 continue; 237 238 pad = media_entity_remote_pad(spad); 239 if (!pad || !is_media_entity_v4l2_subdev(pad->entity)) 240 continue; 241 242 ret = imx_media_inherit_controls(imxmd, vfd, pad->entity); 243 if (ret) 244 break; 245 } 246 247 return ret; 248 } 249 250 static int imx_media_link_notify(struct media_link *link, u32 flags, 251 unsigned int notification) 252 { 253 struct imx_media_dev *imxmd = container_of(link->graph_obj.mdev, 254 struct imx_media_dev, md); 255 struct media_entity *source = link->source->entity; 256 struct imx_media_pad_vdev *pad_vdev; 257 struct list_head *pad_vdev_list; 258 struct video_device *vfd; 259 struct v4l2_subdev *sd; 260 int pad_idx, ret; 261 262 ret = v4l2_pipeline_link_notify(link, flags, notification); 263 if (ret) 264 return ret; 265 266 /* don't bother if source is not a subdev */ 267 if (!is_media_entity_v4l2_subdev(source)) 268 return 0; 269 270 sd = media_entity_to_v4l2_subdev(source); 271 pad_idx = link->source->index; 272 273 pad_vdev_list = to_pad_vdev_list(sd, pad_idx); 274 if (!pad_vdev_list) { 275 /* nothing to do if source sd has no pad vdev list */ 276 return 0; 277 } 278 279 /* 280 * Before disabling a link, reset controls for all video 281 * devices reachable from this link. 282 * 283 * After enabling a link, refresh controls for all video 284 * devices reachable from this link. 285 */ 286 if (notification == MEDIA_DEV_NOTIFY_PRE_LINK_CH && 287 !(flags & MEDIA_LNK_FL_ENABLED)) { 288 list_for_each_entry(pad_vdev, pad_vdev_list, list) { 289 vfd = pad_vdev->vdev->vfd; 290 if (!vfd->ctrl_handler) 291 continue; 292 dev_dbg(imxmd->md.dev, 293 "reset controls for %s\n", 294 vfd->entity.name); 295 v4l2_ctrl_handler_free(vfd->ctrl_handler); 296 v4l2_ctrl_handler_init(vfd->ctrl_handler, 0); 297 } 298 } else if (notification == MEDIA_DEV_NOTIFY_POST_LINK_CH && 299 (link->flags & MEDIA_LNK_FL_ENABLED)) { 300 list_for_each_entry(pad_vdev, pad_vdev_list, list) { 301 vfd = pad_vdev->vdev->vfd; 302 if (!vfd->ctrl_handler) 303 continue; 304 dev_dbg(imxmd->md.dev, 305 "refresh controls for %s\n", 306 vfd->entity.name); 307 ret = imx_media_inherit_controls(imxmd, vfd, 308 &vfd->entity); 309 if (ret) 310 break; 311 } 312 } 313 314 return ret; 315 } 316 317 static void imx_media_notify(struct v4l2_subdev *sd, unsigned int notification, 318 void *arg) 319 { 320 struct media_entity *entity = &sd->entity; 321 int i; 322 323 if (notification != V4L2_DEVICE_NOTIFY_EVENT) 324 return; 325 326 for (i = 0; i < entity->num_pads; i++) { 327 struct media_pad *pad = &entity->pads[i]; 328 struct imx_media_pad_vdev *pad_vdev; 329 struct list_head *pad_vdev_list; 330 331 pad_vdev_list = to_pad_vdev_list(sd, pad->index); 332 if (!pad_vdev_list) 333 continue; 334 list_for_each_entry(pad_vdev, pad_vdev_list, list) 335 v4l2_event_queue(pad_vdev->vdev->vfd, arg); 336 } 337 } 338 339 static const struct v4l2_async_notifier_operations imx_media_notifier_ops = { 340 .bound = imx_media_subdev_bound, 341 .complete = imx_media_probe_complete, 342 }; 343 344 static const struct media_device_ops imx_media_md_ops = { 345 .link_notify = imx_media_link_notify, 346 }; 347 348 struct imx_media_dev *imx_media_dev_init(struct device *dev, 349 const struct media_device_ops *ops) 350 { 351 struct imx_media_dev *imxmd; 352 int ret; 353 354 imxmd = devm_kzalloc(dev, sizeof(*imxmd), GFP_KERNEL); 355 if (!imxmd) 356 return ERR_PTR(-ENOMEM); 357 358 dev_set_drvdata(dev, imxmd); 359 360 strscpy(imxmd->md.model, "imx-media", sizeof(imxmd->md.model)); 361 imxmd->md.ops = ops ? ops : &imx_media_md_ops; 362 imxmd->md.dev = dev; 363 364 mutex_init(&imxmd->mutex); 365 366 imxmd->v4l2_dev.mdev = &imxmd->md; 367 imxmd->v4l2_dev.notify = imx_media_notify; 368 strscpy(imxmd->v4l2_dev.name, "imx-media", 369 sizeof(imxmd->v4l2_dev.name)); 370 snprintf(imxmd->md.bus_info, sizeof(imxmd->md.bus_info), 371 "platform:%s", dev_name(imxmd->md.dev)); 372 373 media_device_init(&imxmd->md); 374 375 ret = v4l2_device_register(dev, &imxmd->v4l2_dev); 376 if (ret < 0) { 377 v4l2_err(&imxmd->v4l2_dev, 378 "Failed to register v4l2_device: %d\n", ret); 379 goto cleanup; 380 } 381 382 INIT_LIST_HEAD(&imxmd->vdev_list); 383 384 v4l2_async_nf_init(&imxmd->notifier); 385 386 return imxmd; 387 388 cleanup: 389 media_device_cleanup(&imxmd->md); 390 391 return ERR_PTR(ret); 392 } 393 EXPORT_SYMBOL_GPL(imx_media_dev_init); 394 395 int imx_media_dev_notifier_register(struct imx_media_dev *imxmd, 396 const struct v4l2_async_notifier_operations *ops) 397 { 398 int ret; 399 400 /* no subdevs? just bail */ 401 if (list_empty(&imxmd->notifier.asd_list)) { 402 v4l2_err(&imxmd->v4l2_dev, "no subdevs\n"); 403 return -ENODEV; 404 } 405 406 /* prepare the async subdev notifier and register it */ 407 imxmd->notifier.ops = ops ? ops : &imx_media_notifier_ops; 408 ret = v4l2_async_nf_register(&imxmd->v4l2_dev, &imxmd->notifier); 409 if (ret) { 410 v4l2_err(&imxmd->v4l2_dev, 411 "v4l2_async_nf_register failed with %d\n", ret); 412 return ret; 413 } 414 415 return 0; 416 } 417 EXPORT_SYMBOL_GPL(imx_media_dev_notifier_register); 418