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