| /* |
| * Copyright 2017-2018 NXP |
| */ |
| /* |
| * The code contained herein is licensed under the GNU General Public |
| * License. You may obtain a copy of the GNU General Public License |
| * Version 2 or later at the following locations: |
| * |
| * http://www.opensource.org/licenses/gpl-license.html |
| * http://www.gnu.org/copyleft/gpl.html |
| */ |
| |
| #include <linux/bug.h> |
| #include <linux/clk.h> |
| #include <linux/clk-provider.h> |
| #include <linux/device.h> |
| #include <linux/errno.h> |
| #include <linux/i2c.h> |
| #include <linux/kernel.h> |
| #include <linux/list.h> |
| #include <linux/module.h> |
| #include <linux/of.h> |
| #include <linux/of_platform.h> |
| #include <linux/of_device.h> |
| #include <linux/of_graph.h> |
| #include <linux/platform_device.h> |
| #include <linux/pm_runtime.h> |
| #include <linux/types.h> |
| #include <linux/slab.h> |
| #include <media/v4l2-fwnode.h> |
| #include <media/v4l2-async.h> |
| #include <media/v4l2-ctrls.h> |
| #include <media/media-device.h> |
| |
| #include "mxc-media-dev.h" |
| #include "mxc-isi-core.h" |
| #include "mxc-mipi-csi2.h" |
| #include "mxc-parallel-csi.h" |
| |
| /*create default links between registered entities */ |
| static int mxc_md_create_links(struct mxc_md *mxc_md) |
| { |
| struct media_entity *source, *sink; |
| struct mxc_isi_dev *mxc_isi; |
| struct mxc_sensor_info *sensor; |
| struct mxc_mipi_csi2_dev *mipi_csi2; |
| struct mxc_parallel_csi_dev *pcsidev; |
| int num_sensors = mxc_md->subdev_notifier.num_subdevs; |
| int i, j, ret = 0; |
| u16 source_pad, sink_pad; |
| u32 flags; |
| u32 mipi_vc = 0; |
| |
| /* Create links between each ISI's subdev and video node */ |
| flags = MEDIA_LNK_FL_ENABLED; |
| for (i = 0; i < MXC_ISI_MAX_DEVS; i++) { |
| mxc_isi = mxc_md->mxc_isi[i]; |
| if (!mxc_isi) |
| continue; |
| |
| /* Connect ISI source to video device */ |
| source = &mxc_isi->isi_cap.sd.entity; |
| sink = &mxc_isi->isi_cap.vdev.entity; |
| sink_pad = 0; |
| |
| switch (mxc_isi->interface[OUT_PORT]) { |
| case ISI_OUTPUT_INTERFACE_DC0: |
| source_pad = MXC_ISI_SD_PAD_SOURCE_DC0; |
| break; |
| case ISI_OUTPUT_INTERFACE_DC1: |
| source_pad = MXC_ISI_SD_PAD_SOURCE_DC1; |
| break; |
| case ISI_OUTPUT_INTERFACE_MEM: |
| source_pad = MXC_ISI_SD_PAD_SOURCE_MEM; |
| break; |
| default: |
| v4l2_err(&mxc_md->v4l2_dev, "Wrong output interface: %x\n", |
| mxc_isi->interface[OUT_PORT]); |
| return -EINVAL; |
| } |
| |
| ret = media_create_pad_link(source, source_pad, |
| sink, sink_pad, flags); |
| if (ret) { |
| v4l2_err(&mxc_md->v4l2_dev, "Failed created link [%s] %c> [%s]\n", |
| source->name, flags ? '=' : '-', sink->name); |
| break; |
| } |
| |
| /* Notify capture subdev entity ,ISI cap link setup */ |
| ret = media_entity_call(source, link_setup, &source->pads[source_pad], |
| &sink->pads[sink_pad], flags); |
| if (ret) { |
| v4l2_err(&mxc_md->v4l2_dev, "failed call link_setup [%s] %c> [%s]\n", |
| source->name, flags ? '=' : '-', sink->name); |
| break; |
| } |
| |
| v4l2_info(&mxc_md->v4l2_dev, "created link [%s] %c> [%s]\n", |
| source->name, flags ? '=' : '-', sink->name); |
| |
| /* Connect MIPI/HDMI/Mem source to ISI sink */ |
| sink = &mxc_isi->isi_cap.sd.entity; |
| |
| switch (mxc_isi->interface[IN_PORT]) { |
| |
| case ISI_INPUT_INTERFACE_MIPI0_CSI2: |
| if (mxc_md->mipi_csi2[0] == NULL) |
| continue; |
| source = &mxc_md->mipi_csi2[0]->sd.entity; |
| |
| switch (mxc_isi->interface[SUB_IN_PORT]) { |
| case ISI_INPUT_SUB_INTERFACE_VC1: |
| source_pad = MXC_MIPI_CSI2_VC1_PAD_SOURCE; |
| sink_pad = MXC_ISI_SD_PAD_SINK_MIPI0_VC1; |
| break; |
| case ISI_INPUT_SUB_INTERFACE_VC2: |
| source_pad = MXC_MIPI_CSI2_VC2_PAD_SOURCE; |
| sink_pad = MXC_ISI_SD_PAD_SINK_MIPI0_VC2; |
| break; |
| case ISI_INPUT_SUB_INTERFACE_VC3: |
| source_pad = MXC_MIPI_CSI2_VC3_PAD_SOURCE; |
| sink_pad = MXC_ISI_SD_PAD_SINK_MIPI0_VC3; |
| break; |
| default: |
| source_pad = MXC_MIPI_CSI2_VC0_PAD_SOURCE; |
| sink_pad = MXC_ISI_SD_PAD_SINK_MIPI0_VC0; |
| break; |
| } |
| break; |
| |
| case ISI_INPUT_INTERFACE_MIPI1_CSI2: |
| if (mxc_md->mipi_csi2[1] == NULL) |
| continue; |
| source = &mxc_md->mipi_csi2[1]->sd.entity; |
| |
| switch (mxc_isi->interface[SUB_IN_PORT]) { |
| case ISI_INPUT_SUB_INTERFACE_VC1: |
| source_pad = MXC_MIPI_CSI2_VC1_PAD_SOURCE; |
| sink_pad = MXC_ISI_SD_PAD_SINK_MIPI1_VC1; |
| break; |
| case ISI_INPUT_SUB_INTERFACE_VC2: |
| source_pad = MXC_MIPI_CSI2_VC2_PAD_SOURCE; |
| sink_pad = MXC_ISI_SD_PAD_SINK_MIPI1_VC2; |
| break; |
| case ISI_INPUT_SUB_INTERFACE_VC3: |
| source_pad = MXC_MIPI_CSI2_VC3_PAD_SOURCE; |
| sink_pad = MXC_ISI_SD_PAD_SINK_MIPI1_VC3; |
| break; |
| default: |
| source_pad = MXC_MIPI_CSI2_VC0_PAD_SOURCE; |
| sink_pad = MXC_ISI_SD_PAD_SINK_MIPI1_VC0; |
| break; |
| } |
| break; |
| case ISI_INPUT_INTERFACE_PARALLEL_CSI: |
| if (mxc_md->pcsidev == NULL) |
| continue; |
| source = &mxc_md->pcsidev->sd.entity; |
| source_pad = MXC_PARALLEL_CSI_PAD_SOURCE; |
| sink_pad = MXC_ISI_SD_PAD_SINK_PARALLEL_CSI; |
| break; |
| |
| case ISI_INPUT_INTERFACE_HDMI: |
| if (mxc_md->hdmi_rx == NULL) |
| continue; |
| source = &mxc_md->hdmi_rx->sd.entity; |
| source_pad = MXC_HDMI_RX_PAD_SOURCE; |
| sink_pad = MXC_ISI_SD_PAD_SINK_HDMI; |
| break; |
| |
| case ISI_INPUT_INTERFACE_DC0: |
| case ISI_INPUT_INTERFACE_DC1: |
| case ISI_INPUT_INTERFACE_MEM: |
| default: |
| v4l2_err(&mxc_md->v4l2_dev, "Not support input interface: %x\n", |
| mxc_isi->interface[IN_PORT]); |
| return -EINVAL; |
| } |
| /* Create link MIPI/HDMI to ISI */ |
| ret = media_create_pad_link(source, source_pad, sink, sink_pad, flags); |
| if (ret) { |
| v4l2_err(&mxc_md->v4l2_dev, "created link [%s] %c> [%s] fail\n", |
| source->name, flags ? '=' : '-', sink->name); |
| break; |
| } |
| |
| /* Notify ISI subdev entity */ |
| ret = media_entity_call(sink, link_setup, &sink->pads[sink_pad], |
| &source->pads[source_pad], 0); |
| if (ret) |
| break; |
| |
| /* Notify MIPI/HDMI entity */ |
| ret = media_entity_call(source, link_setup, &source->pads[source_pad], |
| &sink->pads[sink_pad], 0); |
| if (ret) |
| break; |
| |
| v4l2_info(&mxc_md->v4l2_dev, "created link [%s] %c> [%s]\n", |
| source->name, flags ? '=' : '-', sink->name); |
| } |
| |
| /* Connect MIPI Sensor to MIPI CSI2 */ |
| for (i = 0; i < num_sensors; i++) { |
| sensor = &mxc_md->sensor[i]; |
| if (sensor == NULL || sensor->sd == NULL) |
| continue; |
| |
| if (mxc_md->parallel_csi && !sensor->mipi_mode) { |
| pcsidev = mxc_md->pcsidev; |
| if (pcsidev == NULL) |
| continue; |
| source = &sensor->sd->entity; |
| sink = &pcsidev->sd.entity; |
| |
| source_pad = 0; |
| sink_pad = MXC_PARALLEL_CSI_PAD_SINK; |
| |
| ret = media_create_pad_link(source, source_pad, sink, sink_pad, |
| MEDIA_LNK_FL_IMMUTABLE | MEDIA_LNK_FL_ENABLED); |
| if (ret) |
| return ret; |
| |
| /* Notify MIPI subdev entity */ |
| ret = media_entity_call(sink, link_setup, &sink->pads[sink_pad], |
| &source->pads[source_pad], 0); |
| if (ret) |
| return ret; |
| |
| /* Notify MIPI sensor subdev entity */ |
| ret = media_entity_call(source, link_setup, &source->pads[source_pad], |
| &sink->pads[sink_pad], 0); |
| if (ret) |
| return ret; |
| v4l2_info(&mxc_md->v4l2_dev, "created link [%s] => [%s]\n", |
| sensor->sd->entity.name, pcsidev->sd.entity.name); |
| } else if (mxc_md->mipi_csi2) { |
| mipi_csi2 = mxc_md->mipi_csi2[sensor->id]; |
| if (mipi_csi2 == NULL) |
| continue; |
| source = &sensor->sd->entity; |
| sink = &mipi_csi2->sd.entity; |
| |
| source_pad = 0; /* sensor source pad: MIPI_CSI2_SENS_VC0_PAD_SOURCE */ |
| sink_pad = source_pad; /* mipi sink pad: MXC_MIPI_CSI2_VC0_PAD_SINK; */ |
| |
| if (mipi_csi2->vchannel == true) |
| mipi_vc = 4; |
| else |
| mipi_vc = 1; |
| |
| for (j = 0; j < mipi_vc; j++) { |
| ret = media_create_pad_link(source, source_pad + j, sink, sink_pad + j, |
| MEDIA_LNK_FL_IMMUTABLE | MEDIA_LNK_FL_ENABLED); |
| if (ret) |
| return ret; |
| |
| /* Notify MIPI subdev entity */ |
| ret = media_entity_call(sink, link_setup, &sink->pads[sink_pad + j], |
| &source->pads[source_pad + j], 0); |
| if (ret) |
| return ret; |
| |
| /* Notify MIPI sensor subdev entity */ |
| ret = media_entity_call(source, link_setup, &source->pads[source_pad + j], |
| &sink->pads[sink_pad + j], 0); |
| if (ret) |
| return ret; |
| } |
| v4l2_info(&mxc_md->v4l2_dev, "created link [%s] => [%s]\n", |
| sensor->sd->entity.name, mipi_csi2->sd.entity.name); |
| } |
| } |
| dev_info(&mxc_md->pdev->dev, "%s\n", __func__); |
| return 0; |
| } |
| |
| static int subdev_notifier_bound(struct v4l2_async_notifier *notifier, |
| struct v4l2_subdev *sd, |
| struct v4l2_async_subdev *asd) |
| { |
| struct mxc_md *mxc_md = notifier_to_mxc_md(notifier); |
| struct mxc_sensor_info *sensor = NULL; |
| int i; |
| |
| dev_dbg(&mxc_md->pdev->dev, "%s\n", __func__); |
| /* Find platform data for this sensor subdev */ |
| for (i = 0; i < ARRAY_SIZE(mxc_md->sensor); i++) { |
| if (mxc_md->sensor[i].asd.match.fwnode.fwnode == |
| of_fwnode_handle(sd->dev->of_node)) |
| sensor = &mxc_md->sensor[i]; |
| } |
| |
| if (sensor == NULL) |
| return -EINVAL; |
| |
| sd->grp_id = GRP_ID_MXC_SENSOR; |
| |
| sensor->sd = sd; |
| |
| mxc_md->num_sensors++; |
| |
| v4l2_info(&mxc_md->v4l2_dev, "Registered sensor subdevice: %s (%d)\n", |
| sd->name, mxc_md->num_sensors); |
| |
| return 0; |
| } |
| |
| static int subdev_notifier_complete(struct v4l2_async_notifier *notifier) |
| { |
| struct mxc_md *mxc_md = notifier_to_mxc_md(notifier); |
| int ret; |
| |
| dev_dbg(&mxc_md->pdev->dev, "%s\n", __func__); |
| mutex_lock(&mxc_md->media_dev.graph_mutex); |
| |
| ret = mxc_md_create_links(mxc_md); |
| if (ret < 0) |
| goto unlock; |
| |
| mxc_md->link_status = 1; |
| |
| ret = v4l2_device_register_subdev_nodes(&mxc_md->v4l2_dev); |
| unlock: |
| mutex_unlock(&mxc_md->media_dev.graph_mutex); |
| if (ret < 0) { |
| v4l2_err(&mxc_md->v4l2_dev, "%s error exit\n", __func__); |
| return ret; |
| } |
| |
| return media_device_register(&mxc_md->media_dev); |
| } |
| |
| |
| /** |
| * mxc_sensor_notify - v4l2_device notification from a sensor subdev |
| */ |
| void mxc_sensor_notify(struct v4l2_subdev *sd, unsigned int notification, |
| void *arg) |
| { |
| return; |
| } |
| |
| /* Register mipi sensor / Parallel CSI / HDMI Rx sub-devices */ |
| static int register_sensor_entities(struct mxc_md *mxc_md) |
| { |
| struct device_node *parent = mxc_md->pdev->dev.of_node; |
| struct device_node *node, *ep, *rem; |
| struct v4l2_fwnode_endpoint endpoint; |
| int index = 0; |
| |
| mxc_md->num_sensors = 0; |
| |
| /* Attach sensors linked to MIPI CSI2 / paralle csi / HDMI Rx */ |
| for_each_available_child_of_node(parent, node) { |
| struct device_node *port; |
| |
| if (!of_node_cmp(node->name, "hdmi_rx")) { |
| mxc_md->sensor[index].asd.match_type = V4L2_ASYNC_MATCH_FWNODE; |
| mxc_md->sensor[index].asd.match.fwnode.fwnode = of_fwnode_handle(node); |
| mxc_md->async_subdevs[index] = &mxc_md->sensor[index].asd; |
| |
| mxc_md->num_sensors++; |
| index++; |
| continue; |
| } |
| |
| if (of_node_cmp(node->name, "csi") && |
| of_node_cmp(node->name, "pcsi")) |
| continue; |
| |
| if (!of_device_is_available(node)) |
| continue; |
| |
| /* csi2 node have only port */ |
| port = of_get_next_child(node, NULL); |
| if (!port) |
| continue; |
| |
| /* port can have only endpoint */ |
| ep = of_get_next_child(port, NULL); |
| if (!ep) |
| return -EINVAL; |
| |
| v4l2_fwnode_endpoint_parse(of_fwnode_handle(ep), &endpoint); |
| if (WARN_ON(endpoint.base.port >= MXC_MAX_SENSORS)) { |
| v4l2_err(&mxc_md->v4l2_dev, "Failed to get sensor endpoint\n"); |
| return -EINVAL; |
| } |
| |
| mxc_md->sensor[index].id = endpoint.base.port; |
| |
| if (!of_node_cmp(node->name, "csi")) |
| mxc_md->sensor[index].mipi_mode = true; |
| |
| /* remote port---sensor node */ |
| rem = of_graph_get_remote_port_parent(ep); |
| of_node_put(ep); |
| if (rem == NULL) { |
| v4l2_info(&mxc_md->v4l2_dev, "Remote device at %s not found\n", |
| ep->full_name); |
| continue; |
| } |
| |
| mxc_md->sensor[index].asd.match_type = V4L2_ASYNC_MATCH_FWNODE; |
| mxc_md->sensor[index].asd.match.fwnode.fwnode = of_fwnode_handle(rem); |
| mxc_md->async_subdevs[index] = &mxc_md->sensor[index].asd; |
| |
| mxc_md->num_sensors++; |
| |
| index++; |
| } |
| |
| return 0; |
| } |
| |
| static int register_isi_entity(struct mxc_md *mxc_md, struct mxc_isi_dev *mxc_isi) |
| { |
| struct v4l2_subdev *sd = &mxc_isi->isi_cap.sd; |
| int ret; |
| |
| dev_dbg(&mxc_md->pdev->dev, "%s\n", __func__); |
| if (WARN_ON(mxc_isi->id >= MXC_ISI_MAX_DEVS)) |
| return -EBUSY; |
| |
| sd->grp_id = GRP_ID_MXC_ISI; |
| |
| ret = v4l2_device_register_subdev(&mxc_md->v4l2_dev, sd); |
| if (!ret) |
| mxc_md->mxc_isi[mxc_isi->id] = mxc_isi; |
| else |
| v4l2_err(&mxc_md->v4l2_dev, "Failed to register ISI.%d (%d)\n", |
| mxc_isi->id, ret); |
| return ret; |
| } |
| |
| static int register_mipi_csi2_entity(struct mxc_md *mxc_md, |
| struct mxc_mipi_csi2_dev *mipi_csi2) |
| { |
| struct v4l2_subdev *sd = &mipi_csi2->sd; |
| int ret; |
| |
| if (WARN_ON(mipi_csi2->id >= MXC_MIPI_CSI2_MAX_DEVS)) |
| return -ENOENT; |
| |
| sd->grp_id = GRP_ID_MXC_MIPI_CSI2; |
| ret = v4l2_device_register_subdev(&mxc_md->v4l2_dev, sd); |
| if (!ret) |
| mxc_md->mipi_csi2[mipi_csi2->id] = mipi_csi2; |
| else |
| v4l2_err(&mxc_md->v4l2_dev, |
| "Failed to register MIPI-CSIS.%d (%d)\n", mipi_csi2->id, ret); |
| return ret; |
| } |
| |
| static int register_parallel_csi_entity(struct mxc_md *mxc_md, |
| struct mxc_parallel_csi_dev *pcsidev) |
| { |
| struct v4l2_subdev *sd = &pcsidev->sd; |
| int ret; |
| |
| sd->grp_id = GRP_ID_MXC_PARALLEL_CSI; |
| ret = v4l2_device_register_subdev(&mxc_md->v4l2_dev, sd); |
| if (!ret) |
| mxc_md->pcsidev = pcsidev; |
| else |
| v4l2_err(&mxc_md->v4l2_dev, |
| "Failed to register PARALLEL CSI ret=(%d)\n", ret); |
| return ret; |
| } |
| |
| static int register_hdmi_rx_entity(struct mxc_md *mxc_md, |
| struct mxc_hdmi_rx_dev *hdmi_rx) |
| { |
| struct v4l2_subdev *sd = &hdmi_rx->sd;; |
| |
| dev_dbg(&mxc_md->pdev->dev, "%s\n", __func__); |
| sd->grp_id = GRP_ID_MXC_HDMI_RX; |
| mxc_md->hdmi_rx = hdmi_rx; |
| |
| return 0; |
| } |
| |
| static int mxc_md_register_platform_entity(struct mxc_md *mxc_md, |
| struct platform_device *pdev, |
| int plat_entity) |
| { |
| struct device *dev = &pdev->dev; |
| int ret = -EPROBE_DEFER; |
| void *drvdata; |
| |
| /* Lock to ensure dev->driver won't change. */ |
| device_lock(dev); |
| |
| if (!dev->driver || !try_module_get(dev->driver->owner)) |
| goto dev_unlock; |
| |
| drvdata = dev_get_drvdata(dev); |
| /* Some subdev didn't probe successfully id drvdata is NULL */ |
| if (drvdata) { |
| switch (plat_entity) { |
| case IDX_ISI: |
| ret = register_isi_entity(mxc_md, drvdata); |
| break; |
| case IDX_MIPI_CSI2: |
| ret = register_mipi_csi2_entity(mxc_md, drvdata); |
| break; |
| case IDX_PARALLEL_CSI: |
| ret = register_parallel_csi_entity(mxc_md, drvdata); |
| break; |
| case IDX_HDMI_RX: |
| ret = register_hdmi_rx_entity(mxc_md, drvdata); |
| break; |
| default: |
| ret = -ENODEV; |
| } |
| } |
| module_put(dev->driver->owner); |
| |
| dev_unlock: |
| device_unlock(dev); |
| if (ret == -EPROBE_DEFER) |
| dev_info(&mxc_md->pdev->dev, "deferring %s device registration\n", |
| dev_name(dev)); |
| else if (ret < 0) |
| dev_err(&mxc_md->pdev->dev, "%s device registration failed (%d)\n", |
| dev_name(dev), ret); |
| |
| return ret; |
| } |
| |
| /* Register ISI, MIPI CSI2 and HDMI Rx Media entities */ |
| static int mxc_md_register_platform_entities(struct mxc_md *mxc_md, |
| struct device_node *parent) |
| { |
| struct device_node *node; |
| int ret = 0; |
| |
| for_each_available_child_of_node(parent, node) { |
| struct platform_device *pdev; |
| int plat_entity = -1; |
| |
| pdev = of_find_device_by_node(node); |
| if (!pdev) |
| continue; |
| |
| /* If driver of any entity isn't ready try all again later. */ |
| if (!strcmp(node->name, ISI_OF_NODE_NAME)) |
| plat_entity = IDX_ISI; |
| else if (!strcmp(node->name, MIPI_CSI2_OF_NODE_NAME)) |
| plat_entity = IDX_MIPI_CSI2; |
| else if (!strcmp(node->name, PARALLEL_CSI_OF_NODE_NAME)) |
| plat_entity = IDX_PARALLEL_CSI; |
| else if (!strcmp(node->name, MXC_HDMI_RX_NODE_NAME)) |
| plat_entity = IDX_HDMI_RX; |
| |
| if (plat_entity >= 0) |
| ret = mxc_md_register_platform_entity(mxc_md, pdev, |
| plat_entity); |
| put_device(&pdev->dev); |
| if (ret < 0) |
| break; |
| } |
| |
| return ret; |
| } |
| |
| static void mxc_md_prepare_for_m2m(struct mxc_md *mxc_md, |
| struct device_node *parent) |
| { |
| struct device *dev = &mxc_md->pdev->dev; |
| struct device_node *node; |
| |
| for_each_available_child_of_node(parent, node) { |
| if (!strcmp(node->name, ISI_OF_NODE_NAME)) { |
| mxc_md->nr_isi++; |
| |
| /* achive ISI channel0 driver data */ |
| if (of_alias_get_id(node, "isi") == 0) { |
| struct platform_device *pdev = |
| of_find_device_by_node(node); |
| |
| if (pdev && pdev->dev.driver) { |
| device_lock(&pdev->dev); |
| mxc_md->mxc_isi[0] = dev_get_drvdata(&pdev->dev); |
| mxc_md->mxc_isi[0]->skip_m2m = 0; |
| device_unlock(&pdev->dev); |
| } |
| put_device(&pdev->dev); |
| } |
| } |
| } |
| |
| dev_dbg(dev, "%s: nr_isi = %d\n", __func__, mxc_md->nr_isi); |
| } |
| |
| static void mxc_md_unregister_entities(struct mxc_md *mxc_md) |
| { |
| int i; |
| |
| for (i = 0; i < MXC_ISI_MAX_DEVS; i++) { |
| struct mxc_isi_dev *dev = mxc_md->mxc_isi[i]; |
| if (dev == NULL) |
| continue; |
| v4l2_device_unregister_subdev(&dev->isi_cap.sd); |
| mxc_md->mxc_isi[i] = NULL; |
| } |
| for (i = 0; i < MXC_MIPI_CSI2_MAX_DEVS; i++) { |
| if (mxc_md->mipi_csi2[i] == NULL) |
| continue; |
| v4l2_device_unregister_subdev(&mxc_md->mipi_csi2[i]->sd); |
| mxc_md->mipi_csi2[i] = NULL; |
| } |
| |
| if (mxc_md->pcsidev) { |
| v4l2_device_unregister_subdev(&mxc_md->pcsidev->sd); |
| mxc_md->pcsidev = NULL; |
| } |
| |
| v4l2_info(&mxc_md->v4l2_dev, "Unregistered all entities\n"); |
| } |
| |
| static int mxc_md_do_clean(struct mxc_md *mxc_md, struct media_pad *pad) |
| { |
| struct device *dev = &mxc_md->pdev->dev; |
| struct media_pad *remote_pad; |
| struct v4l2_subdev *subdev; |
| struct mxc_isi_dev *mxc_isi; |
| |
| remote_pad = media_entity_remote_pad(pad); |
| if (remote_pad == NULL) { |
| dev_err(dev, "%s get remote pad fail\n", __func__); |
| return -ENODEV; |
| } |
| |
| subdev = media_entity_to_v4l2_subdev(remote_pad->entity); |
| if (subdev == NULL) { |
| dev_err(dev, "%s media entity to v4l2 subdev fail\n", __func__); |
| return -ENODEV; |
| } |
| |
| mxc_isi = v4l2_get_subdevdata(subdev); |
| if (mxc_isi == NULL) { |
| dev_err(dev, "%s Can't get subdev %s data\n", __func__, subdev->name); |
| return -ENODEV; |
| } |
| |
| if (mxc_isi->id == 0) |
| mxc_isi->skip_m2m = 1; |
| |
| v4l2_device_unregister_subdev(subdev); |
| media_entity_cleanup(&subdev->entity); |
| |
| dev_info(dev, "clean ISI channel[%d]\n", mxc_isi->id); |
| |
| return 0; |
| } |
| |
| static int mxc_md_clean_channel(struct mxc_md *mxc_md, int index) |
| { |
| struct mxc_sensor_info *sensor = &mxc_md->sensor[index]; |
| struct mxc_mipi_csi2_dev *mipi_csi2; |
| struct mxc_parallel_csi_dev *pcsidev; |
| struct media_pad *local_pad; |
| struct media_entity *local_en; |
| u32 i, mipi_vc = 0; |
| int ret; |
| |
| if (mxc_md->mipi_csi2[index]) { |
| mipi_csi2 = mxc_md->mipi_csi2[index]; |
| |
| if (mipi_csi2->vchannel == true) |
| mipi_vc = 4; |
| else |
| mipi_vc = 1; |
| |
| local_en = &mipi_csi2->sd.entity; |
| if (local_en == NULL) |
| return -ENODEV; |
| |
| for (i = 0; i < mipi_vc; i++) { |
| local_pad = &local_en->pads[MXC_MIPI_CSI2_VC0_PAD_SOURCE + i]; |
| ret = mxc_md_do_clean(mxc_md, local_pad); |
| if (ret < 0) |
| return -ENODEV; |
| } |
| } else if (mxc_md->parallel_csi && !sensor->mipi_mode) { |
| pcsidev = mxc_md->pcsidev; |
| if (pcsidev == NULL) |
| return -ENODEV; |
| |
| local_en = &pcsidev->sd.entity; |
| if (local_en == NULL) |
| return -ENODEV; |
| |
| local_pad = &local_en->pads[MXC_PARALLEL_CSI_PAD_SOURCE]; |
| ret = mxc_md_do_clean(mxc_md, local_pad); |
| if (ret < 0) |
| return -ENODEV; |
| } |
| |
| return 0; |
| } |
| |
| static int mxc_md_clean_unlink_channels(struct mxc_md *mxc_md) |
| { |
| struct mxc_sensor_info *sensor; |
| int num_subdevs = mxc_md->subdev_notifier.num_subdevs; |
| int i, ret; |
| |
| for (i = 0; i < num_subdevs; i++) { |
| sensor = &mxc_md->sensor[i]; |
| if (sensor->sd != NULL) |
| continue; |
| |
| ret = mxc_md_clean_channel(mxc_md, i); |
| if (ret < 0) { |
| pr_err("%s: clean channel fail(%d)\n", __func__, i); |
| return ret; |
| } |
| } |
| |
| return 0; |
| } |
| |
| static void mxc_md_unregister_all(struct mxc_md *mxc_md) |
| { |
| struct mxc_isi_dev *mxc_isi; |
| int i; |
| |
| for (i = 0; i < MXC_ISI_MAX_DEVS; i++) { |
| mxc_isi = mxc_md->mxc_isi[i]; |
| if (!mxc_isi) |
| continue; |
| |
| if (mxc_isi->id == 0) |
| mxc_isi->skip_m2m = 1; |
| |
| v4l2_device_unregister_subdev(&mxc_isi->isi_cap.sd); |
| media_entity_cleanup(&mxc_isi->isi_cap.sd.entity); |
| |
| dev_info(&mxc_isi->pdev->dev, "%s unregister ISI channel[%d]\n", |
| __func__, mxc_isi->id); |
| } |
| } |
| |
| static int mxc_md_link_notify(struct media_link *link, unsigned int flags, |
| unsigned int notification) |
| { |
| return 0; |
| } |
| |
| static const struct media_device_ops mxc_md_ops = { |
| .link_notify = mxc_md_link_notify, |
| }; |
| |
| |
| static int mxc_md_probe(struct platform_device *pdev) |
| { |
| struct device *dev = &pdev->dev; |
| struct v4l2_device *v4l2_dev; |
| struct mxc_md *mxc_md; |
| int ret; |
| |
| mxc_md = devm_kzalloc(dev, sizeof(*mxc_md), GFP_KERNEL); |
| if (!mxc_md) |
| return -ENOMEM; |
| |
| mxc_md->pdev = pdev; |
| platform_set_drvdata(pdev, mxc_md); |
| |
| mxc_md->parallel_csi = of_property_read_bool(dev->of_node, "parallel_csi"); |
| |
| /* register media device */ |
| strlcpy(mxc_md->media_dev.model, "FSL Capture Media Deivce", |
| sizeof(mxc_md->media_dev.model)); |
| mxc_md->media_dev.ops = &mxc_md_ops; |
| mxc_md->media_dev.dev = dev; |
| |
| /* register v4l2 device */ |
| v4l2_dev = &mxc_md->v4l2_dev; |
| v4l2_dev->mdev = &mxc_md->media_dev; |
| v4l2_dev->notify = mxc_sensor_notify; |
| strlcpy(v4l2_dev->name, "mx8-img-md", sizeof(v4l2_dev->name)); |
| |
| media_device_init(&mxc_md->media_dev); |
| |
| ret = v4l2_device_register(dev, &mxc_md->v4l2_dev); |
| if (ret < 0) { |
| v4l2_err(v4l2_dev, "Failed to register v4l2_device: %d\n", ret); |
| goto err_md; |
| } |
| |
| /* prepare for registration m2m */ |
| mxc_md_prepare_for_m2m(mxc_md, dev->of_node); |
| |
| ret = mxc_md_register_platform_entities(mxc_md, dev->of_node); |
| if (ret < 0) |
| goto err_v4l2_dev; |
| |
| ret = register_sensor_entities(mxc_md); |
| if (ret < 0) |
| goto err_m_ent; |
| |
| if (mxc_md->num_sensors > 0) { |
| mxc_md->subdev_notifier.subdevs = mxc_md->async_subdevs; |
| mxc_md->subdev_notifier.num_subdevs = mxc_md->num_sensors; |
| mxc_md->subdev_notifier.bound = subdev_notifier_bound; |
| mxc_md->subdev_notifier.complete = subdev_notifier_complete; |
| mxc_md->num_sensors = 0; |
| mxc_md->link_status = 0; |
| |
| ret = v4l2_async_notifier_register(&mxc_md->v4l2_dev, |
| &mxc_md->subdev_notifier); |
| if (ret < 0) { |
| dev_warn(&mxc_md->pdev->dev, "Sensor register failed\n"); |
| goto err_m_ent; |
| } |
| |
| if (!mxc_md->link_status) { |
| if (mxc_md->num_sensors > 0) { |
| ret = subdev_notifier_complete(&mxc_md->subdev_notifier); |
| if (ret < 0) |
| goto err_m_ent; |
| |
| mxc_md_clean_unlink_channels(mxc_md); |
| } else { |
| /* no sensors connected */ |
| mxc_md_unregister_all(mxc_md); |
| } |
| } |
| } |
| |
| return 0; |
| |
| err_m_ent: |
| mxc_md_unregister_entities(mxc_md); |
| err_v4l2_dev: |
| v4l2_device_unregister(&mxc_md->v4l2_dev); |
| err_md: |
| media_device_cleanup(&mxc_md->media_dev); |
| return ret; |
| } |
| |
| static int mxc_md_remove(struct platform_device *pdev) |
| { |
| struct mxc_md *mxc_md = platform_get_drvdata(pdev); |
| |
| if (!mxc_md) |
| return 0; |
| |
| v4l2_async_notifier_unregister(&mxc_md->subdev_notifier); |
| |
| v4l2_device_unregister(&mxc_md->v4l2_dev); |
| mxc_md_unregister_entities(mxc_md); |
| media_device_unregister(&mxc_md->media_dev); |
| media_device_cleanup(&mxc_md->media_dev); |
| |
| return 0; |
| } |
| |
| static const struct of_device_id mxc_md_of_match[] = { |
| { .compatible = "fsl,mxc-md",}, |
| { /* sentinel */ }, |
| }; |
| MODULE_DEVICE_TABLE(of, mxc_md_of_match); |
| |
| |
| static struct platform_driver mxc_md_driver = { |
| .driver = { |
| .name = MXC_MD_DRIVER_NAME, |
| .of_match_table = mxc_md_of_match, |
| }, |
| .probe = mxc_md_probe, |
| .remove = mxc_md_remove, |
| }; |
| |
| module_platform_driver(mxc_md_driver); |
| |
| MODULE_AUTHOR("Freescale Semiconductor, Inc."); |
| MODULE_DESCRIPTION("MXC Media Device driver"); |
| MODULE_LICENSE("GPL"); |
| MODULE_ALIAS("platform:" MXC_MD_DRIVER_NAME); |