diff --git a/drivers/media/platform/soc_camera/soc_camera.c b/drivers/media/platform/soc_camera/soc_camera.c index dc626b9a177dc0..f4308fed543186 100644 --- a/drivers/media/platform/soc_camera/soc_camera.c +++ b/drivers/media/platform/soc_camera/soc_camera.c @@ -36,6 +36,7 @@ #include #include #include +#include #include #include @@ -1585,6 +1586,130 @@ static void scan_async_host(struct soc_camera_host *ici) #define scan_async_host(ici) do {} while (0) #endif +#ifdef CONFIG_OF + +struct soc_of_info { + struct soc_camera_async_subdev sasd; + struct soc_camera_async_client sasc; + struct v4l2_async_subdev *subdev; +}; + +static int soc_of_bind(struct soc_camera_host *ici, + struct device_node *ep, + struct device_node *remote) +{ + struct soc_camera_device *icd; + struct soc_camera_desc sdesc = {.host_desc.bus_id = ici->nr,}; + struct soc_camera_async_client *sasc; + struct soc_of_info *info; + struct i2c_client *client; + char clk_name[V4L2_SUBDEV_NAME_SIZE]; + int ret; + + /* allocate a new subdev and add match info to it */ + info = devm_kzalloc(ici->v4l2_dev.dev, sizeof(struct soc_of_info), + GFP_KERNEL); + if (!info) + return -ENOMEM; + + info->sasd.asd.match.of.node = remote; + info->sasd.asd.match_type = V4L2_ASYNC_MATCH_OF; + info->subdev = &info->sasd.asd; + + /* Or shall this be managed by the soc-camera device? */ + sasc = &info->sasc; + + /* HACK: just need a != NULL */ + sdesc.host_desc.board_info = ERR_PTR(-ENODATA); + + ret = soc_camera_dyn_pdev(&sdesc, sasc); + if (ret < 0) + goto eallocpdev; + + sasc->sensor = &info->sasd.asd; + + icd = soc_camera_add_pdev(sasc); + if (!icd) { + ret = -ENOMEM; + goto eaddpdev; + } + + sasc->notifier.subdevs = &info->subdev; + sasc->notifier.num_subdevs = 1; + sasc->notifier.bound = soc_camera_async_bound; + sasc->notifier.unbind = soc_camera_async_unbind; + sasc->notifier.complete = soc_camera_async_complete; + + icd->sasc = sasc; + icd->parent = ici->v4l2_dev.dev; + + client = of_find_i2c_device_by_node(remote); + + if (client) + snprintf(clk_name, sizeof(clk_name), "%d-%04x", + client->adapter->nr, client->addr); + else + snprintf(clk_name, sizeof(clk_name), "of-%s", + of_node_full_name(remote)); + + icd->clk = v4l2_clk_register(&soc_camera_clk_ops, clk_name, "mclk", icd); + if (IS_ERR(icd->clk)) { + ret = PTR_ERR(icd->clk); + goto eclkreg; + } + + ret = v4l2_async_notifier_register(&ici->v4l2_dev, &sasc->notifier); + if (!ret) + return 0; +eclkreg: + icd->clk = NULL; + platform_device_del(sasc->pdev); +eaddpdev: + platform_device_put(sasc->pdev); +eallocpdev: + devm_kfree(ici->v4l2_dev.dev, sasc); + dev_err(ici->v4l2_dev.dev, "group probe failed: %d\n", ret); + + return ret; +} + +static void scan_of_host(struct soc_camera_host *ici) +{ + struct device *dev = ici->v4l2_dev.dev; + struct device_node *np = dev->of_node; + struct device_node *epn = NULL, *ren; + unsigned int i; + + for (i = 0; ; i++) { + epn = of_graph_get_next_endpoint(np, epn); + if (!epn) + break; + + ren = of_graph_get_remote_port(epn); + if (!ren) { + dev_notice(dev, "no remote for %s\n", + of_node_full_name(epn)); + continue; + } + + /* so we now have a remote node to connect */ + if (!i) + soc_of_bind(ici, epn, ren->parent); + + of_node_put(epn); + of_node_put(ren); + + if (i) { + dev_err(dev, "multiple subdevices aren't supported yet!\n"); + break; + } + } +} + +#else +static inline void scan_of_host(struct soc_camera_host *ici) { } +#endif + /* Called during host-driver probe */ static int soc_camera_probe(struct soc_camera_host *ici, struct soc_camera_device *icd) @@ -1836,7 +1961,9 @@ int soc_camera_host_register(struct soc_camera_host *ici) mutex_init(&ici->host_lock); mutex_init(&ici->clk_lock); - if (ici->asd_sizes) + if (ici->v4l2_dev.dev->of_node) + scan_of_host(ici); + else if (ici->asd_sizes) /* * No OF, host with a list of subdevices. Don't try to mix * modes by initialising some groups statically and some