source "drivers/media/platform/rockchip/Kconfig"
source "drivers/media/platform/samsung/Kconfig"
source "drivers/media/platform/st/Kconfig"
+source "drivers/media/platform/starfive/Kconfig"
source "drivers/media/platform/sunxi/Kconfig"
source "drivers/media/platform/ti/Kconfig"
source "drivers/media/platform/verisilicon/Kconfig"
obj-y += rockchip/
obj-y += samsung/
obj-y += st/
+obj-y += starfive/
obj-y += sunxi/
obj-y += ti/
obj-y += verisilicon/
--- /dev/null
+# SPDX-License-Identifier: GPL-2.0-only
+
+comment "Starfive media platform drivers"
+
+config VIDEO_STARFIVE_CAMSS
+ tristate "Starfive Camera Subsystem driver"
+ depends on V4L_PLATFORM_DRIVERS
+ depends on VIDEO_DEV && OF
+ select MEDIA_CONTROLLER
+ select VIDEO_V4L2_SUBDEV_API
+ select VIDEOBUF2_DMA_CONTIG
+ select V4L2_FWNODE
+ help
+ Enable this to support for the Starfive Camera subsystem
+ found on Starfive JH7110 SoC.
+
+ To compile this driver as a module, choose M here: the
+ module will be called stf-camss.
--- /dev/null
+# SPDX-License-Identifier: GPL-2.0
+#
+# Makefile for StarFive camera subsystem driver.
+#
+
+starfive-camss-objs += \
+ stf_camss.o \
+ stf_isp.o \
+ stf_isp_hw_ops.o \
+ stf_video.o \
+ stf_vin.o \
+ stf_vin_hw_ops.o
+
+obj-$(CONFIG_VIDEO_STARFIVE_CAMSS) += starfive-camss.o \
--- /dev/null
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * stf_camss.c
+ *
+ * Starfive Camera Subsystem driver
+ *
+ * Copyright (C) 2021-2023 StarFive Technology Co., Ltd.
+ */
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_graph.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/videodev2.h>
+#include <media/media-device.h>
+#include <media/v4l2-async.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-mc.h>
+
+#include "stf_camss.h"
+
+static struct clk_bulk_data stfcamss_clocks[] = {
+ { .id = "clk_apb_func" },
+ { .id = "clk_wrapper_clk_c" },
+ { .id = "clk_dvp_inv" },
+ { .id = "clk_axiwr" },
+ { .id = "clk_mipi_rx0_pxl" },
+ { .id = "clk_ispcore_2x" },
+ { .id = "clk_isp_axi" },
+};
+
+static struct reset_control_bulk_data stfcamss_resets[] = {
+ { .id = "rst_wrapper_p" },
+ { .id = "rst_wrapper_c" },
+ { .id = "rst_axird" },
+ { .id = "rst_axiwr" },
+ { .id = "rst_isp_top_n" },
+ { .id = "rst_isp_top_axi" },
+};
+
+int stfcamss_get_mem_res(struct platform_device *pdev,
+ struct stfcamss *stfcamss)
+{
+ stfcamss->syscon_base =
+ devm_platform_ioremap_resource_byname(pdev, "syscon");
+ if (IS_ERR(stfcamss->syscon_base))
+ return PTR_ERR(stfcamss->syscon_base);
+
+ stfcamss->isp_base =
+ devm_platform_ioremap_resource_byname(pdev, "isp");
+ if (IS_ERR(stfcamss->isp_base))
+ return PTR_ERR(stfcamss->isp_base);
+
+ return 0;
+}
+
+/*
+ * stfcamss_find_sensor - Find a linked media entity which represents a sensor
+ * @entity: Media entity to start searching from
+ *
+ * Return a pointer to sensor media entity or NULL if not found
+ */
+struct media_entity *stfcamss_find_sensor(struct media_entity *entity)
+{
+ struct media_pad *pad;
+
+ while (1) {
+ if (!entity->pads)
+ return NULL;
+
+ pad = &entity->pads[0];
+ if (!(pad->flags & MEDIA_PAD_FL_SINK))
+ return NULL;
+
+ pad = media_pad_remote_pad_first(pad);
+ if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
+ return NULL;
+
+ entity = pad->entity;
+
+ if (entity->function == MEDIA_ENT_F_CAM_SENSOR)
+ return entity;
+ }
+}
+
+/*
+ * stfcamss_of_parse_endpoint_node - Parse port endpoint node
+ * @dev: Device
+ * @node: Device node to be parsed
+ * @csd: Parsed data from port endpoint node
+ *
+ * Return 0 on success or a negative error code on failure
+ */
+static int stfcamss_of_parse_endpoint_node(struct device *dev,
+ struct device_node *node,
+ struct stfcamss_async_subdev *csd)
+{
+ struct v4l2_fwnode_endpoint vep = { { 0 } };
+
+ v4l2_fwnode_endpoint_parse(of_fwnode_handle(node), &vep);
+ st_debug(ST_CAMSS, "%s: vep.base.port = 0x%x, id = 0x%x\n",
+ __func__, vep.base.port, vep.base.id);
+
+ csd->port = vep.base.port;
+ switch (csd->port) {
+ case PORT_NUMBER_DVP_SENSOR:
+ break;
+ case PORT_NUMBER_CSI2RX:
+ break;
+ default:
+ break;
+ };
+
+ return 0;
+}
+
+/*
+ * stfcamss_of_parse_ports - Parse ports node
+ * @stfcamss: STFCAMSS device
+ *
+ * Return number of "port" nodes found in "ports" node
+ */
+static int stfcamss_of_parse_ports(struct stfcamss *stfcamss)
+{
+ struct device *dev = stfcamss->dev;
+ struct device_node *node = NULL;
+ struct device_node *remote = NULL;
+ int ret, num_subdevs = 0;
+
+ for_each_endpoint_of_node(dev->of_node, node) {
+ struct stfcamss_async_subdev *csd;
+
+ if (!of_device_is_available(node))
+ continue;
+
+ remote = of_graph_get_remote_port_parent(node);
+ if (!remote) {
+ st_err(ST_CAMSS, "Cannot get remote parent\n");
+ ret = -EINVAL;
+ goto err_cleanup;
+ }
+
+ csd = v4l2_async_nf_add_fwnode(&stfcamss->notifier,
+ of_fwnode_handle(remote),
+ struct stfcamss_async_subdev);
+ of_node_put(remote);
+ if (IS_ERR(csd)) {
+ ret = PTR_ERR(csd);
+ goto err_cleanup;
+ }
+
+ ret = stfcamss_of_parse_endpoint_node(dev, node, csd);
+ if (ret < 0)
+ goto err_cleanup;
+
+ num_subdevs++;
+ }
+
+ return num_subdevs;
+
+err_cleanup:
+ of_node_put(node);
+ return ret;
+}
+
+/*
+ * stfcamss_init_subdevices - Initialize subdev structures and resources
+ * @stfcamss: STFCAMSS device
+ *
+ * Return 0 on success or a negative error code on failure
+ */
+static int stfcamss_init_subdevices(struct stfcamss *stfcamss)
+{
+ int ret;
+
+ ret = stf_isp_subdev_init(stfcamss);
+ if (ret < 0) {
+ st_err(ST_CAMSS,
+ "Failed to init stf_isp sub-device: %d\n", ret);
+ return ret;
+ }
+
+ ret = stf_vin_subdev_init(stfcamss);
+ if (ret < 0) {
+ st_err(ST_CAMSS,
+ "Failed to init stf_vin sub-device: %d\n", ret);
+ return ret;
+ }
+ return ret;
+}
+
+static int stfcamss_register_subdevices(struct stfcamss *stfcamss)
+{
+ int ret;
+ struct stf_vin_dev *vin_dev = stfcamss->vin_dev;
+ struct stf_isp_dev *isp_dev = stfcamss->isp_dev;
+
+ ret = stf_isp_register(isp_dev, &stfcamss->v4l2_dev);
+ if (ret < 0) {
+ st_err(ST_CAMSS,
+ "Failed to register stf isp%d entity: %d\n", 0, ret);
+ goto err_reg_isp;
+ }
+
+ ret = stf_vin_register(vin_dev, &stfcamss->v4l2_dev);
+ if (ret < 0) {
+ st_err(ST_CAMSS,
+ "Failed to register vin entity: %d\n", ret);
+ goto err_reg_vin;
+ }
+
+ ret = media_create_pad_link(&isp_dev->subdev.entity,
+ STF_ISP_PAD_SRC,
+ &vin_dev->line[VIN_LINE_ISP].subdev.entity,
+ STF_VIN_PAD_SINK,
+ 0);
+ if (ret < 0) {
+ st_err(ST_CAMSS,
+ "Failed to link %s->%s entities: %d\n",
+ isp_dev->subdev.entity.name,
+ vin_dev->line[VIN_LINE_ISP].subdev.entity.name,
+ ret);
+ goto err_link;
+ }
+
+ ret = media_create_pad_link(
+ &isp_dev->subdev.entity,
+ STF_ISP_PAD_SRC_SS0,
+ &vin_dev->line[VIN_LINE_ISP_SS0].subdev.entity,
+ STF_VIN_PAD_SINK,
+ 0);
+
+ if (ret < 0) {
+ st_err(ST_CAMSS,
+ "Failed to link %s->%s entities: %d\n",
+ isp_dev->subdev.entity.name,
+ vin_dev->line[VIN_LINE_ISP_SS0].subdev.entity.name,
+ ret);
+ goto err_link;
+ }
+
+ ret = media_create_pad_link(
+ &isp_dev->subdev.entity,
+ STF_ISP_PAD_SRC_SS1,
+ &vin_dev->line[VIN_LINE_ISP_SS1].subdev.entity,
+ STF_VIN_PAD_SINK,
+ 0);
+ if (ret < 0) {
+ st_err(ST_CAMSS,
+ "Failed to link %s->%s entities: %d\n",
+ isp_dev->subdev.entity.name,
+ vin_dev->line[VIN_LINE_ISP_SS1].subdev.entity.name,
+ ret);
+ goto err_link;
+ }
+
+ ret = media_create_pad_link(
+ &isp_dev->subdev.entity,
+ STF_ISP_PAD_SRC_RAW,
+ &vin_dev->line[VIN_LINE_ISP_RAW].subdev.entity,
+ STF_VIN_PAD_SINK,
+ 0);
+ if (ret < 0) {
+ st_err(ST_CAMSS,
+ "Failed to link %s->%s entities: %d\n",
+ isp_dev->subdev.entity.name,
+ vin_dev->line[VIN_LINE_ISP_RAW].subdev.entity.name,
+ ret);
+ goto err_link;
+ }
+
+ return ret;
+
+err_link:
+ stf_vin_unregister(stfcamss->vin_dev);
+err_reg_vin:
+ stf_isp_unregister(stfcamss->isp_dev);
+err_reg_isp:
+ return ret;
+}
+
+static void stfcamss_unregister_subdevices(struct stfcamss *stfcamss)
+{
+ stf_isp_unregister(stfcamss->isp_dev);
+ stf_vin_unregister(stfcamss->vin_dev);
+}
+
+static int stfcamss_register_media_subdev_nod(struct v4l2_async_notifier *async,
+ struct v4l2_subdev *sd)
+{
+ struct stfcamss *stfcamss =
+ container_of(async, struct stfcamss, notifier);
+ struct host_data *host_data = v4l2_get_subdev_hostdata(sd);
+ struct media_entity *sensor;
+ struct media_entity *input;
+ int ret;
+ int i, j;
+
+ for (i = 0; host_data->host_entity[i] && (i < HOST_ENTITY_MAX); i++) {
+ sensor = &sd->entity;
+ input = host_data->host_entity[i];
+
+ for (j = 0; j < sensor->num_pads; j++) {
+ if (sensor->pads[j].flags & MEDIA_PAD_FL_SOURCE)
+ break;
+ }
+
+ if (j == sensor->num_pads) {
+ st_err(ST_CAMSS, "No source pad in external entity\n");
+ return -EINVAL;
+ }
+
+ ret = media_create_pad_link(
+ sensor,
+ j,
+ input,
+ STF_PAD_SINK,
+ sensor->function == MEDIA_ENT_F_CAM_SENSOR ?
+ MEDIA_LNK_FL_IMMUTABLE | MEDIA_LNK_FL_ENABLED :
+ 0);
+ if (ret < 0) {
+ st_err(ST_CAMSS, "Failed to link %s->%s entities: %d\n",
+ sensor->name, input->name, ret);
+ return ret;
+ }
+ }
+
+ ret = v4l2_device_register_subdev_nodes(&stfcamss->v4l2_dev);
+ if (ret < 0)
+ return ret;
+
+ if (stfcamss->media_dev.devnode)
+ return ret;
+
+ st_debug(ST_CAMSS, "stfcamss register media device\n");
+ return media_device_register(&stfcamss->media_dev);
+}
+
+static int stfcamss_subdev_notifier_bound(struct v4l2_async_notifier *async,
+ struct v4l2_subdev *subdev,
+ struct v4l2_async_subdev *asd)
+{
+ struct stfcamss *stfcamss =
+ container_of(async, struct stfcamss, notifier);
+ struct stfcamss_async_subdev *csd =
+ container_of(asd, struct stfcamss_async_subdev, asd);
+ enum port_num port = csd->port;
+ struct stf_isp_dev *isp_dev = stfcamss->isp_dev;
+ struct stf_vin_dev *vin_dev = stfcamss->vin_dev;
+ struct host_data *host_data = &stfcamss->host_data;
+
+ switch (port) {
+ case PORT_NUMBER_DVP_SENSOR:
+ host_data->host_entity[0] = NULL;
+ host_data->host_entity[1] = NULL;
+ /* not support DVP sensor */
+ break;
+ case PORT_NUMBER_CSI2RX:
+ host_data->host_entity[0] =
+ &vin_dev->line[VIN_LINE_WR].subdev.entity;
+ host_data->host_entity[1] = &isp_dev->subdev.entity;
+ break;
+ default:
+ break;
+ };
+
+ v4l2_set_subdev_hostdata(subdev, host_data);
+ stfcamss_register_media_subdev_nod(async, subdev);
+
+ return 0;
+}
+
+static const struct v4l2_async_notifier_operations
+stfcamss_subdev_notifier_ops = {
+ .bound = stfcamss_subdev_notifier_bound,
+};
+
+static const struct media_device_ops stfcamss_media_ops = {
+ .link_notify = v4l2_pipeline_link_notify,
+};
+
+/*
+ * stfcamss_probe - Probe STFCAMSS platform device
+ * @pdev: Pointer to STFCAMSS platform device
+ *
+ * Return 0 on success or a negative error code on failure
+ */
+static int stfcamss_probe(struct platform_device *pdev)
+{
+ struct stfcamss *stfcamss;
+ struct device *dev = &pdev->dev;
+ int ret = 0, num_subdevs;
+
+ stfcamss = devm_kzalloc(dev, sizeof(struct stfcamss), GFP_KERNEL);
+ if (!stfcamss)
+ return -ENOMEM;
+
+ stfcamss->isp_dev = devm_kzalloc(dev, sizeof(*stfcamss->isp_dev),
+ GFP_KERNEL);
+ if (!stfcamss->isp_dev) {
+ ret = -ENOMEM;
+ goto err_cam;
+ }
+
+ stfcamss->vin_dev = devm_kzalloc(dev, sizeof(*stfcamss->vin_dev),
+ GFP_KERNEL);
+ if (!stfcamss->vin_dev) {
+ ret = -ENOMEM;
+ goto err_cam;
+ }
+
+ stfcamss->irq = platform_get_irq(pdev, 0);
+ if (stfcamss->irq <= 0) {
+ st_err(ST_CAMSS, "Could not get irq\n");
+ goto err_cam;
+ }
+
+ stfcamss->isp_irq = platform_get_irq(pdev, 1);
+ if (stfcamss->isp_irq <= 0) {
+ st_err(ST_CAMSS, "Could not get isp irq\n");
+ goto err_cam;
+ }
+
+ stfcamss->isp_irq_csi = platform_get_irq(pdev, 2);
+ if (stfcamss->isp_irq_csi <= 0) {
+ st_err(ST_CAMSS, "Could not get isp csi irq\n");
+ goto err_cam;
+ }
+
+ stfcamss->isp_irq_csiline = platform_get_irq(pdev, 3);
+ if (stfcamss->isp_irq_csiline <= 0) {
+ st_err(ST_CAMSS, "Could not get isp irq csiline\n");
+ goto err_cam;
+ }
+
+ pm_runtime_enable(dev);
+
+ stfcamss->nclks = ARRAY_SIZE(stfcamss_clocks);
+ stfcamss->sys_clk = stfcamss_clocks;
+
+ ret = devm_clk_bulk_get(dev, stfcamss->nclks, stfcamss->sys_clk);
+ if (ret) {
+ st_err(ST_CAMSS, "Failed to get clk controls\n");
+ return ret;
+ }
+
+ stfcamss->nrsts = ARRAY_SIZE(stfcamss_resets);
+ stfcamss->sys_rst = stfcamss_resets;
+
+ ret = devm_reset_control_bulk_get_shared(dev, stfcamss->nrsts,
+ stfcamss->sys_rst);
+ if (ret) {
+ st_err(ST_CAMSS, "Failed to get reset controls\n");
+ return ret;
+ }
+
+ ret = stfcamss_get_mem_res(pdev, stfcamss);
+ if (ret) {
+ st_err(ST_CAMSS, "Could not map registers\n");
+ goto err_cam;
+ }
+
+ stfcamss->dev = dev;
+ platform_set_drvdata(pdev, stfcamss);
+
+ v4l2_async_nf_init(&stfcamss->notifier);
+
+ num_subdevs = stfcamss_of_parse_ports(stfcamss);
+ if (num_subdevs < 0) {
+ ret = num_subdevs;
+ goto err_cam_noti;
+ }
+
+ ret = stfcamss_init_subdevices(stfcamss);
+ if (ret < 0) {
+ st_err(ST_CAMSS, "Failed to init subdevice: %d\n", ret);
+ goto err_cam_noti;
+ }
+
+ stfcamss->media_dev.dev = stfcamss->dev;
+ strscpy(stfcamss->media_dev.model, "Starfive Camera Subsystem",
+ sizeof(stfcamss->media_dev.model));
+ strscpy(stfcamss->media_dev.serial, "0123456789ABCDEF",
+ sizeof(stfcamss->media_dev.serial));
+ snprintf(stfcamss->media_dev.bus_info,
+ sizeof(stfcamss->media_dev.bus_info),
+ "%s:%s", dev_bus_name(dev), pdev->name);
+ stfcamss->media_dev.hw_revision = 0x01;
+ stfcamss->media_dev.ops = &stfcamss_media_ops;
+ media_device_init(&stfcamss->media_dev);
+
+ stfcamss->v4l2_dev.mdev = &stfcamss->media_dev;
+
+ ret = v4l2_device_register(stfcamss->dev, &stfcamss->v4l2_dev);
+ if (ret < 0) {
+ st_err(ST_CAMSS, "Failed to register V4L2 device: %d\n", ret);
+ goto err_cam_noti_med;
+ }
+
+ ret = stfcamss_register_subdevices(stfcamss);
+ if (ret < 0) {
+ st_err(ST_CAMSS, "Failed to register subdevice: %d\n", ret);
+ goto err_cam_noti_med_vreg;
+ }
+
+ if (num_subdevs) {
+ stfcamss->notifier.ops = &stfcamss_subdev_notifier_ops;
+ ret = v4l2_async_nf_register(&stfcamss->v4l2_dev,
+ &stfcamss->notifier);
+ if (ret) {
+ st_err(ST_CAMSS,
+ "Failed to register async subdev nodes: %d\n",
+ ret);
+ goto err_cam_noti_med_vreg_sub;
+ }
+ } else {
+ ret = v4l2_device_register_subdev_nodes(&stfcamss->v4l2_dev);
+ if (ret < 0) {
+ st_err(ST_CAMSS,
+ "Failed to register subdev nodes: %d\n",
+ ret);
+ goto err_cam_noti_med_vreg_sub;
+ }
+
+ ret = media_device_register(&stfcamss->media_dev);
+ if (ret < 0) {
+ st_err(ST_CAMSS,
+ "Failed to register media device: %d\n",
+ ret);
+ goto err_cam_noti_med_vreg_sub_medreg;
+ }
+ }
+
+ dev_info(dev, "stfcamss probe success!\n");
+ return 0;
+
+err_cam_noti_med_vreg_sub_medreg:
+err_cam_noti_med_vreg_sub:
+ stfcamss_unregister_subdevices(stfcamss);
+err_cam_noti_med_vreg:
+ v4l2_device_unregister(&stfcamss->v4l2_dev);
+err_cam_noti_med:
+ media_device_cleanup(&stfcamss->media_dev);
+err_cam_noti:
+ v4l2_async_nf_cleanup(&stfcamss->notifier);
+err_cam:
+ return ret;
+}
+
+/*
+ * stfcamss_remove - Remove STFCAMSS platform device
+ * @pdev: Pointer to STFCAMSS platform device
+ *
+ * Always returns 0.
+ */
+static int stfcamss_remove(struct platform_device *pdev)
+{
+ struct stfcamss *stfcamss = platform_get_drvdata(pdev);
+
+ dev_info(&pdev->dev, "remove done\n");
+
+ stfcamss_unregister_subdevices(stfcamss);
+ v4l2_device_unregister(&stfcamss->v4l2_dev);
+ media_device_cleanup(&stfcamss->media_dev);
+ pm_runtime_disable(&pdev->dev);
+
+ return 0;
+}
+
+static const struct of_device_id stfcamss_of_match[] = {
+ { .compatible = "starfive,jh7110-camss" },
+ { /* end node */ },
+};
+
+MODULE_DEVICE_TABLE(of, stfcamss_of_match);
+
+#ifdef CONFIG_PM_SLEEP
+static int stfcamss_suspend(struct device *dev)
+{
+ struct stfcamss *stfcamss = dev_get_drvdata(dev);
+ struct stf_vin_dev *vin_dev = stfcamss->vin_dev;
+ struct media_entity *entity;
+ struct media_pad *pad;
+ struct v4l2_subdev *subdev;
+ struct stfcamss_video *video;
+ struct video_device *vdev;
+ int i = 0;
+
+ for (i = 0; i < VIN_LINE_MAX; i++) {
+ if (vin_dev->line[i].stream_count) {
+ vin_dev->line[i].stream_count++;
+ video = &vin_dev->line[i].video_out;
+ vdev = &vin_dev->line[i].video_out.vdev;
+ entity = &vdev->entity;
+ while (1) {
+ pad = &entity->pads[0];
+ if (!(pad->flags & MEDIA_PAD_FL_SINK))
+ break;
+
+ pad = media_pad_remote_pad_first(pad);
+ if (!pad ||
+ !is_media_entity_v4l2_subdev(pad->entity))
+ break;
+
+ entity = pad->entity;
+ subdev = media_entity_to_v4l2_subdev(entity);
+
+ v4l2_subdev_call(subdev, video, s_stream, 0);
+ }
+ video_device_pipeline_stop(vdev);
+ video->ops->flush_buffers(video, VB2_BUF_STATE_ERROR);
+ v4l2_pipeline_pm_put(&vdev->entity);
+ }
+ }
+
+ return pm_runtime_force_suspend(dev);
+}
+
+static int stfcamss_resume(struct device *dev)
+{
+ struct stfcamss *stfcamss = dev_get_drvdata(dev);
+ struct stf_vin_dev *vin_dev = stfcamss->vin_dev;
+ struct media_entity *entity;
+ struct media_pad *pad;
+ struct v4l2_subdev *subdev;
+ struct stfcamss_video *video;
+ struct video_device *vdev;
+ int i = 0;
+ int ret = 0;
+
+ pm_runtime_force_resume(dev);
+
+ for (i = 0; i < VIN_LINE_MAX; i++) {
+ if (vin_dev->line[i].stream_count) {
+ vin_dev->line[i].stream_count--;
+ video = &vin_dev->line[i].video_out;
+ vdev = &vin_dev->line[i].video_out.vdev;
+
+ ret = v4l2_pipeline_pm_get(&vdev->entity);
+ if (ret < 0)
+ goto err;
+
+ ret = video_device_pipeline_start(
+ vdev, &video->stfcamss->pipe);
+ if (ret < 0)
+ goto err_pm_put;
+
+ entity = &vdev->entity;
+ while (1) {
+ pad = &entity->pads[0];
+ if (!(pad->flags & MEDIA_PAD_FL_SINK))
+ break;
+
+ pad = media_pad_remote_pad_first(pad);
+ if (!pad ||
+ !is_media_entity_v4l2_subdev(pad->entity))
+ break;
+
+ entity = pad->entity;
+ subdev = media_entity_to_v4l2_subdev(entity);
+
+ ret = v4l2_subdev_call(subdev, video,
+ s_stream, 1);
+ if (ret < 0 && ret != -ENOIOCTLCMD)
+ goto err_pipeline_stop;
+ }
+ }
+ }
+
+ return 0;
+
+err_pipeline_stop:
+ video_device_pipeline_stop(vdev);
+err_pm_put:
+ v4l2_pipeline_pm_put(&vdev->entity);
+err:
+ return ret;
+}
+#endif /* CONFIG_PM_SLEEP */
+
+#ifdef CONFIG_PM
+static int stfcamss_runtime_suspend(struct device *dev)
+{
+ struct stfcamss *stfcamss = dev_get_drvdata(dev);
+
+ reset_control_assert(stfcamss->sys_rst[STF_RST_ISP_TOP_AXI].rstc);
+ reset_control_assert(stfcamss->sys_rst[STF_RST_ISP_TOP_N].rstc);
+ clk_disable_unprepare(stfcamss->sys_clk[STF_CLK_ISP_AXI].clk);
+ clk_disable_unprepare(stfcamss->sys_clk[STF_CLK_ISPCORE_2X].clk);
+
+ return 0;
+}
+
+static int stfcamss_runtime_resume(struct device *dev)
+{
+ struct stfcamss *stfcamss = dev_get_drvdata(dev);
+
+ clk_prepare_enable(stfcamss->sys_clk[STF_CLK_ISPCORE_2X].clk);
+ clk_prepare_enable(stfcamss->sys_clk[STF_CLK_ISP_AXI].clk);
+ reset_control_deassert(stfcamss->sys_rst[STF_RST_ISP_TOP_N].rstc);
+ reset_control_deassert(stfcamss->sys_rst[STF_RST_ISP_TOP_AXI].rstc);
+
+ return 0;
+}
+#endif /* CONFIG_PM */
+
+static const struct dev_pm_ops stfcamss_pm_ops = {
+ SET_SYSTEM_SLEEP_PM_OPS(stfcamss_suspend, stfcamss_resume)
+ SET_RUNTIME_PM_OPS(stfcamss_runtime_suspend,
+ stfcamss_runtime_resume,
+ NULL)
+};
+
+static struct platform_driver stfcamss_driver = {
+ .probe = stfcamss_probe,
+ .remove = stfcamss_remove,
+ .driver = {
+ .name = DRV_NAME,
+ .pm = &stfcamss_pm_ops,
+ .of_match_table = of_match_ptr(stfcamss_of_match),
+ },
+};
+
+module_platform_driver(stfcamss_driver);
+
+MODULE_AUTHOR("StarFive Corporation");
+MODULE_DESCRIPTION("StarFive Camera Subsystem driver");
+MODULE_LICENSE("GPL");
--- /dev/null
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * stf_camss.h
+ *
+ * Starfive Camera Subsystem driver
+ *
+ * Copyright (C) 2021-2023 StarFive Technology Co., Ltd.
+ */
+
+#ifndef STF_CAMSS_H
+#define STF_CAMSS_H
+
+#include <linux/clk.h>
+#include <linux/reset.h>
+#include <media/v4l2-device.h>
+
+enum sensor_type {
+ SENSOR_VIN,
+ /* need replace sensor */
+ SENSOR_ISP,
+};
+
+enum subdev_type {
+ SUBDEV_TYPE_VIN,
+ SUBDEV_TYPE_ISP,
+};
+
+#include "stf_common.h"
+#include "stf_isp.h"
+#include "stf_vin.h"
+
+#define DRV_NAME "starfive-camss"
+#define STF_DVP_NAME "stf_dvp"
+#define STF_CSI_NAME "cdns_csi2rx"
+#define STF_ISP_NAME "stf_isp"
+#define STF_VIN_NAME "stf_vin"
+
+#define STF_PAD_SINK 0
+#define STF_PAD_SRC 1
+#define STF_PADS_NUM 2
+
+enum port_num {
+ PORT_NUMBER_DVP_SENSOR = 0,
+ PORT_NUMBER_CSI2RX
+};
+
+enum stf_clk {
+ STF_CLK_APB_FUNC = 0,
+ STF_CLK_WRAPPER_CLK_C,
+ STF_CLK_DVP_INV,
+ STF_CLK_AXIWR,
+ STF_CLK_MIPI_RX0_PXL,
+ STF_CLK_ISPCORE_2X,
+ STF_CLK_ISP_AXI,
+ STF_CLK_NUM
+};
+
+enum stf_rst {
+ STF_RST_WRAPPER_P = 0,
+ STF_RST_WRAPPER_C,
+ STF_RST_AXIRD,
+ STF_RST_AXIWR,
+ STF_RST_ISP_TOP_N,
+ STF_RST_ISP_TOP_AXI,
+ STF_RST_NUM
+};
+
+#define HOST_ENTITY_MAX 2
+
+struct host_data {
+ struct media_entity *host_entity[HOST_ENTITY_MAX];
+};
+
+struct stfcamss {
+ struct v4l2_device v4l2_dev;
+ struct media_device media_dev;
+ struct media_pipeline pipe;
+ struct device *dev;
+ struct stf_vin_dev *vin_dev;
+ struct stf_isp_dev *isp_dev;
+ struct v4l2_async_notifier notifier;
+ struct host_data host_data;
+ void __iomem *syscon_base;
+ void __iomem *isp_base;
+ int irq;
+ int isp_irq;
+ int isp_irq_csi;
+ int isp_irq_csiline;
+ struct clk_bulk_data *sys_clk;
+ int nclks;
+ struct reset_control_bulk_data *sys_rst;
+ int nrsts;
+ struct regmap *stf_aon_syscon;
+ u32 aon_gp_reg;
+};
+
+struct stfcamss_async_subdev {
+ struct v4l2_async_subdev asd; /* must be first */
+ enum port_num port;
+};
+
+struct media_entity *stfcamss_find_sensor(struct media_entity *entity);
+
+#endif /* STF_CAMSS_H */
--- /dev/null
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * stf_common.h
+ *
+ * StarFive Camera Subsystem - Common definitions
+ *
+ * Copyright (C) 2021-2023 StarFive Technology Co., Ltd.
+ */
+
+#ifndef STF_COMMON_H
+#define STF_COMMON_H
+
+#include <linux/io.h>
+#include <linux/kern_levels.h>
+
+enum {
+ ST_DVP = 0x0001,
+ ST_ISP = 0x0002,
+ ST_VIN = 0x0004,
+ ST_VIDEO = 0x0008,
+ ST_CAMSS = 0x0010,
+ ST_SENSOR = 0x0020,
+};
+
+enum {
+ ST_NONE = 0x00,
+ ST_ERR = 0x01,
+ ST_WARN = 0x02,
+ ST_INFO = 0x03,
+ ST_DEBUG = 0x04,
+};
+
+#ifdef STF_DEBUG
+#define STFDBG_LEVEL ST_DEBUG
+#define STFDBG_MASK 0x7F
+#else
+#define STFDBG_LEVEL ST_ERR
+#define STFDBG_MASK 0x7F
+#endif
+
+#define ST_MODULE2STRING(__module) ({ \
+ char *__str; \
+ \
+ switch (__module) { \
+ case ST_DVP: \
+ __str = "st_dvp"; \
+ break; \
+ case ST_ISP: \
+ __str = "st_isp"; \
+ break; \
+ case ST_VIN: \
+ __str = "st_vin"; \
+ break; \
+ case ST_VIDEO: \
+ __str = "st_video"; \
+ break; \
+ case ST_CAMSS: \
+ __str = "st_camss"; \
+ break; \
+ case ST_SENSOR: \
+ __str = "st_sensor"; \
+ break; \
+ default: \
+ __str = "unknown"; \
+ break; \
+ } \
+ \
+ __str; \
+ })
+
+#define st_debug(module, __fmt, arg...) \
+ do { \
+ if (STFDBG_LEVEL > ST_INFO) { \
+ if (STFDBG_MASK & (module)) \
+ pr_err("[%s] debug: " __fmt, \
+ ST_MODULE2STRING((module)), \
+ ## arg); \
+ } \
+ } while (0)
+
+#define st_info(module, __fmt, arg...) \
+ do { \
+ if (STFDBG_LEVEL > ST_WARN) { \
+ if (STFDBG_MASK & (module)) \
+ pr_err("[%s] info: " __fmt, \
+ ST_MODULE2STRING((module)), \
+ ## arg); \
+ } \
+ } while (0)
+
+#define st_warn(module, __fmt, arg...) \
+ do { \
+ if (STFDBG_LEVEL > ST_ERR) { \
+ if (STFDBG_MASK & (module)) \
+ pr_err("[%s] warn: " __fmt, \
+ ST_MODULE2STRING((module)), \
+ ## arg); \
+ } \
+ } while (0)
+
+#define st_err(module, __fmt, arg...) \
+ do { \
+ if (STFDBG_LEVEL > ST_NONE) { \
+ if (STFDBG_MASK & (module)) \
+ pr_err("[%s] error: " __fmt, \
+ ST_MODULE2STRING((module)), \
+ ## arg); \
+ } \
+ } while (0)
+
+#define st_err_ratelimited(module, fmt, ...) \
+ do { \
+ static DEFINE_RATELIMIT_STATE(_rs, \
+ DEFAULT_RATELIMIT_INTERVAL, \
+ DEFAULT_RATELIMIT_BURST); \
+ if (__ratelimit(&_rs) && STFDBG_LEVEL > ST_NONE) { \
+ if (STFDBG_MASK & (module)) \
+ pr_err("[%s] error: " fmt, \
+ ST_MODULE2STRING((module)), \
+ ##__VA_ARGS__); \
+ } \
+ } while (0)
+
+static inline u32 reg_read(void __iomem *base, u32 reg)
+{
+ return ioread32(base + reg);
+}
+
+static inline void reg_write(void __iomem *base, u32 reg, u32 val)
+{
+ iowrite32(val, base + reg);
+}
+
+static inline void reg_set_bit(void __iomem *base, u32 reg, u32 mask, u32 val)
+{
+ u32 value;
+
+ value = ioread32(base + reg) & ~mask;
+ val &= mask;
+ val |= value;
+ iowrite32(val, base + reg);
+}
+
+static inline void reg_set(void __iomem *base, u32 reg, u32 mask)
+{
+ iowrite32(ioread32(base + reg) | mask, base + reg);
+}
+
+#endif /* STF_COMMON_H */
--- /dev/null
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * stf_isp.c
+ *
+ * StarFive Camera Subsystem - ISP Module
+ *
+ * Copyright (C) 2021-2023 StarFive Technology Co., Ltd.
+ */
+#include <linux/firmware.h>
+#include <linux/stf_isp_ioctl.h>
+#include <media/v4l2-event.h>
+
+#include "stf_camss.h"
+
+#define SINK_FORMATS_INDEX 0
+#define UO_FORMATS_INDEX 1
+#define RAW_FORMATS_INDEX 2
+
+static int user_config_isp;
+
+static int isp_set_selection(struct v4l2_subdev *sd,
+ struct v4l2_subdev_state *state,
+ struct v4l2_subdev_selection *sel);
+
+static struct v4l2_rect *
+__isp_get_compose(struct stf_isp_dev *isp_dev,
+ struct v4l2_subdev_state *state,
+ enum v4l2_subdev_format_whence which);
+
+static struct v4l2_rect *
+__isp_get_crop(struct stf_isp_dev *isp_dev,
+ struct v4l2_subdev_state *state,
+ enum v4l2_subdev_format_whence which);
+
+static struct v4l2_rect *
+__isp_get_scale(struct stf_isp_dev *isp_dev,
+ struct v4l2_subdev_state *state,
+ struct v4l2_subdev_selection *sel);
+
+static const struct isp_format isp_formats_sink[] = {
+ { MEDIA_BUS_FMT_SRGGB10_1X10, 10 },
+ { MEDIA_BUS_FMT_SGRBG10_1X10, 10 },
+ { MEDIA_BUS_FMT_SGBRG10_1X10, 10 },
+ { MEDIA_BUS_FMT_SBGGR10_1X10, 10 },
+};
+
+static const struct isp_format isp_formats_raw[] = {
+ { MEDIA_BUS_FMT_SRGGB12_1X12, 12 },
+ { MEDIA_BUS_FMT_SGRBG12_1X12, 12 },
+ { MEDIA_BUS_FMT_SGBRG12_1X12, 12 },
+ { MEDIA_BUS_FMT_SBGGR12_1X12, 12 },
+};
+
+static const struct isp_format isp_formats_compat_raw10[] = {
+ { MEDIA_BUS_FMT_SRGGB10_1X10, 10 },
+ { MEDIA_BUS_FMT_SGRBG10_1X10, 10 },
+ { MEDIA_BUS_FMT_SGBRG10_1X10, 10 },
+ { MEDIA_BUS_FMT_SBGGR10_1X10, 10 },
+};
+
+static const struct isp_format isp_formats_compat_raw8[] = {
+ { MEDIA_BUS_FMT_SRGGB8_1X8, 8 },
+ { MEDIA_BUS_FMT_SGRBG8_1X8, 8 },
+ { MEDIA_BUS_FMT_SGBRG8_1X8, 8 },
+ { MEDIA_BUS_FMT_SBGGR8_1X8, 8 },
+};
+
+static const struct isp_format isp_formats_uo[] = {
+ { MEDIA_BUS_FMT_Y12_1X12, 8 },
+};
+
+static const struct isp_format_table isp_formats_st7110[] = {
+ { isp_formats_sink, ARRAY_SIZE(isp_formats_sink) },
+ { isp_formats_uo, ARRAY_SIZE(isp_formats_uo) },
+ { isp_formats_raw, ARRAY_SIZE(isp_formats_raw) },
+};
+
+int stf_isp_subdev_init(struct stfcamss *stfcamss)
+{
+ struct stf_isp_dev *isp_dev = stfcamss->isp_dev;
+ struct device *dev = stfcamss->dev;
+
+ isp_dev->sdev_type = SUBDEV_TYPE_ISP;
+ isp_dev->hw_ops = &isp_ops;
+ isp_dev->stfcamss = stfcamss;
+ isp_dev->formats = isp_formats_st7110;
+ isp_dev->nformats = ARRAY_SIZE(isp_formats_st7110);
+ isp_dev->reg_buf = devm_kzalloc(dev, sizeof(*isp_dev->reg_buf),
+ GFP_KERNEL);
+ if (!isp_dev->reg_buf)
+ return -ENOMEM;
+
+ mutex_init(&isp_dev->stream_lock);
+ mutex_init(&isp_dev->power_lock);
+ mutex_init(&isp_dev->setfile_lock);
+ atomic_set(&isp_dev->shadow_count, 0);
+ return 0;
+}
+
+static int isp_set_power(struct v4l2_subdev *sd, int on)
+{
+ struct stf_isp_dev *isp_dev = v4l2_get_subdevdata(sd);
+
+ mutex_lock(&isp_dev->power_lock);
+ if (on) {
+ if (isp_dev->power_count == 0)
+ st_debug(ST_ISP, "turn on isp\n");
+ isp_dev->power_count++;
+ } else {
+ if (isp_dev->power_count == 0)
+ goto exit;
+ isp_dev->power_count--;
+ }
+exit:
+ mutex_unlock(&isp_dev->power_lock);
+
+ return 0;
+}
+
+static struct v4l2_mbus_framefmt *
+__isp_get_format(struct stf_isp_dev *isp_dev,
+ struct v4l2_subdev_state *state,
+ unsigned int pad,
+ enum v4l2_subdev_format_whence which)
+{
+ if (which == V4L2_SUBDEV_FORMAT_TRY)
+ return v4l2_subdev_get_try_format(&isp_dev->subdev, state, pad);
+
+ return &isp_dev->fmt[pad];
+}
+
+static int isp_get_interface_type(struct media_entity *entity)
+{
+ struct v4l2_subdev *subdev;
+ struct media_pad *pad = &entity->pads[0];
+
+ if (!(pad->flags & MEDIA_PAD_FL_SINK))
+ return -EINVAL;
+
+ pad = media_pad_remote_pad_first(pad);
+ if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
+ return -EINVAL;
+
+ subdev = media_entity_to_v4l2_subdev(pad->entity);
+
+ st_debug(ST_ISP, "interface subdev name %s\n", subdev->name);
+ if (!strncmp(subdev->name, STF_CSI_NAME, strlen(STF_CSI_NAME)))
+ return INTERFACE_CSI;
+
+ return -EINVAL;
+}
+
+static int isp_set_stream(struct v4l2_subdev *sd, int enable)
+{
+ struct stf_isp_dev *isp_dev = v4l2_get_subdevdata(sd);
+ int ret = 0, interface_type;
+ struct v4l2_mbus_framefmt *fmt;
+ struct v4l2_event src_ch = { 0 };
+
+ fmt = __isp_get_format(isp_dev, NULL, STF_ISP_PAD_SINK,
+ V4L2_SUBDEV_FORMAT_ACTIVE);
+ mutex_lock(&isp_dev->stream_lock);
+ if (enable) {
+ if (isp_dev->stream_count == 0) {
+ isp_dev->hw_ops->isp_clk_enable(isp_dev);
+ if (!user_config_isp)
+ isp_dev->hw_ops->isp_config_set(isp_dev);
+ interface_type = isp_get_interface_type(&sd->entity);
+ if (interface_type < 0) {
+ st_err(ST_ISP,
+ "%s, pipeline not config\n", __func__);
+ goto exit;
+ }
+ isp_dev->hw_ops->isp_set_format(isp_dev,
+ isp_dev->rect,
+ fmt->code,
+ interface_type);
+ isp_dev->hw_ops->isp_reset(isp_dev);
+ isp_dev->hw_ops->isp_stream_set(isp_dev, enable);
+ user_config_isp = 0;
+ }
+ isp_dev->stream_count++;
+ } else {
+ if (isp_dev->stream_count == 0)
+ goto exit;
+ if (isp_dev->stream_count == 1) {
+ isp_dev->hw_ops->isp_stream_set(isp_dev, enable);
+ isp_dev->hw_ops->isp_clk_disable(isp_dev);
+ }
+ isp_dev->stream_count--;
+ }
+ src_ch.type = V4L2_EVENT_SOURCE_CHANGE,
+ src_ch.u.src_change.changes = isp_dev->stream_count,
+
+ v4l2_subdev_notify_event(sd, &src_ch);
+exit:
+ mutex_unlock(&isp_dev->stream_lock);
+
+ return ret;
+}
+
+static int isp_sensor_fmt_to_index(struct stf_isp_dev *isp_dev)
+{
+ int ret, idx;
+ struct media_entity *sensor;
+ struct v4l2_subdev *subdev;
+ struct v4l2_subdev_format fmt;
+ const struct isp_format_table *formats;
+
+ if (!isp_dev)
+ return -EINVAL;
+
+ sensor = stfcamss_find_sensor(&isp_dev->subdev.entity);
+ if (!sensor)
+ return -EINVAL;
+
+ subdev = media_entity_to_v4l2_subdev(sensor);
+ st_debug(ST_ISP, "Found sensor = %s\n", sensor->name);
+
+ fmt.pad = 0;
+ fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+ ret = v4l2_subdev_call(subdev, pad, get_fmt, NULL, &fmt);
+ if (ret) {
+ st_warn(ST_ISP, "Sonser get format failed !!\n");
+ return -EINVAL;
+ }
+
+ st_debug(ST_ISP, "Got sensor format 0x%x !!\n", fmt.format.code);
+
+ formats = &isp_dev->formats[SINK_FORMATS_INDEX];
+ for (idx = 0; idx < formats->nfmts; idx++) {
+ if (formats->fmts[idx].code == fmt.format.code) {
+ st_info(ST_ISP, "Match sensor format to index %d\n",
+ idx);
+ return idx;
+ }
+ }
+ return -ERANGE;
+}
+
+static int isp_fmt_to_index(const struct isp_format_table *f_table,
+ __u32 mbus_code, unsigned int pad)
+{
+ int i;
+
+ for (i = 0; i < f_table->nfmts; i++) {
+ if (mbus_code == f_table->fmts[i].code)
+ break;
+
+ if (pad == STF_ISP_PAD_SRC_RAW) {
+ if (mbus_code == isp_formats_compat_raw10[i].code ||
+ mbus_code == isp_formats_compat_raw8[i].code)
+ break;
+ }
+ }
+
+ return i;
+}
+
+static void isp_try_format(struct stf_isp_dev *isp_dev,
+ struct v4l2_subdev_state *state,
+ unsigned int pad,
+ struct v4l2_mbus_framefmt *fmt,
+ enum v4l2_subdev_format_whence which)
+{
+ const struct isp_format_table *formats;
+ unsigned int i;
+ u32 code = fmt->code;
+ u32 bpp;
+
+ switch (pad) {
+ case STF_ISP_PAD_SINK:
+ /* Set format on sink pad */
+ formats = &isp_dev->formats[SINK_FORMATS_INDEX];
+ fmt->width = clamp_t(u32,
+ fmt->width, STFCAMSS_FRAME_MIN_WIDTH,
+ STFCAMSS_FRAME_MAX_WIDTH);
+ fmt->height = clamp_t(u32,
+ fmt->height, STFCAMSS_FRAME_MIN_HEIGHT,
+ STFCAMSS_FRAME_MAX_HEIGHT);
+ fmt->height &= ~0x1;
+ fmt->field = V4L2_FIELD_NONE;
+ fmt->colorspace = V4L2_COLORSPACE_SRGB;
+ fmt->flags = 0;
+ break;
+
+ case STF_ISP_PAD_SRC:
+ case STF_ISP_PAD_SRC_SS0:
+ case STF_ISP_PAD_SRC_SS1:
+ formats = &isp_dev->formats[UO_FORMATS_INDEX];
+ break;
+
+ case STF_ISP_PAD_SRC_RAW:
+ formats = &isp_dev->formats[RAW_FORMATS_INDEX];
+ break;
+ }
+
+ i = isp_fmt_to_index(formats, fmt->code, pad);
+ st_debug(ST_ISP, "%s pad=%d, code=%x format to index = %d\n",
+ __func__, pad, code, i);
+
+ if (i >= formats->nfmts && pad == STF_ISP_PAD_SRC_RAW) {
+ int sensor_idx;
+
+ sensor_idx = isp_sensor_fmt_to_index(isp_dev);
+ if (sensor_idx)
+ i = sensor_idx;
+ }
+
+ if (pad != STF_ISP_PAD_SINK)
+ *fmt = *__isp_get_format(isp_dev, state,
+ STF_ISP_PAD_SINK, which);
+
+ if (i >= formats->nfmts) {
+ fmt->code = formats->fmts[0].code;
+ bpp = formats->fmts[0].bpp;
+ st_info(ST_ISP, "Use default index 0 format = 0x%x\n",
+ fmt->code);
+ } else {
+ if (pad == STF_ISP_PAD_SRC_RAW) {
+ fmt->code = formats->fmts[i].code;
+ bpp = formats->fmts[i].bpp;
+ st_info(ST_ISP,
+ "use format from sink index %d = 0x%x\n",
+ i, fmt->code);
+ } else {
+ fmt->code = code;
+ bpp = formats->fmts[i].bpp;
+ st_info(ST_ISP, "Use input format = 0x%x\n", fmt->code);
+ }
+ }
+
+ switch (pad) {
+ case STF_ISP_PAD_SINK:
+ break;
+
+ case STF_ISP_PAD_SRC:
+ isp_dev->rect[ISP_COMPOSE].bpp = bpp;
+ break;
+
+ case STF_ISP_PAD_SRC_SS0:
+ isp_dev->rect[ISP_SCALE_SS0].bpp = bpp;
+ break;
+
+ case STF_ISP_PAD_SRC_SS1:
+ isp_dev->rect[ISP_SCALE_SS1].bpp = bpp;
+ break;
+
+ case STF_ISP_PAD_SRC_RAW:
+ isp_dev->rect[ISP_CROP].bpp = bpp;
+ break;
+ }
+}
+
+static int isp_enum_mbus_code(struct v4l2_subdev *sd,
+ struct v4l2_subdev_state *state,
+ struct v4l2_subdev_mbus_code_enum *code)
+{
+ struct stf_isp_dev *isp_dev = v4l2_get_subdevdata(sd);
+ const struct isp_format_table *formats;
+
+ if (code->index >= isp_dev->nformats)
+ return -EINVAL;
+ if (code->pad == STF_ISP_PAD_SINK) {
+ formats = &isp_dev->formats[SINK_FORMATS_INDEX];
+ code->code = formats->fmts[code->index].code;
+ } else {
+ struct v4l2_mbus_framefmt *sink_fmt;
+
+ sink_fmt = __isp_get_format(isp_dev, state, STF_ISP_PAD_SINK,
+ code->which);
+
+ code->code = sink_fmt->code;
+ if (!code->code)
+ return -EINVAL;
+ }
+ code->flags = 0;
+
+ return 0;
+}
+
+static int isp_enum_frame_size(struct v4l2_subdev *sd,
+ struct v4l2_subdev_state *state,
+ struct v4l2_subdev_frame_size_enum *fse)
+{
+ struct stf_isp_dev *isp_dev = v4l2_get_subdevdata(sd);
+ struct v4l2_mbus_framefmt format;
+
+ if (fse->index != 0)
+ return -EINVAL;
+
+ format.code = fse->code;
+ format.width = 1;
+ format.height = 1;
+ isp_try_format(isp_dev, state, fse->pad, &format, fse->which);
+ fse->min_width = format.width;
+ fse->min_height = format.height;
+
+ if (format.code != fse->code)
+ return -EINVAL;
+
+ format.code = fse->code;
+ format.width = -1;
+ format.height = -1;
+ isp_try_format(isp_dev, state, fse->pad, &format, fse->which);
+ fse->max_width = format.width;
+ fse->max_height = format.height;
+
+ return 0;
+}
+
+static int isp_get_format(struct v4l2_subdev *sd,
+ struct v4l2_subdev_state *state,
+ struct v4l2_subdev_format *fmt)
+{
+ struct stf_isp_dev *isp_dev = v4l2_get_subdevdata(sd);
+ struct v4l2_mbus_framefmt *format;
+
+ format = __isp_get_format(isp_dev, state, fmt->pad, fmt->which);
+ if (!format)
+ return -EINVAL;
+
+ fmt->format = *format;
+
+ return 0;
+}
+
+static int isp_set_format(struct v4l2_subdev *sd,
+ struct v4l2_subdev_state *state,
+ struct v4l2_subdev_format *fmt)
+{
+ struct stf_isp_dev *isp_dev = v4l2_get_subdevdata(sd);
+ struct v4l2_mbus_framefmt *format;
+ struct v4l2_subdev_selection sel = { 0 };
+ struct v4l2_rect *rect = NULL;
+ int ret;
+
+ st_debug(ST_ISP, "%s pad=%d, code=%x, which=%d\n",
+ __func__, fmt->reserved[0], fmt->format.code, fmt->which);
+ format = __isp_get_format(isp_dev, state, fmt->pad, fmt->which);
+ if (!format)
+ return -EINVAL;
+
+ mutex_lock(&isp_dev->stream_lock);
+ if (isp_dev->stream_count) {
+ fmt->format = *format;
+ if (fmt->reserved[0] != 0) {
+ sel.which = fmt->which;
+ sel.pad = fmt->reserved[0];
+
+ switch (fmt->reserved[0]) {
+ case STF_ISP_PAD_SRC:
+ rect = __isp_get_compose(isp_dev,
+ state, fmt->which);
+ break;
+
+ case STF_ISP_PAD_SRC_SS0:
+ case STF_ISP_PAD_SRC_SS1:
+ rect = __isp_get_scale(isp_dev, state, &sel);
+ break;
+
+ case STF_ISP_PAD_SRC_RAW:
+ rect = __isp_get_crop(isp_dev,
+ state, fmt->which);
+ break;
+
+ default:
+ break;
+ }
+ if (rect) {
+ fmt->format.width = rect->width;
+ fmt->format.height = rect->height;
+ }
+ }
+ mutex_unlock(&isp_dev->stream_lock);
+ goto out;
+ } else {
+ isp_try_format(isp_dev, state, fmt->pad,
+ &fmt->format, fmt->which);
+ *format = fmt->format;
+ }
+ mutex_unlock(&isp_dev->stream_lock);
+
+ /* Propagate the format from sink to source */
+ if (fmt->pad == STF_ISP_PAD_SINK) {
+ /* Reset sink pad compose selection */
+ sel.which = fmt->which;
+ sel.pad = STF_ISP_PAD_SINK;
+ sel.target = V4L2_SEL_TGT_CROP;
+ sel.r.width = fmt->format.width;
+ sel.r.height = fmt->format.height;
+ ret = isp_set_selection(sd, state, &sel);
+ if (ret < 0)
+ return ret;
+ }
+
+out:
+ return 0;
+}
+
+static struct v4l2_rect *
+__isp_get_compose(struct stf_isp_dev *isp_dev,
+ struct v4l2_subdev_state *state,
+ enum v4l2_subdev_format_whence which)
+{
+ if (which == V4L2_SUBDEV_FORMAT_TRY)
+ return v4l2_subdev_get_try_compose(&isp_dev->subdev, state,
+ STF_ISP_PAD_SINK);
+
+ return &isp_dev->rect[ISP_COMPOSE].rect;
+}
+
+static struct v4l2_rect *
+__isp_get_crop(struct stf_isp_dev *isp_dev,
+ struct v4l2_subdev_state *state,
+ enum v4l2_subdev_format_whence which)
+{
+ if (which == V4L2_SUBDEV_FORMAT_TRY)
+ return v4l2_subdev_get_try_crop(&isp_dev->subdev, state,
+ STF_ISP_PAD_SINK);
+
+ return &isp_dev->rect[ISP_CROP].rect;
+}
+
+static struct v4l2_rect *
+__isp_get_scale(struct stf_isp_dev *isp_dev,
+ struct v4l2_subdev_state *state,
+ struct v4l2_subdev_selection *sel)
+{
+ int pad;
+
+ if (sel->which == V4L2_SUBDEV_FORMAT_TRY)
+ return v4l2_subdev_get_try_compose(&isp_dev->subdev, state,
+ STF_ISP_PAD_SINK);
+ if (sel->pad != STF_ISP_PAD_SRC_SS0 && sel->pad != STF_ISP_PAD_SRC_SS1)
+ return NULL;
+
+ pad = sel->pad == STF_ISP_PAD_SRC_SS0 ? ISP_SCALE_SS0 : ISP_SCALE_SS1;
+ return &isp_dev->rect[pad].rect;
+}
+
+static void isp_try_crop(struct stf_isp_dev *isp_dev,
+ struct v4l2_subdev_state *state,
+ struct v4l2_rect *rect,
+ enum v4l2_subdev_format_whence which)
+{
+ struct v4l2_mbus_framefmt *fmt;
+
+ fmt = __isp_get_format(isp_dev, state, STF_ISP_PAD_SINK, which);
+
+ if (rect->width > fmt->width)
+ rect->width = fmt->width;
+
+ if (rect->width + rect->left > fmt->width)
+ rect->left = fmt->width - rect->width;
+
+ if (rect->height > fmt->height)
+ rect->height = fmt->height;
+
+ if (rect->height + rect->top > fmt->height)
+ rect->top = fmt->height - rect->height;
+
+ if (rect->width < STFCAMSS_FRAME_MIN_WIDTH) {
+ rect->left = 0;
+ rect->width = STFCAMSS_FRAME_MIN_WIDTH;
+ }
+
+ if (rect->height < STFCAMSS_FRAME_MIN_HEIGHT) {
+ rect->top = 0;
+ rect->height = STFCAMSS_FRAME_MIN_HEIGHT;
+ }
+ rect->height &= ~0x1;
+}
+
+static void isp_try_compose(struct stf_isp_dev *isp_dev,
+ struct v4l2_subdev_state *state,
+ struct v4l2_rect *rect,
+ enum v4l2_subdev_format_whence which)
+{
+ struct v4l2_rect *crop;
+
+ crop = __isp_get_crop(isp_dev, state, which);
+
+ if (rect->width > crop->width)
+ rect->width = crop->width;
+
+ if (rect->height > crop->height)
+ rect->height = crop->height;
+
+ if (crop->width > rect->width * SCALER_RATIO_MAX)
+ rect->width =
+ (crop->width + SCALER_RATIO_MAX - 1) / SCALER_RATIO_MAX;
+
+ if (crop->height > rect->height * SCALER_RATIO_MAX)
+ rect->height =
+ (crop->height + SCALER_RATIO_MAX - 1) / SCALER_RATIO_MAX;
+
+ if (rect->width < STFCAMSS_FRAME_MIN_WIDTH)
+ rect->width = STFCAMSS_FRAME_MIN_WIDTH;
+
+ if (rect->height < STFCAMSS_FRAME_MIN_HEIGHT)
+ rect->height = STFCAMSS_FRAME_MIN_HEIGHT;
+ rect->height &= ~0x1;
+}
+
+static void isp_try_scale(struct stf_isp_dev *isp_dev,
+ struct v4l2_subdev_state *state,
+ struct v4l2_rect *rect,
+ enum v4l2_subdev_format_whence which)
+{
+ struct v4l2_rect *compose;
+
+ compose = __isp_get_compose(isp_dev, state, which);
+
+ if (rect->width > compose->width)
+ rect->width = compose->width;
+
+ if (rect->width + rect->left > compose->width)
+ rect->left = compose->width - rect->width;
+
+ if (rect->height > compose->height)
+ rect->height = compose->height;
+
+ if (rect->height + rect->top > compose->height)
+ rect->top = compose->height - rect->height;
+
+ if (rect->width < STFCAMSS_FRAME_MIN_WIDTH) {
+ rect->left = 0;
+ rect->width = STFCAMSS_FRAME_MIN_WIDTH;
+ }
+
+ if (rect->height < STFCAMSS_FRAME_MIN_HEIGHT) {
+ rect->top = 0;
+ rect->height = STFCAMSS_FRAME_MIN_HEIGHT;
+ }
+
+ rect->height &= ~0x1;
+}
+
+static int isp_get_selection(struct v4l2_subdev *sd,
+ struct v4l2_subdev_state *state,
+ struct v4l2_subdev_selection *sel)
+{
+ struct stf_isp_dev *isp_dev = v4l2_get_subdevdata(sd);
+ struct v4l2_subdev_format fmt = { 0 };
+ struct v4l2_rect *rect;
+ int ret;
+
+ switch (sel->target) {
+ case V4L2_SEL_TGT_CROP_BOUNDS:
+ case V4L2_SEL_TGT_CROP_DEFAULT:
+ fmt.pad = sel->pad;
+ fmt.which = sel->which;
+ ret = isp_get_format(sd, state, &fmt);
+ if (ret < 0)
+ return ret;
+
+ sel->r.left = 0;
+ sel->r.top = 0;
+ sel->r.width = fmt.format.width;
+ sel->r.height = fmt.format.height;
+ break;
+
+ case V4L2_SEL_TGT_CROP:
+ rect = __isp_get_crop(isp_dev, state, sel->which);
+ if (!rect)
+ return -EINVAL;
+
+ sel->r = *rect;
+ break;
+
+ case V4L2_SEL_TGT_COMPOSE_BOUNDS:
+ case V4L2_SEL_TGT_COMPOSE_DEFAULT:
+ if (sel->pad > STF_ISP_PAD_SRC_SS1)
+ return -EINVAL;
+ rect = __isp_get_crop(isp_dev, state, sel->which);
+ if (!rect)
+ return -EINVAL;
+
+ sel->r.left = rect->left;
+ sel->r.top = rect->top;
+ sel->r.width = rect->width;
+ sel->r.height = rect->height;
+ break;
+
+ case V4L2_SEL_TGT_COMPOSE:
+ if (sel->pad > STF_ISP_PAD_SRC_SS1)
+ return -EINVAL;
+ if (sel->pad == STF_ISP_PAD_SRC_SS0 ||
+ sel->pad == STF_ISP_PAD_SRC_SS1) {
+ rect = __isp_get_scale(isp_dev, state, sel);
+ if (!rect)
+ return -EINVAL;
+ } else {
+ rect = __isp_get_compose(isp_dev, state, sel->which);
+ if (!rect)
+ return -EINVAL;
+ }
+ sel->r = *rect;
+ break;
+
+ default:
+ return -EINVAL;
+ }
+
+ st_info(ST_ISP, "%s pad = %d, left = %d, %d, %d, %d\n",
+ __func__, sel->pad, sel->r.left,
+ sel->r.top, sel->r.width, sel->r.height);
+ return 0;
+}
+
+static int isp_set_selection(struct v4l2_subdev *sd,
+ struct v4l2_subdev_state *state,
+ struct v4l2_subdev_selection *sel)
+{
+ struct stf_isp_dev *isp_dev = v4l2_get_subdevdata(sd);
+ struct v4l2_rect *rect;
+ int ret = 0;
+
+ if (sel->target == V4L2_SEL_TGT_COMPOSE &&
+ (sel->pad == STF_ISP_PAD_SINK || sel->pad == STF_ISP_PAD_SRC)) {
+ struct v4l2_subdev_format fmt = { 0 };
+ int i;
+
+ rect = __isp_get_compose(isp_dev, state, sel->which);
+ if (!rect)
+ return -EINVAL;
+
+ mutex_lock(&isp_dev->stream_lock);
+ if (isp_dev->stream_count) {
+ sel->r = *rect;
+ mutex_unlock(&isp_dev->stream_lock);
+ ret = 0;
+ goto out;
+ } else {
+ isp_try_compose(isp_dev, state, &sel->r, sel->which);
+ *rect = sel->r;
+ }
+ mutex_unlock(&isp_dev->stream_lock);
+
+ /* Reset source pad format width and height */
+ fmt.which = sel->which;
+ fmt.pad = STF_ISP_PAD_SRC;
+ ret = isp_get_format(sd, state, &fmt);
+ if (ret < 0)
+ return ret;
+
+ fmt.format.width = rect->width;
+ fmt.format.height = rect->height;
+ ret = isp_set_format(sd, state, &fmt);
+
+ /* Reset scale */
+ for (i = STF_ISP_PAD_SRC_SS0; i <= STF_ISP_PAD_SRC_SS1; i++) {
+ struct v4l2_subdev_selection scale = { 0 };
+
+ scale.which = sel->which;
+ scale.target = V4L2_SEL_TGT_COMPOSE;
+ scale.r = *rect;
+ scale.pad = i;
+ ret = isp_set_selection(sd, state, &scale);
+ }
+ } else if (sel->target == V4L2_SEL_TGT_COMPOSE &&
+ ((sel->pad == STF_ISP_PAD_SRC_SS0) ||
+ (sel->pad == STF_ISP_PAD_SRC_SS1))) {
+ struct v4l2_subdev_format fmt = { 0 };
+
+ rect = __isp_get_scale(isp_dev, state, sel);
+ if (!rect)
+ return -EINVAL;
+
+ mutex_lock(&isp_dev->stream_lock);
+ if (isp_dev->stream_count) {
+ sel->r = *rect;
+ mutex_unlock(&isp_dev->stream_lock);
+ ret = 0;
+ goto out;
+ } else {
+ isp_try_scale(isp_dev, state, &sel->r, sel->which);
+ *rect = sel->r;
+ }
+ mutex_unlock(&isp_dev->stream_lock);
+
+ /* Reset source pad format width and height */
+ fmt.which = sel->which;
+ fmt.pad = sel->pad;
+ ret = isp_get_format(sd, state, &fmt);
+ if (ret < 0)
+ return ret;
+
+ fmt.format.width = rect->width;
+ fmt.format.height = rect->height;
+ ret = isp_set_format(sd, state, &fmt);
+ } else if (sel->target == V4L2_SEL_TGT_CROP) {
+ struct v4l2_subdev_selection compose = { 0 };
+ int i;
+
+ rect = __isp_get_crop(isp_dev, state, sel->which);
+ if (!rect)
+ return -EINVAL;
+
+ mutex_lock(&isp_dev->stream_lock);
+ if (isp_dev->stream_count) {
+ sel->r = *rect;
+ mutex_unlock(&isp_dev->stream_lock);
+ ret = 0;
+ goto out;
+ } else {
+ isp_try_crop(isp_dev, state, &sel->r, sel->which);
+ *rect = sel->r;
+ }
+ mutex_unlock(&isp_dev->stream_lock);
+
+ /* Reset source compose selection */
+ compose.which = sel->which;
+ compose.target = V4L2_SEL_TGT_COMPOSE;
+ compose.r.width = rect->width;
+ compose.r.height = rect->height;
+ compose.pad = STF_ISP_PAD_SINK;
+ ret = isp_set_selection(sd, state, &compose);
+
+ /* Reset source pad format width and height */
+ for (i = STF_ISP_PAD_SRC_RAW; i < STF_ISP_PAD_MAX; i++) {
+ struct v4l2_subdev_format fmt = { 0 };
+
+ fmt.which = sel->which;
+ fmt.pad = i;
+ ret = isp_get_format(sd, state, &fmt);
+ if (ret < 0)
+ return ret;
+
+ fmt.format.width = rect->width;
+ fmt.format.height = rect->height;
+ ret = isp_set_format(sd, state, &fmt);
+ }
+ } else {
+ ret = -EINVAL;
+ }
+
+ st_info(ST_ISP, "%s pad = %d, left = %d, %d, %d, %d\n",
+ __func__, sel->pad, sel->r.left,
+ sel->r.top, sel->r.width, sel->r.height);
+out:
+ return ret;
+}
+
+static int isp_init_formats(struct v4l2_subdev *sd,
+ struct v4l2_subdev_fh *fh)
+{
+ struct v4l2_subdev_format format = {
+ .pad = STF_ISP_PAD_SINK,
+ .which =
+ fh ? V4L2_SUBDEV_FORMAT_TRY : V4L2_SUBDEV_FORMAT_ACTIVE,
+ .format = {
+ .code = MEDIA_BUS_FMT_RGB565_2X8_LE,
+ .width = 1920,
+ .height = 1080
+ }
+ };
+
+ return isp_set_format(sd, fh ? fh->state : NULL, &format);
+}
+
+static int isp_link_setup(struct media_entity *entity,
+ const struct media_pad *local,
+ const struct media_pad *remote, u32 flags)
+{
+ if (flags & MEDIA_LNK_FL_ENABLED)
+ if (media_pad_remote_pad_first(local))
+ return -EBUSY;
+ return 0;
+}
+
+static int stf_isp_load_setfile(struct stf_isp_dev *isp_dev, char *file_name)
+{
+ struct device *dev = isp_dev->stfcamss->dev;
+ const struct firmware *fw;
+ u8 *buf = NULL;
+ int *regval_num;
+ int ret;
+
+ st_debug(ST_ISP, "%s, file_name %s\n", __func__, file_name);
+ ret = request_firmware(&fw, file_name, dev);
+ if (ret < 0) {
+ st_err(ST_ISP, "firmware request failed (%d)\n", ret);
+ return ret;
+ }
+ buf = devm_kzalloc(dev, fw->size, GFP_KERNEL);
+ if (!buf)
+ return -ENOMEM;
+ memcpy(buf, fw->data, fw->size);
+
+ mutex_lock(&isp_dev->setfile_lock);
+ if (isp_dev->setfile.state == 1)
+ devm_kfree(dev, isp_dev->setfile.data);
+ isp_dev->setfile.data = buf;
+ isp_dev->setfile.size = fw->size;
+ isp_dev->setfile.state = 1;
+ regval_num = (int *)&buf[fw->size - sizeof(unsigned int)];
+ isp_dev->setfile.settings.regval_num = *regval_num;
+ isp_dev->setfile.settings.regval = (struct regval_t *)buf;
+ mutex_unlock(&isp_dev->setfile_lock);
+
+ st_debug(ST_ISP, "stf_isp setfile loaded size: %zu B, reg_nul: %d\n",
+ fw->size, isp_dev->setfile.settings.regval_num);
+
+ release_firmware(fw);
+ return ret;
+}
+
+static long stf_isp_ioctl(struct v4l2_subdev *sd, unsigned int cmd, void *arg)
+{
+ struct stf_isp_dev *isp_dev = v4l2_get_subdevdata(sd);
+ struct stfisp_fw_info *fw_info = arg;
+ int ret = -ENOIOCTLCMD;
+
+ switch (cmd) {
+ case VIDIOC_STFISP_LOAD_FW:
+ if (IS_ERR(fw_info)) {
+ st_err(ST_ISP, "fw_info failed, params invalid\n");
+ return -EINVAL;
+ }
+
+ ret = stf_isp_load_setfile(isp_dev, fw_info->filename);
+ break;
+
+ case VIDIOC_STFISP_GET_REG:
+ ret = isp_dev->hw_ops->isp_reg_read(isp_dev, arg);
+ break;
+
+ case VIDIOC_STFISP_SET_REG:
+ ret = isp_dev->hw_ops->isp_reg_write(isp_dev, arg);
+ break;
+
+ case VIDIOC_STFISP_SHADOW_LOCK:
+ if (atomic_add_unless(&isp_dev->shadow_count, 1, 1))
+ ret = 0;
+ else
+ ret = -EBUSY;
+ break;
+
+ case VIDIOC_STFISP_SHADOW_UNLOCK:
+ if (atomic_dec_if_positive(&isp_dev->shadow_count) < 0)
+ ret = -EINVAL;
+ else
+ ret = 0;
+ break;
+
+ case VIDIOC_STFISP_SHADOW_UNLOCK_N_TRIGGER:
+ isp_dev->hw_ops->isp_shadow_trigger(isp_dev);
+ if (atomic_dec_if_positive(&isp_dev->shadow_count) < 0)
+ ret = -EINVAL;
+ else
+ ret = 0;
+ break;
+
+ case VIDIOC_STFISP_SET_USER_CONFIG_ISP:
+ st_debug(ST_ISP, "Set user config isp\n");
+ user_config_isp = 1;
+ break;
+
+ default:
+ break;
+ }
+ return ret;
+}
+
+int isp_close(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+{
+ struct stf_isp_dev *isp_dev = v4l2_get_subdevdata(sd);
+
+ while (atomic_dec_if_positive(&isp_dev->shadow_count) > 0)
+ st_warn(ST_ISP, "unlock the shadow lock!\n");
+
+ return 0;
+}
+
+static int stf_isp_subscribe_event(struct v4l2_subdev *sd,
+ struct v4l2_fh *fh,
+ struct v4l2_event_subscription *sub)
+{
+ switch (sub->type) {
+ case V4L2_EVENT_SOURCE_CHANGE:
+ return v4l2_src_change_event_subdev_subscribe(sd, fh, sub);
+ default:
+ st_debug(ST_ISP, "unspport subscribe_event\n");
+ return -EINVAL;
+ }
+}
+
+static const struct v4l2_subdev_core_ops isp_core_ops = {
+ .s_power = isp_set_power,
+ .ioctl = stf_isp_ioctl,
+ .subscribe_event = stf_isp_subscribe_event,
+ .unsubscribe_event = v4l2_event_subdev_unsubscribe,
+};
+
+static const struct v4l2_subdev_video_ops isp_video_ops = {
+ .s_stream = isp_set_stream,
+};
+
+static const struct v4l2_subdev_pad_ops isp_pad_ops = {
+ .enum_mbus_code = isp_enum_mbus_code,
+ .enum_frame_size = isp_enum_frame_size,
+ .get_fmt = isp_get_format,
+ .set_fmt = isp_set_format,
+ .get_selection = isp_get_selection,
+ .set_selection = isp_set_selection,
+};
+
+static const struct v4l2_subdev_ops isp_v4l2_ops = {
+ .core = &isp_core_ops,
+ .video = &isp_video_ops,
+ .pad = &isp_pad_ops,
+};
+
+static const struct v4l2_subdev_internal_ops isp_v4l2_internal_ops = {
+ .open = isp_init_formats,
+ .close = isp_close,
+};
+
+static const struct media_entity_operations isp_media_ops = {
+ .link_setup = isp_link_setup,
+ .link_validate = v4l2_subdev_link_validate,
+};
+
+int stf_isp_register(struct stf_isp_dev *isp_dev, struct v4l2_device *v4l2_dev)
+{
+ struct v4l2_subdev *sd = &isp_dev->subdev;
+ struct media_pad *pads = isp_dev->pads;
+ int ret;
+
+ v4l2_subdev_init(sd, &isp_v4l2_ops);
+ sd->internal_ops = &isp_v4l2_internal_ops;
+ sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE | V4L2_SUBDEV_FL_HAS_EVENTS;
+ snprintf(sd->name, ARRAY_SIZE(sd->name), "%s%d", STF_ISP_NAME, 0);
+ v4l2_set_subdevdata(sd, isp_dev);
+
+ ret = isp_init_formats(sd, NULL);
+ if (ret < 0) {
+ st_err(ST_ISP, "Failed to init format: %d\n", ret);
+ return ret;
+ }
+
+ pads[STF_ISP_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
+ pads[STF_ISP_PAD_SRC].flags = MEDIA_PAD_FL_SOURCE;
+ pads[STF_ISP_PAD_SRC_SS0].flags = MEDIA_PAD_FL_SOURCE;
+ pads[STF_ISP_PAD_SRC_SS1].flags = MEDIA_PAD_FL_SOURCE;
+ pads[STF_ISP_PAD_SRC_RAW].flags = MEDIA_PAD_FL_SOURCE;
+
+ sd->entity.function = MEDIA_ENT_F_PROC_VIDEO_PIXEL_FORMATTER;
+ sd->entity.ops = &isp_media_ops;
+ ret = media_entity_pads_init(&sd->entity, STF_ISP_PAD_MAX, pads);
+ if (ret < 0) {
+ st_err(ST_ISP, "Failed to init media entity: %d\n", ret);
+ return ret;
+ }
+
+ ret = v4l2_device_register_subdev(v4l2_dev, sd);
+ if (ret < 0) {
+ st_err(ST_ISP, "Failed to register subdev: %d\n", ret);
+ goto err_sreg;
+ }
+
+ return 0;
+
+err_sreg:
+ media_entity_cleanup(&sd->entity);
+ return ret;
+}
+
+int stf_isp_unregister(struct stf_isp_dev *isp_dev)
+{
+ v4l2_device_unregister_subdev(&isp_dev->subdev);
+ media_entity_cleanup(&isp_dev->subdev.entity);
+ mutex_destroy(&isp_dev->stream_lock);
+ mutex_destroy(&isp_dev->power_lock);
+ mutex_destroy(&isp_dev->setfile_lock);
+ return 0;
+}
--- /dev/null
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * stf_isp.h
+ *
+ * StarFive Camera Subsystem - ISP Module
+ *
+ * Copyright (C) 2021-2023 StarFive Technology Co., Ltd.
+ */
+
+#ifndef STF_ISP_H
+#define STF_ISP_H
+
+#include <media/media-entity.h>
+#include <media/v4l2-subdev.h>
+
+#define STF_ISP_SETFILE "stf_isp0_fw.bin"
+
+#define ISP_RAW_DATA_BITS 12
+#define SCALER_RATIO_MAX 1
+#define STF_ISP_REG_OFFSET_MAX 0x0FFF
+#define STF_ISP_REG_DELAY_MAX 100
+
+/* isp registers */
+#define ISP_REG_CSI_INPUT_EN_AND_STATUS 0x00000000
+#define ISP_REG_CSIINTS_ADDR 0x00000008
+#define ISP_REG_CSI_MODULE_CFG 0x00000010
+#define ISP_REG_SENSOR 0x00000014
+#define ISP_REG_RAW_FORMAT_CFG 0x00000018
+#define ISP_REG_PIC_CAPTURE_START_CFG 0x0000001C
+#define ISP_REG_PIC_CAPTURE_END_CFG 0x00000020
+#define ISP_REG_DUMP_CFG_0 0x00000024
+#define ISP_REG_DUMP_CFG_1 0x00000028
+#define ISP_REG_SCD_CFG_0 0x00000098
+#define ISP_REG_SCD_CFG_1 0x0000009C
+#define ISP_REG_SC_CFG_1 0x000000BC
+#define ISP_REG_ISP_CTRL_0 0x00000A00
+#define ISP_REG_ISP_CTRL_1 0x00000A08
+#define ISP_REG_PIPELINE_XY_SIZE 0x00000A0C
+#define ISP_REG_IESHD_ADDR 0x00000A50
+#define ISP_REG_Y_PLANE_START_ADDR 0x00000A80
+#define ISP_REG_UV_PLANE_START_ADDR 0x00000A84
+#define ISP_REG_STRIDE 0x00000A88
+#define ISP_REG_PIXEL_COORDINATE_GEN 0x00000A8C
+#define ISP_REG_SS0AY 0x00000A94
+#define ISP_REG_SS0AUV 0x00000A98
+#define ISP_REG_SS0S 0x00000A9C
+#define ISP_REG_SS0IW 0x00000AA8
+#define ISP_REG_SS1AY 0x00000AAC
+#define ISP_REG_SS1AUV 0x00000AB0
+#define ISP_REG_SS1S 0x00000AB4
+#define ISP_REG_SS1IW 0x00000AC0
+#define ISP_REG_YHIST_CFG_4 0x00000CD8
+#define ISP_REG_ITIIWSR 0x00000B20
+#define ISP_REG_ITIDWLSR 0x00000B24
+#define ISP_REG_ITIDWYSAR 0x00000B28
+#define ISP_REG_ITIDWUSAR 0x00000B2C
+#define ISP_REG_ITIDRYSAR 0x00000B30
+#define ISP_REG_ITIDRUSAR 0x00000B34
+#define ISP_REG_ITIPDFR 0x00000B38
+#define ISP_REG_ITIDRLSR 0x00000B3C
+#define ISP_REG_ITIBSR 0x00000B40
+#define ISP_REG_ITIAIR 0x00000B44
+#define ISP_REG_ITIDPSR 0x00000B48
+
+/* The output line of ISP */
+enum isp_line_id {
+ STF_ISP_LINE_INVALID = -1,
+ STF_ISP_LINE_SRC = 1,
+ STF_ISP_LINE_SRC_SS0,
+ STF_ISP_LINE_SRC_SS1,
+ STF_ISP_LINE_SRC_RAW,
+ STF_ISP_LINE_MAX = STF_ISP_LINE_SRC_RAW
+};
+
+/* pad id for media framework */
+enum isp_pad_id {
+ STF_ISP_PAD_SINK = 0,
+ STF_ISP_PAD_SRC,
+ STF_ISP_PAD_SRC_SS0,
+ STF_ISP_PAD_SRC_SS1,
+ STF_ISP_PAD_SRC_RAW,
+ STF_ISP_PAD_MAX
+};
+
+enum {
+ EN_INT_NONE = 0,
+ EN_INT_ISP_DONE = (0x1 << 24),
+ EN_INT_CSI_DONE = (0x1 << 25),
+ EN_INT_SC_DONE = (0x1 << 26),
+ EN_INT_LINE_INT = (0x1 << 27),
+ EN_INT_ALL = (0xF << 24),
+};
+
+enum {
+ INTERFACE_DVP = 0,
+ INTERFACE_CSI,
+};
+
+struct isp_format {
+ u32 code;
+ u8 bpp;
+};
+
+struct isp_format_table {
+ const struct isp_format *fmts;
+ int nfmts;
+};
+
+struct regval_t {
+ u32 addr;
+ u32 val;
+ u32 mask;
+ u32 delay_ms;
+};
+
+struct reg_table {
+ const struct regval_t *regval;
+ int regval_num;
+};
+
+struct isp_stream_format {
+ struct v4l2_rect rect;
+ u32 bpp;
+};
+
+struct isp_setfile {
+ struct reg_table settings;
+ const u8 *data;
+ unsigned int size;
+ unsigned int state;
+};
+
+enum {
+ ISP_CROP = 0,
+ ISP_COMPOSE,
+ ISP_SCALE_SS0,
+ ISP_SCALE_SS1,
+ ISP_RECT_MAX
+};
+
+struct stf_isp_dev {
+ enum subdev_type sdev_type; /* This member must be first */
+ struct stfcamss *stfcamss;
+ struct v4l2_subdev subdev;
+ struct media_pad pads[STF_ISP_PAD_MAX];
+ struct v4l2_mbus_framefmt fmt[STF_ISP_PAD_MAX];
+ struct isp_stream_format rect[ISP_RECT_MAX];
+ const struct isp_format_table *formats;
+ unsigned int nformats;
+ const struct isp_hw_ops *hw_ops;
+ struct mutex power_lock; /* serialize power control*/
+ int power_count;
+ struct mutex stream_lock; /* serialize stream control */
+ int stream_count;
+ atomic_t shadow_count;
+
+ struct mutex setfile_lock; /* protects setting files */
+ struct isp_setfile setfile;
+
+ union reg_buf *reg_buf;
+};
+
+struct isp_hw_ops {
+ int (*isp_clk_enable)(struct stf_isp_dev *isp_dev);
+ int (*isp_clk_disable)(struct stf_isp_dev *isp_dev);
+ int (*isp_reset)(struct stf_isp_dev *isp_dev);
+ int (*isp_config_set)(struct stf_isp_dev *isp_dev);
+ int (*isp_set_format)(struct stf_isp_dev *isp_dev,
+ struct isp_stream_format *crop,
+ u32 mcode, int type);
+ int (*isp_stream_set)(struct stf_isp_dev *isp_dev, int on);
+ int (*isp_reg_read)(struct stf_isp_dev *isp_dev, void *arg);
+ int (*isp_reg_write)(struct stf_isp_dev *isp_dev, void *arg);
+ int (*isp_shadow_trigger)(struct stf_isp_dev *isp_dev);
+};
+
+extern const struct isp_hw_ops isp_ops;
+
+int stf_isp_subdev_init(struct stfcamss *stfcamss);
+int stf_isp_register(struct stf_isp_dev *isp_dev, struct v4l2_device *v4l2_dev);
+int stf_isp_unregister(struct stf_isp_dev *isp_dev);
+
+#endif /* STF_ISP_H */
--- /dev/null
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * stf_isp_hw_ops.c
+ *
+ * Register interface file for StarFive ISP driver
+ *
+ * Copyright (C) 2021-2023 StarFive Technology Co., Ltd.
+ *
+ */
+#include <linux/delay.h>
+#include <linux/stf_isp_ioctl.h>
+
+#include "stf_camss.h"
+
+static const struct regval_t isp_reg_init_config_list[] = {
+ /* config DC */
+ {0x00000044, 0x00000000, 0, 0},
+ /* config DEC */
+ {0x00000030, 0x00000000, 0, 0},
+ /* config OBC */
+ {0x00000034, 0x000000BB, 0, 0},
+ {0x000002E0, 0x40404040, 0, 0},
+ {0x000002E4, 0x40404040, 0, 0},
+ {0x000002E8, 0x40404040, 0, 0},
+ {0x000002EC, 0x40404040, 0, 0},
+ {0x000002F0, 0x00000000, 0, 0},
+ {0x000002F4, 0x00000000, 0, 0},
+ {0x000002F8, 0x00000000, 0, 0},
+ {0x000002FC, 0x00000000, 0, 0},
+ /* config LCBQ */
+ {0x00000074, 0x00009900, 0, 0},
+ {0x0000007C, 0x01E40040, 0, 0},
+ {0x00000300, 0x01000100, 0, 0},
+ {0x00000304, 0x01000100, 0, 0},
+ {0x00000308, 0x01000100, 0, 0},
+ {0x0000030C, 0x01000100, 0, 0},
+ {0x00000310, 0x01000100, 0, 0},
+ {0x00000314, 0x01000100, 0, 0},
+ {0x00000318, 0x01000100, 0, 0},
+ {0x0000031C, 0x01000100, 0, 0},
+ {0x00000320, 0x01000100, 0, 0},
+ {0x00000324, 0x01000100, 0, 0},
+ {0x00000328, 0x01000100, 0, 0},
+ {0x0000032C, 0x01000100, 0, 0},
+ {0x00000330, 0x00000100, 0, 0},
+ {0x00000334, 0x01000100, 0, 0},
+ {0x00000338, 0x01000100, 0, 0},
+ {0x0000033C, 0x01000100, 0, 0},
+ {0x00000340, 0x01000100, 0, 0},
+ {0x00000344, 0x01000100, 0, 0},
+ {0x00000348, 0x01000100, 0, 0},
+ {0x0000034C, 0x01000100, 0, 0},
+ {0x00000350, 0x01000100, 0, 0},
+ {0x00000354, 0x01000100, 0, 0},
+ {0x00000358, 0x01000100, 0, 0},
+ {0x0000035C, 0x01000100, 0, 0},
+ {0x00000360, 0x01000100, 0, 0},
+ {0x00000364, 0x00000100, 0, 0},
+ {0x00000368, 0x01000100, 0, 0},
+ {0x0000036C, 0x01000100, 0, 0},
+ {0x00000370, 0x01000100, 0, 0},
+ {0x00000374, 0x01000100, 0, 0},
+ {0x00000378, 0x01000100, 0, 0},
+ {0x0000037C, 0x01000100, 0, 0},
+ {0x00000380, 0x01000100, 0, 0},
+ {0x00000384, 0x01000100, 0, 0},
+ {0x00000388, 0x01000100, 0, 0},
+ {0x0000038C, 0x01000100, 0, 0},
+ {0x00000390, 0x01000100, 0, 0},
+ {0x00000394, 0x01000100, 0, 0},
+ {0x00000398, 0x00000100, 0, 0},
+ {0x0000039C, 0x01000100, 0, 0},
+ {0x000003A0, 0x01000100, 0, 0},
+ {0x000003A4, 0x01000100, 0, 0},
+ {0x000003A8, 0x01000100, 0, 0},
+ {0x000003AC, 0x01000100, 0, 0},
+ {0x000003B0, 0x01000100, 0, 0},
+ {0x000003B4, 0x01000100, 0, 0},
+ {0x000003B8, 0x01000100, 0, 0},
+ {0x000003BC, 0x01000100, 0, 0},
+ {0x000003C0, 0x01000100, 0, 0},
+ {0x000003C4, 0x01000100, 0, 0},
+ {0x000003C8, 0x01000100, 0, 0},
+ {0x000003CC, 0x00000100, 0, 0},
+ {0x00000400, 0x00000000, 0, 0},
+ {0x00000404, 0x00000000, 0, 0},
+ {0x00000408, 0x00000000, 0, 0},
+ {0x0000040C, 0x00000000, 0, 0},
+ {0x00000410, 0x00000000, 0, 0},
+ {0x00000414, 0x00000000, 0, 0},
+ {0x00000418, 0x00000000, 0, 0},
+ {0x0000041C, 0x00000000, 0, 0},
+ {0x00000420, 0x00000000, 0, 0},
+ {0x00000424, 0x00000000, 0, 0},
+ {0x00000428, 0x00000000, 0, 0},
+ {0x0000042C, 0x00000000, 0, 0},
+ {0x00000430, 0x00000000, 0, 0},
+ {0x00000434, 0x00000000, 0, 0},
+ {0x00000438, 0x00000000, 0, 0},
+ {0x0000043C, 0x00000000, 0, 0},
+ {0x00000440, 0x00000000, 0, 0},
+ {0x00000444, 0x00000000, 0, 0},
+ {0x00000448, 0x00000000, 0, 0},
+ {0x0000044C, 0x00000000, 0, 0},
+ {0x00000450, 0x00000000, 0, 0},
+ {0x00000454, 0x00000000, 0, 0},
+ {0x00000458, 0x00000000, 0, 0},
+ {0x0000045C, 0x00000000, 0, 0},
+ {0x00000460, 0x00000000, 0, 0},
+ {0x00000464, 0x00000000, 0, 0},
+ {0x00000468, 0x00000000, 0, 0},
+ {0x0000046C, 0x00000000, 0, 0},
+ {0x00000470, 0x00000000, 0, 0},
+ {0x00000474, 0x00000000, 0, 0},
+ {0x00000478, 0x00000000, 0, 0},
+ {0x0000047C, 0x00000000, 0, 0},
+ {0x00000480, 0x00000000, 0, 0},
+ {0x00000484, 0x00000000, 0, 0},
+ {0x00000488, 0x00000000, 0, 0},
+ {0x0000048C, 0x00000000, 0, 0},
+ {0x00000490, 0x00000000, 0, 0},
+ {0x00000494, 0x00000000, 0, 0},
+ {0x00000498, 0x00000000, 0, 0},
+ {0x0000049C, 0x00000000, 0, 0},
+ {0x000004A0, 0x00000000, 0, 0},
+ {0x000004A4, 0x00000000, 0, 0},
+ {0x000004A8, 0x00000000, 0, 0},
+ {0x000004AC, 0x00000000, 0, 0},
+ {0x000004B0, 0x00000000, 0, 0},
+ {0x000004B4, 0x00000000, 0, 0},
+ {0x000004B8, 0x00000000, 0, 0},
+ {0x000004BC, 0x00000000, 0, 0},
+ {0x000004C0, 0x00000000, 0, 0},
+ {0x000004C4, 0x00000000, 0, 0},
+ {0x000004C8, 0x00000000, 0, 0},
+ {0x000004CC, 0x00000000, 0, 0},
+ /* config OECF */
+ {0x00000100, 0x00100000, 0, 0},
+ {0x00000104, 0x00400020, 0, 0},
+ {0x00000108, 0x00800060, 0, 0},
+ {0x0000010C, 0x00C000A0, 0, 0},
+ {0x00000110, 0x010000E0, 0, 0},
+ {0x00000114, 0x02000180, 0, 0},
+ {0x00000118, 0x03000280, 0, 0},
+ {0x0000011C, 0x03FE0380, 0, 0},
+ {0x00000120, 0x00100000, 0, 0},
+ {0x00000124, 0x00400020, 0, 0},
+ {0x00000128, 0x00800060, 0, 0},
+ {0x0000012C, 0x00C000A0, 0, 0},
+ {0x00000130, 0x010000E0, 0, 0},
+ {0x00000134, 0x02000180, 0, 0},
+ {0x00000138, 0x03000280, 0, 0},
+ {0x0000013C, 0x03FE0380, 0, 0},
+ {0x00000140, 0x00100000, 0, 0},
+ {0x00000144, 0x00400020, 0, 0},
+ {0x00000148, 0x00800060, 0, 0},
+ {0x0000014C, 0x00C000A0, 0, 0},
+ {0x00000150, 0x010000E0, 0, 0},
+ {0x00000154, 0x02000180, 0, 0},
+ {0x00000158, 0x03000280, 0, 0},
+ {0x0000015C, 0x03FE0380, 0, 0},
+ {0x00000160, 0x00100000, 0, 0},
+ {0x00000164, 0x00400020, 0, 0},
+ {0x00000168, 0x00800060, 0, 0},
+ {0x0000016C, 0x00C000A0, 0, 0},
+ {0x00000170, 0x010000E0, 0, 0},
+ {0x00000174, 0x02000180, 0, 0},
+ {0x00000178, 0x03000280, 0, 0},
+ {0x0000017C, 0x03FE0380, 0, 0},
+ {0x00000180, 0x00100000, 0, 0},
+ {0x00000184, 0x00400020, 0, 0},
+ {0x00000188, 0x00800060, 0, 0},
+ {0x0000018C, 0x00C000A0, 0, 0},
+ {0x00000190, 0x010000E0, 0, 0},
+ {0x00000194, 0x02000180, 0, 0},
+ {0x00000198, 0x03000280, 0, 0},
+ {0x0000019C, 0x03FE0380, 0, 0},
+ {0x000001A0, 0x00100000, 0, 0},
+ {0x000001A4, 0x00400020, 0, 0},
+ {0x000001A8, 0x00800060, 0, 0},
+ {0x000001AC, 0x00C000A0, 0, 0},
+ {0x000001B0, 0x010000E0, 0, 0},
+ {0x000001B4, 0x02000180, 0, 0},
+ {0x000001B8, 0x03000280, 0, 0},
+ {0x000001BC, 0x03FE0380, 0, 0},
+ {0x000001C0, 0x00100000, 0, 0},
+ {0x000001C4, 0x00400020, 0, 0},
+ {0x000001C8, 0x00800060, 0, 0},
+ {0x000001CC, 0x00C000A0, 0, 0},
+ {0x000001D0, 0x010000E0, 0, 0},
+ {0x000001D4, 0x02000180, 0, 0},
+ {0x000001D8, 0x03000280, 0, 0},
+ {0x000001DC, 0x03FE0380, 0, 0},
+ {0x000001E0, 0x00100000, 0, 0},
+ {0x000001E4, 0x00400020, 0, 0},
+ {0x000001E8, 0x00800060, 0, 0},
+ {0x000001EC, 0x00C000A0, 0, 0},
+ {0x000001F0, 0x010000E0, 0, 0},
+ {0x000001F4, 0x02000180, 0, 0},
+ {0x000001F8, 0x03000280, 0, 0},
+ {0x000001FC, 0x03FE0380, 0, 0},
+ {0x00000200, 0x00800080, 0, 0},
+ {0x00000204, 0x00800080, 0, 0},
+ {0x00000208, 0x00800080, 0, 0},
+ {0x0000020C, 0x00800080, 0, 0},
+ {0x00000210, 0x00800080, 0, 0},
+ {0x00000214, 0x00800080, 0, 0},
+ {0x00000218, 0x00800080, 0, 0},
+ {0x0000021C, 0x00800080, 0, 0},
+ {0x00000220, 0x00800080, 0, 0},
+ {0x00000224, 0x00800080, 0, 0},
+ {0x00000228, 0x00800080, 0, 0},
+ {0x0000022C, 0x00800080, 0, 0},
+ {0x00000230, 0x00800080, 0, 0},
+ {0x00000234, 0x00800080, 0, 0},
+ {0x00000238, 0x00800080, 0, 0},
+ {0x0000023C, 0x00800080, 0, 0},
+ {0x00000240, 0x00800080, 0, 0},
+ {0x00000244, 0x00800080, 0, 0},
+ {0x00000248, 0x00800080, 0, 0},
+ {0x0000024C, 0x00800080, 0, 0},
+ {0x00000250, 0x00800080, 0, 0},
+ {0x00000254, 0x00800080, 0, 0},
+ {0x00000258, 0x00800080, 0, 0},
+ {0x0000025C, 0x00800080, 0, 0},
+ {0x00000260, 0x00800080, 0, 0},
+ {0x00000264, 0x00800080, 0, 0},
+ {0x00000268, 0x00800080, 0, 0},
+ {0x0000026C, 0x00800080, 0, 0},
+ {0x00000270, 0x00800080, 0, 0},
+ {0x00000274, 0x00800080, 0, 0},
+ {0x00000278, 0x00800080, 0, 0},
+ {0x0000027C, 0x00800080, 0, 0},
+ /* config OECFHM */
+ {0x000003D0, 0x04000000, 0, 0},
+ {0x000003D4, 0x0C000800, 0, 0},
+ {0x000003D8, 0x00000FFF, 0, 0},
+ {0x000003DC, 0x08000800, 0, 0},
+ {0x000003E0, 0x08000800, 0, 0},
+ {0x000003E4, 0x00000800, 0, 0},
+ /* config LCCF */
+ {0x00000050, 0x021C03C0, 0, 0},
+ {0x00000058, 0x0000000B, 0, 0},
+ {0x000000E0, 0x00000000, 0, 0},
+ {0x000000E4, 0x00000000, 0, 0},
+ {0x000000E8, 0x00000000, 0, 0},
+ {0x000000EC, 0x00000000, 0, 0},
+ /* config AWB */
+ {0x00000280, 0x00000000, 0, 0},
+ {0x00000284, 0x00000000, 0, 0},
+ {0x00000288, 0x00000000, 0, 0},
+ {0x0000028C, 0x00000000, 0, 0},
+ {0x00000290, 0x00000000, 0, 0},
+ {0x00000294, 0x00000000, 0, 0},
+ {0x00000298, 0x00000000, 0, 0},
+ {0x0000029C, 0x00000000, 0, 0},
+ {0x000002A0, 0x00000000, 0, 0},
+ {0x000002A4, 0x00000000, 0, 0},
+ {0x000002A8, 0x00000000, 0, 0},
+ {0x000002AC, 0x00000000, 0, 0},
+ {0x000002B0, 0x00000000, 0, 0},
+ {0x000002B4, 0x00000000, 0, 0},
+ {0x000002B8, 0x00000000, 0, 0},
+ {0x000002BC, 0x00000000, 0, 0},
+ {0x000002C0, 0x00800080, 0, 0},
+ {0x000002C4, 0x00800080, 0, 0},
+ {0x000002C8, 0x00800080, 0, 0},
+ {0x000002CC, 0x00800080, 0, 0},
+ {0x000002D0, 0x00800080, 0, 0},
+ {0x000002D4, 0x00800080, 0, 0},
+ {0x000002D8, 0x00800080, 0, 0},
+ {0x000002DC, 0x00800080, 0, 0},
+ /* config CTC and DBC filter */
+ {0x00000A10, 0x41400040, 0, 0},
+ {0x00000A14, 0x02000200, 0, 0},
+ /* config CFA */
+ {0x00000018, 0x000011BB, 0, 0},
+ {0x00000A1C, 0x00000032, 0, 0},
+ /* config CCM */
+ {0x00000C40, 0x00060000, 0, 0},
+ {0x00000C44, 0x00000000, 0, 0},
+ {0x00000C48, 0x00000000, 0, 0},
+ {0x00000C4C, 0x00000000, 0, 0},
+ {0x00000C50, 0x00000000, 0, 0},
+ {0x00000C54, 0x00000000, 0, 0},
+ {0x00000C58, 0x00000000, 0, 0},
+ {0x00000C5C, 0x00000000, 0, 0},
+ {0x00000C60, 0x00000000, 0, 0},
+ {0x00000C64, 0x00000000, 0, 0},
+ {0x00000C68, 0x00000000, 0, 0},
+ {0x00000C6C, 0x00000000, 0, 0},
+ {0x00000C70, 0x00000080, 0, 0},
+ {0x00000C74, 0x00000000, 0, 0},
+ {0x00000C78, 0x00000000, 0, 0},
+ {0x00000C7C, 0x00000000, 0, 0},
+ {0x00000C80, 0x00000080, 0, 0},
+ {0x00000C84, 0x00000000, 0, 0},
+ {0x00000C88, 0x00000000, 0, 0},
+ {0x00000C8C, 0x00000000, 0, 0},
+ {0x00000C90, 0x00000080, 0, 0},
+ {0x00000C94, 0x00000000, 0, 0},
+ {0x00000C98, 0x00000000, 0, 0},
+ {0x00000C9C, 0x00000000, 0, 0},
+ {0x00000CA0, 0x00000700, 0, 0},
+ {0x00000CA4, 0x00000200, 0, 0},
+ /* config GMARGB */
+ {0x00000E00, 0x24000000, 0, 0},
+ {0x00000E04, 0x08000020, 0, 0},
+ {0x00000E08, 0x08000040, 0, 0},
+ {0x00000E0C, 0x08000060, 0, 0},
+ {0x00000E10, 0x08000080, 0, 0},
+ {0x00000E14, 0x080000A0, 0, 0},
+ {0x00000E18, 0x080000C0, 0, 0},
+ {0x00000E1C, 0x080000E0, 0, 0},
+ {0x00000E20, 0x08000100, 0, 0},
+ {0x00000E24, 0x08000180, 0, 0},
+ {0x00000E28, 0x08000200, 0, 0},
+ {0x00000E2C, 0x08000280, 0, 0},
+ {0x00000E30, 0x08000300, 0, 0},
+ {0x00000E34, 0x08000380, 0, 0},
+ {0x00000E38, 0x080003FE, 0, 0},
+ /* config R2Y */
+ {0x00000E40, 0x0000004C, 0, 0},
+ {0x00000E44, 0x00000097, 0, 0},
+ {0x00000E48, 0x0000001D, 0, 0},
+ {0x00000E4C, 0x000001D5, 0, 0},
+ {0x00000E50, 0x000001AC, 0, 0},
+ {0x00000E54, 0x00000080, 0, 0},
+ {0x00000E58, 0x00000080, 0, 0},
+ {0x00000E5C, 0x00000194, 0, 0},
+ {0x00000E60, 0x000001EC, 0, 0},
+ /* config YCRV */
+ {0x00000F00, 0x00000000, 0, 0},
+ {0x00000F04, 0x00000010, 0, 0},
+ {0x00000F08, 0x00000020, 0, 0},
+ {0x00000F0C, 0x00000030, 0, 0},
+ {0x00000F10, 0x00000040, 0, 0},
+ {0x00000F14, 0x00000050, 0, 0},
+ {0x00000F18, 0x00000060, 0, 0},
+ {0x00000F1C, 0x00000070, 0, 0},
+ {0x00000F20, 0x00000080, 0, 0},
+ {0x00000F24, 0x00000090, 0, 0},
+ {0x00000F28, 0x000000A0, 0, 0},
+ {0x00000F2C, 0x000000B0, 0, 0},
+ {0x00000F30, 0x000000C0, 0, 0},
+ {0x00000F34, 0x000000D0, 0, 0},
+ {0x00000F38, 0x000000E0, 0, 0},
+ {0x00000F3C, 0x000000F0, 0, 0},
+ {0x00000F40, 0x00000100, 0, 0},
+ {0x00000F44, 0x00000110, 0, 0},
+ {0x00000F48, 0x00000120, 0, 0},
+ {0x00000F4C, 0x00000130, 0, 0},
+ {0x00000F50, 0x00000140, 0, 0},
+ {0x00000F54, 0x00000150, 0, 0},
+ {0x00000F58, 0x00000160, 0, 0},
+ {0x00000F5C, 0x00000170, 0, 0},
+ {0x00000F60, 0x00000180, 0, 0},
+ {0x00000F64, 0x00000190, 0, 0},
+ {0x00000F68, 0x000001A0, 0, 0},
+ {0x00000F6C, 0x000001B0, 0, 0},
+ {0x00000F70, 0x000001C0, 0, 0},
+ {0x00000F74, 0x000001D0, 0, 0},
+ {0x00000F78, 0x000001E0, 0, 0},
+ {0x00000F7C, 0x000001F0, 0, 0},
+ {0x00000F80, 0x00000200, 0, 0},
+ {0x00000F84, 0x00000210, 0, 0},
+ {0x00000F88, 0x00000220, 0, 0},
+ {0x00000F8C, 0x00000230, 0, 0},
+ {0x00000F90, 0x00000240, 0, 0},
+ {0x00000F94, 0x00000250, 0, 0},
+ {0x00000F98, 0x00000260, 0, 0},
+ {0x00000F9C, 0x00000270, 0, 0},
+ {0x00000FA0, 0x00000280, 0, 0},
+ {0x00000FA4, 0x00000290, 0, 0},
+ {0x00000FA8, 0x000002A0, 0, 0},
+ {0x00000FAC, 0x000002B0, 0, 0},
+ {0x00000FB0, 0x000002C0, 0, 0},
+ {0x00000FB4, 0x000002D0, 0, 0},
+ {0x00000FB8, 0x000002E0, 0, 0},
+ {0x00000FBC, 0x000002F0, 0, 0},
+ {0x00000FC0, 0x00000300, 0, 0},
+ {0x00000FC4, 0x00000310, 0, 0},
+ {0x00000FC8, 0x00000320, 0, 0},
+ {0x00000FCC, 0x00000330, 0, 0},
+ {0x00000FD0, 0x00000340, 0, 0},
+ {0x00000FD4, 0x00000350, 0, 0},
+ {0x00000FD8, 0x00000360, 0, 0},
+ {0x00000FDC, 0x00000370, 0, 0},
+ {0x00000FE0, 0x00000380, 0, 0},
+ {0x00000FE4, 0x00000390, 0, 0},
+ {0x00000FE8, 0x000003A0, 0, 0},
+ {0x00000FEC, 0x000003B0, 0, 0},
+ {0x00000FF0, 0x000003C0, 0, 0},
+ {0x00000FF4, 0x000003D0, 0, 0},
+ {0x00000FF8, 0x000003E0, 0, 0},
+ {0x00000FFC, 0x000003F0, 0, 0},
+ /* config Sharp */
+ {0x00000E80, 0x00070F00, 0, 0},
+ {0x00000E84, 0x00180F00, 0, 0},
+ {0x00000E88, 0x00800F00, 0, 0},
+ {0x00000E8C, 0x01000F00, 0, 0},
+ {0x00000E90, 0x00100F00, 0, 0},
+ {0x00000E94, 0x00600F00, 0, 0},
+ {0x00000E98, 0x01000F00, 0, 0},
+ {0x00000E9C, 0x01900F00, 0, 0},
+ {0x00000EA0, 0x00000F00, 0, 0},
+ {0x00000EA4, 0x00000F00, 0, 0},
+ {0x00000EA8, 0x00000F00, 0, 0},
+ {0x00000EAC, 0x00000F00, 0, 0},
+ {0x00000EB0, 0x00000F00, 0, 0},
+ {0x00000EB4, 0x00000F00, 0, 0},
+ {0x00000EB8, 0x00000F00, 0, 0},
+ {0x00000EBC, 0x10000000, 0, 0},
+ {0x00000EC0, 0x10000000, 0, 0},
+ {0x00000EC4, 0x10000000, 0, 0},
+ {0x00000EC8, 0x10000000, 0, 0},
+ {0x00000ECC, 0x10000000, 0, 0},
+ {0x00000ED0, 0x10000000, 0, 0},
+ {0x00000ED4, 0x88000D7C, 0, 0},
+ {0x00000ED8, 0x00C00040, 0, 0},
+ {0x00000EDC, 0xFF000000, 0, 0},
+ {0x00000EE0, 0x00A00040, 0, 0},
+ {0x00000EE4, 0x00000000, 0, 0},
+ {0x00000EE8, 0x00000000, 0, 0},
+ /* config DNYUV */
+ {0x00000C00, 0x00777777, 0, 0},
+ {0x00000C04, 0x00007777, 0, 0},
+ {0x00000C08, 0x00777777, 0, 0},
+ {0x00000C0C, 0x00007777, 0, 0},
+ {0x00000C10, 0x00600040, 0, 0},
+ {0x00000C14, 0x00D80090, 0, 0},
+ {0x00000C18, 0x01E60144, 0, 0},
+ {0x00000C1C, 0x00600040, 0, 0},
+ {0x00000C20, 0x00D80090, 0, 0},
+ {0x00000C24, 0x01E60144, 0, 0},
+ /* config SAT */
+ {0x00000A30, 0x00000100, 0, 0},
+ {0x00000A34, 0x001F0001, 0, 0},
+ {0x00000A38, 0x00000000, 0, 0},
+ {0x00000A3C, 0x00000100, 0, 0},
+ {0x00000A40, 0x00000008, 0, 0},
+ {0x00000A54, 0x04010001, 0, 0},
+ {0x00000A58, 0x03FF0001, 0, 0},
+ /* config OBA */
+ {0x00000090, 0x04380000, 0, 0},
+ {0x00000094, 0x04390780, 0, 0},
+ /* config SC */
+ {0x0000009C, 0x01000000, 0, 0},
+ {0x000000B8, 0x000C0000, 0, 0},
+ {0x000000BC, 0xC010151D, 0, 0},
+ {0x000000C0, 0x01F1BF08, 0, 0},
+ {0x000000C4, 0xFF00FF00, 0, 0},
+ {0x000000C8, 0xFF00FF00, 0, 0},
+ {0x000000CC, 0xFFFF0000, 0, 0},
+ {0x000000D0, 0xFFFF0000, 0, 0},
+ {0x000000D4, 0xFFFF0000, 0, 0},
+ {0x000000D8, 0x01050107, 0, 0},
+ {0x000004D0, 0x00000000, 0, 0},
+ {0x000004D4, 0x00000000, 0, 0},
+ {0x000004D8, 0x00000000, 0, 0},
+ {0x000004DC, 0x00000000, 0, 0},
+ {0x000004E0, 0x00000000, 0, 0},
+ {0x000004E4, 0x00000000, 0, 0},
+ {0x000004E8, 0x00000000, 0, 0},
+ {0x000004EC, 0x00000000, 0, 0},
+ {0x000004F0, 0x00100000, 0, 0},
+ {0x000004F4, 0x00000000, 0, 0},
+ {0x000004F8, 0x03D20000, 0, 0},
+ {0x000004FC, 0x00000000, 0, 0},
+ {0x00000500, 0x00950000, 0, 0},
+ {0x00000504, 0x00000000, 0, 0},
+ {0x00000508, 0x00253000, 0, 0},
+ {0x0000050C, 0x00000000, 0, 0},
+ {0x00000510, 0x00000000, 0, 0},
+ {0x00000514, 0x00000000, 0, 0},
+ {0x00000518, 0x00000000, 0, 0},
+ {0x0000051C, 0x00000000, 0, 0},
+ {0x00000520, 0x00000000, 0, 0},
+ {0x00000524, 0x00000000, 0, 0},
+ {0x00000528, 0x00000000, 0, 0},
+ {0x0000052C, 0x00000000, 0, 0},
+ {0x00000530, 0x00000000, 0, 0},
+ {0x00000534, 0x00000000, 0, 0},
+ {0x00000538, 0xFFFFFFF0, 0, 0},
+ {0x0000053C, 0x8FFFFFFF, 0, 0},
+ {0x00000540, 0x0000001E, 0, 0},
+ {0x00000544, 0x00000000, 0, 0},
+ {0x00000548, 0x00000000, 0, 0},
+ {0x0000054C, 0xF0F20000, 0, 0},
+ {0x000005D0, 0xFF00FF00, 0, 0},
+ {0x000005D4, 0xFF00FF00, 0, 0},
+ /* config YHIST */
+ {0x00000CC8, 0x00000000, 0, 0},
+ {0x00000CCC, 0x0437077F, 0, 0},
+ {0x00000CD0, 0x00010002, 0, 0},
+ {0x00000CD4, 0x00000000, 0, 0},
+ /* config CBAR */
+ {0x00000600, 0x043E0782, 0, 0},
+ {0x00000604, 0x00000000, 0, 0},
+ {0x00000608, 0x0437077F, 0, 0},
+ {0x0000060C, 0x00443150, 0, 0},
+ {0x00000610, 0x00000000, 0, 0},
+ {0x00000614, 0x08880888, 0, 0},
+ {0x00000618, 0x02220222, 0, 0},
+ {0x0000061C, 0x04440444, 0, 0},
+ {0x00000620, 0x08880888, 0, 0},
+ {0x00000624, 0x0AAA0AAA, 0, 0},
+ {0x00000628, 0x0CCC0CCC, 0, 0},
+ {0x0000062C, 0x0EEE0EEE, 0, 0},
+ {0x00000630, 0x0FFF0FFF, 0, 0},
+ {0x00000634, 0x08880888, 0, 0},
+ {0x00000638, 0x02220222, 0, 0},
+ {0x0000063C, 0x04440444, 0, 0},
+ {0x00000640, 0x08880888, 0, 0},
+ {0x00000644, 0x0AAA0AAA, 0, 0},
+ {0x00000648, 0x0CCC0CCC, 0, 0},
+ {0x0000064C, 0x0EEE0EEE, 0, 0},
+ {0x00000650, 0x0FFF0FFF, 0, 0},
+ /* config sensor */
+ {0x00000014, 0x0000000c, 0, 0},
+ /* config CROP */
+ {0x0000001C, 0x00000000, 0, 0},
+ {0x00000020, 0x0437077F, 0, 0},
+ /* config isp pileline X/Y size */
+ {0x00000A0C, 0x04380780, 0, 0},
+ /* config CSI dump */
+ {0x00000028, 0x00030B80, 0, 0},
+ /* config UO */
+ {0x00000A88, 0x00000780, 0, 0},
+ /* NV12 */
+ {0x00000A8C, 0x00000000, 0, 0},
+ /* NV21
+ *{0x00000A8C, 0x00000020, 0, 0},
+ */
+ {0x00000A90, 0x00000000, 0, 0},
+ {0x00000A9C, 0x00000780, 0, 0},
+ {0x00000AA0, 0x00000002, 0, 0},
+ {0x00000AA4, 0x00000002, 0, 0},
+ {0x00000AA8, 0x07800438, 0, 0},
+ {0x00000AB4, 0x00000780, 0, 0},
+ {0x00000AB8, 0x00000002, 0, 0},
+ {0x00000ABC, 0x00000002, 0, 0},
+ {0x00000AC0, 0x07800438, 0, 0},
+ {0x00000AC4, 0x00000000, 0, 0},
+ /* config TIL */
+ {0x00000B20, 0x04380780, 0, 0},
+ {0x00000B24, 0x00000960, 0, 0},
+ {0x00000B38, 0x00030003, 0, 0},
+ {0x00000B3C, 0x00000960, 0, 0},
+ {0x00000B44, 0x00000000, 0, 0},
+ {0x00000B48, 0x00000000, 0, 0},
+ /* Enable DEC/OBC/OECF/LCCF/AWB/SC/DUMP */
+ {0x00000010, 0x000A00D6, 0x00000000, 0x00},
+ /* Enable CFA/CAR/CCM/GMARGB/R2Y/SHRP/SAT/DNYUV/YCRV/YHIST/CTC/DBC */
+ {0x00000A08, 0x107A01BE, 0x00000000, 0x00},
+};
+
+const struct reg_table isp_reg_init_settings[] = {
+ {isp_reg_init_config_list, ARRAY_SIZE(isp_reg_init_config_list)},
+};
+
+static const struct regval_t isp_reg_start_config_list[] = {
+ /* ENABLE UO/Multi-Frame and Reset ISP */
+ {0x00000A00, 0x00120002, 0x00000000, 0x0A},
+ /* ENABLE UO/Multi-Frame and Leave ISP reset */
+ {0x00000A00, 0x00120000, 0x00000000, 0x0A},
+ /* Config ISP shadow mode as next-vsync */
+ {0x00000A50, 0x00000002, 0x00000000, 0x00},
+ /* ENABLE UO/Multi-Frame and Enable ISP */
+ {0x00000A00, 0x00120001, 0x00000000, 0x0A},
+ /* Config CSI shadow mode as immediate to fetch current setting */
+ {0x00000008, 0x00010004, 0x00000000, 0x0A},
+ /* Config CSI shadow mode as next-vsync */
+ {0x00000008, 0x00020004, 0x00000000, 0x00},
+ /* Enable CSI */
+ {0x00000000, 0x00000001, 0x00000000, 0x0A},
+};
+
+const struct reg_table isp_reg_start_settings[] = {
+ {isp_reg_start_config_list, ARRAY_SIZE(isp_reg_start_config_list)},
+};
+
+static struct regval_t isp_format_reg_list[] = {
+ {0x0000001C, 0x00000000, 0x00000000, 0},
+ {0x00000020, 0x0437077F, 0x00000000, 0},
+ {0x00000A0C, 0x04380780, 0x00000000, 0},
+ {0x00000A88, 0x00000780, 0x00000000, 0},
+ {0x00000018, 0x000011BB, 0x00000000, 0},
+ {0x00000A08, 0x10000000, 0xF0000000, 0},
+ {0x00000028, 0x00030B80, 0x0003FFFF, 0},
+ {0x00000AA8, 0x07800438, 0x00000000, 0},
+ {0x00000A9C, 0x00000780, 0x00000000, 0},
+ {0x00000AC0, 0x07800438, 0x00000000, 0},
+ {0x00000AB4, 0x00000780, 0x00000000, 0},
+ {0x00000B20, 0x04380780, 0x00000000, 0},
+ {0x00000B24, 0x00000960, 0x00000000, 0},
+ {0x00000B3C, 0x00000960, 0x00000000, 0},
+ {0x00000014, 0x00000008, 0x00000000, 0},
+};
+
+const struct reg_table isp_format_settings[] = {
+ {isp_format_reg_list, ARRAY_SIZE(isp_format_reg_list)},
+};
+
+static void isp_load_regs(void __iomem *ispbase, const struct reg_table *table)
+{
+ int j;
+ u32 delay_ms, reg_addr, mask, val;
+
+ for (j = 0; j < table->regval_num; j++) {
+ delay_ms = table->regval[j].delay_ms;
+ reg_addr = table->regval[j].addr;
+ val = table->regval[j].val;
+ mask = table->regval[j].mask;
+
+ if (reg_addr % 4 ||
+ reg_addr > STF_ISP_REG_OFFSET_MAX ||
+ delay_ms > STF_ISP_REG_DELAY_MAX)
+ continue;
+
+ if (mask)
+ reg_set_bit(ispbase, reg_addr, mask, val);
+ else
+ reg_write(ispbase, reg_addr, val);
+
+ if (delay_ms)
+ usleep_range(1000 * delay_ms, 1000 * delay_ms + 100);
+ }
+}
+
+static void isp_load_regs_exclude_csi_isp_enable(void __iomem *ispbase,
+ const struct reg_table *table)
+{
+ int j;
+ u32 delay_ms, reg_addr, mask, val;
+
+ for (j = 0; j < table->regval_num; j++) {
+ delay_ms = table->regval[j].delay_ms;
+ reg_addr = table->regval[j].addr;
+ val = table->regval[j].val;
+ mask = table->regval[j].mask;
+
+ if (reg_addr % 4 ||
+ reg_addr > STF_ISP_REG_OFFSET_MAX ||
+ delay_ms > STF_ISP_REG_DELAY_MAX ||
+ (reg_addr == ISP_REG_CSI_INPUT_EN_AND_STATUS && (val & 0x01)) ||
+ (reg_addr == ISP_REG_ISP_CTRL_0 && (val & 0x01)))
+ continue;
+
+ if (mask)
+ reg_set_bit(ispbase, reg_addr, mask, val);
+ else
+ reg_write(ispbase, reg_addr, val);
+ if (delay_ms)
+ usleep_range(1000 * delay_ms, 1000 * delay_ms + 100);
+ }
+}
+
+static int stf_isp_clk_enable(struct stf_isp_dev *isp_dev)
+{
+ struct stfcamss *stfcamss = isp_dev->stfcamss;
+
+ clk_prepare_enable(stfcamss->sys_clk[STF_CLK_WRAPPER_CLK_C].clk);
+ reset_control_deassert(stfcamss->sys_rst[STF_RST_WRAPPER_C].rstc);
+ reset_control_deassert(stfcamss->sys_rst[STF_RST_WRAPPER_P].rstc);
+
+ return 0;
+}
+
+static int stf_isp_clk_disable(struct stf_isp_dev *isp_dev)
+{
+ struct stfcamss *stfcamss = isp_dev->stfcamss;
+
+ reset_control_assert(stfcamss->sys_rst[STF_RST_WRAPPER_C].rstc);
+ reset_control_assert(stfcamss->sys_rst[STF_RST_WRAPPER_P].rstc);
+ clk_disable_unprepare(stfcamss->sys_clk[STF_CLK_WRAPPER_CLK_C].clk);
+
+ return 0;
+}
+
+static void __iomem *stf_isp_get_ispbase(struct stf_isp_dev *isp_dev)
+{
+ void __iomem *base = isp_dev->stfcamss->isp_base;
+
+ return base;
+}
+
+static int stf_isp_reset(struct stf_isp_dev *isp_dev)
+{
+ void __iomem *ispbase = stf_isp_get_ispbase(isp_dev);
+
+ reg_set_bit(ispbase, ISP_REG_ISP_CTRL_0, BIT(1), BIT(1));
+ reg_set_bit(ispbase, ISP_REG_ISP_CTRL_0, BIT(1), 0);
+
+ return 0;
+}
+
+static int stf_isp_config_set(struct stf_isp_dev *isp_dev)
+{
+ void __iomem *ispbase = stf_isp_get_ispbase(isp_dev);
+
+ mutex_lock(&isp_dev->setfile_lock);
+ isp_load_regs(ispbase, isp_reg_init_settings);
+ if (isp_dev->setfile.state) {
+ st_info(ST_ISP, "%s, Program extra ISP setting!\n", __func__);
+ isp_load_regs_exclude_csi_isp_enable(ispbase,
+ &isp_dev->setfile.settings);
+ }
+
+ mutex_unlock(&isp_dev->setfile_lock);
+
+ return 0;
+}
+
+static int stf_isp_set_format(struct stf_isp_dev *isp_dev,
+ struct isp_stream_format *crop_array,
+ u32 mcode,
+ int type)
+{
+ struct v4l2_rect *crop = &crop_array[ISP_COMPOSE].rect;
+ u32 bpp = crop_array[ISP_COMPOSE].bpp;
+ void __iomem *ispbase = stf_isp_get_ispbase(isp_dev);
+ u32 val, val1;
+
+ st_debug(ST_ISP, "interface type is %d(%s)\n",
+ type, type == INTERFACE_CSI ? "CSI" : "DVP");
+
+ val = crop->left + (crop->top << 16);
+ isp_format_reg_list[0].addr = ISP_REG_PIC_CAPTURE_START_CFG;
+ isp_format_reg_list[0].val = val;
+
+ val = (crop->width + crop->left - 1)
+ + ((crop->height + crop->top - 1) << 16);
+ isp_format_reg_list[1].addr = ISP_REG_PIC_CAPTURE_END_CFG;
+ isp_format_reg_list[1].val = val;
+
+ val = crop->width + (crop->height << 16);
+ isp_format_reg_list[2].addr = ISP_REG_PIPELINE_XY_SIZE;
+ isp_format_reg_list[2].val = val;
+
+ isp_format_reg_list[3].addr = ISP_REG_STRIDE;
+ isp_format_reg_list[3].val = ALIGN(crop->width * bpp / 8,
+ STFCAMSS_FRAME_WIDTH_ALIGN_8);
+
+ switch (mcode) {
+ case MEDIA_BUS_FMT_SRGGB10_1X10:
+ case MEDIA_BUS_FMT_SRGGB8_1X8:
+ /* 3 2 3 2 1 0 1 0 B Gb B Gb Gr R Gr R */
+ val = 0x0000EE44;
+ val1 = 0x00000000;
+ break;
+ case MEDIA_BUS_FMT_SGRBG10_1X10:
+ case MEDIA_BUS_FMT_SGRBG8_1X8:
+ /* 2 3 2 3 0 1 0 1, Gb B Gb B R Gr R Gr */
+ val = 0x0000BB11;
+ val1 = 0x20000000;
+ break;
+ case MEDIA_BUS_FMT_SGBRG10_1X10:
+ case MEDIA_BUS_FMT_SGBRG8_1X8:
+ /* 1 0 1 0 3 2 3 2, Gr R Gr R B Gb B Gb */
+ val = 0x000044EE;
+ val1 = 0x30000000;
+ break;
+ case MEDIA_BUS_FMT_SBGGR10_1X10:
+ case MEDIA_BUS_FMT_SBGGR8_1X8:
+ /* 0 1 0 1 2 3 2 3 R Gr R Gr Gb B Gb B */
+ val = 0x000011BB;
+ val1 = 0x10000000;
+ break;
+ default:
+ st_err(ST_ISP, "UNKNOWN format\n");
+ val = 0x000011BB;
+ val1 = 0x10000000;
+ break;
+ }
+
+ isp_format_reg_list[4].addr = ISP_REG_RAW_FORMAT_CFG;
+ isp_format_reg_list[4].val = val;
+
+ isp_format_reg_list[5].addr = ISP_REG_ISP_CTRL_1;
+ isp_format_reg_list[5].val = val1;
+ isp_format_reg_list[5].mask = 0xF0000000;
+
+ st_info(ST_ISP,
+ "src left: %d, top: %d, width = %d, height = %d, bpp = %d\n",
+ crop->left, crop->top, crop->width, crop->height, bpp);
+
+ crop = &crop_array[ISP_CROP].rect;
+ bpp = crop_array[ISP_CROP].bpp;
+ val = ALIGN(crop->width * bpp / 8, STFCAMSS_FRAME_WIDTH_ALIGN_128);
+ isp_format_reg_list[6].addr = ISP_REG_DUMP_CFG_1;
+ isp_format_reg_list[6].val = val | 3 << 16;
+ isp_format_reg_list[6].mask = 0x0003FFFF;
+
+ st_info(ST_ISP,
+ "raw left: %d, top: %d, width = %d, height = %d, bpp = %d\n",
+ crop->left, crop->top, crop->width, crop->height, bpp);
+
+ crop = &crop_array[ISP_SCALE_SS0].rect;
+ bpp = crop_array[ISP_SCALE_SS0].bpp;
+ isp_format_reg_list[7].addr = ISP_REG_SS0IW;
+ isp_format_reg_list[7].val = (crop->width << 16) + crop->height;
+ isp_format_reg_list[8].addr = ISP_REG_SS0S;
+ isp_format_reg_list[8].val =
+ ALIGN(crop->width * bpp / 8, STFCAMSS_FRAME_WIDTH_ALIGN_8);
+
+ st_info(ST_ISP,
+ "ss0 left: %d, top: %d, width = %d, height = %d, bpp = %d\n",
+ crop->left, crop->top, crop->width, crop->height, bpp);
+
+ crop = &crop_array[ISP_SCALE_SS1].rect;
+ bpp = crop_array[ISP_SCALE_SS1].bpp;
+ isp_format_reg_list[9].addr = ISP_REG_SS1IW;
+ isp_format_reg_list[9].val = (crop->width << 16) + crop->height;
+ isp_format_reg_list[10].addr = ISP_REG_SS1S;
+ isp_format_reg_list[10].val =
+ ALIGN(crop->width * bpp / 8, STFCAMSS_FRAME_WIDTH_ALIGN_8);
+
+ isp_format_reg_list[14].addr = ISP_REG_SENSOR;
+ isp_format_reg_list[14].val = 0x00000000;
+ if (type == INTERFACE_CSI)
+ isp_format_reg_list[14].val |= 0x01;
+
+ isp_load_regs(ispbase, isp_format_settings);
+ return 0;
+}
+
+static int stf_isp_stream_set(struct stf_isp_dev *isp_dev, int on)
+{
+ void __iomem *ispbase = stf_isp_get_ispbase(isp_dev);
+
+ if (on)
+ isp_load_regs(ispbase, isp_reg_start_settings);
+
+ return 0;
+}
+
+static int stf_isp_reg_read(struct stf_isp_dev *isp_dev, void *arg)
+{
+ void __iomem *ispbase = stf_isp_get_ispbase(isp_dev);
+ struct isp_reg_param *reg_param = arg;
+ union reg_buf *reg_buf = isp_dev->reg_buf;
+ u32 size;
+ unsigned long r;
+
+ if (!reg_param->reg_buf) {
+ st_err(ST_ISP,
+ "Failed to access register.The pointer is NULL!\n");
+ return -EINVAL;
+ }
+
+ size = 0;
+ switch (reg_param->reg_info.method) {
+ case STF_ISP_REG_METHOD_ONE_REG:
+ break;
+
+ case STF_ISP_REG_METHOD_SERIES:
+ if (reg_param->reg_info.length > STF_ISP_REG_BUF_SIZE) {
+ st_err(ST_ISP,
+ "The length = 0x%08X is out of size!\n",
+ reg_param->reg_info.length);
+ return -EINVAL;
+ }
+ break;
+
+ case STF_ISP_REG_METHOD_MODULE:
+ st_err(ST_ISP, "The method = %d is not supported!\n",
+ reg_param->reg_info.method);
+ return -ENOTTY;
+
+ case STF_ISP_REG_METHOD_TABLE:
+ if (reg_param->reg_info.length > STF_ISP_REG_TBL_BUF_SIZE) {
+ st_err(ST_ISP,
+ "The length = 0x%08X is out of size!\n",
+ reg_param->reg_info.length);
+ return -EINVAL;
+ }
+ size = sizeof(u32) * reg_param->reg_info.length * 2;
+ break;
+
+ case STF_ISP_REG_METHOD_TABLE_2:
+ if (reg_param->reg_info.length > STF_ISP_REG_TBL_2_BUF_SIZE) {
+ st_err(ST_ISP,
+ "The length = 0x%08X is out of size!\n",
+ reg_param->reg_info.length);
+ return -EINVAL;
+ }
+ size = sizeof(u32) * reg_param->reg_info.length * 3;
+ break;
+
+ case STF_ISP_REG_METHOD_TABLE_3:
+ if (reg_param->reg_info.length > STF_ISP_REG_TBL_3_BUF_SIZE) {
+ st_err(ST_ISP,
+ "The length = 0x%08X is out of size!\n",
+ reg_param->reg_info.length);
+ return -EINVAL;
+ }
+ size = sizeof(u32) * reg_param->reg_info.length * 4;
+ break;
+
+ case STF_ISP_REG_METHOD_SMPL_PACK:
+ st_err(ST_ISP, "The method = %d is not supported!\n",
+ reg_param->reg_info.method);
+ return -ENOTTY;
+
+ case STF_ISP_REG_METHOD_SOFT_RDMA:
+ st_err(ST_ISP, "The method = %d is not supported!\n",
+ reg_param->reg_info.method);
+ return -ENOTTY;
+
+ default:
+ st_err(ST_ISP, "The method = %d is not supported!\n",
+ reg_param->reg_info.method);
+ return -ENOTTY;
+ }
+
+ memset(reg_buf, 0, sizeof(union reg_buf));
+ if (size) {
+ r = copy_from_user((u8 *)reg_buf->buffer,
+ (u8 *)reg_param->reg_buf->buffer, size);
+ if (r) {
+ st_err(ST_ISP, "Failed to copy buffer data\n");
+ return -EIO;
+ }
+ }
+
+ size = 0;
+ switch (reg_param->reg_info.method) {
+ case STF_ISP_REG_METHOD_ONE_REG:
+ reg_buf->buffer[0] = reg_read(ispbase,
+ reg_param->reg_info.offset);
+ size = sizeof(u32);
+ break;
+
+ case STF_ISP_REG_METHOD_SERIES:
+ for (r = 0; r < reg_param->reg_info.length; r++)
+ reg_buf->buffer[r] =
+ reg_read(ispbase,
+ reg_param->reg_info.offset + (r * 4));
+
+ size = sizeof(u32) * reg_param->reg_info.length;
+ break;
+
+ case STF_ISP_REG_METHOD_MODULE:
+ break;
+
+ case STF_ISP_REG_METHOD_TABLE:
+ for (r = 0; r < reg_param->reg_info.length; r++)
+ reg_buf->reg_tbl[r].value =
+ reg_read(ispbase, reg_buf->reg_tbl[r].offset);
+
+ size = sizeof(u32) * reg_param->reg_info.length * 2;
+ break;
+
+ case STF_ISP_REG_METHOD_TABLE_2:
+ for (r = 0; r < reg_param->reg_info.length; r++) {
+ if (reg_buf->reg_tbl2[r].mask)
+ reg_buf->reg_tbl2[r].value =
+ reg_read(ispbase, reg_buf->reg_tbl2[r].offset) &
+ reg_buf->reg_tbl2[r].mask;
+ else
+ reg_buf->reg_tbl2[r].value =
+ reg_read(ispbase,
+ reg_buf->reg_tbl2[r].offset);
+ }
+
+ size = sizeof(u32) * reg_param->reg_info.length * 3;
+ break;
+
+ case STF_ISP_REG_METHOD_TABLE_3:
+ for (r = 0; r < reg_param->reg_info.length; r++) {
+ if (reg_buf->reg_tbl3[r].mask)
+ reg_buf->reg_tbl3[r].value =
+ reg_read(ispbase, reg_buf->reg_tbl3[r].offset) &
+ reg_buf->reg_tbl3[r].mask;
+ else
+ reg_buf->reg_tbl3[r].value =
+ reg_read(ispbase,
+ reg_buf->reg_tbl3[r].offset);
+
+ if (reg_buf->reg_tbl3[r].delay_ms)
+ usleep_range(1000 * reg_buf->reg_tbl3[r].delay_ms,
+ 1000 * reg_buf->reg_tbl3[r].delay_ms + 100);
+ }
+
+ size = sizeof(u32) * reg_param->reg_info.length * 4;
+ break;
+
+ case STF_ISP_REG_METHOD_SMPL_PACK:
+ break;
+
+ case STF_ISP_REG_METHOD_SOFT_RDMA:
+ break;
+
+ default:
+ break;
+ }
+
+ r = copy_to_user((u8 *)reg_param->reg_buf->buffer,
+ (u8 *)reg_buf->buffer, size);
+ if (r) {
+ st_err(ST_ISP,
+ "Failed to copy the reg_param->buffer value\n");
+ return -EIO;
+ }
+
+ return 0;
+}
+
+static int stf_isp_soft_rdma(struct stf_isp_dev *isp_dev, u32 rdma_addr)
+{
+ void __iomem *ispbase = stf_isp_get_ispbase(isp_dev);
+ struct isp_rdma_info *rdma_info = NULL;
+ s32 len;
+ u32 offset;
+ int ret = 0;
+
+ rdma_info = phys_to_virt(rdma_addr);
+ while (1) {
+ if (rdma_info->tag == RDMA_END)
+ break;
+
+ if (rdma_info->tag == RDMA_WR_ONE) {
+ reg_write(ispbase, rdma_info->offset, rdma_info->param);
+ rdma_info++;
+ } else if (rdma_info->tag == RDMA_WR_SRL) {
+ offset = rdma_info->offset;
+ len = rdma_info->param;
+ rdma_info++;
+ while (len > 0) {
+ reg_write(ispbase, offset, rdma_info->param);
+ offset += 4;
+ len--;
+ if (len > 0) {
+ reg_write(ispbase,
+ offset, rdma_info->value);
+ len--;
+ }
+ offset += 4;
+ rdma_info++;
+ }
+ } else if (rdma_info->tag == RDMA_LINK) {
+ rdma_info = phys_to_virt(rdma_info->param);
+ } else if (rdma_info->tag == RDMA_SINT) {
+ /* not support this command. */
+ rdma_info++;
+ } else {
+ rdma_info++;
+ }
+ }
+
+ return ret;
+}
+
+static int stf_isp_reg_write(struct stf_isp_dev *isp_dev, void *arg)
+{
+ void __iomem *ispbase = stf_isp_get_ispbase(isp_dev);
+ struct isp_reg_param *reg_param = arg;
+ struct isp_rdma_info *rdma_info = NULL;
+ union reg_buf *reg_buf = isp_dev->reg_buf;
+ s32 len;
+ u32 offset;
+ u32 size;
+ unsigned long r;
+ int ret = 0;
+
+ if (!reg_param->reg_buf &&
+ reg_param->reg_info.method != STF_ISP_REG_METHOD_SOFT_RDMA) {
+ st_err(ST_ISP, "Failed to access register.\n");
+ return -EINVAL;
+ }
+
+ size = 0;
+ switch (reg_param->reg_info.method) {
+ case STF_ISP_REG_METHOD_ONE_REG:
+ size = sizeof(u32);
+ break;
+
+ case STF_ISP_REG_METHOD_SERIES:
+ if (reg_param->reg_info.length > STF_ISP_REG_BUF_SIZE) {
+ st_err(ST_ISP,
+ "The length = 0x%08X is out of size!\n",
+ reg_param->reg_info.length);
+ return -EINVAL;
+ }
+ size = sizeof(u32) * reg_param->reg_info.length;
+ break;
+
+ case STF_ISP_REG_METHOD_MODULE:
+ st_err(ST_ISP, "The method = %d is not supported!\n",
+ reg_param->reg_info.method);
+ return -ENOTTY;
+
+ case STF_ISP_REG_METHOD_TABLE:
+ if (reg_param->reg_info.length > STF_ISP_REG_TBL_BUF_SIZE) {
+ st_err(ST_ISP,
+ "The length = 0x%08X is out of size!\n",
+ reg_param->reg_info.length);
+ return -EINVAL;
+ }
+ size = sizeof(u32) * reg_param->reg_info.length * 2;
+ break;
+
+ case STF_ISP_REG_METHOD_TABLE_2:
+ if (reg_param->reg_info.length > STF_ISP_REG_TBL_2_BUF_SIZE) {
+ st_err(ST_ISP,
+ "The length = 0x%08X is out of size!\n",
+ reg_param->reg_info.length);
+ return -EINVAL;
+ }
+ size = sizeof(u32) * reg_param->reg_info.length * 3;
+ break;
+
+ case STF_ISP_REG_METHOD_TABLE_3:
+ if (reg_param->reg_info.length > STF_ISP_REG_TBL_3_BUF_SIZE) {
+ st_err(ST_ISP,
+ "The length = 0x%08X is out of size!\n",
+ reg_param->reg_info.length);
+ return -EINVAL;
+ }
+ size = sizeof(u32) * reg_param->reg_info.length * 4;
+ break;
+
+ case STF_ISP_REG_METHOD_SMPL_PACK:
+ if (reg_param->reg_info.length >
+ STF_ISP_REG_SMPL_PACK_BUF_SIZE) {
+ st_err(ST_ISP,
+ "The length = 0x%08X is out of size!\n",
+ reg_param->reg_info.length);
+ return -EINVAL;
+ }
+ size = sizeof(u32) * reg_param->reg_info.length * 2;
+ break;
+
+ case STF_ISP_REG_METHOD_SOFT_RDMA:
+ break;
+
+ default:
+ st_err(ST_ISP, "The method = %d is not supported!\n",
+ reg_param->reg_info.method);
+ return -ENOTTY;
+ }
+
+ memset(reg_buf, 0, sizeof(union reg_buf));
+ if (size) {
+ r = copy_from_user((u8 *)reg_buf->buffer,
+ (u8 *)reg_param->reg_buf->buffer, size);
+ if (r) {
+ st_err(ST_ISP, "Failed to copy buffer data\n");
+ return -EIO;
+ }
+ }
+
+ switch (reg_param->reg_info.method) {
+ case STF_ISP_REG_METHOD_ONE_REG:
+ reg_write(ispbase,
+ reg_param->reg_info.offset, reg_buf->buffer[0]);
+ break;
+
+ case STF_ISP_REG_METHOD_SERIES:
+ for (r = 0; r < reg_param->reg_info.length; r++) {
+ reg_write(ispbase,
+ reg_param->reg_info.offset + (r * 4),
+ reg_buf->buffer[r]);
+ }
+ break;
+
+ case STF_ISP_REG_METHOD_MODULE:
+ /* This mode is not supported.*/
+ break;
+
+ case STF_ISP_REG_METHOD_TABLE:
+ for (r = 0; r < reg_param->reg_info.length; r++) {
+ reg_write(ispbase, reg_buf->reg_tbl[r].offset,
+ reg_buf->reg_tbl[r].value);
+ }
+ break;
+
+ case STF_ISP_REG_METHOD_TABLE_2:
+ for (r = 0; r < reg_param->reg_info.length; r++) {
+ if (reg_buf->reg_tbl2[r].mask) {
+ reg_set_bit(ispbase,
+ reg_buf->reg_tbl2[r].offset,
+ reg_buf->reg_tbl2[r].mask,
+ reg_buf->reg_tbl2[r].value);
+ } else {
+ reg_write(ispbase, reg_buf->reg_tbl2[r].offset,
+ reg_buf->reg_tbl2[r].value);
+ }
+ }
+ break;
+
+ case STF_ISP_REG_METHOD_TABLE_3:
+ for (r = 0; r < reg_param->reg_info.length; r++) {
+ if (reg_buf->reg_tbl3[r].mask)
+ reg_set_bit(ispbase,
+ reg_buf->reg_tbl3[r].offset,
+ reg_buf->reg_tbl3[r].mask,
+ reg_buf->reg_tbl3[r].value);
+ else
+ reg_write(ispbase, reg_buf->reg_tbl3[r].offset,
+ reg_buf->reg_tbl3[r].value);
+
+ if (reg_buf->reg_tbl3[r].delay_ms)
+ usleep_range(1000 * reg_buf->reg_tbl3[r].delay_ms,
+ 1000 * reg_buf->reg_tbl3[r].delay_ms + 100);
+ }
+ break;
+
+ case STF_ISP_REG_METHOD_SMPL_PACK:
+ size = reg_param->reg_info.length;
+ rdma_info = ®_buf->rdma_cmd[0];
+ while (size) {
+ if (rdma_info->tag == RDMA_END)
+ break;
+
+ if (rdma_info->tag == RDMA_WR_ONE) {
+ reg_write(ispbase,
+ rdma_info->offset, rdma_info->param);
+ rdma_info++;
+ size--;
+ } else if (rdma_info->tag == RDMA_WR_SRL) {
+ offset = rdma_info->offset;
+ len = rdma_info->param;
+ rdma_info++;
+ size--;
+ while (size && (len > 0)) {
+ reg_write(ispbase,
+ offset, rdma_info->param);
+ offset += 4;
+ len--;
+ if (len > 0) {
+ reg_write(ispbase,
+ offset,
+ rdma_info->value);
+ len--;
+ }
+ offset += 4;
+ rdma_info++;
+ size--;
+ }
+ } else {
+ rdma_info++;
+ size--;
+ }
+ }
+ break;
+
+ case STF_ISP_REG_METHOD_SOFT_RDMA:
+ /*
+ * Simulation the hardware RDMA behavior to debug and verify
+ * the RDMA chain.
+ */
+ ret = stf_isp_soft_rdma(isp_dev, reg_param->reg_info.offset);
+ break;
+
+ default:
+ break;
+ }
+
+ return ret;
+}
+
+static int stf_isp_shadow_trigger(struct stf_isp_dev *isp_dev)
+{
+ void __iomem *ispbase = stf_isp_get_ispbase(isp_dev);
+
+ /* shadow update */
+ reg_set_bit(ispbase,
+ ISP_REG_CSIINTS_ADDR, (BIT(17) | BIT(16)), 0x30000);
+ reg_set_bit(ispbase, ISP_REG_IESHD_ADDR, (BIT(1) | BIT(0)), 0x3);
+
+ return 0;
+}
+
+const struct isp_hw_ops isp_ops = {
+ .isp_clk_enable = stf_isp_clk_enable,
+ .isp_clk_disable = stf_isp_clk_disable,
+ .isp_reset = stf_isp_reset,
+ .isp_config_set = stf_isp_config_set,
+ .isp_set_format = stf_isp_set_format,
+ .isp_stream_set = stf_isp_stream_set,
+ .isp_reg_read = stf_isp_reg_read,
+ .isp_reg_write = stf_isp_reg_write,
+ .isp_shadow_trigger = stf_isp_shadow_trigger,
+};
--- /dev/null
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * stf_video.c
+ *
+ * StarFive Camera Subsystem - V4L2 device node
+ *
+ * Copyright (C) 2021-2023 StarFive Technology Co., Ltd.
+ */
+
+#include <media/media-entity.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-mc.h>
+#include <media/videobuf2-dma-contig.h>
+
+#include "stf_camss.h"
+#include "stf_video.h"
+
+static const struct stfcamss_format_info formats_pix_wr[] = {
+ { MEDIA_BUS_FMT_SRGGB10_1X10, V4L2_PIX_FMT_SRGGB10, 1,
+ { { 1, 1 } }, { { 1, 1 } }, { 10 } },
+ { MEDIA_BUS_FMT_SGRBG10_1X10, V4L2_PIX_FMT_SGRBG10, 1,
+ { { 1, 1 } }, { { 1, 1 } }, { 10 } },
+ { MEDIA_BUS_FMT_SGBRG10_1X10, V4L2_PIX_FMT_SGBRG10, 1,
+ { { 1, 1 } }, { { 1, 1 } }, { 10 } },
+ { MEDIA_BUS_FMT_SBGGR10_1X10, V4L2_PIX_FMT_SBGGR10, 1,
+ { { 1, 1 } }, { { 1, 1 } }, { 10 } },
+};
+
+static const struct stfcamss_format_info formats_raw_isp[] = {
+ { MEDIA_BUS_FMT_SBGGR12_1X12, V4L2_PIX_FMT_SBGGR12, 1,
+ { { 1, 1 } }, { { 1, 1 } }, { 12 } },
+ { MEDIA_BUS_FMT_SRGGB12_1X12, V4L2_PIX_FMT_SRGGB12, 1,
+ { { 1, 1 } }, { { 1, 1 } }, { 12 } },
+ { MEDIA_BUS_FMT_SGRBG12_1X12, V4L2_PIX_FMT_SGRBG12, 1,
+ { { 1, 1 } }, { { 1, 1 } }, { 12 } },
+ { MEDIA_BUS_FMT_SGBRG12_1X12, V4L2_PIX_FMT_SGBRG12, 1,
+ { { 1, 1 } }, { { 1, 1 } }, { 12 } },
+};
+
+static const struct stfcamss_format_info formats_pix_isp[] = {
+ { MEDIA_BUS_FMT_Y12_1X12, V4L2_PIX_FMT_NV12, 1,
+ { { 1, 1 } }, { { 2, 3 } }, { 8 } },
+};
+
+/* -----------------------------------------------------------------------------
+ * Helper functions
+ */
+
+static int video_find_format(u32 code, u32 pixelformat,
+ const struct stfcamss_format_info *formats,
+ unsigned int nformats)
+{
+ int i;
+
+ for (i = 0; i < nformats; i++) {
+ if (formats[i].code == code &&
+ formats[i].pixelformat == pixelformat)
+ return i;
+ }
+
+ for (i = 0; i < nformats; i++)
+ if (formats[i].code == code)
+ return i;
+
+ for (i = 0; i < nformats; i++)
+ if (formats[i].pixelformat == pixelformat)
+ return i;
+
+ return -EINVAL;
+}
+
+static int __video_try_fmt(struct stfcamss_video *video, struct v4l2_format *f)
+{
+ struct v4l2_pix_format *pix;
+ const struct stfcamss_format_info *fi;
+ u32 width, height;
+ u32 bpl;
+ int i;
+
+ st_debug(ST_VIDEO, "%s, fmt.type = 0x%x\n", __func__, f->type);
+ pix = &f->fmt.pix;
+
+ for (i = 0; i < video->nformats; i++)
+ if (pix->pixelformat == video->formats[i].pixelformat)
+ break;
+
+ if (i == video->nformats)
+ i = 0; /* default format */
+
+ fi = &video->formats[i];
+ width = pix->width;
+ height = pix->height;
+
+ memset(pix, 0, sizeof(*pix));
+
+ pix->pixelformat = fi->pixelformat;
+ pix->width = clamp_t(u32, width, STFCAMSS_FRAME_MIN_WIDTH,
+ STFCAMSS_FRAME_MAX_WIDTH);
+ pix->height = clamp_t(u32, height, STFCAMSS_FRAME_MIN_HEIGHT,
+ STFCAMSS_FRAME_MAX_HEIGHT);
+ bpl = pix->width / fi->hsub[0].numerator *
+ fi->hsub[0].denominator * fi->bpp[0] / 8;
+ bpl = ALIGN(bpl, video->bpl_alignment);
+ pix->bytesperline = bpl;
+ pix->sizeimage = pix->height / fi->vsub[0].numerator *
+ fi->vsub[0].denominator * bpl;
+
+ pix->field = V4L2_FIELD_NONE;
+ pix->colorspace = V4L2_COLORSPACE_SRGB;
+ pix->flags = 0;
+ pix->ycbcr_enc =
+ V4L2_MAP_YCBCR_ENC_DEFAULT(pix->colorspace);
+ pix->quantization = V4L2_MAP_QUANTIZATION_DEFAULT(true,
+ pix->colorspace,
+ pix->ycbcr_enc);
+ pix->xfer_func = V4L2_MAP_XFER_FUNC_DEFAULT(pix->colorspace);
+
+ st_info(ST_VIDEO, "w, h = %d, %d, bpp = %d\n",
+ pix->width, pix->height, fi->bpp[0]);
+ st_info(ST_VIDEO, "i = %d, s = 0x%x\n", i, pix->sizeimage);
+
+ return 0;
+}
+
+static int stf_video_init_format(struct stfcamss_video *video)
+{
+ int ret;
+ struct v4l2_format format = {
+ .type = video->type,
+ .fmt.pix = {
+ .width = 1920,
+ .height = 1080,
+ .pixelformat = V4L2_PIX_FMT_RGB565,
+ },
+ };
+
+ ret = __video_try_fmt(video, &format);
+
+ if (ret < 0)
+ return ret;
+
+ video->active_fmt = format;
+
+ return 0;
+}
+
+/* -----------------------------------------------------------------------------
+ * Video queue operations
+ */
+
+static int video_queue_setup(struct vb2_queue *q,
+ unsigned int *num_buffers,
+ unsigned int *num_planes,
+ unsigned int sizes[],
+ struct device *alloc_devs[])
+{
+ struct stfcamss_video *video = vb2_get_drv_priv(q);
+ const struct v4l2_pix_format *format = &video->active_fmt.fmt.pix;
+
+ st_debug(ST_VIDEO, "%s, planes = %d\n", __func__, *num_planes);
+
+ if (*num_planes) {
+ if (*num_planes != 1)
+ return -EINVAL;
+
+ if (sizes[0] < format->sizeimage)
+ return -EINVAL;
+ }
+
+ *num_planes = 1;
+ sizes[0] = format->sizeimage;
+ if (!sizes[0])
+ st_err(ST_VIDEO, "%s: error size is zero!!!\n", __func__);
+
+ st_info(ST_VIDEO, "%s, planes = %d, size = %d\n",
+ __func__, *num_planes, sizes[0]);
+
+ return 0;
+}
+
+static int video_buf_init(struct vb2_buffer *vb)
+{
+ struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+ struct stfcamss_video *video = vb2_get_drv_priv(vb->vb2_queue);
+ struct stfcamss_buffer *buffer =
+ container_of(vbuf, struct stfcamss_buffer, vb);
+ const struct v4l2_pix_format *fmt = &video->active_fmt.fmt.pix;
+ dma_addr_t *paddr;
+
+ buffer->sizeimage = 0;
+
+ paddr = vb2_plane_cookie(vb, 0);
+ buffer->sizeimage = vb2_plane_size(vb, 0);
+ buffer->addr[0] = *paddr;
+ if (fmt->pixelformat == V4L2_PIX_FMT_NV12 ||
+ fmt->pixelformat == V4L2_PIX_FMT_NV21 ||
+ fmt->pixelformat == V4L2_PIX_FMT_NV16 ||
+ fmt->pixelformat == V4L2_PIX_FMT_NV61)
+ buffer->addr[1] =
+ buffer->addr[0] + fmt->bytesperline * fmt->height;
+
+ return 0;
+}
+
+static int video_buf_prepare(struct vb2_buffer *vb)
+{
+ struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+ struct stfcamss_video *video = vb2_get_drv_priv(vb->vb2_queue);
+ const struct v4l2_pix_format *fmt = &video->active_fmt.fmt.pix;
+
+ if (fmt->sizeimage > vb2_plane_size(vb, 0)) {
+ st_err(ST_VIDEO, "sizeimage = %d, plane size = %d\n",
+ fmt->sizeimage, (unsigned int)vb2_plane_size(vb, 0));
+ return -EINVAL;
+ }
+ vb2_set_plane_payload(vb, 0, fmt->sizeimage);
+
+ vbuf->field = V4L2_FIELD_NONE;
+
+ return 0;
+}
+
+static void video_buf_queue(struct vb2_buffer *vb)
+{
+ struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+ struct stfcamss_video *video = vb2_get_drv_priv(vb->vb2_queue);
+ struct stfcamss_buffer *buffer =
+ container_of(vbuf, struct stfcamss_buffer, vb);
+
+ video->ops->queue_buffer(video, buffer);
+}
+
+/*
+ * video_mbus_to_pix - Convert v4l2_mbus_framefmt to v4l2_pix_format
+ * @mbus: v4l2_mbus_framefmt format (input)
+ * @pix: v4l2_pix_format_mplane format (output)
+ * @f: a pointer to formats array element to be used for the conversion
+ * @alignment: bytesperline alignment value
+ *
+ * Fill the output pix structure with information from the input mbus format.
+ *
+ * Return 0 on success or a negative error code otherwise
+ */
+static int video_mbus_to_pix(const struct v4l2_mbus_framefmt *mbus,
+ struct v4l2_pix_format *pix,
+ const struct stfcamss_format_info *f,
+ unsigned int alignment)
+{
+ u32 bytesperline;
+
+ memset(pix, 0, sizeof(*pix));
+ v4l2_fill_pix_format(pix, mbus);
+ pix->pixelformat = f->pixelformat;
+ bytesperline = pix->width / f->hsub[0].numerator *
+ f->hsub[0].denominator * f->bpp[0] / 8;
+ bytesperline = ALIGN(bytesperline, alignment);
+ pix->bytesperline = bytesperline;
+ pix->sizeimage = pix->height / f->vsub[0].numerator *
+ f->vsub[0].denominator * bytesperline;
+ return 0;
+}
+
+static struct v4l2_subdev *video_remote_subdev(struct stfcamss_video *video,
+ u32 *pad)
+{
+ struct media_pad *remote;
+
+ remote = media_pad_remote_pad_first(&video->pad);
+
+ if (!remote || !is_media_entity_v4l2_subdev(remote->entity))
+ return NULL;
+
+ if (pad)
+ *pad = remote->index;
+
+ return media_entity_to_v4l2_subdev(remote->entity);
+}
+
+static int video_get_subdev_format(struct stfcamss_video *video,
+ struct v4l2_format *format)
+{
+ struct v4l2_pix_format *pix = &video->active_fmt.fmt.pix;
+ struct v4l2_subdev_format fmt;
+ struct v4l2_subdev *subdev;
+ u32 pixelformat;
+ u32 pad;
+ int ret;
+
+ subdev = video_remote_subdev(video, &pad);
+ if (!subdev)
+ return -EPIPE;
+
+ fmt.pad = pad;
+ fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+
+ ret = v4l2_subdev_call(subdev, pad, get_fmt, NULL, &fmt);
+ if (ret)
+ return ret;
+
+ pixelformat = pix->pixelformat;
+ ret = video_find_format(fmt.format.code, pixelformat,
+ video->formats, video->nformats);
+ if (ret < 0)
+ return ret;
+
+ format->type = video->type;
+
+ return video_mbus_to_pix(&fmt.format, &format->fmt.pix,
+ &video->formats[ret], video->bpl_alignment);
+}
+
+static int video_check_format(struct stfcamss_video *video)
+{
+ struct v4l2_pix_format *pix = &video->active_fmt.fmt.pix;
+ struct v4l2_format format;
+ struct v4l2_pix_format *sd_pix = &format.fmt.pix;
+ int ret;
+
+ sd_pix->pixelformat = pix->pixelformat;
+ ret = video_get_subdev_format(video, &format);
+ if (ret < 0)
+ return ret;
+
+ if (pix->pixelformat != sd_pix->pixelformat ||
+ pix->height > sd_pix->height ||
+ pix->width > sd_pix->width ||
+ pix->field != format.fmt.pix.field) {
+ st_err(ST_VIDEO,
+ "%s, not match:\n"
+ "0x%x 0x%x\n0x%x 0x%x\n0x%x 0x%x\n",
+ __func__,
+ pix->pixelformat, sd_pix->pixelformat,
+ pix->height, sd_pix->height,
+ pix->field, format.fmt.pix.field);
+ return -EPIPE;
+ }
+
+ return 0;
+}
+
+static int video_start_streaming(struct vb2_queue *q, unsigned int count)
+{
+ struct stfcamss_video *video = vb2_get_drv_priv(q);
+ struct video_device *vdev = &video->vdev;
+ struct media_entity *entity;
+ struct media_pad *pad;
+ struct v4l2_subdev *subdev;
+ int ret;
+
+ ret = video_device_pipeline_start(vdev, &video->stfcamss->pipe);
+ if (ret < 0) {
+ st_err(ST_VIDEO, "Failed to media_pipeline_start: %d\n", ret);
+ return ret;
+ }
+
+ ret = video_check_format(video);
+ if (ret < 0)
+ goto error;
+ entity = &vdev->entity;
+ while (1) {
+ pad = &entity->pads[0];
+ if (!(pad->flags & MEDIA_PAD_FL_SINK))
+ break;
+
+ pad = media_pad_remote_pad_first(pad);
+ if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
+ break;
+
+ entity = pad->entity;
+ subdev = media_entity_to_v4l2_subdev(entity);
+
+ ret = v4l2_subdev_call(subdev, video, s_stream, 1);
+ if (ret < 0 && ret != -ENOIOCTLCMD)
+ goto error;
+ }
+ return 0;
+
+error:
+ video_device_pipeline_stop(vdev);
+ video->ops->flush_buffers(video, VB2_BUF_STATE_QUEUED);
+ return ret;
+}
+
+static void video_stop_streaming(struct vb2_queue *q)
+{
+ struct stfcamss_video *video = vb2_get_drv_priv(q);
+ struct video_device *vdev = &video->vdev;
+ struct media_entity *entity;
+ struct media_pad *pad;
+ struct v4l2_subdev *subdev;
+
+ entity = &vdev->entity;
+ while (1) {
+ pad = &entity->pads[0];
+ if (!(pad->flags & MEDIA_PAD_FL_SINK))
+ break;
+
+ pad = media_pad_remote_pad_first(pad);
+ if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
+ break;
+
+ entity = pad->entity;
+ subdev = media_entity_to_v4l2_subdev(entity);
+
+ v4l2_subdev_call(subdev, video, s_stream, 0);
+ }
+
+ video_device_pipeline_stop(vdev);
+ video->ops->flush_buffers(video, VB2_BUF_STATE_ERROR);
+}
+
+static const struct vb2_ops stf_video_vb2_q_ops = {
+ .queue_setup = video_queue_setup,
+ .wait_prepare = vb2_ops_wait_prepare,
+ .wait_finish = vb2_ops_wait_finish,
+ .buf_init = video_buf_init,
+ .buf_prepare = video_buf_prepare,
+ .buf_queue = video_buf_queue,
+ .start_streaming = video_start_streaming,
+ .stop_streaming = video_stop_streaming,
+};
+
+/* -----------------------------------------------------------------------------
+ * V4L2 ioctls
+ */
+
+static int video_querycap(struct file *file, void *fh,
+ struct v4l2_capability *cap)
+{
+ struct stfcamss_video *video = video_drvdata(file);
+
+ strscpy(cap->driver, "stf camss", sizeof(cap->driver));
+ strscpy(cap->card, "Starfive Camera Subsystem", sizeof(cap->card));
+ snprintf(cap->bus_info, sizeof(cap->bus_info), "platform:%s",
+ dev_name(video->stfcamss->dev));
+ return 0;
+}
+
+static int video_get_pfmt_by_index(struct stfcamss_video *video, int ndx)
+{
+ int i, j, k;
+
+ /* find index "i" of "k"th unique pixelformat in formats array */
+ k = -1;
+ for (i = 0; i < video->nformats; i++) {
+ for (j = 0; j < i; j++) {
+ if (video->formats[i].pixelformat ==
+ video->formats[j].pixelformat)
+ break;
+ }
+
+ if (j == i)
+ k++;
+
+ if (k == ndx)
+ return i;
+ }
+
+ return -EINVAL;
+}
+
+static int video_get_pfmt_by_mcode(struct stfcamss_video *video, u32 mcode)
+{
+ int i;
+
+ for (i = 0; i < video->nformats; i++) {
+ if (video->formats[i].code == mcode)
+ return i;
+ }
+
+ return -EINVAL;
+}
+
+static int video_enum_fmt(struct file *file, void *fh, struct v4l2_fmtdesc *f)
+{
+ struct stfcamss_video *video = video_drvdata(file);
+ int i;
+
+ st_debug(ST_VIDEO, "%s:\n0x%x 0x%x\n 0x%x, 0x%x\n0x%x\n",
+ __func__,
+ f->type, video->type,
+ f->index, video->nformats,
+ f->mbus_code);
+
+ if (f->type != video->type)
+ return -EINVAL;
+ if (f->index >= video->nformats)
+ return -EINVAL;
+
+ if (f->mbus_code) {
+ /* Each entry in formats[] table has unique mbus_code */
+ if (f->index > 0)
+ return -EINVAL;
+
+ i = video_get_pfmt_by_mcode(video, f->mbus_code);
+ } else {
+ i = video_get_pfmt_by_index(video, f->index);
+ }
+
+ if (i < 0)
+ return -EINVAL;
+
+ f->pixelformat = video->formats[i].pixelformat;
+
+ return 0;
+}
+
+static int video_enum_framesizes(struct file *file, void *fh,
+ struct v4l2_frmsizeenum *fsize)
+{
+ struct v4l2_subdev_frame_size_enum fse = {0};
+ struct v4l2_subdev_mbus_code_enum code = {0};
+ struct stfcamss_video *video = video_drvdata(file);
+ struct video_device *vdev = &video->vdev;
+ struct media_entity *entity = &vdev->entity;
+ struct media_entity *sensor;
+ struct v4l2_subdev *subdev;
+ struct media_pad *pad;
+ bool support_selection = false;
+ int i;
+ int ret;
+
+ for (i = 0; i < video->nformats; i++) {
+ if (video->formats[i].pixelformat == fsize->pixel_format)
+ break;
+ }
+
+ if (i == video->nformats)
+ return -EINVAL;
+
+ entity = &vdev->entity;
+ while (1) {
+ pad = &entity->pads[0];
+ if (!(pad->flags & MEDIA_PAD_FL_SINK))
+ break;
+
+ pad = media_pad_remote_pad_first(pad);
+ if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
+ break;
+
+ entity = pad->entity;
+ subdev = media_entity_to_v4l2_subdev(entity);
+
+ if (v4l2_subdev_has_op(subdev, pad, set_selection)) {
+ support_selection = true;
+ break;
+ }
+ }
+
+ if (support_selection) {
+ if (fsize->index)
+ return -EINVAL;
+ fsize->type = V4L2_FRMSIZE_TYPE_CONTINUOUS;
+ fsize->stepwise.min_width = STFCAMSS_FRAME_MIN_WIDTH;
+ fsize->stepwise.max_width = STFCAMSS_FRAME_MAX_WIDTH;
+ fsize->stepwise.min_height = STFCAMSS_FRAME_MIN_HEIGHT;
+ fsize->stepwise.max_height = STFCAMSS_FRAME_MAX_HEIGHT;
+ fsize->stepwise.step_width = 1;
+ fsize->stepwise.step_height = 1;
+ } else {
+ entity = &vdev->entity;
+ sensor = stfcamss_find_sensor(entity);
+ if (!sensor)
+ return -ENOTTY;
+
+ subdev = media_entity_to_v4l2_subdev(sensor);
+ code.index = 0;
+ code.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+ ret = v4l2_subdev_call(subdev, pad,
+ enum_mbus_code, NULL, &code);
+ if (ret < 0)
+ return -EINVAL;
+ fse.index = fsize->index;
+ fse.code = code.code;
+ fse.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+ ret = v4l2_subdev_call(subdev, pad,
+ enum_frame_size, NULL, &fse);
+ if (ret < 0)
+ return -EINVAL;
+ fsize->type = V4L2_FRMSIZE_TYPE_DISCRETE;
+ fsize->discrete.width = fse.min_width;
+ fsize->discrete.height = fse.min_height;
+ }
+
+ return 0;
+}
+
+static int video_enum_frameintervals(struct file *file, void *fh,
+ struct v4l2_frmivalenum *fival)
+{
+ int ret = 0;
+ struct stfcamss_video *video = video_drvdata(file);
+ struct video_device *vdev = &video->vdev;
+ struct media_entity *entity = &vdev->entity;
+ struct media_entity *sensor;
+ struct v4l2_subdev *subdev;
+ struct v4l2_subdev_mbus_code_enum code = {0};
+ struct v4l2_subdev_frame_interval_enum fie = {0};
+
+ sensor = stfcamss_find_sensor(entity);
+ if (!sensor)
+ return -ENOTTY;
+ fie.index = fival->index;
+ fie.width = fival->width;
+ fie.height = fival->height;
+ fie.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+ subdev = media_entity_to_v4l2_subdev(sensor);
+
+ code.index = 0;
+ code.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+
+ ret = video_find_format(0, fival->pixel_format,
+ video->formats, video->nformats);
+ if (ret < 0)
+ return -EINVAL;
+
+ ret = v4l2_subdev_call(subdev, pad, enum_mbus_code, NULL, &code);
+ if (ret < 0)
+ return -EINVAL;
+
+ fie.code = code.code;
+ ret = v4l2_subdev_call(subdev, pad, enum_frame_interval, NULL, &fie);
+ if (ret < 0)
+ return ret;
+
+ fival->type = V4L2_FRMIVAL_TYPE_DISCRETE;
+ fival->discrete = fie.interval;
+
+ return 0;
+}
+
+static int video_g_fmt(struct file *file, void *fh, struct v4l2_format *f)
+{
+ struct stfcamss_video *video = video_drvdata(file);
+
+ st_debug(ST_VIDEO, "%s, fmt.type = 0x%x\n", __func__, f->type);
+ st_debug(ST_VIDEO, "%s, active_fmt.type = 0x%x,0x%x\n",
+ __func__, video->active_fmt.type,
+ video->active_fmt.fmt.pix.pixelformat);
+ *f = video->active_fmt;
+ return 0;
+}
+
+static int video_entity_s_fmt(struct stfcamss_video *video,
+ struct media_entity *entity,
+ struct v4l2_subdev_state *state,
+ struct v4l2_subdev_format *fmt,
+ u32 dst_code)
+{
+ struct v4l2_subdev *subdev;
+ struct media_pad *pad;
+ struct v4l2_mbus_framefmt *mf = &fmt->format;
+ u32 width, height, code;
+ int ret, index = 0;
+
+ code = mf->code;
+ width = mf->width;
+ height = mf->height;
+ subdev = media_entity_to_v4l2_subdev(entity);
+ while (1) {
+ if (index >= entity->num_pads)
+ break;
+ pad = &entity->pads[index];
+ pad = media_pad_remote_pad_first(pad);
+ if (pad && is_media_entity_v4l2_subdev(pad->entity)) {
+ fmt->pad = index;
+ if (index)
+ mf->code = dst_code;
+ ret = v4l2_subdev_call(subdev, pad,
+ set_fmt, state, fmt);
+ st_warn(ST_VIDEO,
+ "\"%s\":%d pad fmt set to 0x%x %ux%u\n",
+ subdev->name, fmt->pad, mf->code,
+ mf->width, mf->height);
+ if (mf->code != code ||
+ mf->width != width ||
+ mf->height != height) {
+ st_warn(ST_VIDEO,
+ "\"%s\":%d,changed fmt to 0x%x %ux%u\n",
+ subdev->name, fmt->pad, mf->code,
+ mf->width, mf->height);
+ }
+ if (index)
+ ret = video_entity_s_fmt(video, pad->entity,
+ state, fmt, dst_code);
+ }
+
+ if (ret < 0 && ret != -ENOIOCTLCMD)
+ break;
+ index++;
+ }
+ return ret;
+}
+
+static int video_getcrop_pad_id(int video_id)
+{
+ return stf_vin_map_isp_pad(video_id, STF_ISP_PAD_SRC);
+}
+
+static int video_pipeline_s_fmt(struct stfcamss_video *video,
+ struct v4l2_subdev_state *state,
+ struct v4l2_format *f)
+{
+ struct video_device *vdev = &video->vdev;
+ struct media_entity *entity = &vdev->entity;
+ struct v4l2_subdev *subdev;
+ int ret, index;
+ struct v4l2_subdev_format fmt = {
+ .which = V4L2_SUBDEV_FORMAT_ACTIVE,
+ .reserved = {video_getcrop_pad_id(video->id)}
+ };
+ struct v4l2_mbus_framefmt *mf = &fmt.format;
+ struct v4l2_pix_format *pix = &f->fmt.pix;
+ struct media_entity *sensor;
+ u32 width, height, code;
+ struct media_pad *pad;
+
+ /* pix to mbus format */
+ index = video_find_format(mf->code,
+ pix->pixelformat,
+ video->formats, video->nformats);
+ if (index < 0)
+ return index;
+ v4l2_fill_mbus_format(mf, pix, video->formats[index].code);
+
+ code = mf->code;
+ width = mf->width;
+ height = mf->height;
+ sensor = stfcamss_find_sensor(entity);
+ if (sensor) {
+ subdev = media_entity_to_v4l2_subdev(sensor);
+ ret = v4l2_subdev_call(subdev, pad, set_fmt, state, &fmt);
+ st_warn(ST_VIDEO,
+ "\"%s\":%d pad fmt set to 0x%x %ux%u\n",
+ subdev->name, fmt.pad, mf->code,
+ mf->width, mf->height);
+ if (mf->code != code ||
+ mf->width != width ||
+ mf->height != height) {
+ st_warn(ST_VIDEO,
+ "\"%s\":%d, changed fmt to 0x%x %ux%u\n",
+ subdev->name, fmt.pad, mf->code,
+ mf->width, mf->height);
+ }
+ } else {
+ st_err(ST_VIDEO, "Can't find sensor\n");
+ return -ENOTTY;
+ }
+ /*
+ * Starting from sensor subdevice, walk within
+ * pipeline and set format on each subdevice
+ */
+ sensor = stfcamss_find_sensor(entity);
+ pad = media_pad_remote_pad_first(&sensor->pads[0]);
+ ret = video_entity_s_fmt(video, pad->entity, state, &fmt, code);
+
+ if (ret < 0 && ret != -ENOIOCTLCMD)
+ return ret;
+
+ index = video_find_format(mf->code,
+ video->formats[index].pixelformat,
+ video->formats, video->nformats);
+ st_debug(ST_VIDEO, "%s, code=%x, index=%d\n",
+ __func__, mf->code, index);
+
+ if (index < 0)
+ return index;
+
+ video_mbus_to_pix(mf, pix, &video->formats[index],
+ video->bpl_alignment);
+
+ ret = __video_try_fmt(video, f);
+ if (ret < 0)
+ return ret;
+
+ return 0;
+}
+
+static int video_s_fmt(struct file *file, void *fh, struct v4l2_format *f)
+{
+ struct stfcamss_video *video = video_drvdata(file);
+ int ret;
+
+ st_debug(ST_VIDEO, "%s, fmt.type = 0x%x, v4l2fmt=%x\n",
+ __func__, f->type, f->fmt.pix.pixelformat);
+
+ if (vb2_is_busy(&video->vb2_q))
+ return -EBUSY;
+
+ ret = __video_try_fmt(video, f);
+ if (ret < 0)
+ return ret;
+
+ ret = video_pipeline_s_fmt(video, NULL, f);
+
+ st_debug(ST_VIDEO, "%s, pixelformat=0x%x, ret=%d\n",
+ __func__, f->fmt.pix.pixelformat, ret);
+ if (ret < 0)
+ return ret;
+
+ video->active_fmt = *f;
+
+ return 0;
+}
+
+static int video_try_fmt(struct file *file, void *fh, struct v4l2_format *f)
+{
+ struct stfcamss_video *video = video_drvdata(file);
+
+ return __video_try_fmt(video, f);
+}
+
+static int video_enum_input(struct file *file, void *fh,
+ struct v4l2_input *input)
+{
+ if (input->index > 0)
+ return -EINVAL;
+
+ strscpy(input->name, "camera", sizeof(input->name));
+ input->type = V4L2_INPUT_TYPE_CAMERA;
+
+ return 0;
+}
+
+static int video_g_input(struct file *file, void *fh, unsigned int *input)
+{
+ *input = 0;
+
+ return 0;
+}
+
+static int video_s_input(struct file *file, void *fh, unsigned int input)
+{
+ return input == 0 ? 0 : -EINVAL;
+}
+
+static int video_g_parm(struct file *file, void *priv,
+ struct v4l2_streamparm *p)
+{
+ struct stfcamss_video *video = video_drvdata(file);
+ struct video_device *vdev = &video->vdev;
+ struct media_entity *entity;
+ struct v4l2_subdev *subdev;
+ struct media_pad *pad;
+ int ret, is_support = 0;
+
+ entity = &vdev->entity;
+ while (1) {
+ pad = &entity->pads[0];
+ if (!(pad->flags & MEDIA_PAD_FL_SINK))
+ break;
+
+ pad = media_pad_remote_pad_first(pad);
+ if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
+ break;
+
+ entity = pad->entity;
+ subdev = media_entity_to_v4l2_subdev(entity);
+
+ ret = v4l2_g_parm_cap(vdev, subdev, p);
+ if (ret < 0 && ret != -ENOIOCTLCMD)
+ break;
+ if (!ret)
+ is_support = 1;
+ }
+
+ return is_support ? 0 : ret;
+}
+
+static int video_s_parm(struct file *file, void *priv,
+ struct v4l2_streamparm *p)
+{
+ struct stfcamss_video *video = video_drvdata(file);
+ struct video_device *vdev = &video->vdev;
+ struct media_entity *entity;
+ struct v4l2_subdev *subdev;
+ struct media_pad *pad;
+ struct v4l2_streamparm tmp_p;
+ int ret, is_support = 0;
+
+ entity = &vdev->entity;
+ while (1) {
+ pad = &entity->pads[0];
+ if (!(pad->flags & MEDIA_PAD_FL_SINK))
+ break;
+
+ pad = media_pad_remote_pad_first(pad);
+ if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
+ break;
+
+ entity = pad->entity;
+ subdev = media_entity_to_v4l2_subdev(entity);
+
+ tmp_p = *p;
+ ret = v4l2_s_parm_cap(vdev, subdev, &tmp_p);
+ if (ret < 0 && ret != -ENOIOCTLCMD)
+ break;
+ if (!ret) {
+ is_support = 1;
+ *p = tmp_p;
+ }
+ }
+
+ return is_support ? 0 : ret;
+}
+
+static int video_g_selection(struct file *file, void *fh,
+ struct v4l2_selection *s)
+{
+ struct stfcamss_video *video = video_drvdata(file);
+ struct video_device *vdev = &video->vdev;
+ struct media_entity *entity;
+ struct v4l2_subdev *subdev;
+ struct media_pad *pad;
+ struct v4l2_subdev_selection sel = {
+ .which = V4L2_SUBDEV_FORMAT_ACTIVE,
+ .pad = video_getcrop_pad_id(video->id),
+ .target = s->target,
+ .r = s->r,
+ .flags = s->flags,
+ };
+ int ret;
+
+ st_debug(ST_VIDEO, "%s, target = 0x%x, 0x%x\n",
+ __func__, sel.target, s->target);
+ if (s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE &&
+ s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
+ return -EINVAL;
+
+ entity = &vdev->entity;
+ while (1) {
+ pad = &entity->pads[0];
+ if (!(pad->flags & MEDIA_PAD_FL_SINK))
+ break;
+
+ pad = media_pad_remote_pad_first(pad);
+ if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
+ break;
+
+ entity = pad->entity;
+ subdev = media_entity_to_v4l2_subdev(entity);
+
+ ret = v4l2_subdev_call(subdev, pad, get_selection, NULL, &sel);
+ if (!ret) {
+ s->r = sel.r;
+ s->flags = sel.flags;
+ break;
+ }
+ if (ret != -ENOIOCTLCMD)
+ break;
+ }
+
+ return ret;
+}
+
+static int video_s_selection(struct file *file, void *fh,
+ struct v4l2_selection *s)
+{
+ struct stfcamss_video *video = video_drvdata(file);
+ struct video_device *vdev = &video->vdev;
+ struct media_entity *entity;
+ struct v4l2_subdev *subdev;
+ struct media_pad *pad;
+ struct v4l2_subdev_selection sel = {
+ .which = V4L2_SUBDEV_FORMAT_ACTIVE,
+ .pad = video_getcrop_pad_id(video->id),
+ .target = s->target,
+ .r = s->r,
+ .flags = s->flags,
+ };
+ struct v4l2_pix_format *format = &video->active_fmt.fmt.pix;
+ int ret;
+
+ st_debug(ST_VIDEO, "%s, target = 0x%x, 0x%x\n",
+ __func__, sel.target, s->target);
+ if (s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE &&
+ s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
+ return -EINVAL;
+
+ entity = &vdev->entity;
+ while (1) {
+ pad = &entity->pads[0];
+ if (!(pad->flags & MEDIA_PAD_FL_SINK))
+ break;
+
+ pad = media_pad_remote_pad_first(pad);
+ if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
+ break;
+
+ entity = pad->entity;
+ subdev = media_entity_to_v4l2_subdev(entity);
+
+ ret = v4l2_subdev_call(subdev, pad, set_selection, NULL, &sel);
+ if (!ret) {
+ s->r = sel.r;
+ s->flags = sel.flags;
+ format->width = s->r.width;
+ format->height = s->r.height;
+ ret = __video_try_fmt(video, &video->active_fmt);
+ if (ret < 0)
+ return ret;
+ break;
+ }
+ if (ret != -ENOIOCTLCMD)
+ break;
+ }
+
+ st_debug(ST_VIDEO, "ret = 0x%x, -EINVAL = 0x%x\n", ret, -EINVAL);
+
+ return ret;
+}
+
+static int video_subscribe_event(struct v4l2_fh *fh,
+ const struct v4l2_event_subscription *sub)
+{
+ struct v4l2_subdev *subdev = NULL;
+ struct media_entity *entity = &fh->vdev->entity;
+ struct media_entity *sensor;
+
+ sensor = stfcamss_find_sensor(entity);
+ if (sensor)
+ subdev = media_entity_to_v4l2_subdev(sensor);
+
+ if (!subdev)
+ return -ENOTTY;
+
+ fh->ctrl_handler = subdev->ctrl_handler;
+ return v4l2_ctrl_subscribe_event(fh, sub);
+}
+
+static int video_unsubscribe_event(struct v4l2_fh *fh,
+ const struct v4l2_event_subscription *sub)
+{
+ struct v4l2_subdev *subdev = NULL;
+ struct media_entity *entity = &fh->vdev->entity;
+ struct media_entity *sensor;
+
+ sensor = stfcamss_find_sensor(entity);
+ if (sensor)
+ subdev = media_entity_to_v4l2_subdev(sensor);
+
+ if (!subdev)
+ return -ENOTTY;
+
+ fh->ctrl_handler = subdev->ctrl_handler;
+ return v4l2_event_unsubscribe(fh, sub);
+}
+
+static const struct v4l2_ioctl_ops stf_vid_vin_ioctl_ops = {
+ .vidioc_querycap = video_querycap,
+ .vidioc_enum_fmt_vid_cap = video_enum_fmt,
+ .vidioc_enum_fmt_vid_out = video_enum_fmt,
+ .vidioc_enum_framesizes = video_enum_framesizes,
+ .vidioc_enum_frameintervals = video_enum_frameintervals,
+ .vidioc_g_fmt_vid_cap = video_g_fmt,
+ .vidioc_s_fmt_vid_cap = video_s_fmt,
+ .vidioc_try_fmt_vid_cap = video_try_fmt,
+ .vidioc_g_fmt_vid_out = video_g_fmt,
+ .vidioc_s_fmt_vid_out = video_s_fmt,
+ .vidioc_try_fmt_vid_out = video_try_fmt,
+ .vidioc_reqbufs = vb2_ioctl_reqbufs,
+ .vidioc_querybuf = vb2_ioctl_querybuf,
+ .vidioc_qbuf = vb2_ioctl_qbuf,
+ .vidioc_expbuf = vb2_ioctl_expbuf,
+ .vidioc_dqbuf = vb2_ioctl_dqbuf,
+ .vidioc_create_bufs = vb2_ioctl_create_bufs,
+ .vidioc_prepare_buf = vb2_ioctl_prepare_buf,
+ .vidioc_streamon = vb2_ioctl_streamon,
+ .vidioc_streamoff = vb2_ioctl_streamoff,
+ .vidioc_enum_input = video_enum_input,
+ .vidioc_g_input = video_g_input,
+ .vidioc_s_input = video_s_input,
+ .vidioc_g_parm = video_g_parm,
+ .vidioc_s_parm = video_s_parm,
+ .vidioc_subscribe_event = video_subscribe_event,
+ .vidioc_unsubscribe_event = video_unsubscribe_event,
+};
+
+static const struct v4l2_ioctl_ops stf_vid_isp_ioctl_ops = {
+ .vidioc_querycap = video_querycap,
+ .vidioc_enum_fmt_vid_cap = video_enum_fmt,
+ .vidioc_enum_fmt_vid_out = video_enum_fmt,
+ .vidioc_enum_framesizes = video_enum_framesizes,
+ .vidioc_enum_frameintervals = video_enum_frameintervals,
+ .vidioc_g_fmt_vid_cap = video_g_fmt,
+ .vidioc_s_fmt_vid_cap = video_s_fmt,
+ .vidioc_try_fmt_vid_cap = video_try_fmt,
+ .vidioc_g_fmt_vid_out = video_g_fmt,
+ .vidioc_s_fmt_vid_out = video_s_fmt,
+ .vidioc_try_fmt_vid_out = video_try_fmt,
+ .vidioc_reqbufs = vb2_ioctl_reqbufs,
+ .vidioc_querybuf = vb2_ioctl_querybuf,
+ .vidioc_qbuf = vb2_ioctl_qbuf,
+ .vidioc_expbuf = vb2_ioctl_expbuf,
+ .vidioc_dqbuf = vb2_ioctl_dqbuf,
+ .vidioc_create_bufs = vb2_ioctl_create_bufs,
+ .vidioc_prepare_buf = vb2_ioctl_prepare_buf,
+ .vidioc_streamon = vb2_ioctl_streamon,
+ .vidioc_streamoff = vb2_ioctl_streamoff,
+ .vidioc_enum_input = video_enum_input,
+ .vidioc_g_input = video_g_input,
+ .vidioc_s_input = video_s_input,
+ .vidioc_g_parm = video_g_parm,
+ .vidioc_s_parm = video_s_parm,
+ .vidioc_s_selection = video_s_selection,
+ .vidioc_g_selection = video_g_selection,
+ .vidioc_subscribe_event = video_subscribe_event,
+ .vidioc_unsubscribe_event = video_unsubscribe_event,
+};
+
+/* -----------------------------------------------------------------------------
+ * V4L2 file operations
+ */
+
+static int video_open(struct file *file)
+{
+ struct video_device *vdev = video_devdata(file);
+ struct stfcamss_video *video = video_drvdata(file);
+ struct v4l2_fh *vfh;
+ int ret;
+
+ mutex_lock(&video->lock);
+
+ vfh = kzalloc(sizeof(*vfh), GFP_KERNEL);
+ if (!vfh) {
+ ret = -ENOMEM;
+ goto error_alloc;
+ }
+
+ v4l2_fh_init(vfh, vdev);
+ v4l2_fh_add(vfh);
+
+ file->private_data = vfh;
+
+ ret = v4l2_pipeline_pm_get(&vdev->entity);
+ if (ret < 0) {
+ st_err(ST_VIDEO,
+ "Failed to power up pipeline: %d\n", ret);
+ goto error_pm_use;
+ }
+ mutex_unlock(&video->lock);
+
+ return 0;
+
+error_pm_use:
+ v4l2_fh_release(file);
+error_alloc:
+ mutex_unlock(&video->lock);
+ return ret;
+}
+
+static int video_release(struct file *file)
+{
+ struct video_device *vdev = video_devdata(file);
+
+ vb2_fop_release(file);
+ v4l2_pipeline_pm_put(&vdev->entity);
+ file->private_data = NULL;
+
+ return 0;
+}
+
+static const struct v4l2_file_operations stf_vid_fops = {
+ .owner = THIS_MODULE,
+ .unlocked_ioctl = video_ioctl2,
+ .open = video_open,
+ .release = video_release,
+ .poll = vb2_fop_poll,
+ .mmap = vb2_fop_mmap,
+ .read = vb2_fop_read,
+};
+
+/* -----------------------------------------------------------------------------
+ * STFCAMSS video core
+ */
+
+static void stf_video_release(struct video_device *vdev)
+{
+ struct stfcamss_video *video = video_get_drvdata(vdev);
+
+ media_entity_cleanup(&vdev->entity);
+
+ mutex_destroy(&video->q_lock);
+ mutex_destroy(&video->lock);
+}
+
+int stf_video_register(struct stfcamss_video *video,
+ struct v4l2_device *v4l2_dev, const char *name)
+{
+ struct video_device *vdev;
+ struct vb2_queue *q;
+ struct media_pad *pad = &video->pad;
+ int ret;
+ enum isp_pad_id isp_pad;
+
+ vdev = &video->vdev;
+
+ mutex_init(&video->q_lock);
+
+ q = &video->vb2_q;
+ q->drv_priv = video;
+ q->mem_ops = &vb2_dma_contig_memops;
+ q->ops = &stf_video_vb2_q_ops;
+ q->type = video->type;
+ q->io_modes = VB2_DMABUF | VB2_MMAP | VB2_READ;
+ q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+ q->buf_struct_size = sizeof(struct stfcamss_buffer);
+ q->dev = video->stfcamss->dev;
+ q->lock = &video->q_lock;
+ q->min_buffers_needed = STFCAMSS_MIN_BUFFERS;
+ ret = vb2_queue_init(q);
+ if (ret < 0) {
+ st_err(ST_VIDEO, "Failed to init vb2 queue: %d\n", ret);
+ goto err_vb2_init;
+ }
+
+ pad->flags = MEDIA_PAD_FL_SINK;
+ ret = media_entity_pads_init(&vdev->entity, 1, pad);
+ if (ret < 0) {
+ st_err(ST_VIDEO, "Failed to init video entity: %d\n", ret);
+ goto err_vb2_init;
+ }
+
+ mutex_init(&video->lock);
+
+ isp_pad = stf_vin_map_isp_pad(video->id, STF_ISP_PAD_SRC);
+ if (video->id == VIN_LINE_WR) {
+ video->formats = formats_pix_wr;
+ video->nformats = ARRAY_SIZE(formats_pix_wr);
+ video->bpl_alignment = STFCAMSS_FRAME_WIDTH_ALIGN_8;
+ vdev->ioctl_ops = &stf_vid_vin_ioctl_ops;
+ } else if (isp_pad == STF_ISP_PAD_SRC ||
+ isp_pad == STF_ISP_PAD_SRC_SS0 ||
+ isp_pad == STF_ISP_PAD_SRC_SS1) {
+ video->formats = formats_pix_isp;
+ video->nformats = ARRAY_SIZE(formats_pix_isp);
+ video->bpl_alignment = STFCAMSS_FRAME_WIDTH_ALIGN_8;
+ vdev->ioctl_ops = &stf_vid_isp_ioctl_ops;
+ } else {
+ video->formats = formats_raw_isp;
+ video->nformats = ARRAY_SIZE(formats_raw_isp);
+ video->bpl_alignment = STFCAMSS_FRAME_WIDTH_ALIGN_128;
+ vdev->ioctl_ops = &stf_vid_isp_ioctl_ops;
+ }
+
+ ret = stf_video_init_format(video);
+ if (ret < 0) {
+ st_err(ST_VIDEO, "Failed to init format: %d\n", ret);
+ goto err_vid_init_format;
+ }
+
+ vdev->fops = &stf_vid_fops;
+ vdev->device_caps = V4L2_CAP_VIDEO_CAPTURE;
+ vdev->vfl_dir = VFL_DIR_RX;
+ vdev->device_caps |= V4L2_CAP_STREAMING | V4L2_CAP_READWRITE;
+ vdev->release = stf_video_release;
+ vdev->v4l2_dev = v4l2_dev;
+ vdev->queue = &video->vb2_q;
+ vdev->lock = &video->lock;
+ strscpy(vdev->name, name, sizeof(vdev->name));
+
+ ret = video_register_device(vdev, VFL_TYPE_VIDEO, video->id);
+ if (ret < 0) {
+ st_err(ST_VIDEO,
+ "Failed to register video device: %d\n",
+ ret);
+ goto err_vid_reg;
+ }
+
+ video_set_drvdata(vdev, video);
+ return 0;
+
+err_vid_reg:
+err_vid_init_format:
+ media_entity_cleanup(&vdev->entity);
+ mutex_destroy(&video->lock);
+err_vb2_init:
+ mutex_destroy(&video->q_lock);
+ return ret;
+}
+
+void stf_video_unregister(struct stfcamss_video *video)
+{
+ vb2_video_unregister_device(&video->vdev);
+}
--- /dev/null
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * stf_video.h
+ *
+ * StarFive Camera Subsystem - V4L2 device node
+ *
+ * Copyright (C) 2021-2023 StarFive Technology Co., Ltd.
+ */
+
+#ifndef STF_VIDEO_H
+#define STF_VIDEO_H
+
+#include <linux/mutex.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-dev.h>
+#include <media/v4l2-fh.h>
+#include <media/v4l2-ioctl.h>
+#include <media/videobuf2-v4l2.h>
+
+#define STFCAMSS_FRAME_MIN_WIDTH 64
+#define STFCAMSS_FRAME_MAX_WIDTH 1920
+#define STFCAMSS_FRAME_MIN_HEIGHT 64
+#define STFCAMSS_FRAME_MAX_HEIGHT 1080
+#define STFCAMSS_FRAME_WIDTH_ALIGN_8 8
+#define STFCAMSS_FRAME_WIDTH_ALIGN_128 128
+#define STFCAMSS_MIN_BUFFERS 2
+
+#define STFCAMSS_MAX_ENTITY_NAME_LEN 27
+
+struct stfcamss_buffer {
+ struct vb2_v4l2_buffer vb;
+ dma_addr_t addr[3];
+ struct list_head queue;
+ int sizeimage;
+};
+
+struct fract {
+ u8 numerator;
+ u8 denominator;
+};
+
+/*
+ * struct stfcamss_format_info - ISP media bus format information
+ * @code: V4L2 media bus format code
+ * @pixelformat: V4L2 pixel format FCC identifier
+ * @planes: Number of planes
+ * @hsub: Horizontal subsampling (for each plane)
+ * @vsub: Vertical subsampling (for each plane)
+ * @bpp: Bits per pixel when stored in memory (for each plane)
+ */
+struct stfcamss_format_info {
+ u32 code;
+ u32 pixelformat;
+ u8 planes;
+ struct fract hsub[3];
+ struct fract vsub[3];
+ u8 bpp[3];
+};
+
+struct stfcamss_video {
+ struct stfcamss *stfcamss;
+ u8 id;
+ struct vb2_queue vb2_q;
+ struct video_device vdev;
+ struct media_pad pad;
+ struct media_pipeline pipe;
+ struct v4l2_format active_fmt;
+ enum v4l2_buf_type type;
+ const struct stfcamss_video_ops *ops;
+ struct mutex lock; /* serialize device access */
+ struct mutex q_lock; /* protects the queue */
+ unsigned int bpl_alignment;
+ const struct stfcamss_format_info *formats;
+ unsigned int nformats;
+};
+
+struct stfcamss_video_ops {
+ int (*queue_buffer)(struct stfcamss_video *vid,
+ struct stfcamss_buffer *buf);
+ int (*flush_buffers)(struct stfcamss_video *vid,
+ enum vb2_buffer_state state);
+};
+
+int stf_video_register(struct stfcamss_video *video,
+ struct v4l2_device *v4l2_dev, const char *name);
+
+void stf_video_unregister(struct stfcamss_video *video);
+
+#endif /* STF_VIDEO_H */
--- /dev/null
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * stf_vin.c
+ *
+ * StarFive Camera Subsystem - VIN Module
+ *
+ * Copyright (C) 2021-2023 StarFive Technology Co., Ltd.
+ */
+#include <linux/pm_runtime.h>
+
+#include "stf_camss.h"
+
+#define vin_line_array(ptr_line) \
+ ((const struct vin_line (*)[]) &(ptr_line)[-((ptr_line)->id)])
+
+#define line_to_vin_dev(ptr_line) \
+ container_of(vin_line_array(ptr_line), struct stf_vin_dev, line)
+
+#define VIN_FRAME_DROP_MAX_VAL 90
+#define VIN_FRAME_DROP_MIN_VAL 4
+#define VIN_FRAME_PER_SEC_MAX_VAL 90
+
+/* ISP ctrl need 1 sec to let frames become stable. */
+#define VIN_FRAME_DROP_SEC_FOR_ISP_CTRL 1
+
+static const struct vin_format vin_formats_wr[] = {
+ { MEDIA_BUS_FMT_SRGGB10_1X10, 10},
+ { MEDIA_BUS_FMT_SGRBG10_1X10, 10},
+ { MEDIA_BUS_FMT_SGBRG10_1X10, 10},
+ { MEDIA_BUS_FMT_SBGGR10_1X10, 10},
+};
+
+static const struct vin_format vin_formats_raw[] = {
+ { MEDIA_BUS_FMT_SBGGR12_1X12, 12},
+ { MEDIA_BUS_FMT_SRGGB12_1X12, 12},
+ { MEDIA_BUS_FMT_SGRBG12_1X12, 12},
+ { MEDIA_BUS_FMT_SGBRG12_1X12, 12},
+};
+
+static const struct vin_format vin_formats_uo[] = {
+ { MEDIA_BUS_FMT_Y12_1X12, 8},
+};
+
+static const struct vin_format_table vin_formats_table[] = {
+ /* VIN_LINE_WR */
+ { vin_formats_wr, ARRAY_SIZE(vin_formats_wr) },
+ /* VIN_LINE_ISP */
+ { vin_formats_uo, ARRAY_SIZE(vin_formats_uo) },
+ /* VIN_LINE_ISP_SS0 */
+ { vin_formats_uo, ARRAY_SIZE(vin_formats_uo) },
+ /* VIN_LINE_ISP_SS1 */
+ { vin_formats_uo, ARRAY_SIZE(vin_formats_uo) },
+ /* VIN_LINE_ISP_RAW */
+ { vin_formats_raw, ARRAY_SIZE(vin_formats_raw) },
+};
+
+static void vin_buffer_done(struct vin_line *line);
+static void vin_change_buffer(struct vin_line *line);
+static struct stfcamss_buffer *vin_buf_get_pending(struct vin_output *output);
+static void vin_output_init_addrs(struct vin_line *line);
+static void vin_init_outputs(struct vin_line *line);
+static struct v4l2_mbus_framefmt *
+__vin_get_format(struct vin_line *line,
+ struct v4l2_subdev_state *state,
+ unsigned int pad,
+ enum v4l2_subdev_format_whence which);
+
+static char *vin_get_line_subdevname(int line_id)
+{
+ char *name = NULL;
+
+ switch (line_id) {
+ case VIN_LINE_WR:
+ name = "wr";
+ break;
+ case VIN_LINE_ISP:
+ name = "isp0";
+ break;
+ case VIN_LINE_ISP_SS0:
+ name = "isp0_ss0";
+ break;
+ case VIN_LINE_ISP_SS1:
+ name = "isp0_ss1";
+ break;
+ case VIN_LINE_ISP_RAW:
+ name = "isp0_raw";
+ break;
+ default:
+ name = "unknown";
+ break;
+ }
+ return name;
+}
+
+static enum isp_line_id vin_map_isp_line(enum vin_line_id line)
+{
+ enum isp_line_id line_id;
+
+ if (line > VIN_LINE_WR && line < VIN_LINE_MAX) {
+ line_id = line % STF_ISP_LINE_MAX;
+ if (line_id == 0)
+ line_id = STF_ISP_LINE_SRC_RAW;
+ } else {
+ line_id = STF_ISP_LINE_INVALID;
+ }
+
+ return line_id;
+}
+
+enum isp_pad_id stf_vin_map_isp_pad(enum vin_line_id line, enum isp_pad_id def)
+{
+ enum isp_pad_id pad_id;
+
+ if (line == VIN_LINE_WR)
+ pad_id = STF_ISP_PAD_SINK;
+ else if ((line > VIN_LINE_WR) && (line < VIN_LINE_MAX))
+ pad_id = vin_map_isp_line(line);
+ else
+ pad_id = def;
+
+ return pad_id;
+}
+
+int stf_vin_subdev_init(struct stfcamss *stfcamss)
+{
+ struct device *dev = stfcamss->dev;
+ struct stf_vin_dev *vin_dev = stfcamss->vin_dev;
+ int i, ret = 0;
+
+ vin_dev->stfcamss = stfcamss;
+ vin_dev->hw_ops = &vin_ops;
+
+ vin_dev->isr_ops = devm_kzalloc(dev, sizeof(*vin_dev->isr_ops),
+ GFP_KERNEL);
+ if (!vin_dev->isr_ops)
+ return -ENOMEM;
+ vin_dev->isr_ops->isr_buffer_done = vin_buffer_done;
+ vin_dev->isr_ops->isr_change_buffer = vin_change_buffer;
+
+ atomic_set(&vin_dev->ref_count, 0);
+
+ ret = devm_request_irq(dev,
+ stfcamss->irq,
+ vin_dev->hw_ops->vin_wr_irq_handler,
+ 0, "vin_axiwr_irq", vin_dev);
+ if (ret) {
+ st_err(ST_VIN, "failed to request irq\n");
+ goto out;
+ }
+
+ ret = devm_request_irq(dev,
+ stfcamss->isp_irq,
+ vin_dev->hw_ops->vin_isp_irq_handler,
+ 0, "vin_isp_irq", vin_dev);
+ if (ret) {
+ st_err(ST_VIN, "failed to request isp irq\n");
+ goto out;
+ }
+
+ ret = devm_request_irq(dev,
+ stfcamss->isp_irq_csi,
+ vin_dev->hw_ops->vin_isp_irq_csi_handler,
+ 0, "vin_isp_irq_csi", vin_dev);
+ if (ret) {
+ st_err(ST_VIN, "failed to request isp raw irq\n");
+ goto out;
+ }
+
+ ret = devm_request_irq(dev,
+ stfcamss->isp_irq_csiline,
+ vin_dev->hw_ops->vin_isp_irq_csiline_handler,
+ 0, "vin_isp_irq_csiline", vin_dev);
+ if (ret) {
+ st_err(ST_VIN, "failed to request isp irq csiline\n");
+ goto out;
+ }
+
+ mutex_init(&vin_dev->power_lock);
+ vin_dev->power_count = 0;
+
+ for (i = 0; i < STF_DUMMY_MODULE_NUMS; i++) {
+ struct dummy_buffer *dummy_buffer = &vin_dev->dummy_buffer[i];
+
+ mutex_init(&dummy_buffer->stream_lock);
+ dummy_buffer->nums =
+ i == 0 ? VIN_DUMMY_BUFFER_NUMS : ISP_DUMMY_BUFFER_NUMS;
+ dummy_buffer->stream_count = 0;
+ dummy_buffer->buffer =
+ devm_kzalloc(dev,
+ dummy_buffer->nums * sizeof(struct vin_dummy_buffer),
+ GFP_KERNEL);
+ atomic_set(&dummy_buffer->frame_skip, 0);
+ }
+
+ for (i = VIN_LINE_WR;
+ i < STF_ISP_LINE_MAX + 1; i++) {
+ struct vin_line *l = &vin_dev->line[i];
+
+ l->video_out.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ l->video_out.stfcamss = stfcamss;
+ l->id = i;
+ l->sdev_type = SUBDEV_TYPE_VIN;
+ l->formats = vin_formats_table[i].fmts;
+ l->nformats = vin_formats_table[i].nfmts;
+ spin_lock_init(&l->output_lock);
+
+ mutex_init(&l->stream_lock);
+ l->stream_count = 0;
+ mutex_init(&l->power_lock);
+ l->power_count = 0;
+ }
+
+ return 0;
+out:
+ return ret;
+}
+
+static enum link vin_get_link(struct media_entity *entity)
+{
+ struct v4l2_subdev *subdev;
+ struct media_pad *pad;
+ bool isp = false;
+
+ while (1) {
+ pad = &entity->pads[0];
+ if (!(pad->flags & MEDIA_PAD_FL_SINK))
+ return LINK_ERROR;
+
+ pad = media_pad_remote_pad_first(pad);
+ if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
+ return LINK_ERROR;
+
+ entity = pad->entity;
+ subdev = media_entity_to_v4l2_subdev(entity);
+
+ if (!strncmp(subdev->name, STF_CSI_NAME,
+ strlen(STF_CSI_NAME))) {
+ if (isp)
+ return LINK_CSI_TO_ISP;
+ else
+ return LINK_CSI_TO_WR;
+ } else if (!strncmp(subdev->name, STF_DVP_NAME,
+ strlen(STF_DVP_NAME))) {
+ if (isp)
+ return LINK_DVP_TO_ISP;
+ else
+ return LINK_DVP_TO_WR;
+ } else if (!strncmp(subdev->name, STF_ISP_NAME,
+ strlen(STF_ISP_NAME))) {
+ isp = true;
+ } else {
+ return LINK_ERROR;
+ }
+ }
+}
+
+static int vin_set_power(struct v4l2_subdev *sd, int on)
+{
+ struct vin_line *line = v4l2_get_subdevdata(sd);
+ struct stf_vin_dev *vin_dev = line_to_vin_dev(line);
+ struct stfcamss *stfcamss = vin_dev->stfcamss;
+ enum link link;
+
+ mutex_lock(&line->power_lock);
+ if (on) {
+ if (line->power_count == 0)
+ vin_init_outputs(line);
+ line->power_count++;
+ } else {
+ if (line->power_count == 0) {
+ st_err(ST_VIN, "line power off on power_count = 0\n");
+ goto exit_line;
+ }
+ line->power_count--;
+ }
+exit_line:
+ mutex_unlock(&line->power_lock);
+
+ mutex_lock(&vin_dev->power_lock);
+ link = vin_get_link(&sd->entity);
+ if (link == LINK_ERROR)
+ goto exit;
+
+ if (on) {
+ if (vin_dev->power_count == 0) {
+ pm_runtime_get_sync(stfcamss->dev);
+ vin_dev->hw_ops->vin_clk_enable(vin_dev, link);
+ }
+ vin_dev->power_count++;
+ } else {
+ if (vin_dev->power_count == 0) {
+ st_err(ST_VIN, "vin_dev power off on power_count=0\n");
+ goto exit;
+ }
+ if (vin_dev->power_count == 1) {
+ vin_dev->hw_ops->vin_clk_disable(vin_dev, link);
+ pm_runtime_put_sync(stfcamss->dev);
+ }
+ vin_dev->power_count--;
+ }
+exit:
+
+ mutex_unlock(&vin_dev->power_lock);
+
+ return 0;
+}
+
+static unsigned int vin_get_frame_skip(struct vin_line *line)
+{
+ unsigned int frame_skip = 0;
+ unsigned int isp_ctrl_skip_frames = 0;
+ struct media_entity *sensor;
+ struct v4l2_subdev_frame_interval fi = {
+ .pad = 0,
+ };
+
+ sensor = stfcamss_find_sensor(&line->subdev.entity);
+ if (sensor) {
+ int fps = 0;
+ struct v4l2_subdev *subdev =
+ media_entity_to_v4l2_subdev(sensor);
+
+ v4l2_subdev_call(subdev, video, g_frame_interval, &fi);
+ fps = fi.interval.denominator;
+
+ if (fps > 0 && fps <= 90)
+ isp_ctrl_skip_frames =
+ fps * VIN_FRAME_DROP_SEC_FOR_ISP_CTRL;
+
+ if (!fps)
+ st_debug(ST_VIN,
+ "%s, Failed to get sensor fps !\n", __func__);
+
+ if (isp_ctrl_skip_frames <= VIN_FRAME_DROP_MIN_VAL)
+ isp_ctrl_skip_frames = VIN_FRAME_DROP_MIN_VAL;
+
+ v4l2_subdev_call(subdev, sensor, g_skip_frames, &frame_skip);
+
+ frame_skip += isp_ctrl_skip_frames;
+
+ if (frame_skip > VIN_FRAME_DROP_MAX_VAL)
+ frame_skip = VIN_FRAME_DROP_MAX_VAL;
+ st_debug(ST_VIN, "%s, frame_skip %d\n", __func__, frame_skip);
+ }
+
+ return frame_skip;
+}
+
+static int vin_enable_output(struct vin_line *line)
+{
+ struct vin_output *output = &line->output;
+ unsigned long flags;
+
+ spin_lock_irqsave(&line->output_lock, flags);
+
+ output->state = VIN_OUTPUT_IDLE;
+
+ output->buf[0] = vin_buf_get_pending(output);
+
+ if (!output->buf[0] && output->buf[1]) {
+ output->buf[0] = output->buf[1];
+ output->buf[1] = NULL;
+ }
+
+ if (output->buf[0])
+ output->state = VIN_OUTPUT_SINGLE;
+
+ output->sequence = 0;
+
+ vin_output_init_addrs(line);
+ spin_unlock_irqrestore(&line->output_lock, flags);
+ return 0;
+}
+
+static int vin_disable_output(struct vin_line *line)
+{
+ struct vin_output *output = &line->output;
+ unsigned long flags;
+
+ spin_lock_irqsave(&line->output_lock, flags);
+
+ output->state = VIN_OUTPUT_OFF;
+
+ spin_unlock_irqrestore(&line->output_lock, flags);
+ return 0;
+}
+
+static u32 vin_line_to_dummy_module(struct vin_line *line)
+{
+ u32 dummy_module = 0;
+
+ switch (line->id) {
+ case VIN_LINE_WR:
+ dummy_module = STF_DUMMY_VIN;
+ break;
+ case VIN_LINE_ISP:
+ case VIN_LINE_ISP_SS0:
+ case VIN_LINE_ISP_SS1:
+ case VIN_LINE_ISP_RAW:
+ dummy_module = STF_DUMMY_ISP;
+ break;
+ default:
+ dummy_module = STF_DUMMY_VIN;
+ break;
+ }
+
+ return dummy_module;
+}
+
+static int vin_alloc_dummy_buffer(struct stf_vin_dev *vin_dev,
+ struct v4l2_mbus_framefmt *fmt,
+ int dummy_module)
+{
+ struct device *dev = vin_dev->stfcamss->dev;
+ struct dummy_buffer *dummy_buffer =
+ &vin_dev->dummy_buffer[dummy_module];
+ struct vin_dummy_buffer *buffer = NULL;
+ int ret = 0, i;
+ u32 aligns;
+
+ for (i = 0; i < dummy_buffer->nums; i++) {
+ buffer = &vin_dev->dummy_buffer[dummy_module].buffer[i];
+ buffer->width = fmt->width;
+ buffer->height = fmt->height;
+ buffer->mcode = fmt->code;
+ if (i == STF_VIN_PAD_SINK) {
+ aligns = ALIGN(fmt->width * 4,
+ STFCAMSS_FRAME_WIDTH_ALIGN_8);
+ buffer->buffer_size = PAGE_ALIGN(aligns * fmt->height);
+ } else if (i == STF_ISP_PAD_SRC ||
+ i == STF_ISP_PAD_SRC_SS0 ||
+ i == STF_ISP_PAD_SRC_SS1) {
+ aligns = ALIGN(fmt->width,
+ STFCAMSS_FRAME_WIDTH_ALIGN_8);
+ buffer->buffer_size =
+ PAGE_ALIGN(aligns * fmt->height * 3 / 2);
+ } else if (i == STF_ISP_PAD_SRC_RAW) {
+ aligns = ALIGN(fmt->width * ISP_RAW_DATA_BITS / 8,
+ STFCAMSS_FRAME_WIDTH_ALIGN_128);
+ buffer->buffer_size = PAGE_ALIGN(aligns * fmt->height);
+ } else {
+ continue;
+ }
+
+ buffer->vaddr = dma_alloc_coherent(dev,
+ buffer->buffer_size,
+ &buffer->paddr[0],
+ GFP_DMA | GFP_KERNEL);
+
+ if (buffer->vaddr) {
+ if (i == STF_ISP_PAD_SRC ||
+ i == STF_ISP_PAD_SRC_SS0 ||
+ i == STF_ISP_PAD_SRC_SS1)
+ buffer->paddr[1] =
+ (dma_addr_t)(buffer->paddr[0] + aligns * fmt->height);
+ else
+ st_debug(ST_VIN, "signal plane\n");
+ }
+ }
+
+ return ret;
+}
+
+static void vin_free_dummy_buffer(struct stf_vin_dev *vin_dev, int dummy_module)
+{
+ struct device *dev = vin_dev->stfcamss->dev;
+ struct dummy_buffer *dummy_buffer =
+ &vin_dev->dummy_buffer[dummy_module];
+ struct vin_dummy_buffer *buffer = NULL;
+ int i;
+
+ for (i = 0; i < dummy_buffer->nums; i++) {
+ buffer = &dummy_buffer->buffer[i];
+ if (buffer->vaddr)
+ dma_free_coherent(dev, buffer->buffer_size,
+ buffer->vaddr, buffer->paddr[0]);
+ memset(buffer, 0, sizeof(struct vin_dummy_buffer));
+ }
+}
+
+static void vin_set_dummy_buffer(struct vin_line *line, u32 pad)
+{
+ struct stf_vin_dev *vin_dev = line_to_vin_dev(line);
+ int dummy_module = vin_line_to_dummy_module(line);
+ struct dummy_buffer *dummy_buffer =
+ &vin_dev->dummy_buffer[dummy_module];
+ struct vin_dummy_buffer *buffer = NULL;
+
+ switch (pad) {
+ case STF_VIN_PAD_SINK:
+ if (line->id == VIN_LINE_WR) {
+ buffer = &dummy_buffer->buffer[STF_VIN_PAD_SINK];
+ vin_dev->hw_ops->vin_wr_set_ping_addr(vin_dev,
+ buffer->paddr[0]);
+ vin_dev->hw_ops->vin_wr_set_pong_addr(vin_dev,
+ buffer->paddr[0]);
+ } else {
+ buffer = &dummy_buffer->buffer[STF_ISP_PAD_SRC];
+ vin_dev->hw_ops->vin_isp_set_yuv_addr(vin_dev,
+ buffer->paddr[0],
+ buffer->paddr[1]);
+
+ buffer = &dummy_buffer->buffer[STF_ISP_PAD_SRC_SS0];
+ vin_dev->hw_ops->vin_isp_set_ss0_addr(vin_dev,
+ buffer->paddr[0],
+ buffer->paddr[1]);
+
+ buffer = &dummy_buffer->buffer[STF_ISP_PAD_SRC_SS1];
+ vin_dev->hw_ops->vin_isp_set_ss1_addr(vin_dev,
+ buffer->paddr[0],
+ buffer->paddr[1]);
+
+ buffer = &dummy_buffer->buffer[STF_ISP_PAD_SRC_RAW];
+ vin_dev->hw_ops->vin_isp_set_raw_addr(vin_dev,
+ buffer->paddr[0]);
+ }
+ break;
+ case STF_ISP_PAD_SRC:
+ buffer = &dummy_buffer->buffer[STF_ISP_PAD_SRC];
+ vin_dev->hw_ops->vin_isp_set_yuv_addr(vin_dev,
+ buffer->paddr[0],
+ buffer->paddr[1]);
+ break;
+ case STF_ISP_PAD_SRC_SS0:
+ buffer = &dummy_buffer->buffer[STF_ISP_PAD_SRC_SS0];
+ vin_dev->hw_ops->vin_isp_set_ss0_addr(vin_dev,
+ buffer->paddr[0],
+ buffer->paddr[1]);
+ break;
+ case STF_ISP_PAD_SRC_SS1:
+ buffer = &dummy_buffer->buffer[STF_ISP_PAD_SRC_SS1];
+ vin_dev->hw_ops->vin_isp_set_ss1_addr(vin_dev,
+ buffer->paddr[0],
+ buffer->paddr[1]);
+ break;
+ case STF_ISP_PAD_SRC_RAW:
+ buffer = &dummy_buffer->buffer[STF_ISP_PAD_SRC_RAW];
+ vin_dev->hw_ops->vin_isp_set_raw_addr(vin_dev,
+ buffer->paddr[0]);
+ break;
+ default:
+ break;
+ }
+}
+
+static int vin_set_stream(struct v4l2_subdev *sd, int enable)
+{
+ struct vin_line *line = v4l2_get_subdevdata(sd);
+ struct stf_vin_dev *vin_dev = line_to_vin_dev(line);
+ int dummy_module = vin_line_to_dummy_module(line);
+ struct dummy_buffer *dummy_buffer =
+ &vin_dev->dummy_buffer[dummy_module];
+ struct v4l2_mbus_framefmt *fmt;
+ struct media_entity *sensor;
+ struct v4l2_subdev *sensor_sd;
+ struct v4l2_subdev_format sensor_fmt;
+ enum link link;
+ int ret;
+
+ sensor = stfcamss_find_sensor(&line->subdev.entity);
+ if (!sensor)
+ return -EINVAL;
+
+ sensor_sd = media_entity_to_v4l2_subdev(sensor);
+
+ sensor_fmt.pad = 0;
+ sensor_fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+ ret = v4l2_subdev_call(sensor_sd, pad, get_fmt, NULL, &sensor_fmt);
+ if (ret) {
+ st_warn(ST_VIN, "Sensor get format failed !!\n");
+ return -EINVAL;
+ }
+
+ fmt = __vin_get_format(line, NULL,
+ STF_VIN_PAD_SINK, V4L2_SUBDEV_FORMAT_ACTIVE);
+ mutex_lock(&dummy_buffer->stream_lock);
+ if (enable) {
+ if (dummy_buffer->stream_count == 0) {
+ vin_alloc_dummy_buffer(vin_dev, fmt, dummy_module);
+ vin_set_dummy_buffer(line, STF_VIN_PAD_SINK);
+ atomic_set(&dummy_buffer->frame_skip,
+ vin_get_frame_skip(line));
+ }
+ dummy_buffer->stream_count++;
+ } else {
+ if (dummy_buffer->stream_count == 1) {
+ vin_free_dummy_buffer(vin_dev, dummy_module);
+ /* set buffer addr to zero */
+ vin_set_dummy_buffer(line, STF_VIN_PAD_SINK);
+ } else {
+ vin_set_dummy_buffer(line,
+ stf_vin_map_isp_pad(line->id, STF_ISP_PAD_SINK));
+ }
+
+ dummy_buffer->stream_count--;
+ }
+ mutex_unlock(&dummy_buffer->stream_lock);
+
+ mutex_lock(&line->stream_lock);
+ link = vin_get_link(&sd->entity);
+ if (link == LINK_ERROR)
+ goto exit;
+
+ if (enable) {
+ if (line->stream_count == 0) {
+ vin_dev->hw_ops->vin_stream_set(vin_dev, 1,
+ sensor_fmt, link);
+ if (line->id == VIN_LINE_WR) {
+ vin_dev->hw_ops->vin_wr_irq_enable(vin_dev, 1);
+ vin_dev->hw_ops->vin_wr_stream_set(vin_dev, 1);
+ }
+ }
+ line->stream_count++;
+ } else {
+ if (line->stream_count == 1) {
+ if (line->id == VIN_LINE_WR) {
+ vin_dev->hw_ops->vin_wr_irq_enable(vin_dev, 0);
+ vin_dev->hw_ops->vin_wr_stream_set(vin_dev, 0);
+ }
+ vin_dev->hw_ops->vin_stream_set(vin_dev, 0,
+ sensor_fmt, link);
+ }
+ line->stream_count--;
+ }
+exit:
+ mutex_unlock(&line->stream_lock);
+
+ if (enable)
+ vin_enable_output(line);
+ else
+ vin_disable_output(line);
+
+ return 0;
+}
+
+static struct v4l2_mbus_framefmt *
+__vin_get_format(struct vin_line *line,
+ struct v4l2_subdev_state *state,
+ unsigned int pad,
+ enum v4l2_subdev_format_whence which)
+{
+ if (which == V4L2_SUBDEV_FORMAT_TRY)
+ return v4l2_subdev_get_try_format(&line->subdev, state, pad);
+ return &line->fmt[pad];
+}
+
+static void vin_try_format(struct vin_line *line,
+ struct v4l2_subdev_state *state,
+ unsigned int pad,
+ struct v4l2_mbus_framefmt *fmt,
+ enum v4l2_subdev_format_whence which)
+{
+ unsigned int i;
+
+ switch (pad) {
+ case STF_VIN_PAD_SINK:
+ /* Set format on sink pad */
+ for (i = 0; i < line->nformats; i++)
+ if (fmt->code == line->formats[i].code)
+ break;
+
+ /* If not found, use UYVY as default */
+ if (i >= line->nformats)
+ fmt->code = line->formats[0].code;
+
+ fmt->width = clamp_t(u32, fmt->width,
+ STFCAMSS_FRAME_MIN_WIDTH,
+ STFCAMSS_FRAME_MAX_WIDTH);
+ fmt->height = clamp_t(u32, fmt->height,
+ STFCAMSS_FRAME_MIN_HEIGHT,
+ STFCAMSS_FRAME_MAX_HEIGHT);
+
+ fmt->field = V4L2_FIELD_NONE;
+ fmt->colorspace = V4L2_COLORSPACE_SRGB;
+ fmt->flags = 0;
+ break;
+
+ case STF_VIN_PAD_SRC:
+ /* Set and return a format same as sink pad */
+ *fmt = *__vin_get_format(line, state, STF_VIN_PAD_SINK, which);
+ break;
+ }
+
+ fmt->colorspace = V4L2_COLORSPACE_SRGB;
+}
+
+static int vin_enum_mbus_code(struct v4l2_subdev *sd,
+ struct v4l2_subdev_state *state,
+ struct v4l2_subdev_mbus_code_enum *code)
+{
+ struct vin_line *line = v4l2_get_subdevdata(sd);
+
+ if (code->index >= line->nformats)
+ return -EINVAL;
+ if (code->pad == STF_VIN_PAD_SINK) {
+ code->code = line->formats[code->index].code;
+ } else {
+ struct v4l2_mbus_framefmt *sink_fmt;
+
+ sink_fmt = __vin_get_format(line, state, STF_VIN_PAD_SINK,
+ code->which);
+
+ code->code = sink_fmt->code;
+ if (!code->code)
+ return -EINVAL;
+ }
+ code->flags = 0;
+
+ return 0;
+}
+
+static int vin_enum_frame_size(struct v4l2_subdev *sd,
+ struct v4l2_subdev_state *state,
+ struct v4l2_subdev_frame_size_enum *fse)
+{
+ struct vin_line *line = v4l2_get_subdevdata(sd);
+ struct v4l2_mbus_framefmt format;
+
+ if (fse->index != 0)
+ return -EINVAL;
+
+ format.code = fse->code;
+ format.width = 1;
+ format.height = 1;
+ vin_try_format(line, state, fse->pad, &format, fse->which);
+ fse->min_width = format.width;
+ fse->min_height = format.height;
+
+ if (format.code != fse->code)
+ return -EINVAL;
+
+ format.code = fse->code;
+ format.width = -1;
+ format.height = -1;
+ vin_try_format(line, state, fse->pad, &format, fse->which);
+ fse->max_width = format.width;
+ fse->max_height = format.height;
+
+ return 0;
+}
+
+static int vin_get_format(struct v4l2_subdev *sd,
+ struct v4l2_subdev_state *state,
+ struct v4l2_subdev_format *fmt)
+{
+ struct vin_line *line = v4l2_get_subdevdata(sd);
+ struct v4l2_mbus_framefmt *format;
+
+ format = __vin_get_format(line, state, fmt->pad, fmt->which);
+ if (!format)
+ return -EINVAL;
+
+ fmt->format = *format;
+
+ return 0;
+}
+
+static int vin_set_format(struct v4l2_subdev *sd,
+ struct v4l2_subdev_state *state,
+ struct v4l2_subdev_format *fmt)
+{
+ struct vin_line *line = v4l2_get_subdevdata(sd);
+ struct v4l2_mbus_framefmt *format;
+
+ st_debug(ST_VIDEO, "%s, pad %d, fmt code %x\n",
+ __func__, fmt->pad, fmt->format.code);
+
+ format = __vin_get_format(line, state, fmt->pad, fmt->which);
+ if (!format)
+ return -EINVAL;
+
+ mutex_lock(&line->stream_lock);
+ if (line->stream_count) {
+ fmt->format = *format;
+ mutex_unlock(&line->stream_lock);
+ goto out;
+ } else {
+ vin_try_format(line, state, fmt->pad, &fmt->format, fmt->which);
+ *format = fmt->format;
+ }
+ mutex_unlock(&line->stream_lock);
+
+ if (fmt->pad == STF_VIN_PAD_SINK) {
+ /* Propagate the format from sink to source */
+ format = __vin_get_format(line, state, STF_VIN_PAD_SRC,
+ fmt->which);
+
+ *format = fmt->format;
+ vin_try_format(line, state, STF_VIN_PAD_SRC, format,
+ fmt->which);
+ }
+
+out:
+ return 0;
+}
+
+static int vin_init_formats(struct v4l2_subdev *sd,
+ struct v4l2_subdev_fh *fh)
+{
+ struct v4l2_subdev_format format = {
+ .pad = STF_VIN_PAD_SINK,
+ .which = fh ?
+ V4L2_SUBDEV_FORMAT_TRY : V4L2_SUBDEV_FORMAT_ACTIVE,
+ .format = {
+ .code = MEDIA_BUS_FMT_RGB565_2X8_LE,
+ .width = 1920,
+ .height = 1080
+ }
+ };
+
+ return vin_set_format(sd, fh ? fh->state : NULL, &format);
+}
+
+static void vin_output_init_addrs(struct vin_line *line)
+{
+ struct vin_output *output = &line->output;
+ struct stf_vin_dev *vin_dev = line_to_vin_dev(line);
+ dma_addr_t ping_addr;
+ dma_addr_t pong_addr;
+ dma_addr_t y_addr, uv_addr;
+
+ output->active_buf = 0;
+
+ if (output->buf[0]) {
+ ping_addr = output->buf[0]->addr[0];
+ y_addr = output->buf[0]->addr[0];
+ uv_addr = output->buf[0]->addr[1];
+ } else {
+ return;
+ }
+
+ if (output->buf[1])
+ pong_addr = output->buf[1]->addr[0];
+ else
+ pong_addr = ping_addr;
+
+ switch (vin_map_isp_line(line->id)) {
+ case STF_ISP_LINE_SRC:
+ vin_dev->hw_ops->vin_isp_set_yuv_addr(vin_dev, y_addr, uv_addr);
+ break;
+ case STF_ISP_LINE_SRC_SS0:
+ vin_dev->hw_ops->vin_isp_set_ss0_addr(vin_dev, y_addr, uv_addr);
+ break;
+ case STF_ISP_LINE_SRC_SS1:
+ vin_dev->hw_ops->vin_isp_set_ss1_addr(vin_dev, y_addr, uv_addr);
+ break;
+ case STF_ISP_LINE_SRC_RAW:
+ vin_dev->hw_ops->vin_isp_set_raw_addr(vin_dev, y_addr);
+ break;
+ default:
+ if (line->id == VIN_LINE_WR) {
+ vin_dev->hw_ops->vin_wr_set_ping_addr(vin_dev,
+ ping_addr);
+ vin_dev->hw_ops->vin_wr_set_pong_addr(vin_dev,
+ ping_addr);
+ }
+ break;
+ }
+}
+
+static void vin_init_outputs(struct vin_line *line)
+{
+ struct vin_output *output = &line->output;
+
+ output->state = VIN_OUTPUT_OFF;
+ output->buf[0] = NULL;
+ output->buf[1] = NULL;
+ output->active_buf = 0;
+ INIT_LIST_HEAD(&output->pending_bufs);
+ INIT_LIST_HEAD(&output->ready_bufs);
+}
+
+static void vin_buf_add_ready(struct vin_output *output,
+ struct stfcamss_buffer *buffer)
+{
+ INIT_LIST_HEAD(&buffer->queue);
+ list_add_tail(&buffer->queue, &output->ready_bufs);
+}
+
+static struct stfcamss_buffer *vin_buf_get_ready(struct vin_output *output)
+{
+ struct stfcamss_buffer *buffer = NULL;
+
+ if (!list_empty(&output->ready_bufs)) {
+ buffer = list_first_entry(&output->ready_bufs,
+ struct stfcamss_buffer,
+ queue);
+ list_del(&buffer->queue);
+ }
+
+ return buffer;
+}
+
+static void vin_buf_add_pending(struct vin_output *output,
+ struct stfcamss_buffer *buffer)
+{
+ INIT_LIST_HEAD(&buffer->queue);
+ list_add_tail(&buffer->queue, &output->pending_bufs);
+}
+
+static struct stfcamss_buffer *vin_buf_get_pending(struct vin_output *output)
+{
+ struct stfcamss_buffer *buffer = NULL;
+
+ if (!list_empty(&output->pending_bufs)) {
+ buffer = list_first_entry(&output->pending_bufs,
+ struct stfcamss_buffer,
+ queue);
+ list_del(&buffer->queue);
+ }
+
+ return buffer;
+}
+
+static void vin_buf_update_on_last(struct vin_line *line)
+{
+ struct vin_output *output = &line->output;
+
+ switch (output->state) {
+ case VIN_OUTPUT_CONTINUOUS:
+ output->state = VIN_OUTPUT_SINGLE;
+ output->active_buf = !output->active_buf;
+ break;
+ case VIN_OUTPUT_SINGLE:
+ output->state = VIN_OUTPUT_STOPPING;
+ break;
+ default:
+ st_err_ratelimited(ST_VIN, "Last buff in wrong state! %d\n",
+ output->state);
+ break;
+ }
+}
+
+static void vin_buf_update_on_next(struct vin_line *line)
+{
+ struct vin_output *output = &line->output;
+
+ switch (output->state) {
+ case VIN_OUTPUT_CONTINUOUS:
+ output->active_buf = !output->active_buf;
+ break;
+ case VIN_OUTPUT_SINGLE:
+ default:
+ break;
+ }
+}
+
+static void vin_buf_update_on_new(struct vin_line *line,
+ struct vin_output *output,
+ struct stfcamss_buffer *new_buf)
+{
+ switch (output->state) {
+ case VIN_OUTPUT_SINGLE:
+ vin_buf_add_pending(output, new_buf);
+ break;
+ case VIN_OUTPUT_IDLE:
+ st_warn(ST_VIN, "Output idle buffer set!\n");
+ if (!output->buf[0]) {
+ output->buf[0] = new_buf;
+ vin_output_init_addrs(line);
+ output->state = VIN_OUTPUT_SINGLE;
+ } else {
+ vin_buf_add_pending(output, new_buf);
+ st_warn(ST_VIN, "Output idle with buffer set!\n");
+ }
+ break;
+ case VIN_OUTPUT_STOPPING:
+ if (output->last_buffer) {
+ output->buf[output->active_buf] = output->last_buffer;
+ output->last_buffer = NULL;
+ } else {
+ st_err(ST_VIN, "stop state lost lastbuffer!\n");
+ }
+ output->state = VIN_OUTPUT_SINGLE;
+ vin_buf_add_pending(output, new_buf);
+ break;
+ case VIN_OUTPUT_CONTINUOUS:
+ default:
+ vin_buf_add_pending(output, new_buf);
+ break;
+ }
+}
+
+static void vin_buf_flush(struct vin_output *output,
+ enum vb2_buffer_state state)
+{
+ struct stfcamss_buffer *buf;
+ struct stfcamss_buffer *t;
+
+ list_for_each_entry_safe(buf, t, &output->pending_bufs, queue) {
+ vb2_buffer_done(&buf->vb.vb2_buf, state);
+ list_del(&buf->queue);
+ }
+ list_for_each_entry_safe(buf, t, &output->ready_bufs, queue) {
+ vb2_buffer_done(&buf->vb.vb2_buf, state);
+ list_del(&buf->queue);
+ }
+}
+
+static void vin_buffer_done(struct vin_line *line)
+{
+ struct stfcamss_buffer *ready_buf;
+ struct vin_output *output = &line->output;
+ unsigned long flags;
+ u64 ts = ktime_get_ns();
+
+ if (output->state == VIN_OUTPUT_OFF ||
+ output->state == VIN_OUTPUT_RESERVED)
+ return;
+
+ spin_lock_irqsave(&line->output_lock, flags);
+
+ while ((ready_buf = vin_buf_get_ready(output))) {
+ ready_buf->vb.vb2_buf.timestamp = ts;
+ ready_buf->vb.sequence = output->sequence++;
+
+ vb2_buffer_done(&ready_buf->vb.vb2_buf, VB2_BUF_STATE_DONE);
+ }
+
+ spin_unlock_irqrestore(&line->output_lock, flags);
+}
+
+static void vin_change_buffer(struct vin_line *line)
+{
+ struct stfcamss_buffer *ready_buf;
+ struct vin_output *output = &line->output;
+ struct stf_vin_dev *vin_dev = line_to_vin_dev(line);
+ dma_addr_t *new_addr;
+ unsigned long flags;
+ u32 active_index;
+
+ if (output->state == VIN_OUTPUT_OFF ||
+ output->state == VIN_OUTPUT_STOPPING ||
+ output->state == VIN_OUTPUT_RESERVED ||
+ output->state == VIN_OUTPUT_IDLE)
+ return;
+
+ spin_lock_irqsave(&line->output_lock, flags);
+
+ active_index = output->active_buf;
+
+ ready_buf = output->buf[active_index];
+ if (!ready_buf) {
+ st_err_ratelimited(ST_VIN, "Missing ready buf %d %d!\n",
+ active_index, output->state);
+ active_index = !active_index;
+ ready_buf = output->buf[active_index];
+ if (!ready_buf) {
+ st_err_ratelimited(ST_VIN, "Missing ready buf2 %d %d!\n",
+ active_index, output->state);
+ goto out_unlock;
+ }
+ }
+
+ /* Get next buffer */
+ output->buf[active_index] = vin_buf_get_pending(output);
+ if (!output->buf[active_index]) {
+ /* No next buffer - set same address */
+ new_addr = ready_buf->addr;
+ vin_buf_update_on_last(line);
+ } else {
+ new_addr = output->buf[active_index]->addr;
+ vin_buf_update_on_next(line);
+ }
+
+ if (output->state == VIN_OUTPUT_STOPPING) {
+ output->last_buffer = ready_buf;
+ } else {
+ switch (vin_map_isp_line(line->id)) {
+ case STF_ISP_LINE_SRC:
+ vin_dev->hw_ops->vin_isp_set_yuv_addr(vin_dev,
+ new_addr[0],
+ new_addr[1]);
+ break;
+ case STF_ISP_LINE_SRC_SS0:
+ vin_dev->hw_ops->vin_isp_set_ss0_addr(vin_dev,
+ new_addr[0],
+ new_addr[1]);
+ break;
+ case STF_ISP_LINE_SRC_SS1:
+ vin_dev->hw_ops->vin_isp_set_ss1_addr(vin_dev,
+ new_addr[0],
+ new_addr[1]);
+ break;
+ case STF_ISP_LINE_SRC_RAW:
+ vin_dev->hw_ops->vin_isp_set_raw_addr(vin_dev,
+ new_addr[0]);
+ break;
+ default:
+ if (line->id == VIN_LINE_WR) {
+ vin_dev->hw_ops->vin_wr_set_ping_addr(vin_dev,
+ new_addr[0]);
+ vin_dev->hw_ops->vin_wr_set_pong_addr(vin_dev,
+ new_addr[0]);
+ }
+ break;
+ }
+
+ vin_buf_add_ready(output, ready_buf);
+ }
+
+ spin_unlock_irqrestore(&line->output_lock, flags);
+ return;
+
+out_unlock:
+ spin_unlock_irqrestore(&line->output_lock, flags);
+}
+
+static int vin_queue_buffer(struct stfcamss_video *vid,
+ struct stfcamss_buffer *buf)
+{
+ struct vin_line *line = container_of(vid, struct vin_line, video_out);
+ struct vin_output *output;
+ unsigned long flags;
+
+ output = &line->output;
+
+ spin_lock_irqsave(&line->output_lock, flags);
+
+ vin_buf_update_on_new(line, output, buf);
+
+ spin_unlock_irqrestore(&line->output_lock, flags);
+
+ return 0;
+}
+
+static int vin_flush_buffers(struct stfcamss_video *vid,
+ enum vb2_buffer_state state)
+{
+ struct vin_line *line = container_of(vid, struct vin_line, video_out);
+ struct vin_output *output = &line->output;
+ unsigned long flags;
+
+ spin_lock_irqsave(&line->output_lock, flags);
+
+ vin_buf_flush(output, state);
+ if (output->buf[0])
+ vb2_buffer_done(&output->buf[0]->vb.vb2_buf, state);
+
+ if (output->buf[1])
+ vb2_buffer_done(&output->buf[1]->vb.vb2_buf, state);
+
+ if (output->last_buffer) {
+ vb2_buffer_done(&output->last_buffer->vb.vb2_buf, state);
+ output->last_buffer = NULL;
+ }
+ output->buf[0] = NULL;
+ output->buf[1] = NULL;
+
+ spin_unlock_irqrestore(&line->output_lock, flags);
+ return 0;
+}
+
+static int vin_link_setup(struct media_entity *entity,
+ const struct media_pad *local,
+ const struct media_pad *remote, u32 flags)
+{
+ if (flags & MEDIA_LNK_FL_ENABLED)
+ if (media_pad_remote_pad_first(local))
+ return -EBUSY;
+ return 0;
+}
+
+static const struct v4l2_subdev_core_ops vin_core_ops = {
+ .s_power = vin_set_power,
+};
+
+static const struct v4l2_subdev_video_ops vin_video_ops = {
+ .s_stream = vin_set_stream,
+};
+
+static const struct v4l2_subdev_pad_ops vin_pad_ops = {
+ .enum_mbus_code = vin_enum_mbus_code,
+ .enum_frame_size = vin_enum_frame_size,
+ .get_fmt = vin_get_format,
+ .set_fmt = vin_set_format,
+};
+
+static const struct v4l2_subdev_ops vin_v4l2_ops = {
+ .core = &vin_core_ops,
+ .video = &vin_video_ops,
+ .pad = &vin_pad_ops,
+};
+
+static const struct v4l2_subdev_internal_ops vin_v4l2_internal_ops = {
+ .open = vin_init_formats,
+};
+
+static const struct stfcamss_video_ops stfcamss_vin_video_ops = {
+ .queue_buffer = vin_queue_buffer,
+ .flush_buffers = vin_flush_buffers,
+};
+
+static const struct media_entity_operations vin_media_ops = {
+ .link_setup = vin_link_setup,
+ .link_validate = v4l2_subdev_link_validate,
+};
+
+int stf_vin_register(struct stf_vin_dev *vin_dev, struct v4l2_device *v4l2_dev)
+{
+ struct v4l2_subdev *sd;
+ struct stfcamss_video *video_out;
+ struct media_pad *pads;
+ int ret;
+ int i;
+
+ for (i = 0; i < STF_ISP_LINE_MAX + 1; i++) {
+ char name[32];
+ char *sub_name = vin_get_line_subdevname(i);
+
+ sd = &vin_dev->line[i].subdev;
+ pads = vin_dev->line[i].pads;
+ video_out = &vin_dev->line[i].video_out;
+ video_out->id = i;
+
+ v4l2_subdev_init(sd, &vin_v4l2_ops);
+ sd->internal_ops = &vin_v4l2_internal_ops;
+ sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+ snprintf(sd->name, ARRAY_SIZE(sd->name), "%s%d_%s",
+ STF_VIN_NAME, 0, sub_name);
+ v4l2_set_subdevdata(sd, &vin_dev->line[i]);
+
+ ret = vin_init_formats(sd, NULL);
+ if (ret < 0) {
+ st_err(ST_VIN, "Failed to init format: %d\n", ret);
+ goto err_init;
+ }
+
+ pads[STF_VIN_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
+ pads[STF_VIN_PAD_SRC].flags = MEDIA_PAD_FL_SOURCE;
+
+ sd->entity.function =
+ MEDIA_ENT_F_PROC_VIDEO_PIXEL_FORMATTER;
+ sd->entity.ops = &vin_media_ops;
+ ret = media_entity_pads_init(&sd->entity,
+ STF_VIN_PADS_NUM, pads);
+ if (ret < 0) {
+ st_err(ST_VIN, "Failed to init media entity: %d\n",
+ ret);
+ goto err_init;
+ }
+
+ ret = v4l2_device_register_subdev(v4l2_dev, sd);
+ if (ret < 0) {
+ st_err(ST_VIN, "Failed to register subdev: %d\n", ret);
+ goto err_reg_subdev;
+ }
+
+ video_out->ops = &stfcamss_vin_video_ops;
+ video_out->bpl_alignment = 16 * 8;
+
+ snprintf(name, ARRAY_SIZE(name), "%s_%s%d",
+ sd->name, "video", i);
+ ret = stf_video_register(video_out, v4l2_dev, name);
+ if (ret < 0) {
+ st_err(ST_VIN, "Failed to register video node: %d\n",
+ ret);
+ goto err_vid_reg;
+ }
+
+ ret = media_create_pad_link(
+ &sd->entity, STF_VIN_PAD_SRC,
+ &video_out->vdev.entity, 0,
+ MEDIA_LNK_FL_IMMUTABLE | MEDIA_LNK_FL_ENABLED);
+ if (ret < 0) {
+ st_err(ST_VIN, "Failed to link %s->%s entities: %d\n",
+ sd->entity.name, video_out->vdev.entity.name,
+ ret);
+ goto err_create_link;
+ }
+ }
+
+ return 0;
+
+err_create_link:
+ stf_video_unregister(video_out);
+err_vid_reg:
+ v4l2_device_unregister_subdev(sd);
+err_reg_subdev:
+ media_entity_cleanup(&sd->entity);
+err_init:
+ for (i--; i >= 0; i--) {
+ sd = &vin_dev->line[i].subdev;
+ video_out = &vin_dev->line[i].video_out;
+
+ stf_video_unregister(video_out);
+ v4l2_device_unregister_subdev(sd);
+ media_entity_cleanup(&sd->entity);
+ }
+ return ret;
+}
+
+int stf_vin_unregister(struct stf_vin_dev *vin_dev)
+{
+ struct v4l2_subdev *sd;
+ struct stfcamss_video *video_out;
+ int i;
+
+ mutex_destroy(&vin_dev->power_lock);
+ for (i = 0; i < STF_DUMMY_MODULE_NUMS; i++)
+ mutex_destroy(&vin_dev->dummy_buffer[i].stream_lock);
+
+ for (i = 0; i < STF_ISP_LINE_MAX + 1; i++) {
+ sd = &vin_dev->line[i].subdev;
+ video_out = &vin_dev->line[i].video_out;
+
+ stf_video_unregister(video_out);
+ v4l2_device_unregister_subdev(sd);
+ media_entity_cleanup(&sd->entity);
+ mutex_destroy(&vin_dev->line[i].stream_lock);
+ mutex_destroy(&vin_dev->line[i].power_lock);
+ }
+ return 0;
+}
--- /dev/null
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * stf_vin.h
+ *
+ * StarFive Camera Subsystem - VIN Module
+ *
+ * Copyright (C) 2021-2023 StarFive Technology Co., Ltd.
+ */
+
+#ifndef STF_VIN_H
+#define STF_VIN_H
+
+#include <linux/interrupt.h>
+#include <linux/spinlock_types.h>
+#include <media/v4l2-subdev.h>
+
+#include "stf_video.h"
+
+#define SYSCONSAIF_SYSCFG(x) (x)
+
+/* syscon offset 0 */
+#define U0_VIN_CNFG_AXI_DVP_EN BIT(2)
+
+/* syscon offset 20 */
+#define U0_VIN_CNFG_AXIWR0_CHANNEL_SEL GENMASK(3, 0)
+#define U0_VIN_CNFG_AXIWR0_EN BIT(4)
+
+/* syscon offset 32 */
+#define U0_VIN_CNFG_AXIWR0_INTR_CLEAN BIT(0)
+#define U0_VIN_CNFG_AXIWR0_INTR_MASK BIT(1)
+#define U0_VIN_CNFG_AXIWR0_PIX_CNT_END GENMASK(12, 2)
+#define U0_VIN_CNFG_AXIWR0_PIX_CT GENMASK(14, 13)
+#define U0_VIN_CNFG_AXIWR0_PIXEL_HEIGH_BIT_SEL GENMASK(16, 15)
+
+/* syscon offset 36 */
+#define U0_VIN_CNFG_DVP_HS_POS BIT(1)
+#define U0_VIN_CNFG_DVP_SWAP_EN BIT(2)
+#define U0_VIN_CNFG_DVP_VS_POS BIT(3)
+#define U0_VIN_CNFG_GEN_EN_AXIRD BIT(4)
+#define U0_VIN_CNFG_ISP_DVP_EN0 BIT(5)
+#define U0_VIN_CNFG_MIPI_BYTE_EN_ISP0 GENMASK(7, 6)
+#define U0_VIN_CNFG_MIPI_CHANNEL_SEL0 GENMASK(11, 8)
+#define U0_VIN_CNFG_P_I_MIPI_HAEDER_EN0 BIT(12)
+#define U0_VIN_CNFG_PIX_NUM GENMASK(16, 13)
+
+#define STF_VIN_PAD_SINK 0
+#define STF_VIN_PAD_SRC 1
+#define STF_VIN_PADS_NUM 2
+
+#define ISP_DUMMY_BUFFER_NUMS STF_ISP_PAD_MAX
+#define VIN_DUMMY_BUFFER_NUMS 1
+
+enum {
+ STF_DUMMY_VIN,
+ STF_DUMMY_ISP,
+ STF_DUMMY_MODULE_NUMS,
+};
+
+enum link {
+ LINK_ERROR = -1,
+ LINK_DVP_TO_WR,
+ LINK_DVP_TO_ISP,
+ LINK_CSI_TO_WR,
+ LINK_CSI_TO_ISP,
+};
+
+struct vin_format {
+ u32 code;
+ u8 bpp;
+};
+
+struct vin_format_table {
+ const struct vin_format *fmts;
+ int nfmts;
+};
+
+enum vin_output_state {
+ VIN_OUTPUT_OFF,
+ VIN_OUTPUT_RESERVED,
+ VIN_OUTPUT_SINGLE,
+ VIN_OUTPUT_CONTINUOUS,
+ VIN_OUTPUT_IDLE,
+ VIN_OUTPUT_STOPPING
+};
+
+struct vin_output {
+ int active_buf;
+ struct stfcamss_buffer *buf[2];
+ struct stfcamss_buffer *last_buffer;
+ struct list_head pending_bufs;
+ struct list_head ready_bufs;
+ enum vin_output_state state;
+ unsigned int sequence;
+ unsigned int frame_skip;
+};
+
+/*
+ * The vin output lines include all isp controller lines,
+ * and one vin_wr output line.
+ */
+enum vin_line_id {
+ VIN_LINE_NONE = -1,
+ VIN_LINE_WR = 0,
+ VIN_LINE_ISP,
+ VIN_LINE_ISP_SS0,
+ VIN_LINE_ISP_SS1,
+ VIN_LINE_ISP_RAW,
+ VIN_LINE_MAX,
+};
+
+struct vin_line {
+ enum subdev_type sdev_type; /* must be frist */
+ enum vin_line_id id;
+ struct v4l2_subdev subdev;
+ struct media_pad pads[STF_VIN_PADS_NUM];
+ struct v4l2_mbus_framefmt fmt[STF_VIN_PADS_NUM];
+ struct stfcamss_video video_out;
+ struct mutex stream_lock; /* serialize stream control */
+ int stream_count;
+ struct mutex power_lock; /* serialize pipeline control in power process*/
+ int power_count;
+ struct vin_output output; /* pipeline and stream states */
+ spinlock_t output_lock;
+ const struct vin_format *formats;
+ unsigned int nformats;
+};
+
+struct vin_dummy_buffer {
+ dma_addr_t paddr[3];
+ void *vaddr;
+ u32 buffer_size;
+ u32 width;
+ u32 height;
+ u32 mcode;
+};
+
+struct dummy_buffer {
+ struct vin_dummy_buffer *buffer;
+ u32 nums;
+ struct mutex stream_lock; /* protects buffer data */
+ int stream_count;
+ atomic_t frame_skip;
+};
+
+struct vin_isr_ops {
+ void (*isr_buffer_done)(struct vin_line *line);
+ void (*isr_change_buffer)(struct vin_line *line);
+};
+
+struct stf_vin_dev {
+ struct stfcamss *stfcamss;
+ struct vin_line line[VIN_LINE_MAX];
+ struct dummy_buffer dummy_buffer[STF_DUMMY_MODULE_NUMS];
+ const struct vin_hw_ops *hw_ops;
+ struct vin_isr_ops *isr_ops;
+ atomic_t ref_count;
+ struct mutex power_lock; /* serialize power control*/
+ int power_count;
+};
+
+struct vin_hw_ops {
+ int (*vin_clk_enable)(struct stf_vin_dev *vin_dev, enum link link);
+ int (*vin_clk_disable)(struct stf_vin_dev *vin_dev, enum link link);
+ int (*vin_wr_stream_set)(struct stf_vin_dev *vin_dev, int on);
+ int (*vin_stream_set)(struct stf_vin_dev *vin_dev, int on,
+ struct v4l2_subdev_format sensor_fmt,
+ enum link link);
+ void (*vin_wr_irq_enable)(struct stf_vin_dev *vin_dev, int enable);
+ void (*vin_wr_set_ping_addr)(struct stf_vin_dev *vin_dev,
+ dma_addr_t addr);
+ void (*vin_wr_set_pong_addr)(struct stf_vin_dev *vin_dev,
+ dma_addr_t addr);
+ void (*vin_isp_set_yuv_addr)(struct stf_vin_dev *vin_dev,
+ dma_addr_t y_addr, dma_addr_t uv_addr);
+ void (*vin_isp_set_raw_addr)(struct stf_vin_dev *vin_dev,
+ dma_addr_t raw_addr);
+ void (*vin_isp_set_ss0_addr)(struct stf_vin_dev *vin_dev,
+ dma_addr_t y_addr, dma_addr_t uv_addr);
+ void (*vin_isp_set_ss1_addr)(struct stf_vin_dev *vin_dev,
+ dma_addr_t y_addr, dma_addr_t uv_addr);
+ irqreturn_t (*vin_wr_irq_handler)(int irq, void *priv);
+ irqreturn_t (*vin_isp_irq_handler)(int irq, void *priv);
+ irqreturn_t (*vin_isp_irq_csi_handler)(int irq, void *priv);
+ irqreturn_t (*vin_isp_irq_csiline_handler)(int irq, void *priv);
+};
+
+extern const struct vin_hw_ops vin_ops;
+
+int stf_vin_subdev_init(struct stfcamss *stfcamss);
+int stf_vin_register(struct stf_vin_dev *vin_dev, struct v4l2_device *v4l2_dev);
+int stf_vin_unregister(struct stf_vin_dev *vin_dev);
+enum isp_pad_id stf_vin_map_isp_pad(enum vin_line_id line, enum isp_pad_id def);
+
+#endif /* STF_VIN_H */
--- /dev/null
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * stf_vin_hw_ops.c
+ *
+ * Register interface file for StarFive VIN module driver
+ *
+ * Copyright (C) 2021-2023 StarFive Technology Co., Ltd.
+ */
+#include "stf_camss.h"
+
+static void vin_intr_clear(void __iomem *syscon_base)
+{
+ reg_set_bit(syscon_base, SYSCONSAIF_SYSCFG(28),
+ U0_VIN_CNFG_AXIWR0_INTR_CLEAN, 0x1);
+ reg_set_bit(syscon_base, SYSCONSAIF_SYSCFG(28),
+ U0_VIN_CNFG_AXIWR0_INTR_CLEAN, 0x0);
+}
+
+static irqreturn_t stf_vin_wr_irq_handler(int irq, void *priv)
+{
+ struct stf_vin_dev *vin_dev = priv;
+ struct stfcamss *stfcamss = vin_dev->stfcamss;
+ struct dummy_buffer *dummy_buffer =
+ &vin_dev->dummy_buffer[STF_DUMMY_VIN];
+
+ if (atomic_dec_if_positive(&dummy_buffer->frame_skip) < 0) {
+ vin_dev->isr_ops->isr_change_buffer(&vin_dev->line[VIN_LINE_WR]);
+ vin_dev->isr_ops->isr_buffer_done(&vin_dev->line[VIN_LINE_WR]);
+ }
+
+ vin_intr_clear(stfcamss->syscon_base);
+
+ return IRQ_HANDLED;
+}
+
+static void __iomem *stf_vin_get_ispbase(struct stf_vin_dev *vin_dev)
+{
+ void __iomem *base = vin_dev->stfcamss->isp_base;
+
+ return base;
+}
+
+static irqreturn_t stf_vin_isp_irq_handler(int irq, void *priv)
+{
+ struct stf_vin_dev *vin_dev = priv;
+ void __iomem *ispbase = stf_vin_get_ispbase(vin_dev);
+ u32 int_status;
+
+ int_status = reg_read(ispbase, ISP_REG_ISP_CTRL_0);
+
+ if (int_status & BIT(24)) {
+ if ((int_status & BIT(11)))
+ vin_dev->isr_ops->isr_buffer_done(
+ &vin_dev->line[VIN_LINE_ISP_SS0]);
+
+ if ((int_status & BIT(12)))
+ vin_dev->isr_ops->isr_buffer_done(
+ &vin_dev->line[VIN_LINE_ISP_SS1]);
+
+ if ((int_status & BIT(20)))
+ vin_dev->isr_ops->isr_buffer_done(
+ &vin_dev->line[VIN_LINE_ISP]);
+
+ if (int_status & BIT(25))
+ vin_dev->isr_ops->isr_buffer_done(
+ &vin_dev->line[VIN_LINE_ISP_RAW]);
+
+ /* clear interrupt */
+ reg_write(ispbase,
+ ISP_REG_ISP_CTRL_0,
+ (int_status & ~EN_INT_ALL) |
+ EN_INT_ISP_DONE |
+ EN_INT_CSI_DONE |
+ EN_INT_SC_DONE);
+
+ } else {
+ st_debug(ST_VIN, "%s, Unknown interrupt!\n", __func__);
+ }
+
+ return IRQ_HANDLED;
+}
+
+static irqreturn_t stf_vin_isp_irq_csi_handler(int irq, void *priv)
+{
+ struct stf_vin_dev *vin_dev = priv;
+ void __iomem *ispbase = stf_vin_get_ispbase(vin_dev);
+ u32 int_status;
+
+ int_status = reg_read(ispbase, ISP_REG_ISP_CTRL_0);
+
+ if (int_status & BIT(25)) {
+ vin_dev->isr_ops->isr_buffer_done(
+ &vin_dev->line[VIN_LINE_ISP_RAW]);
+
+ /* clear interrupt */
+ reg_write(ispbase,
+ ISP_REG_ISP_CTRL_0,
+ (int_status & ~EN_INT_ALL) | EN_INT_CSI_DONE);
+ } else {
+ st_debug(ST_VIN, "%s, Unknown interrupt!!!\n", __func__);
+ }
+
+ return IRQ_HANDLED;
+}
+
+static irqreturn_t stf_vin_isp_irq_csiline_handler(int irq, void *priv)
+{
+ struct stf_vin_dev *vin_dev = priv;
+ struct stf_isp_dev *isp_dev;
+ void __iomem *ispbase = stf_vin_get_ispbase(vin_dev);
+ u32 int_status, value;
+
+ isp_dev = vin_dev->stfcamss->isp_dev;
+
+ int_status = reg_read(ispbase, ISP_REG_ISP_CTRL_0);
+ if (int_status & BIT(27)) {
+ struct dummy_buffer *dummy_buffer =
+ &vin_dev->dummy_buffer[STF_DUMMY_ISP];
+
+ if (!atomic_read(&isp_dev->shadow_count)) {
+ if (atomic_dec_if_positive(&dummy_buffer->frame_skip) < 0) {
+ if ((int_status & BIT(11)))
+ vin_dev->isr_ops->isr_change_buffer(
+ &vin_dev->line[VIN_LINE_ISP_SS0]);
+ if ((int_status & BIT(12)))
+ vin_dev->isr_ops->isr_change_buffer(
+ &vin_dev->line[VIN_LINE_ISP_SS1]);
+ if ((int_status & BIT(20)))
+ vin_dev->isr_ops->isr_change_buffer(
+ &vin_dev->line[VIN_LINE_ISP]);
+ value = reg_read(ispbase,
+ ISP_REG_CSI_MODULE_CFG);
+ if ((value & BIT(19)))
+ vin_dev->isr_ops->isr_change_buffer(
+ &vin_dev->line[VIN_LINE_ISP_RAW]);
+ }
+
+ /* shadow update */
+ reg_set_bit(ispbase, ISP_REG_CSIINTS_ADDR,
+ 0x30000, 0x30000);
+ reg_set_bit(ispbase, ISP_REG_IESHD_ADDR,
+ BIT(1) | BIT(0), 0x3);
+ } else {
+ st_err_ratelimited(ST_VIN, "skip this frame\n");
+ }
+
+ /* clear interrupt */
+ reg_write(ispbase, ISP_REG_ISP_CTRL_0,
+ (int_status & ~EN_INT_ALL) | EN_INT_LINE_INT);
+ } else {
+ st_debug(ST_VIN, "%s, Unknown interrupt!!!\n", __func__);
+ }
+
+ return IRQ_HANDLED;
+}
+
+static int stf_vin_clk_enable(struct stf_vin_dev *vin_dev, enum link link)
+{
+ struct stfcamss *stfcamss = vin_dev->stfcamss;
+
+ clk_set_rate(stfcamss->sys_clk[STF_CLK_APB_FUNC].clk, 49500000);
+
+ switch (link) {
+ case LINK_DVP_TO_WR:
+ break;
+ case LINK_DVP_TO_ISP:
+ break;
+ case LINK_CSI_TO_WR:
+ clk_set_rate(stfcamss->sys_clk[STF_CLK_MIPI_RX0_PXL].clk,
+ 198000000);
+ reset_control_deassert(stfcamss->sys_rst[STF_RST_AXIWR].rstc);
+ clk_set_parent(stfcamss->sys_clk[STF_CLK_AXIWR].clk,
+ stfcamss->sys_clk[STF_CLK_MIPI_RX0_PXL].clk);
+ break;
+ case LINK_CSI_TO_ISP:
+ clk_set_rate(stfcamss->sys_clk[STF_CLK_MIPI_RX0_PXL].clk,
+ 198000000);
+ clk_set_parent(stfcamss->sys_clk[STF_CLK_WRAPPER_CLK_C].clk,
+ stfcamss->sys_clk[STF_CLK_MIPI_RX0_PXL].clk);
+ break;
+ default:
+ return -EINVAL;
+ }
+
+ return 0;
+}
+
+static int stf_vin_clk_disable(struct stf_vin_dev *vin_dev, enum link link)
+{
+ struct stfcamss *stfcamss = vin_dev->stfcamss;
+
+ switch (link) {
+ case LINK_DVP_TO_WR:
+ break;
+ case LINK_DVP_TO_ISP:
+ break;
+ case LINK_CSI_TO_WR:
+ reset_control_assert(stfcamss->sys_rst[STF_RST_AXIWR].rstc);
+ break;
+ case LINK_CSI_TO_ISP:
+ break;
+ default:
+ return -EINVAL;
+ }
+
+ return 0;
+}
+
+static int stf_vin_wr_stream_set(struct stf_vin_dev *vin_dev, int on)
+{
+ struct stfcamss *stfcamss = vin_dev->stfcamss;
+
+ /* make the axiwr alway on */
+ if (on)
+ reg_set(stfcamss->syscon_base,
+ SYSCONSAIF_SYSCFG(20), U0_VIN_CNFG_AXIWR0_EN);
+
+ return 0;
+}
+
+static int stf_vin_stream_set(struct stf_vin_dev *vin_dev, int on,
+ struct v4l2_subdev_format sensor_fmt,
+ enum link link)
+{
+ struct stfcamss *stfcamss = vin_dev->stfcamss;
+
+ switch (link) {
+ case LINK_DVP_TO_WR:
+ break;
+ case LINK_DVP_TO_ISP:
+ break;
+ case LINK_CSI_TO_WR:
+ reg_set_bit(stfcamss->syscon_base, SYSCONSAIF_SYSCFG(20),
+ U0_VIN_CNFG_AXIWR0_CHANNEL_SEL, 0 << 0);
+ reg_set_bit(stfcamss->syscon_base, SYSCONSAIF_SYSCFG(28),
+ U0_VIN_CNFG_AXIWR0_PIX_CT, 1 << 13);
+ reg_set_bit(stfcamss->syscon_base, SYSCONSAIF_SYSCFG(28),
+ U0_VIN_CNFG_AXIWR0_PIXEL_HEIGH_BIT_SEL, 0 << 15);
+ reg_set_bit(stfcamss->syscon_base, SYSCONSAIF_SYSCFG(28),
+ U0_VIN_CNFG_AXIWR0_PIX_CNT_END,
+ (sensor_fmt.format.width / 4 - 1) << 2);
+ break;
+ case LINK_CSI_TO_ISP:
+ reg_set_bit(stfcamss->syscon_base, SYSCONSAIF_SYSCFG(36),
+ U0_VIN_CNFG_MIPI_BYTE_EN_ISP0, 0 << 6);
+ reg_set_bit(stfcamss->syscon_base, SYSCONSAIF_SYSCFG(36),
+ U0_VIN_CNFG_MIPI_CHANNEL_SEL0, 0 << 8);
+ reg_set_bit(stfcamss->syscon_base, SYSCONSAIF_SYSCFG(36),
+ U0_VIN_CNFG_PIX_NUM, 0 << 13);
+
+ if (sensor_fmt.format.code == MEDIA_BUS_FMT_SRGGB10_1X10 ||
+ sensor_fmt.format.code == MEDIA_BUS_FMT_SGRBG10_1X10 ||
+ sensor_fmt.format.code == MEDIA_BUS_FMT_SGBRG10_1X10 ||
+ sensor_fmt.format.code == MEDIA_BUS_FMT_SBGGR10_1X10)
+ reg_set_bit(stfcamss->syscon_base,
+ SYSCONSAIF_SYSCFG(36),
+ U0_VIN_CNFG_P_I_MIPI_HAEDER_EN0,
+ 1 << 12);
+ break;
+ default:
+ return -EINVAL;
+ }
+
+ return 0;
+}
+
+static void stf_vin_wr_irq_enable(struct stf_vin_dev *vin_dev, int enable)
+{
+ struct stfcamss *stfcamss = vin_dev->stfcamss;
+ unsigned int value = 0;
+
+ if (enable) {
+ value = ~(0x1 << 1);
+ reg_set_bit(stfcamss->syscon_base, SYSCONSAIF_SYSCFG(28),
+ U0_VIN_CNFG_AXIWR0_INTR_MASK, value);
+ } else {
+ /* clear vin interrupt */
+ value = 0x1 << 1;
+ reg_set_bit(stfcamss->syscon_base, SYSCONSAIF_SYSCFG(28),
+ U0_VIN_CNFG_AXIWR0_INTR_CLEAN, 0x1);
+ reg_set_bit(stfcamss->syscon_base, SYSCONSAIF_SYSCFG(28),
+ U0_VIN_CNFG_AXIWR0_INTR_CLEAN, 0x0);
+ reg_set_bit(stfcamss->syscon_base, SYSCONSAIF_SYSCFG(28),
+ U0_VIN_CNFG_AXIWR0_INTR_MASK, value);
+ }
+}
+
+static void stf_vin_wr_set_ping_addr(struct stf_vin_dev *vin_dev,
+ dma_addr_t addr)
+{
+ struct stfcamss *stfcamss = vin_dev->stfcamss;
+
+ /* set the start address */
+ reg_write(stfcamss->syscon_base, SYSCONSAIF_SYSCFG(32), (long)addr);
+}
+
+static void stf_vin_wr_set_pong_addr(struct stf_vin_dev *vin_dev,
+ dma_addr_t addr)
+{
+ struct stfcamss *stfcamss = vin_dev->stfcamss;
+
+ /* set the start address */
+ reg_write(stfcamss->syscon_base, SYSCONSAIF_SYSCFG(24), (long)addr);
+}
+
+static void stf_vin_isp_set_yuv_addr(struct stf_vin_dev *vin_dev,
+ dma_addr_t y_addr, dma_addr_t uv_addr)
+{
+ void __iomem *ispbase = stf_vin_get_ispbase(vin_dev);
+
+ reg_write(ispbase, ISP_REG_Y_PLANE_START_ADDR, y_addr);
+ reg_write(ispbase, ISP_REG_UV_PLANE_START_ADDR, uv_addr);
+}
+
+static void stf_vin_isp_set_raw_addr(struct stf_vin_dev *vin_dev,
+ dma_addr_t raw_addr)
+{
+ void __iomem *ispbase = stf_vin_get_ispbase(vin_dev);
+
+ reg_write(ispbase, ISP_REG_DUMP_CFG_0, raw_addr);
+}
+
+static void stf_vin_isp_set_ss0_addr(struct stf_vin_dev *vin_dev,
+ dma_addr_t y_addr, dma_addr_t uv_addr)
+{
+ void __iomem *ispbase = stf_vin_get_ispbase(vin_dev);
+
+ reg_write(ispbase, ISP_REG_SS0AY, y_addr);
+ reg_write(ispbase, ISP_REG_SS0AUV, uv_addr);
+}
+
+static void stf_vin_isp_set_ss1_addr(struct stf_vin_dev *vin_dev,
+ dma_addr_t y_addr, dma_addr_t uv_addr)
+{
+ void __iomem *ispbase = stf_vin_get_ispbase(vin_dev);
+
+ reg_write(ispbase, ISP_REG_SS1AY, y_addr);
+ reg_write(ispbase, ISP_REG_SS1AUV, uv_addr);
+}
+
+const struct vin_hw_ops vin_ops = {
+ .vin_clk_enable = stf_vin_clk_enable,
+ .vin_clk_disable = stf_vin_clk_disable,
+ .vin_wr_stream_set = stf_vin_wr_stream_set,
+ .vin_wr_irq_enable = stf_vin_wr_irq_enable,
+ .vin_stream_set = stf_vin_stream_set,
+ .vin_wr_set_ping_addr = stf_vin_wr_set_ping_addr,
+ .vin_wr_set_pong_addr = stf_vin_wr_set_pong_addr,
+ .vin_isp_set_yuv_addr = stf_vin_isp_set_yuv_addr,
+ .vin_isp_set_raw_addr = stf_vin_isp_set_raw_addr,
+ .vin_isp_set_ss0_addr = stf_vin_isp_set_ss0_addr,
+ .vin_isp_set_ss1_addr = stf_vin_isp_set_ss1_addr,
+ .vin_wr_irq_handler = stf_vin_wr_irq_handler,
+ .vin_isp_irq_handler = stf_vin_isp_irq_handler,
+ .vin_isp_irq_csi_handler = stf_vin_isp_irq_csi_handler,
+ .vin_isp_irq_csiline_handler = stf_vin_isp_irq_csiline_handler,
+};
--- /dev/null
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * stf_isp_ioctl.h
+ *
+ * StarFive Camera Subsystem - ISP Module
+ *
+ * Copyright (C) 2021-2023 StarFive Technology Co., Ltd.
+ */
+
+#ifndef STF_ISP_IOCTL_H
+#define STF_ISP_IOCTL_H
+
+#include <media/v4l2-ctrls.h>
+
+#define FILENAME_MAX_LEN 30
+
+#define ISP_IOC ('V')
+#define STF_ISP_REG_BUF_SIZE (768)
+#define STF_ISP_REG_TBL_BUF_SIZE (STF_ISP_REG_BUF_SIZE / 2)
+#define STF_ISP_REG_TBL_2_BUF_SIZE (STF_ISP_REG_BUF_SIZE / 3)
+#define STF_ISP_REG_TBL_3_BUF_SIZE (STF_ISP_REG_BUF_SIZE / 4)
+#define STF_ISP_REG_SMPL_PACK_BUF_SIZE (STF_ISP_REG_BUF_SIZE / 2)
+#define RDMA_WR_ONE (0xA0)
+#define RDMA_WR_SRL (0xA1)
+#define RDMA_LINK (0xA2)
+#define RDMA_SINT (0xA3)
+#define RDMA_END (0xAF)
+#define ENABLE_SS0_SS1
+
+enum _STF_ISP_IOCTL {
+ STF_ISP_IOCTL_LOAD_FW = BASE_VIDIOC_PRIVATE + 1,
+ STF_ISP_IOCTL_DMABUF_ALLOC,
+ STF_ISP_IOCTL_DMABUF_FREE,
+ STF_ISP_IOCTL_GET_HW_VER,
+ STF_ISP_IOCTL_REG,
+ STF_ISP_IOCTL_SHADOW_LOCK,
+ STF_ISP_IOCTL_SHADOW_UNLOCK,
+ STF_ISP_IOCTL_SHADOW_UNLOCK_N_TRIGGER,
+ STF_ISP_IOCTL_SET_USER_CONFIG_ISP,
+ STF_ISP_IOCTL_MAX
+};
+
+enum _STF_ISP_REG_METHOD {
+ STF_ISP_REG_METHOD_ONE_REG = 0,
+ STF_ISP_REG_METHOD_SERIES,
+ STF_ISP_REG_METHOD_MODULE,
+ STF_ISP_REG_METHOD_TABLE,
+ STF_ISP_REG_METHOD_TABLE_2,
+ STF_ISP_REG_METHOD_TABLE_3,
+ STF_ISP_REG_METHOD_SMPL_PACK,
+ STF_ISP_REG_METHOD_SOFT_RDMA,
+ STF_ISP_REG_METHOD_MAX
+};
+
+struct stfisp_fw_info {
+ char __user filename[FILENAME_MAX_LEN];
+};
+
+struct dmabuf_create {
+ __u32 fd;
+ __u32 size;
+ __u32 paddr;
+};
+
+struct isp_rdma_info {
+ u32 param;
+ union {
+ u32 value;
+ struct {
+ u32 offset : 24;
+ u32 tag : 8;
+ };
+ };
+};
+
+struct isp_reg_info {
+ /* access method of register */
+ u8 method;
+ /* offset indicated which register will be read/write */
+ u32 offset;
+ /* length for indicated how much register will be read/write */
+ u32 length;
+};
+
+union reg_buf {
+ u32 buffer[STF_ISP_REG_BUF_SIZE];
+ struct {
+ u32 offset;
+ u32 value;
+ } reg_tbl[STF_ISP_REG_TBL_BUF_SIZE];
+ struct {
+ u32 offset;
+ u32 value;
+ u32 mask;
+ } reg_tbl2[STF_ISP_REG_TBL_2_BUF_SIZE];
+ struct {
+ u32 offset;
+ u32 value;
+ u32 mask;
+ u32 delay_ms;
+ } reg_tbl3[STF_ISP_REG_TBL_3_BUF_SIZE];
+ struct isp_rdma_info rdma_cmd[STF_ISP_REG_SMPL_PACK_BUF_SIZE];
+};
+
+struct isp_reg_param {
+ /* register read/write information */
+ struct isp_reg_info reg_info;
+ /* buffer */
+ union reg_buf *reg_buf;
+};
+
+#define VIDIOC_STFISP_LOAD_FW \
+ _IOW(ISP_IOC, STF_ISP_IOCTL_LOAD_FW, struct stfisp_fw_info)
+#define VIDIOC_STFISP_GET_REG \
+ _IOWR(ISP_IOC, STF_ISP_IOCTL_REG, struct isp_reg_param)
+#define VIDIOC_STFISP_SET_REG \
+ _IOW(ISP_IOC, STF_ISP_IOCTL_REG, struct isp_reg_param)
+#define VIDIOC_STFISP_SHADOW_LOCK \
+ _IO(ISP_IOC, STF_ISP_IOCTL_SHADOW_LOCK)
+#define VIDIOC_STFISP_SHADOW_UNLOCK \
+ _IO(ISP_IOC, STF_ISP_IOCTL_SHADOW_UNLOCK)
+#define VIDIOC_STFISP_SHADOW_UNLOCK_N_TRIGGER \
+ _IO(ISP_IOC, STF_ISP_IOCTL_SHADOW_UNLOCK_N_TRIGGER)
+#define VIDIOC_STFISP_SET_USER_CONFIG_ISP \
+ _IO(ISP_IOC, STF_ISP_IOCTL_SET_USER_CONFIG_ISP)
+
+#endif /* STF_ISP_IOCTL_H */
projects / platform / kernel / linux-starfive.git / commitdiff