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