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