From nobody Sat Apr 11 23:43:09 2026 Return-Path: X-Spam-Checker-Version: SpamAssassin 3.4.0 (2014-02-07) on aws-us-west-2-korg-lkml-1.web.codeaurora.org Received: from vger.kernel.org (vger.kernel.org [23.128.96.18]) by smtp.lore.kernel.org (Postfix) with ESMTP id 8882FC6FD1E for ; Fri, 10 Mar 2023 12:06:44 +0000 (UTC) Received: (majordomo@vger.kernel.org) by vger.kernel.org via listexpand id S230197AbjCJMGn convert rfc822-to-8bit (ORCPT ); Fri, 10 Mar 2023 07:06:43 -0500 Received: from lindbergh.monkeyblade.net ([23.128.96.19]:54188 "EHLO lindbergh.monkeyblade.net" rhost-flags-OK-OK-OK-OK) by vger.kernel.org with ESMTP id S230154AbjCJMGL (ORCPT ); Fri, 10 Mar 2023 07:06:11 -0500 Received: from ex01.ufhost.com (ex01.ufhost.com [61.152.239.75]) by lindbergh.monkeyblade.net (Postfix) with ESMTPS id 4F6A7F92C8; Fri, 10 Mar 2023 04:06:00 -0800 (PST) Received: from EXMBX166.cuchost.com (unknown [175.102.18.54]) (using TLSv1 with cipher DHE-RSA-AES256-SHA (256/256 bits)) (Client CN "EXMBX166", Issuer "EXMBX166" (not verified)) by ex01.ufhost.com (Postfix) with ESMTP id 91C4724E274; Fri, 10 Mar 2023 20:05:57 +0800 (CST) Received: from EXMBX073.cuchost.com (172.16.6.83) by EXMBX166.cuchost.com (172.16.6.76) with Microsoft SMTP Server (TLS) id 15.0.1497.42; Fri, 10 Mar 2023 20:05:57 +0800 Received: from xiaofei.localdomain (180.164.60.184) by EXMBX073.cuchost.com (172.16.6.83) with Microsoft SMTP Server (TLS) id 15.0.1497.42; Fri, 10 Mar 2023 20:05:56 +0800 From: Jack Zhu To: Mauro Carvalho Chehab , Maxime Ripard , Rob Herring , Krzysztof Kozlowski , Robert Foss , "Todor Tomov" , Philipp Zabel , "Laurent Pinchart" , Jernej Skrabec , Andrzej Pietrasiewicz , Hans Verkuil , Eugen Hristev CC: , , , , Subject: [PATCH v2 6/6] media: starfive: Add Starfive Camera Subsystem driver Date: Fri, 10 Mar 2023 20:05:53 +0800 Message-ID: <20230310120553.60586-7-jack.zhu@starfivetech.com> X-Mailer: git-send-email 2.34.1 In-Reply-To: <20230310120553.60586-1-jack.zhu@starfivetech.com> References: <20230310120553.60586-1-jack.zhu@starfivetech.com> MIME-Version: 1.0 X-Originating-IP: [180.164.60.184] X-ClientProxiedBy: EXCAS066.cuchost.com (172.16.6.26) To EXMBX073.cuchost.com (172.16.6.83) X-YovoleRuleAgent: yovoleflag Content-Transfer-Encoding: quoted-printable Precedence: bulk List-ID: X-Mailing-List: linux-kernel@vger.kernel.org Content-Type: text/plain; charset="utf-8" Add the driver for Starfive Camera Subsystem found on Starfive JH7110 SoC. It is used for handing image sensor data. Signed-off-by: Jack Zhu --- drivers/media/platform/Kconfig | 1 + drivers/media/platform/Makefile | 1 + drivers/media/platform/starfive/Kconfig | 18 + drivers/media/platform/starfive/Makefile | 14 + drivers/media/platform/starfive/stf_camss.c | 728 +++++++++ drivers/media/platform/starfive/stf_camss.h | 104 ++ drivers/media/platform/starfive/stf_common.h | 149 ++ drivers/media/platform/starfive/stf_isp.c | 1079 ++++++++++++++ drivers/media/platform/starfive/stf_isp.h | 183 +++ .../media/platform/starfive/stf_isp_hw_ops.c | 1286 ++++++++++++++++ drivers/media/platform/starfive/stf_video.c | 1286 ++++++++++++++++ drivers/media/platform/starfive/stf_video.h | 89 ++ drivers/media/platform/starfive/stf_vin.c | 1314 +++++++++++++++++ drivers/media/platform/starfive/stf_vin.h | 194 +++ .../media/platform/starfive/stf_vin_hw_ops.c | 357 +++++ include/uapi/linux/stf_isp_ioctl.h | 127 ++ 16 files changed, 6930 insertions(+) create mode 100644 drivers/media/platform/starfive/Kconfig create mode 100644 drivers/media/platform/starfive/Makefile create mode 100644 drivers/media/platform/starfive/stf_camss.c create mode 100644 drivers/media/platform/starfive/stf_camss.h create mode 100644 drivers/media/platform/starfive/stf_common.h create mode 100644 drivers/media/platform/starfive/stf_isp.c create mode 100644 drivers/media/platform/starfive/stf_isp.h create mode 100644 drivers/media/platform/starfive/stf_isp_hw_ops.c create mode 100644 drivers/media/platform/starfive/stf_video.c create mode 100644 drivers/media/platform/starfive/stf_video.h create mode 100644 drivers/media/platform/starfive/stf_vin.c create mode 100644 drivers/media/platform/starfive/stf_vin.h create mode 100644 drivers/media/platform/starfive/stf_vin_hw_ops.c create mode 100644 include/uapi/linux/stf_isp_ioctl.h diff --git a/drivers/media/platform/Kconfig b/drivers/media/platform/Kconfig index ee579916f874..627eaa0ab3ee 100644 --- a/drivers/media/platform/Kconfig +++ b/drivers/media/platform/Kconfig @@ -80,6 +80,7 @@ source "drivers/media/platform/renesas/Kconfig" 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" diff --git a/drivers/media/platform/Makefile b/drivers/media/platform/Makef= ile index 5453bb868e67..5a038498a370 100644 --- a/drivers/media/platform/Makefile +++ b/drivers/media/platform/Makefile @@ -23,6 +23,7 @@ obj-y +=3D renesas/ obj-y +=3D rockchip/ obj-y +=3D samsung/ obj-y +=3D st/ +obj-y +=3D starfive/ obj-y +=3D sunxi/ obj-y +=3D ti/ obj-y +=3D verisilicon/ diff --git a/drivers/media/platform/starfive/Kconfig b/drivers/media/platfo= rm/starfive/Kconfig new file mode 100644 index 000000000000..4bb1e25b66ca --- /dev/null +++ b/drivers/media/platform/starfive/Kconfig @@ -0,0 +1,18 @@ +# 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. diff --git a/drivers/media/platform/starfive/Makefile b/drivers/media/platf= orm/starfive/Makefile new file mode 100644 index 000000000000..bf497dd24bbb --- /dev/null +++ b/drivers/media/platform/starfive/Makefile @@ -0,0 +1,14 @@ +# SPDX-License-Identifier: GPL-2.0 +# +# Makefile for StarFive camera subsystem driver. +# + +starfive-camss-objs +=3D \ + 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) +=3D starfive-camss.o \ diff --git a/drivers/media/platform/starfive/stf_camss.c b/drivers/media/pl= atform/starfive/stf_camss.c new file mode 100644 index 000000000000..525f2d80c5eb --- /dev/null +++ b/drivers/media/platform/starfive/stf_camss.c @@ -0,0 +1,728 @@ +// SPDX-License-Identifier: GPL-2.0 +/* + * stf_camss.c + * + * Starfive Camera Subsystem driver + * + * Copyright (C) 2021-2023 StarFive Technology Co., Ltd. + */ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "stf_camss.h" + +static struct clk_bulk_data stfcamss_clocks[] =3D { + { .id =3D "clk_apb_func" }, + { .id =3D "clk_wrapper_clk_c" }, + { .id =3D "clk_dvp_inv" }, + { .id =3D "clk_axiwr" }, + { .id =3D "clk_mipi_rx0_pxl" }, + { .id =3D "clk_ispcore_2x" }, + { .id =3D "clk_isp_axi" }, +}; + +static struct reset_control_bulk_data stfcamss_resets[] =3D { + { .id =3D "rst_wrapper_p" }, + { .id =3D "rst_wrapper_c" }, + { .id =3D "rst_axird" }, + { .id =3D "rst_axiwr" }, + { .id =3D "rst_isp_top_n" }, + { .id =3D "rst_isp_top_axi" }, +}; + +int stfcamss_get_mem_res(struct platform_device *pdev, + struct stfcamss *stfcamss) +{ + stfcamss->syscon_base =3D + devm_platform_ioremap_resource_byname(pdev, "syscon"); + if (IS_ERR(stfcamss->syscon_base)) + return PTR_ERR(stfcamss->syscon_base); + + stfcamss->isp_base =3D + 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 se= nsor + * @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 =3D &entity->pads[0]; + if (!(pad->flags & MEDIA_PAD_FL_SINK)) + return NULL; + + pad =3D media_pad_remote_pad_first(pad); + if (!pad || !is_media_entity_v4l2_subdev(pad->entity)) + return NULL; + + entity =3D pad->entity; + + if (entity->function =3D=3D 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 =3D { { 0 } }; + + v4l2_fwnode_endpoint_parse(of_fwnode_handle(node), &vep); + st_debug(ST_CAMSS, "%s: vep.base.port =3D 0x%x, id =3D 0x%x\n", + __func__, vep.base.port, vep.base.id); + + csd->port =3D 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 =3D stfcamss->dev; + struct device_node *node =3D NULL; + struct device_node *remote =3D NULL; + int ret, num_subdevs =3D 0; + + for_each_endpoint_of_node(dev->of_node, node) { + struct stfcamss_async_subdev *csd; + + if (!of_device_is_available(node)) + continue; + + remote =3D of_graph_get_remote_port_parent(node); + if (!remote) { + st_err(ST_CAMSS, "Cannot get remote parent\n"); + ret =3D -EINVAL; + goto err_cleanup; + } + + csd =3D v4l2_async_nf_add_fwnode(&stfcamss->notifier, + of_fwnode_handle(remote), + struct stfcamss_async_subdev); + of_node_put(remote); + if (IS_ERR(csd)) { + ret =3D PTR_ERR(csd); + goto err_cleanup; + } + + ret =3D 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 =3D 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 =3D 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 =3D stfcamss->vin_dev; + struct stf_isp_dev *isp_dev =3D stfcamss->isp_dev; + + ret =3D 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 =3D 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 =3D 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 =3D 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 =3D 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 =3D 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 =3D + container_of(async, struct stfcamss, notifier); + struct host_data *host_data =3D v4l2_get_subdev_hostdata(sd); + struct media_entity *sensor; + struct media_entity *input; + int ret; + int i, j; + + for (i =3D 0; host_data->host_entity[i] && (i < HOST_ENTITY_MAX); i++) { + sensor =3D &sd->entity; + input =3D host_data->host_entity[i]; + + for (j =3D 0; j < sensor->num_pads; j++) { + if (sensor->pads[j].flags & MEDIA_PAD_FL_SOURCE) + break; + } + + if (j =3D=3D sensor->num_pads) { + st_err(ST_CAMSS, "No source pad in external entity\n"); + return -EINVAL; + } + + ret =3D media_create_pad_link( + sensor, + j, + input, + STF_PAD_SINK, + sensor->function =3D=3D 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 =3D 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 *asyn= c, + struct v4l2_subdev *subdev, + struct v4l2_async_subdev *asd) +{ + struct stfcamss *stfcamss =3D + container_of(async, struct stfcamss, notifier); + struct stfcamss_async_subdev *csd =3D + container_of(asd, struct stfcamss_async_subdev, asd); + enum port_num port =3D csd->port; + struct stf_isp_dev *isp_dev =3D stfcamss->isp_dev; + struct stf_vin_dev *vin_dev =3D stfcamss->vin_dev; + struct host_data *host_data =3D &stfcamss->host_data; + + switch (port) { + case PORT_NUMBER_DVP_SENSOR: + host_data->host_entity[0] =3D NULL; + host_data->host_entity[1] =3D NULL; + /* not support DVP sensor */ + break; + case PORT_NUMBER_CSI2RX: + host_data->host_entity[0] =3D + &vin_dev->line[VIN_LINE_WR].subdev.entity; + host_data->host_entity[1] =3D &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 =3D { + .bound =3D stfcamss_subdev_notifier_bound, +}; + +static const struct media_device_ops stfcamss_media_ops =3D { + .link_notify =3D 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 =3D &pdev->dev; + int ret =3D 0, num_subdevs; + + stfcamss =3D devm_kzalloc(dev, sizeof(struct stfcamss), GFP_KERNEL); + if (!stfcamss) + return -ENOMEM; + + stfcamss->isp_dev =3D devm_kzalloc(dev, sizeof(*stfcamss->isp_dev), + GFP_KERNEL); + if (!stfcamss->isp_dev) { + ret =3D -ENOMEM; + goto err_cam; + } + + stfcamss->vin_dev =3D devm_kzalloc(dev, sizeof(*stfcamss->vin_dev), + GFP_KERNEL); + if (!stfcamss->vin_dev) { + ret =3D -ENOMEM; + goto err_cam; + } + + stfcamss->irq =3D platform_get_irq(pdev, 0); + if (stfcamss->irq <=3D 0) { + st_err(ST_CAMSS, "Could not get irq\n"); + goto err_cam; + } + + stfcamss->isp_irq =3D platform_get_irq(pdev, 1); + if (stfcamss->isp_irq <=3D 0) { + st_err(ST_CAMSS, "Could not get isp irq\n"); + goto err_cam; + } + + stfcamss->isp_irq_csi =3D platform_get_irq(pdev, 2); + if (stfcamss->isp_irq_csi <=3D 0) { + st_err(ST_CAMSS, "Could not get isp csi irq\n"); + goto err_cam; + } + + stfcamss->isp_irq_csiline =3D platform_get_irq(pdev, 3); + if (stfcamss->isp_irq_csiline <=3D 0) { + st_err(ST_CAMSS, "Could not get isp irq csiline\n"); + goto err_cam; + } + + pm_runtime_enable(dev); + + stfcamss->nclks =3D ARRAY_SIZE(stfcamss_clocks); + stfcamss->sys_clk =3D stfcamss_clocks; + + ret =3D 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 =3D ARRAY_SIZE(stfcamss_resets); + stfcamss->sys_rst =3D stfcamss_resets; + + ret =3D 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 =3D stfcamss_get_mem_res(pdev, stfcamss); + if (ret) { + st_err(ST_CAMSS, "Could not map registers\n"); + goto err_cam; + } + + stfcamss->dev =3D dev; + platform_set_drvdata(pdev, stfcamss); + + v4l2_async_nf_init(&stfcamss->notifier); + + num_subdevs =3D stfcamss_of_parse_ports(stfcamss); + if (num_subdevs < 0) { + ret =3D num_subdevs; + goto err_cam_noti; + } + + ret =3D 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 =3D 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 =3D 0x01; + stfcamss->media_dev.ops =3D &stfcamss_media_ops; + media_device_init(&stfcamss->media_dev); + + stfcamss->v4l2_dev.mdev =3D &stfcamss->media_dev; + + ret =3D 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 =3D 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 =3D &stfcamss_subdev_notifier_ops; + ret =3D 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 =3D 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 =3D 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 =3D 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[] =3D { + { .compatible =3D "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 =3D dev_get_drvdata(dev); + struct stf_vin_dev *vin_dev =3D 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 =3D 0; + + for (i =3D 0; i < VIN_LINE_MAX; i++) { + if (vin_dev->line[i].stream_count) { + vin_dev->line[i].stream_count++; + video =3D &vin_dev->line[i].video_out; + vdev =3D &vin_dev->line[i].video_out.vdev; + entity =3D &vdev->entity; + while (1) { + pad =3D &entity->pads[0]; + if (!(pad->flags & MEDIA_PAD_FL_SINK)) + break; + + pad =3D media_pad_remote_pad_first(pad); + if (!pad || + !is_media_entity_v4l2_subdev(pad->entity)) + break; + + entity =3D pad->entity; + subdev =3D 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 =3D dev_get_drvdata(dev); + struct stf_vin_dev *vin_dev =3D 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 =3D 0; + int ret =3D 0; + + pm_runtime_force_resume(dev); + + for (i =3D 0; i < VIN_LINE_MAX; i++) { + if (vin_dev->line[i].stream_count) { + vin_dev->line[i].stream_count--; + video =3D &vin_dev->line[i].video_out; + vdev =3D &vin_dev->line[i].video_out.vdev; + + ret =3D v4l2_pipeline_pm_get(&vdev->entity); + if (ret < 0) + goto err; + + ret =3D video_device_pipeline_start( + vdev, &video->stfcamss->pipe); + if (ret < 0) + goto err_pm_put; + + entity =3D &vdev->entity; + while (1) { + pad =3D &entity->pads[0]; + if (!(pad->flags & MEDIA_PAD_FL_SINK)) + break; + + pad =3D media_pad_remote_pad_first(pad); + if (!pad || + !is_media_entity_v4l2_subdev(pad->entity)) + break; + + entity =3D pad->entity; + subdev =3D media_entity_to_v4l2_subdev(entity); + + ret =3D v4l2_subdev_call(subdev, video, + s_stream, 1); + if (ret < 0 && ret !=3D -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 =3D 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 =3D 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 =3D { + 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 =3D { + .probe =3D stfcamss_probe, + .remove =3D stfcamss_remove, + .driver =3D { + .name =3D DRV_NAME, + .pm =3D &stfcamss_pm_ops, + .of_match_table =3D of_match_ptr(stfcamss_of_match), + }, +}; + +module_platform_driver(stfcamss_driver); + +MODULE_AUTHOR("StarFive Corporation"); +MODULE_DESCRIPTION("StarFive Camera Subsystem driver"); +MODULE_LICENSE("GPL"); diff --git a/drivers/media/platform/starfive/stf_camss.h b/drivers/media/pl= atform/starfive/stf_camss.h new file mode 100644 index 000000000000..a8c6579d62ed --- /dev/null +++ b/drivers/media/platform/starfive/stf_camss.h @@ -0,0 +1,104 @@ +/* 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 +#include +#include + +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 =3D 0, + PORT_NUMBER_CSI2RX +}; + +enum stf_clk { + STF_CLK_APB_FUNC =3D 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 =3D 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 */ diff --git a/drivers/media/platform/starfive/stf_common.h b/drivers/media/p= latform/starfive/stf_common.h new file mode 100644 index 000000000000..90f5e4c3061c --- /dev/null +++ b/drivers/media/platform/starfive/stf_common.h @@ -0,0 +1,149 @@ +/* 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 +#include + +enum { + ST_DVP =3D 0x0001, + ST_ISP =3D 0x0002, + ST_VIN =3D 0x0004, + ST_VIDEO =3D 0x0008, + ST_CAMSS =3D 0x0010, + ST_SENSOR =3D 0x0020, +}; + +enum { + ST_NONE =3D 0x00, + ST_ERR =3D 0x01, + ST_WARN =3D 0x02, + ST_INFO =3D 0x03, + ST_DEBUG =3D 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 =3D "st_dvp"; \ + break; \ + case ST_ISP: \ + __str =3D "st_isp"; \ + break; \ + case ST_VIN: \ + __str =3D "st_vin"; \ + break; \ + case ST_VIDEO: \ + __str =3D "st_video"; \ + break; \ + case ST_CAMSS: \ + __str =3D "st_camss"; \ + break; \ + case ST_SENSOR: \ + __str =3D "st_sensor"; \ + break; \ + default: \ + __str =3D "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 =3D ioread32(base + reg) & ~mask; + val &=3D mask; + val |=3D 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 */ diff --git a/drivers/media/platform/starfive/stf_isp.c b/drivers/media/plat= form/starfive/stf_isp.c new file mode 100644 index 000000000000..0c67d1d06e03 --- /dev/null +++ b/drivers/media/platform/starfive/stf_isp.c @@ -0,0 +1,1079 @@ +// SPDX-License-Identifier: GPL-2.0 +/* + * stf_isp.c + * + * StarFive Camera Subsystem - ISP Module + * + * Copyright (C) 2021-2023 StarFive Technology Co., Ltd. + */ +#include +#include +#include + +#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[] =3D { + { 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[] =3D { + { 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[] =3D { + { 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[] =3D { + { 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[] =3D { + { MEDIA_BUS_FMT_Y12_1X12, 8 }, +}; + +static const struct isp_format_table isp_formats_st7110[] =3D { + { 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 =3D stfcamss->isp_dev; + struct device *dev =3D stfcamss->dev; + + isp_dev->sdev_type =3D SUBDEV_TYPE_ISP; + isp_dev->hw_ops =3D &isp_ops; + isp_dev->stfcamss =3D stfcamss; + isp_dev->formats =3D isp_formats_st7110; + isp_dev->nformats =3D ARRAY_SIZE(isp_formats_st7110); + isp_dev->reg_buf =3D 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 =3D v4l2_get_subdevdata(sd); + + mutex_lock(&isp_dev->power_lock); + if (on) { + if (isp_dev->power_count =3D=3D 0) + st_debug(ST_ISP, "turn on isp\n"); + isp_dev->power_count++; + } else { + if (isp_dev->power_count =3D=3D 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 =3D=3D 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 =3D &entity->pads[0]; + + if (!(pad->flags & MEDIA_PAD_FL_SINK)) + return -EINVAL; + + pad =3D media_pad_remote_pad_first(pad); + if (!pad || !is_media_entity_v4l2_subdev(pad->entity)) + return -EINVAL; + + subdev =3D 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 =3D v4l2_get_subdevdata(sd); + int ret =3D 0, interface_type; + struct v4l2_mbus_framefmt *fmt; + struct v4l2_event src_ch =3D { 0 }; + + fmt =3D __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 =3D=3D 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 =3D 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 =3D 0; + } + isp_dev->stream_count++; + } else { + if (isp_dev->stream_count =3D=3D 0) + goto exit; + if (isp_dev->stream_count =3D=3D 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 =3D V4L2_EVENT_SOURCE_CHANGE, + src_ch.u.src_change.changes =3D 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 =3D stfcamss_find_sensor(&isp_dev->subdev.entity); + if (!sensor) + return -EINVAL; + + subdev =3D media_entity_to_v4l2_subdev(sensor); + st_debug(ST_ISP, "Found sensor =3D %s\n", sensor->name); + + fmt.pad =3D 0; + fmt.which =3D V4L2_SUBDEV_FORMAT_ACTIVE; + ret =3D 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 =3D &isp_dev->formats[SINK_FORMATS_INDEX]; + for (idx =3D 0; idx < formats->nfmts; idx++) { + if (formats->fmts[idx].code =3D=3D 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 =3D 0; i < f_table->nfmts; i++) { + if (mbus_code =3D=3D f_table->fmts[i].code) + break; + + if (pad =3D=3D STF_ISP_PAD_SRC_RAW) { + if (mbus_code =3D=3D isp_formats_compat_raw10[i].code || + mbus_code =3D=3D 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 =3D fmt->code; + u32 bpp; + + switch (pad) { + case STF_ISP_PAD_SINK: + /* Set format on sink pad */ + formats =3D &isp_dev->formats[SINK_FORMATS_INDEX]; + fmt->width =3D clamp_t(u32, + fmt->width, STFCAMSS_FRAME_MIN_WIDTH, + STFCAMSS_FRAME_MAX_WIDTH); + fmt->height =3D clamp_t(u32, + fmt->height, STFCAMSS_FRAME_MIN_HEIGHT, + STFCAMSS_FRAME_MAX_HEIGHT); + fmt->height &=3D ~0x1; + fmt->field =3D V4L2_FIELD_NONE; + fmt->colorspace =3D V4L2_COLORSPACE_SRGB; + fmt->flags =3D 0; + break; + + case STF_ISP_PAD_SRC: + case STF_ISP_PAD_SRC_SS0: + case STF_ISP_PAD_SRC_SS1: + formats =3D &isp_dev->formats[UO_FORMATS_INDEX]; + break; + + case STF_ISP_PAD_SRC_RAW: + formats =3D &isp_dev->formats[RAW_FORMATS_INDEX]; + break; + } + + i =3D isp_fmt_to_index(formats, fmt->code, pad); + st_debug(ST_ISP, "%s pad=3D%d, code=3D%x format to index =3D %d\n", + __func__, pad, code, i); + + if (i >=3D formats->nfmts && pad =3D=3D STF_ISP_PAD_SRC_RAW) { + int sensor_idx; + + sensor_idx =3D isp_sensor_fmt_to_index(isp_dev); + if (sensor_idx) + i =3D sensor_idx; + } + + if (pad !=3D STF_ISP_PAD_SINK) + *fmt =3D *__isp_get_format(isp_dev, state, + STF_ISP_PAD_SINK, which); + + if (i >=3D formats->nfmts) { + fmt->code =3D formats->fmts[0].code; + bpp =3D formats->fmts[0].bpp; + st_info(ST_ISP, "Use default index 0 format =3D 0x%x\n", + fmt->code); + } else { + if (pad =3D=3D STF_ISP_PAD_SRC_RAW) { + fmt->code =3D formats->fmts[i].code; + bpp =3D formats->fmts[i].bpp; + st_info(ST_ISP, + "use format from sink index %d =3D 0x%x\n", + i, fmt->code); + } else { + fmt->code =3D code; + bpp =3D formats->fmts[i].bpp; + st_info(ST_ISP, "Use input format =3D 0x%x\n", fmt->code); + } + } + + switch (pad) { + case STF_ISP_PAD_SINK: + break; + + case STF_ISP_PAD_SRC: + isp_dev->rect[ISP_COMPOSE].bpp =3D bpp; + break; + + case STF_ISP_PAD_SRC_SS0: + isp_dev->rect[ISP_SCALE_SS0].bpp =3D bpp; + break; + + case STF_ISP_PAD_SRC_SS1: + isp_dev->rect[ISP_SCALE_SS1].bpp =3D bpp; + break; + + case STF_ISP_PAD_SRC_RAW: + isp_dev->rect[ISP_CROP].bpp =3D 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 =3D v4l2_get_subdevdata(sd); + const struct isp_format_table *formats; + + if (code->index >=3D isp_dev->nformats) + return -EINVAL; + if (code->pad =3D=3D STF_ISP_PAD_SINK) { + formats =3D &isp_dev->formats[SINK_FORMATS_INDEX]; + code->code =3D formats->fmts[code->index].code; + } else { + struct v4l2_mbus_framefmt *sink_fmt; + + sink_fmt =3D __isp_get_format(isp_dev, state, STF_ISP_PAD_SINK, + code->which); + + code->code =3D sink_fmt->code; + if (!code->code) + return -EINVAL; + } + code->flags =3D 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 =3D v4l2_get_subdevdata(sd); + struct v4l2_mbus_framefmt format; + + if (fse->index !=3D 0) + return -EINVAL; + + format.code =3D fse->code; + format.width =3D 1; + format.height =3D 1; + isp_try_format(isp_dev, state, fse->pad, &format, fse->which); + fse->min_width =3D format.width; + fse->min_height =3D format.height; + + if (format.code !=3D fse->code) + return -EINVAL; + + format.code =3D fse->code; + format.width =3D -1; + format.height =3D -1; + isp_try_format(isp_dev, state, fse->pad, &format, fse->which); + fse->max_width =3D format.width; + fse->max_height =3D 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 =3D v4l2_get_subdevdata(sd); + struct v4l2_mbus_framefmt *format; + + format =3D __isp_get_format(isp_dev, state, fmt->pad, fmt->which); + if (!format) + return -EINVAL; + + fmt->format =3D *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 =3D v4l2_get_subdevdata(sd); + struct v4l2_mbus_framefmt *format; + struct v4l2_subdev_selection sel =3D { 0 }; + struct v4l2_rect *rect =3D NULL; + int ret; + + st_debug(ST_ISP, "%s pad=3D%d, code=3D%x, which=3D%d\n", + __func__, fmt->reserved[0], fmt->format.code, fmt->which); + format =3D __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 =3D *format; + if (fmt->reserved[0] !=3D 0) { + sel.which =3D fmt->which; + sel.pad =3D fmt->reserved[0]; + + switch (fmt->reserved[0]) { + case STF_ISP_PAD_SRC: + rect =3D __isp_get_compose(isp_dev, + state, fmt->which); + break; + + case STF_ISP_PAD_SRC_SS0: + case STF_ISP_PAD_SRC_SS1: + rect =3D __isp_get_scale(isp_dev, state, &sel); + break; + + case STF_ISP_PAD_SRC_RAW: + rect =3D __isp_get_crop(isp_dev, + state, fmt->which); + break; + + default: + break; + } + if (rect) { + fmt->format.width =3D rect->width; + fmt->format.height =3D rect->height; + } + } + mutex_unlock(&isp_dev->stream_lock); + goto out; + } else { + isp_try_format(isp_dev, state, fmt->pad, + &fmt->format, fmt->which); + *format =3D fmt->format; + } + mutex_unlock(&isp_dev->stream_lock); + + /* Propagate the format from sink to source */ + if (fmt->pad =3D=3D STF_ISP_PAD_SINK) { + /* Reset sink pad compose selection */ + sel.which =3D fmt->which; + sel.pad =3D STF_ISP_PAD_SINK; + sel.target =3D V4L2_SEL_TGT_CROP; + sel.r.width =3D fmt->format.width; + sel.r.height =3D fmt->format.height; + ret =3D 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 =3D=3D 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 =3D=3D 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 =3D=3D V4L2_SUBDEV_FORMAT_TRY) + return v4l2_subdev_get_try_compose(&isp_dev->subdev, state, + STF_ISP_PAD_SINK); + if (sel->pad !=3D STF_ISP_PAD_SRC_SS0 && sel->pad !=3D STF_ISP_PAD_SRC_SS= 1) + return NULL; + + pad =3D sel->pad =3D=3D STF_ISP_PAD_SRC_SS0 ? ISP_SCALE_SS0 : ISP_SCALE_S= S1; + 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 =3D __isp_get_format(isp_dev, state, STF_ISP_PAD_SINK, which); + + if (rect->width > fmt->width) + rect->width =3D fmt->width; + + if (rect->width + rect->left > fmt->width) + rect->left =3D fmt->width - rect->width; + + if (rect->height > fmt->height) + rect->height =3D fmt->height; + + if (rect->height + rect->top > fmt->height) + rect->top =3D fmt->height - rect->height; + + if (rect->width < STFCAMSS_FRAME_MIN_WIDTH) { + rect->left =3D 0; + rect->width =3D STFCAMSS_FRAME_MIN_WIDTH; + } + + if (rect->height < STFCAMSS_FRAME_MIN_HEIGHT) { + rect->top =3D 0; + rect->height =3D STFCAMSS_FRAME_MIN_HEIGHT; + } + rect->height &=3D ~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 =3D __isp_get_crop(isp_dev, state, which); + + if (rect->width > crop->width) + rect->width =3D crop->width; + + if (rect->height > crop->height) + rect->height =3D crop->height; + + if (crop->width > rect->width * SCALER_RATIO_MAX) + rect->width =3D + (crop->width + SCALER_RATIO_MAX - 1) / SCALER_RATIO_MAX; + + if (crop->height > rect->height * SCALER_RATIO_MAX) + rect->height =3D + (crop->height + SCALER_RATIO_MAX - 1) / SCALER_RATIO_MAX; + + if (rect->width < STFCAMSS_FRAME_MIN_WIDTH) + rect->width =3D STFCAMSS_FRAME_MIN_WIDTH; + + if (rect->height < STFCAMSS_FRAME_MIN_HEIGHT) + rect->height =3D STFCAMSS_FRAME_MIN_HEIGHT; + rect->height &=3D ~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 =3D __isp_get_compose(isp_dev, state, which); + + if (rect->width > compose->width) + rect->width =3D compose->width; + + if (rect->width + rect->left > compose->width) + rect->left =3D compose->width - rect->width; + + if (rect->height > compose->height) + rect->height =3D compose->height; + + if (rect->height + rect->top > compose->height) + rect->top =3D compose->height - rect->height; + + if (rect->width < STFCAMSS_FRAME_MIN_WIDTH) { + rect->left =3D 0; + rect->width =3D STFCAMSS_FRAME_MIN_WIDTH; + } + + if (rect->height < STFCAMSS_FRAME_MIN_HEIGHT) { + rect->top =3D 0; + rect->height =3D STFCAMSS_FRAME_MIN_HEIGHT; + } + + rect->height &=3D ~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 =3D v4l2_get_subdevdata(sd); + struct v4l2_subdev_format fmt =3D { 0 }; + struct v4l2_rect *rect; + int ret; + + switch (sel->target) { + case V4L2_SEL_TGT_CROP_BOUNDS: + case V4L2_SEL_TGT_CROP_DEFAULT: + fmt.pad =3D sel->pad; + fmt.which =3D sel->which; + ret =3D isp_get_format(sd, state, &fmt); + if (ret < 0) + return ret; + + sel->r.left =3D 0; + sel->r.top =3D 0; + sel->r.width =3D fmt.format.width; + sel->r.height =3D fmt.format.height; + break; + + case V4L2_SEL_TGT_CROP: + rect =3D __isp_get_crop(isp_dev, state, sel->which); + if (!rect) + return -EINVAL; + + sel->r =3D *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 =3D __isp_get_crop(isp_dev, state, sel->which); + if (!rect) + return -EINVAL; + + sel->r.left =3D rect->left; + sel->r.top =3D rect->top; + sel->r.width =3D rect->width; + sel->r.height =3D rect->height; + break; + + case V4L2_SEL_TGT_COMPOSE: + if (sel->pad > STF_ISP_PAD_SRC_SS1) + return -EINVAL; + if (sel->pad =3D=3D STF_ISP_PAD_SRC_SS0 || + sel->pad =3D=3D STF_ISP_PAD_SRC_SS1) { + rect =3D __isp_get_scale(isp_dev, state, sel); + if (!rect) + return -EINVAL; + } else { + rect =3D __isp_get_compose(isp_dev, state, sel->which); + if (!rect) + return -EINVAL; + } + sel->r =3D *rect; + break; + + default: + return -EINVAL; + } + + st_info(ST_ISP, "%s pad =3D %d, left =3D %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 =3D v4l2_get_subdevdata(sd); + struct v4l2_rect *rect; + int ret =3D 0; + + if (sel->target =3D=3D V4L2_SEL_TGT_COMPOSE && + (sel->pad =3D=3D STF_ISP_PAD_SINK || sel->pad =3D=3D STF_ISP_PAD_SRC)= ) { + struct v4l2_subdev_format fmt =3D { 0 }; + int i; + + rect =3D __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 =3D *rect; + mutex_unlock(&isp_dev->stream_lock); + ret =3D 0; + goto out; + } else { + isp_try_compose(isp_dev, state, &sel->r, sel->which); + *rect =3D sel->r; + } + mutex_unlock(&isp_dev->stream_lock); + + /* Reset source pad format width and height */ + fmt.which =3D sel->which; + fmt.pad =3D STF_ISP_PAD_SRC; + ret =3D isp_get_format(sd, state, &fmt); + if (ret < 0) + return ret; + + fmt.format.width =3D rect->width; + fmt.format.height =3D rect->height; + ret =3D isp_set_format(sd, state, &fmt); + + /* Reset scale */ + for (i =3D STF_ISP_PAD_SRC_SS0; i <=3D STF_ISP_PAD_SRC_SS1; i++) { + struct v4l2_subdev_selection scale =3D { 0 }; + + scale.which =3D sel->which; + scale.target =3D V4L2_SEL_TGT_COMPOSE; + scale.r =3D *rect; + scale.pad =3D i; + ret =3D isp_set_selection(sd, state, &scale); + } + } else if (sel->target =3D=3D V4L2_SEL_TGT_COMPOSE && + ((sel->pad =3D=3D STF_ISP_PAD_SRC_SS0) || + (sel->pad =3D=3D STF_ISP_PAD_SRC_SS1))) { + struct v4l2_subdev_format fmt =3D { 0 }; + + rect =3D __isp_get_scale(isp_dev, state, sel); + if (!rect) + return -EINVAL; + + mutex_lock(&isp_dev->stream_lock); + if (isp_dev->stream_count) { + sel->r =3D *rect; + mutex_unlock(&isp_dev->stream_lock); + ret =3D 0; + goto out; + } else { + isp_try_scale(isp_dev, state, &sel->r, sel->which); + *rect =3D sel->r; + } + mutex_unlock(&isp_dev->stream_lock); + + /* Reset source pad format width and height */ + fmt.which =3D sel->which; + fmt.pad =3D sel->pad; + ret =3D isp_get_format(sd, state, &fmt); + if (ret < 0) + return ret; + + fmt.format.width =3D rect->width; + fmt.format.height =3D rect->height; + ret =3D isp_set_format(sd, state, &fmt); + } else if (sel->target =3D=3D V4L2_SEL_TGT_CROP) { + struct v4l2_subdev_selection compose =3D { 0 }; + int i; + + rect =3D __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 =3D *rect; + mutex_unlock(&isp_dev->stream_lock); + ret =3D 0; + goto out; + } else { + isp_try_crop(isp_dev, state, &sel->r, sel->which); + *rect =3D sel->r; + } + mutex_unlock(&isp_dev->stream_lock); + + /* Reset source compose selection */ + compose.which =3D sel->which; + compose.target =3D V4L2_SEL_TGT_COMPOSE; + compose.r.width =3D rect->width; + compose.r.height =3D rect->height; + compose.pad =3D STF_ISP_PAD_SINK; + ret =3D isp_set_selection(sd, state, &compose); + + /* Reset source pad format width and height */ + for (i =3D STF_ISP_PAD_SRC_RAW; i < STF_ISP_PAD_MAX; i++) { + struct v4l2_subdev_format fmt =3D { 0 }; + + fmt.which =3D sel->which; + fmt.pad =3D i; + ret =3D isp_get_format(sd, state, &fmt); + if (ret < 0) + return ret; + + fmt.format.width =3D rect->width; + fmt.format.height =3D rect->height; + ret =3D isp_set_format(sd, state, &fmt); + } + } else { + ret =3D -EINVAL; + } + + st_info(ST_ISP, "%s pad =3D %d, left =3D %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 =3D { + .pad =3D STF_ISP_PAD_SINK, + .which =3D + fh ? V4L2_SUBDEV_FORMAT_TRY : V4L2_SUBDEV_FORMAT_ACTIVE, + .format =3D { + .code =3D MEDIA_BUS_FMT_RGB565_2X8_LE, + .width =3D 1920, + .height =3D 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_na= me) +{ + struct device *dev =3D isp_dev->stfcamss->dev; + const struct firmware *fw; + u8 *buf =3D NULL; + int *regval_num; + int ret; + + st_debug(ST_ISP, "%s, file_name %s\n", __func__, file_name); + ret =3D request_firmware(&fw, file_name, dev); + if (ret < 0) { + st_err(ST_ISP, "firmware request failed (%d)\n", ret); + return ret; + } + buf =3D 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 =3D=3D 1) + devm_kfree(dev, isp_dev->setfile.data); + isp_dev->setfile.data =3D buf; + isp_dev->setfile.size =3D fw->size; + isp_dev->setfile.state =3D 1; + regval_num =3D (int *)&buf[fw->size - sizeof(unsigned int)]; + isp_dev->setfile.settings.regval_num =3D *regval_num; + isp_dev->setfile.settings.regval =3D (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 =3D v4l2_get_subdevdata(sd); + struct stfisp_fw_info *fw_info =3D arg; + int ret =3D -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 =3D stf_isp_load_setfile(isp_dev, fw_info->filename); + break; + + case VIDIOC_STFISP_GET_REG: + ret =3D isp_dev->hw_ops->isp_reg_read(isp_dev, arg); + break; + + case VIDIOC_STFISP_SET_REG: + ret =3D 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 =3D 0; + else + ret =3D -EBUSY; + break; + + case VIDIOC_STFISP_SHADOW_UNLOCK: + if (atomic_dec_if_positive(&isp_dev->shadow_count) < 0) + ret =3D -EINVAL; + else + ret =3D 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 =3D -EINVAL; + else + ret =3D 0; + break; + + case VIDIOC_STFISP_SET_USER_CONFIG_ISP: + st_debug(ST_ISP, "Set user config isp\n"); + user_config_isp =3D 1; + break; + + default: + break; + } + return ret; +} + +int isp_close(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh) +{ + struct stf_isp_dev *isp_dev =3D 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 =3D { + .s_power =3D isp_set_power, + .ioctl =3D stf_isp_ioctl, + .subscribe_event =3D stf_isp_subscribe_event, + .unsubscribe_event =3D v4l2_event_subdev_unsubscribe, +}; + +static const struct v4l2_subdev_video_ops isp_video_ops =3D { + .s_stream =3D isp_set_stream, +}; + +static const struct v4l2_subdev_pad_ops isp_pad_ops =3D { + .enum_mbus_code =3D isp_enum_mbus_code, + .enum_frame_size =3D isp_enum_frame_size, + .get_fmt =3D isp_get_format, + .set_fmt =3D isp_set_format, + .get_selection =3D isp_get_selection, + .set_selection =3D isp_set_selection, +}; + +static const struct v4l2_subdev_ops isp_v4l2_ops =3D { + .core =3D &isp_core_ops, + .video =3D &isp_video_ops, + .pad =3D &isp_pad_ops, +}; + +static const struct v4l2_subdev_internal_ops isp_v4l2_internal_ops =3D { + .open =3D isp_init_formats, + .close =3D isp_close, +}; + +static const struct media_entity_operations isp_media_ops =3D { + .link_setup =3D isp_link_setup, + .link_validate =3D v4l2_subdev_link_validate, +}; + +int stf_isp_register(struct stf_isp_dev *isp_dev, struct v4l2_device *v4l2= _dev) +{ + struct v4l2_subdev *sd =3D &isp_dev->subdev; + struct media_pad *pads =3D isp_dev->pads; + int ret; + + v4l2_subdev_init(sd, &isp_v4l2_ops); + sd->internal_ops =3D &isp_v4l2_internal_ops; + sd->flags |=3D 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 =3D 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 =3D MEDIA_PAD_FL_SINK; + pads[STF_ISP_PAD_SRC].flags =3D MEDIA_PAD_FL_SOURCE; + pads[STF_ISP_PAD_SRC_SS0].flags =3D MEDIA_PAD_FL_SOURCE; + pads[STF_ISP_PAD_SRC_SS1].flags =3D MEDIA_PAD_FL_SOURCE; + pads[STF_ISP_PAD_SRC_RAW].flags =3D MEDIA_PAD_FL_SOURCE; + + sd->entity.function =3D MEDIA_ENT_F_PROC_VIDEO_PIXEL_FORMATTER; + sd->entity.ops =3D &isp_media_ops; + ret =3D 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 =3D 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; +} diff --git a/drivers/media/platform/starfive/stf_isp.h b/drivers/media/plat= form/starfive/stf_isp.h new file mode 100644 index 000000000000..ca80b8d1f1cc --- /dev/null +++ b/drivers/media/platform/starfive/stf_isp.h @@ -0,0 +1,183 @@ +/* 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 +#include + +#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 =3D -1, + STF_ISP_LINE_SRC =3D 1, + STF_ISP_LINE_SRC_SS0, + STF_ISP_LINE_SRC_SS1, + STF_ISP_LINE_SRC_RAW, + STF_ISP_LINE_MAX =3D STF_ISP_LINE_SRC_RAW +}; + +/* pad id for media framework */ +enum isp_pad_id { + STF_ISP_PAD_SINK =3D 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 =3D 0, + EN_INT_ISP_DONE =3D (0x1 << 24), + EN_INT_CSI_DONE =3D (0x1 << 25), + EN_INT_SC_DONE =3D (0x1 << 26), + EN_INT_LINE_INT =3D (0x1 << 27), + EN_INT_ALL =3D (0xF << 24), +}; + +enum { + INTERFACE_DVP =3D 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 =3D 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 */ diff --git a/drivers/media/platform/starfive/stf_isp_hw_ops.c b/drivers/med= ia/platform/starfive/stf_isp_hw_ops.c new file mode 100644 index 000000000000..b2aee00a78ad --- /dev/null +++ b/drivers/media/platform/starfive/stf_isp_hw_ops.c @@ -0,0 +1,1286 @@ +// 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 +#include + +#include "stf_camss.h" + +static const struct regval_t isp_reg_init_config_list[] =3D { + /* 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[] =3D { + {isp_reg_init_config_list, ARRAY_SIZE(isp_reg_init_config_list)}, +}; + +static const struct regval_t isp_reg_start_config_list[] =3D { + /* 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[] =3D { + {isp_reg_start_config_list, ARRAY_SIZE(isp_reg_start_config_list)}, +}; + +static struct regval_t isp_format_reg_list[] =3D { + {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[] =3D { + {isp_format_reg_list, ARRAY_SIZE(isp_format_reg_list)}, +}; + +static void isp_load_regs(void __iomem *ispbase, const struct reg_table *t= able) +{ + int j; + u32 delay_ms, reg_addr, mask, val; + + for (j =3D 0; j < table->regval_num; j++) { + delay_ms =3D table->regval[j].delay_ms; + reg_addr =3D table->regval[j].addr; + val =3D table->regval[j].val; + mask =3D 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 =3D 0; j < table->regval_num; j++) { + delay_ms =3D table->regval[j].delay_ms; + reg_addr =3D table->regval[j].addr; + val =3D table->regval[j].val; + mask =3D table->regval[j].mask; + + if (reg_addr % 4 || + reg_addr > STF_ISP_REG_OFFSET_MAX || + delay_ms > STF_ISP_REG_DELAY_MAX || + (reg_addr =3D=3D ISP_REG_CSI_INPUT_EN_AND_STATUS && (val & 0x01)) || + (reg_addr =3D=3D 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 =3D 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 =3D 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 =3D isp_dev->stfcamss->isp_base; + + return base; +} + +static int stf_isp_reset(struct stf_isp_dev *isp_dev) +{ + void __iomem *ispbase =3D 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 =3D 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 =3D &crop_array[ISP_COMPOSE].rect; + u32 bpp =3D crop_array[ISP_COMPOSE].bpp; + void __iomem *ispbase =3D stf_isp_get_ispbase(isp_dev); + u32 val, val1; + + st_debug(ST_ISP, "interface type is %d(%s)\n", + type, type =3D=3D INTERFACE_CSI ? "CSI" : "DVP"); + + val =3D crop->left + (crop->top << 16); + isp_format_reg_list[0].addr =3D ISP_REG_PIC_CAPTURE_START_CFG; + isp_format_reg_list[0].val =3D val; + + val =3D (crop->width + crop->left - 1) + + ((crop->height + crop->top - 1) << 16); + isp_format_reg_list[1].addr =3D ISP_REG_PIC_CAPTURE_END_CFG; + isp_format_reg_list[1].val =3D val; + + val =3D crop->width + (crop->height << 16); + isp_format_reg_list[2].addr =3D ISP_REG_PIPELINE_XY_SIZE; + isp_format_reg_list[2].val =3D val; + + isp_format_reg_list[3].addr =3D ISP_REG_STRIDE; + isp_format_reg_list[3].val =3D 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 =3D 0x0000EE44; + val1 =3D 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 =3D 0x0000BB11; + val1 =3D 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 =3D 0x000044EE; + val1 =3D 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 =3D 0x000011BB; + val1 =3D 0x10000000; + break; + default: + st_err(ST_ISP, "UNKNOWN format\n"); + val =3D 0x000011BB; + val1 =3D 0x10000000; + break; + } + + isp_format_reg_list[4].addr =3D ISP_REG_RAW_FORMAT_CFG; + isp_format_reg_list[4].val =3D val; + + isp_format_reg_list[5].addr =3D ISP_REG_ISP_CTRL_1; + isp_format_reg_list[5].val =3D val1; + isp_format_reg_list[5].mask =3D 0xF0000000; + + st_info(ST_ISP, + "src left: %d, top: %d, width =3D %d, height =3D %d, bpp =3D %d\n", + crop->left, crop->top, crop->width, crop->height, bpp); + + crop =3D &crop_array[ISP_CROP].rect; + bpp =3D crop_array[ISP_CROP].bpp; + val =3D ALIGN(crop->width * bpp / 8, STFCAMSS_FRAME_WIDTH_ALIGN_128); + isp_format_reg_list[6].addr =3D ISP_REG_DUMP_CFG_1; + isp_format_reg_list[6].val =3D val | 3 << 16; + isp_format_reg_list[6].mask =3D 0x0003FFFF; + + st_info(ST_ISP, + "raw left: %d, top: %d, width =3D %d, height =3D %d, bpp =3D %d\n", + crop->left, crop->top, crop->width, crop->height, bpp); + + crop =3D &crop_array[ISP_SCALE_SS0].rect; + bpp =3D crop_array[ISP_SCALE_SS0].bpp; + isp_format_reg_list[7].addr =3D ISP_REG_SS0IW; + isp_format_reg_list[7].val =3D (crop->width << 16) + crop->height; + isp_format_reg_list[8].addr =3D ISP_REG_SS0S; + isp_format_reg_list[8].val =3D + ALIGN(crop->width * bpp / 8, STFCAMSS_FRAME_WIDTH_ALIGN_8); + + st_info(ST_ISP, + "ss0 left: %d, top: %d, width =3D %d, height =3D %d, bpp =3D %d\n", + crop->left, crop->top, crop->width, crop->height, bpp); + + crop =3D &crop_array[ISP_SCALE_SS1].rect; + bpp =3D crop_array[ISP_SCALE_SS1].bpp; + isp_format_reg_list[9].addr =3D ISP_REG_SS1IW; + isp_format_reg_list[9].val =3D (crop->width << 16) + crop->height; + isp_format_reg_list[10].addr =3D ISP_REG_SS1S; + isp_format_reg_list[10].val =3D + ALIGN(crop->width * bpp / 8, STFCAMSS_FRAME_WIDTH_ALIGN_8); + + isp_format_reg_list[14].addr =3D ISP_REG_SENSOR; + isp_format_reg_list[14].val =3D 0x00000000; + if (type =3D=3D INTERFACE_CSI) + isp_format_reg_list[14].val |=3D 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 =3D 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 =3D stf_isp_get_ispbase(isp_dev); + struct isp_reg_param *reg_param =3D arg; + union reg_buf *reg_buf =3D 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 =3D 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 =3D 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 =3D %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 =3D 0x%08X is out of size!\n", + reg_param->reg_info.length); + return -EINVAL; + } + size =3D 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 =3D 0x%08X is out of size!\n", + reg_param->reg_info.length); + return -EINVAL; + } + size =3D 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 =3D 0x%08X is out of size!\n", + reg_param->reg_info.length); + return -EINVAL; + } + size =3D sizeof(u32) * reg_param->reg_info.length * 4; + break; + + case STF_ISP_REG_METHOD_SMPL_PACK: + st_err(ST_ISP, "The method =3D %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 =3D %d is not supported!\n", + reg_param->reg_info.method); + return -ENOTTY; + + default: + st_err(ST_ISP, "The method =3D %d is not supported!\n", + reg_param->reg_info.method); + return -ENOTTY; + } + + memset(reg_buf, 0, sizeof(union reg_buf)); + if (size) { + r =3D 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 =3D 0; + switch (reg_param->reg_info.method) { + case STF_ISP_REG_METHOD_ONE_REG: + reg_buf->buffer[0] =3D reg_read(ispbase, + reg_param->reg_info.offset); + size =3D sizeof(u32); + break; + + case STF_ISP_REG_METHOD_SERIES: + for (r =3D 0; r < reg_param->reg_info.length; r++) + reg_buf->buffer[r] =3D + reg_read(ispbase, + reg_param->reg_info.offset + (r * 4)); + + size =3D sizeof(u32) * reg_param->reg_info.length; + break; + + case STF_ISP_REG_METHOD_MODULE: + break; + + case STF_ISP_REG_METHOD_TABLE: + for (r =3D 0; r < reg_param->reg_info.length; r++) + reg_buf->reg_tbl[r].value =3D + reg_read(ispbase, reg_buf->reg_tbl[r].offset); + + size =3D sizeof(u32) * reg_param->reg_info.length * 2; + break; + + case STF_ISP_REG_METHOD_TABLE_2: + for (r =3D 0; r < reg_param->reg_info.length; r++) { + if (reg_buf->reg_tbl2[r].mask) + reg_buf->reg_tbl2[r].value =3D + reg_read(ispbase, reg_buf->reg_tbl2[r].offset) & + reg_buf->reg_tbl2[r].mask; + else + reg_buf->reg_tbl2[r].value =3D + reg_read(ispbase, + reg_buf->reg_tbl2[r].offset); + } + + size =3D sizeof(u32) * reg_param->reg_info.length * 3; + break; + + case STF_ISP_REG_METHOD_TABLE_3: + for (r =3D 0; r < reg_param->reg_info.length; r++) { + if (reg_buf->reg_tbl3[r].mask) + reg_buf->reg_tbl3[r].value =3D + reg_read(ispbase, reg_buf->reg_tbl3[r].offset) & + reg_buf->reg_tbl3[r].mask; + else + reg_buf->reg_tbl3[r].value =3D + 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 =3D 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 =3D 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 =3D stf_isp_get_ispbase(isp_dev); + struct isp_rdma_info *rdma_info =3D NULL; + s32 len; + u32 offset; + int ret =3D 0; + + rdma_info =3D phys_to_virt(rdma_addr); + while (1) { + if (rdma_info->tag =3D=3D RDMA_END) + break; + + if (rdma_info->tag =3D=3D RDMA_WR_ONE) { + reg_write(ispbase, rdma_info->offset, rdma_info->param); + rdma_info++; + } else if (rdma_info->tag =3D=3D RDMA_WR_SRL) { + offset =3D rdma_info->offset; + len =3D rdma_info->param; + rdma_info++; + while (len > 0) { + reg_write(ispbase, offset, rdma_info->param); + offset +=3D 4; + len--; + if (len > 0) { + reg_write(ispbase, + offset, rdma_info->value); + len--; + } + offset +=3D 4; + rdma_info++; + } + } else if (rdma_info->tag =3D=3D RDMA_LINK) { + rdma_info =3D phys_to_virt(rdma_info->param); + } else if (rdma_info->tag =3D=3D 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 =3D stf_isp_get_ispbase(isp_dev); + struct isp_reg_param *reg_param =3D arg; + struct isp_rdma_info *rdma_info =3D NULL; + union reg_buf *reg_buf =3D isp_dev->reg_buf; + s32 len; + u32 offset; + u32 size; + unsigned long r; + int ret =3D 0; + + if (!reg_param->reg_buf && + reg_param->reg_info.method !=3D STF_ISP_REG_METHOD_SOFT_RDMA) { + st_err(ST_ISP, "Failed to access register.\n"); + return -EINVAL; + } + + size =3D 0; + switch (reg_param->reg_info.method) { + case STF_ISP_REG_METHOD_ONE_REG: + size =3D 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 =3D 0x%08X is out of size!\n", + reg_param->reg_info.length); + return -EINVAL; + } + size =3D sizeof(u32) * reg_param->reg_info.length; + break; + + case STF_ISP_REG_METHOD_MODULE: + st_err(ST_ISP, "The method =3D %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 =3D 0x%08X is out of size!\n", + reg_param->reg_info.length); + return -EINVAL; + } + size =3D 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 =3D 0x%08X is out of size!\n", + reg_param->reg_info.length); + return -EINVAL; + } + size =3D 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 =3D 0x%08X is out of size!\n", + reg_param->reg_info.length); + return -EINVAL; + } + size =3D 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 =3D 0x%08X is out of size!\n", + reg_param->reg_info.length); + return -EINVAL; + } + size =3D sizeof(u32) * reg_param->reg_info.length * 2; + break; + + case STF_ISP_REG_METHOD_SOFT_RDMA: + break; + + default: + st_err(ST_ISP, "The method =3D %d is not supported!\n", + reg_param->reg_info.method); + return -ENOTTY; + } + + memset(reg_buf, 0, sizeof(union reg_buf)); + if (size) { + r =3D 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 =3D 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 =3D 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 =3D 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 =3D 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 =3D reg_param->reg_info.length; + rdma_info =3D ®_buf->rdma_cmd[0]; + while (size) { + if (rdma_info->tag =3D=3D RDMA_END) + break; + + if (rdma_info->tag =3D=3D RDMA_WR_ONE) { + reg_write(ispbase, + rdma_info->offset, rdma_info->param); + rdma_info++; + size--; + } else if (rdma_info->tag =3D=3D RDMA_WR_SRL) { + offset =3D rdma_info->offset; + len =3D rdma_info->param; + rdma_info++; + size--; + while (size && (len > 0)) { + reg_write(ispbase, + offset, rdma_info->param); + offset +=3D 4; + len--; + if (len > 0) { + reg_write(ispbase, + offset, + rdma_info->value); + len--; + } + offset +=3D 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 =3D 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 =3D 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 =3D { + .isp_clk_enable =3D stf_isp_clk_enable, + .isp_clk_disable =3D stf_isp_clk_disable, + .isp_reset =3D stf_isp_reset, + .isp_config_set =3D stf_isp_config_set, + .isp_set_format =3D stf_isp_set_format, + .isp_stream_set =3D stf_isp_stream_set, + .isp_reg_read =3D stf_isp_reg_read, + .isp_reg_write =3D stf_isp_reg_write, + .isp_shadow_trigger =3D stf_isp_shadow_trigger, +}; diff --git a/drivers/media/platform/starfive/stf_video.c b/drivers/media/pl= atform/starfive/stf_video.c new file mode 100644 index 000000000000..23d393e6fd78 --- /dev/null +++ b/drivers/media/platform/starfive/stf_video.c @@ -0,0 +1,1286 @@ +// SPDX-License-Identifier: GPL-2.0 +/* + * stf_video.c + * + * StarFive Camera Subsystem - V4L2 device node + * + * Copyright (C) 2021-2023 StarFive Technology Co., Ltd. + */ + +#include +#include +#include +#include +#include + +#include "stf_camss.h" +#include "stf_video.h" + +static const struct stfcamss_format_info formats_pix_wr[] =3D { + { 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[] =3D { + { 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[] =3D { + { 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 =3D 0; i < nformats; i++) { + if (formats[i].code =3D=3D code && + formats[i].pixelformat =3D=3D pixelformat) + return i; + } + + for (i =3D 0; i < nformats; i++) + if (formats[i].code =3D=3D code) + return i; + + for (i =3D 0; i < nformats; i++) + if (formats[i].pixelformat =3D=3D pixelformat) + return i; + + return -EINVAL; +} + +static int __video_try_fmt(struct stfcamss_video *video, struct v4l2_forma= t *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 =3D 0x%x\n", __func__, f->type); + pix =3D &f->fmt.pix; + + for (i =3D 0; i < video->nformats; i++) + if (pix->pixelformat =3D=3D video->formats[i].pixelformat) + break; + + if (i =3D=3D video->nformats) + i =3D 0; /* default format */ + + fi =3D &video->formats[i]; + width =3D pix->width; + height =3D pix->height; + + memset(pix, 0, sizeof(*pix)); + + pix->pixelformat =3D fi->pixelformat; + pix->width =3D clamp_t(u32, width, STFCAMSS_FRAME_MIN_WIDTH, + STFCAMSS_FRAME_MAX_WIDTH); + pix->height =3D clamp_t(u32, height, STFCAMSS_FRAME_MIN_HEIGHT, + STFCAMSS_FRAME_MAX_HEIGHT); + bpl =3D pix->width / fi->hsub[0].numerator * + fi->hsub[0].denominator * fi->bpp[0] / 8; + bpl =3D ALIGN(bpl, video->bpl_alignment); + pix->bytesperline =3D bpl; + pix->sizeimage =3D pix->height / fi->vsub[0].numerator * + fi->vsub[0].denominator * bpl; + + pix->field =3D V4L2_FIELD_NONE; + pix->colorspace =3D V4L2_COLORSPACE_SRGB; + pix->flags =3D 0; + pix->ycbcr_enc =3D + V4L2_MAP_YCBCR_ENC_DEFAULT(pix->colorspace); + pix->quantization =3D V4L2_MAP_QUANTIZATION_DEFAULT(true, + pix->colorspace, + pix->ycbcr_enc); + pix->xfer_func =3D V4L2_MAP_XFER_FUNC_DEFAULT(pix->colorspace); + + st_info(ST_VIDEO, "w, h =3D %d, %d, bpp =3D %d\n", + pix->width, pix->height, fi->bpp[0]); + st_info(ST_VIDEO, "i =3D %d, s =3D 0x%x\n", i, pix->sizeimage); + + return 0; +} + +static int stf_video_init_format(struct stfcamss_video *video) +{ + int ret; + struct v4l2_format format =3D { + .type =3D video->type, + .fmt.pix =3D { + .width =3D 1920, + .height =3D 1080, + .pixelformat =3D V4L2_PIX_FMT_RGB565, + }, + }; + + ret =3D __video_try_fmt(video, &format); + + if (ret < 0) + return ret; + + video->active_fmt =3D 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 =3D vb2_get_drv_priv(q); + const struct v4l2_pix_format *format =3D &video->active_fmt.fmt.pix; + + st_debug(ST_VIDEO, "%s, planes =3D %d\n", __func__, *num_planes); + + if (*num_planes) { + if (*num_planes !=3D 1) + return -EINVAL; + + if (sizes[0] < format->sizeimage) + return -EINVAL; + } + + *num_planes =3D 1; + sizes[0] =3D format->sizeimage; + if (!sizes[0]) + st_err(ST_VIDEO, "%s: error size is zero!!!\n", __func__); + + st_info(ST_VIDEO, "%s, planes =3D %d, size =3D %d\n", + __func__, *num_planes, sizes[0]); + + return 0; +} + +static int video_buf_init(struct vb2_buffer *vb) +{ + struct vb2_v4l2_buffer *vbuf =3D to_vb2_v4l2_buffer(vb); + struct stfcamss_video *video =3D vb2_get_drv_priv(vb->vb2_queue); + struct stfcamss_buffer *buffer =3D + container_of(vbuf, struct stfcamss_buffer, vb); + const struct v4l2_pix_format *fmt =3D &video->active_fmt.fmt.pix; + dma_addr_t *paddr; + + buffer->sizeimage =3D 0; + + paddr =3D vb2_plane_cookie(vb, 0); + buffer->sizeimage =3D vb2_plane_size(vb, 0); + buffer->addr[0] =3D *paddr; + if (fmt->pixelformat =3D=3D V4L2_PIX_FMT_NV12 || + fmt->pixelformat =3D=3D V4L2_PIX_FMT_NV21 || + fmt->pixelformat =3D=3D V4L2_PIX_FMT_NV16 || + fmt->pixelformat =3D=3D V4L2_PIX_FMT_NV61) + buffer->addr[1] =3D + buffer->addr[0] + fmt->bytesperline * fmt->height; + + return 0; +} + +static int video_buf_prepare(struct vb2_buffer *vb) +{ + struct vb2_v4l2_buffer *vbuf =3D to_vb2_v4l2_buffer(vb); + struct stfcamss_video *video =3D vb2_get_drv_priv(vb->vb2_queue); + const struct v4l2_pix_format *fmt =3D &video->active_fmt.fmt.pix; + + if (fmt->sizeimage > vb2_plane_size(vb, 0)) { + st_err(ST_VIDEO, "sizeimage =3D %d, plane size =3D %d\n", + fmt->sizeimage, (unsigned int)vb2_plane_size(vb, 0)); + return -EINVAL; + } + vb2_set_plane_payload(vb, 0, fmt->sizeimage); + + vbuf->field =3D V4L2_FIELD_NONE; + + return 0; +} + +static void video_buf_queue(struct vb2_buffer *vb) +{ + struct vb2_v4l2_buffer *vbuf =3D to_vb2_v4l2_buffer(vb); + struct stfcamss_video *video =3D vb2_get_drv_priv(vb->vb2_queue); + struct stfcamss_buffer *buffer =3D + 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 form= at. + * + * 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 =3D f->pixelformat; + bytesperline =3D pix->width / f->hsub[0].numerator * + f->hsub[0].denominator * f->bpp[0] / 8; + bytesperline =3D ALIGN(bytesperline, alignment); + pix->bytesperline =3D bytesperline; + pix->sizeimage =3D pix->height / f->vsub[0].numerator * + f->vsub[0].denominator * bytesperline; + return 0; +} + +static struct v4l2_subdev *video_remote_subdev(struct stfcamss_video *vide= o, + u32 *pad) +{ + struct media_pad *remote; + + remote =3D media_pad_remote_pad_first(&video->pad); + + if (!remote || !is_media_entity_v4l2_subdev(remote->entity)) + return NULL; + + if (pad) + *pad =3D 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 =3D &video->active_fmt.fmt.pix; + struct v4l2_subdev_format fmt; + struct v4l2_subdev *subdev; + u32 pixelformat; + u32 pad; + int ret; + + subdev =3D video_remote_subdev(video, &pad); + if (!subdev) + return -EPIPE; + + fmt.pad =3D pad; + fmt.which =3D V4L2_SUBDEV_FORMAT_ACTIVE; + + ret =3D v4l2_subdev_call(subdev, pad, get_fmt, NULL, &fmt); + if (ret) + return ret; + + pixelformat =3D pix->pixelformat; + ret =3D video_find_format(fmt.format.code, pixelformat, + video->formats, video->nformats); + if (ret < 0) + return ret; + + format->type =3D 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 =3D &video->active_fmt.fmt.pix; + struct v4l2_format format; + struct v4l2_pix_format *sd_pix =3D &format.fmt.pix; + int ret; + + sd_pix->pixelformat =3D pix->pixelformat; + ret =3D video_get_subdev_format(video, &format); + if (ret < 0) + return ret; + + if (pix->pixelformat !=3D sd_pix->pixelformat || + pix->height > sd_pix->height || + pix->width > sd_pix->width || + pix->field !=3D 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 =3D vb2_get_drv_priv(q); + struct video_device *vdev =3D &video->vdev; + struct media_entity *entity; + struct media_pad *pad; + struct v4l2_subdev *subdev; + int ret; + + ret =3D 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 =3D video_check_format(video); + if (ret < 0) + goto error; + entity =3D &vdev->entity; + while (1) { + pad =3D &entity->pads[0]; + if (!(pad->flags & MEDIA_PAD_FL_SINK)) + break; + + pad =3D media_pad_remote_pad_first(pad); + if (!pad || !is_media_entity_v4l2_subdev(pad->entity)) + break; + + entity =3D pad->entity; + subdev =3D media_entity_to_v4l2_subdev(entity); + + ret =3D v4l2_subdev_call(subdev, video, s_stream, 1); + if (ret < 0 && ret !=3D -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 =3D vb2_get_drv_priv(q); + struct video_device *vdev =3D &video->vdev; + struct media_entity *entity; + struct media_pad *pad; + struct v4l2_subdev *subdev; + + entity =3D &vdev->entity; + while (1) { + pad =3D &entity->pads[0]; + if (!(pad->flags & MEDIA_PAD_FL_SINK)) + break; + + pad =3D media_pad_remote_pad_first(pad); + if (!pad || !is_media_entity_v4l2_subdev(pad->entity)) + break; + + entity =3D pad->entity; + subdev =3D 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 =3D { + .queue_setup =3D video_queue_setup, + .wait_prepare =3D vb2_ops_wait_prepare, + .wait_finish =3D vb2_ops_wait_finish, + .buf_init =3D video_buf_init, + .buf_prepare =3D video_buf_prepare, + .buf_queue =3D video_buf_queue, + .start_streaming =3D video_start_streaming, + .stop_streaming =3D video_stop_streaming, +}; + +/* -----------------------------------------------------------------------= ------ + * V4L2 ioctls + */ + +static int video_querycap(struct file *file, void *fh, + struct v4l2_capability *cap) +{ + struct stfcamss_video *video =3D 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 =3D -1; + for (i =3D 0; i < video->nformats; i++) { + for (j =3D 0; j < i; j++) { + if (video->formats[i].pixelformat =3D=3D + video->formats[j].pixelformat) + break; + } + + if (j =3D=3D i) + k++; + + if (k =3D=3D ndx) + return i; + } + + return -EINVAL; +} + +static int video_get_pfmt_by_mcode(struct stfcamss_video *video, u32 mcode) +{ + int i; + + for (i =3D 0; i < video->nformats; i++) { + if (video->formats[i].code =3D=3D mcode) + return i; + } + + return -EINVAL; +} + +static int video_enum_fmt(struct file *file, void *fh, struct v4l2_fmtdesc= *f) +{ + struct stfcamss_video *video =3D 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 !=3D video->type) + return -EINVAL; + if (f->index >=3D video->nformats) + return -EINVAL; + + if (f->mbus_code) { + /* Each entry in formats[] table has unique mbus_code */ + if (f->index > 0) + return -EINVAL; + + i =3D video_get_pfmt_by_mcode(video, f->mbus_code); + } else { + i =3D video_get_pfmt_by_index(video, f->index); + } + + if (i < 0) + return -EINVAL; + + f->pixelformat =3D 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 =3D {0}; + struct v4l2_subdev_mbus_code_enum code =3D {0}; + struct stfcamss_video *video =3D video_drvdata(file); + struct video_device *vdev =3D &video->vdev; + struct media_entity *entity =3D &vdev->entity; + struct media_entity *sensor; + struct v4l2_subdev *subdev; + struct media_pad *pad; + bool support_selection =3D false; + int i; + int ret; + + for (i =3D 0; i < video->nformats; i++) { + if (video->formats[i].pixelformat =3D=3D fsize->pixel_format) + break; + } + + if (i =3D=3D video->nformats) + return -EINVAL; + + entity =3D &vdev->entity; + while (1) { + pad =3D &entity->pads[0]; + if (!(pad->flags & MEDIA_PAD_FL_SINK)) + break; + + pad =3D media_pad_remote_pad_first(pad); + if (!pad || !is_media_entity_v4l2_subdev(pad->entity)) + break; + + entity =3D pad->entity; + subdev =3D media_entity_to_v4l2_subdev(entity); + + if (v4l2_subdev_has_op(subdev, pad, set_selection)) { + support_selection =3D true; + break; + } + } + + if (support_selection) { + if (fsize->index) + return -EINVAL; + fsize->type =3D V4L2_FRMSIZE_TYPE_CONTINUOUS; + fsize->stepwise.min_width =3D STFCAMSS_FRAME_MIN_WIDTH; + fsize->stepwise.max_width =3D STFCAMSS_FRAME_MAX_WIDTH; + fsize->stepwise.min_height =3D STFCAMSS_FRAME_MIN_HEIGHT; + fsize->stepwise.max_height =3D STFCAMSS_FRAME_MAX_HEIGHT; + fsize->stepwise.step_width =3D 1; + fsize->stepwise.step_height =3D 1; + } else { + entity =3D &vdev->entity; + sensor =3D stfcamss_find_sensor(entity); + if (!sensor) + return -ENOTTY; + + subdev =3D media_entity_to_v4l2_subdev(sensor); + code.index =3D 0; + code.which =3D V4L2_SUBDEV_FORMAT_ACTIVE; + ret =3D v4l2_subdev_call(subdev, pad, + enum_mbus_code, NULL, &code); + if (ret < 0) + return -EINVAL; + fse.index =3D fsize->index; + fse.code =3D code.code; + fse.which =3D V4L2_SUBDEV_FORMAT_ACTIVE; + ret =3D v4l2_subdev_call(subdev, pad, + enum_frame_size, NULL, &fse); + if (ret < 0) + return -EINVAL; + fsize->type =3D V4L2_FRMSIZE_TYPE_DISCRETE; + fsize->discrete.width =3D fse.min_width; + fsize->discrete.height =3D fse.min_height; + } + + return 0; +} + +static int video_enum_frameintervals(struct file *file, void *fh, + struct v4l2_frmivalenum *fival) +{ + int ret =3D 0; + struct stfcamss_video *video =3D video_drvdata(file); + struct video_device *vdev =3D &video->vdev; + struct media_entity *entity =3D &vdev->entity; + struct media_entity *sensor; + struct v4l2_subdev *subdev; + struct v4l2_subdev_mbus_code_enum code =3D {0}; + struct v4l2_subdev_frame_interval_enum fie =3D {0}; + + sensor =3D stfcamss_find_sensor(entity); + if (!sensor) + return -ENOTTY; + fie.index =3D fival->index; + fie.width =3D fival->width; + fie.height =3D fival->height; + fie.which =3D V4L2_SUBDEV_FORMAT_ACTIVE; + subdev =3D media_entity_to_v4l2_subdev(sensor); + + code.index =3D 0; + code.which =3D V4L2_SUBDEV_FORMAT_ACTIVE; + + ret =3D video_find_format(0, fival->pixel_format, + video->formats, video->nformats); + if (ret < 0) + return -EINVAL; + + ret =3D v4l2_subdev_call(subdev, pad, enum_mbus_code, NULL, &code); + if (ret < 0) + return -EINVAL; + + fie.code =3D code.code; + ret =3D v4l2_subdev_call(subdev, pad, enum_frame_interval, NULL, &fie); + if (ret < 0) + return ret; + + fival->type =3D V4L2_FRMIVAL_TYPE_DISCRETE; + fival->discrete =3D fie.interval; + + return 0; +} + +static int video_g_fmt(struct file *file, void *fh, struct v4l2_format *f) +{ + struct stfcamss_video *video =3D video_drvdata(file); + + st_debug(ST_VIDEO, "%s, fmt.type =3D 0x%x\n", __func__, f->type); + st_debug(ST_VIDEO, "%s, active_fmt.type =3D 0x%x,0x%x\n", + __func__, video->active_fmt.type, + video->active_fmt.fmt.pix.pixelformat); + *f =3D 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 =3D &fmt->format; + u32 width, height, code; + int ret, index =3D 0; + + code =3D mf->code; + width =3D mf->width; + height =3D mf->height; + subdev =3D media_entity_to_v4l2_subdev(entity); + while (1) { + if (index >=3D entity->num_pads) + break; + pad =3D &entity->pads[index]; + pad =3D media_pad_remote_pad_first(pad); + if (pad && is_media_entity_v4l2_subdev(pad->entity)) { + fmt->pad =3D index; + if (index) + mf->code =3D dst_code; + ret =3D 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 !=3D code || + mf->width !=3D width || + mf->height !=3D 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 =3D video_entity_s_fmt(video, pad->entity, + state, fmt, dst_code); + } + + if (ret < 0 && ret !=3D -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 =3D &video->vdev; + struct media_entity *entity =3D &vdev->entity; + struct v4l2_subdev *subdev; + int ret, index; + struct v4l2_subdev_format fmt =3D { + .which =3D V4L2_SUBDEV_FORMAT_ACTIVE, + .reserved =3D {video_getcrop_pad_id(video->id)} + }; + struct v4l2_mbus_framefmt *mf =3D &fmt.format; + struct v4l2_pix_format *pix =3D &f->fmt.pix; + struct media_entity *sensor; + u32 width, height, code; + struct media_pad *pad; + + /* pix to mbus format */ + index =3D 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 =3D mf->code; + width =3D mf->width; + height =3D mf->height; + sensor =3D stfcamss_find_sensor(entity); + if (sensor) { + subdev =3D media_entity_to_v4l2_subdev(sensor); + ret =3D 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 !=3D code || + mf->width !=3D width || + mf->height !=3D 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 =3D stfcamss_find_sensor(entity); + pad =3D media_pad_remote_pad_first(&sensor->pads[0]); + ret =3D video_entity_s_fmt(video, pad->entity, state, &fmt, code); + + if (ret < 0 && ret !=3D -ENOIOCTLCMD) + return ret; + + index =3D video_find_format(mf->code, + video->formats[index].pixelformat, + video->formats, video->nformats); + st_debug(ST_VIDEO, "%s, code=3D%x, index=3D%d\n", + __func__, mf->code, index); + + if (index < 0) + return index; + + video_mbus_to_pix(mf, pix, &video->formats[index], + video->bpl_alignment); + + ret =3D __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 =3D video_drvdata(file); + int ret; + + st_debug(ST_VIDEO, "%s, fmt.type =3D 0x%x, v4l2fmt=3D%x\n", + __func__, f->type, f->fmt.pix.pixelformat); + + if (vb2_is_busy(&video->vb2_q)) + return -EBUSY; + + ret =3D __video_try_fmt(video, f); + if (ret < 0) + return ret; + + ret =3D video_pipeline_s_fmt(video, NULL, f); + + st_debug(ST_VIDEO, "%s, pixelformat=3D0x%x, ret=3D%d\n", + __func__, f->fmt.pix.pixelformat, ret); + if (ret < 0) + return ret; + + video->active_fmt =3D *f; + + return 0; +} + +static int video_try_fmt(struct file *file, void *fh, struct v4l2_format *= f) +{ + struct stfcamss_video *video =3D 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 =3D V4L2_INPUT_TYPE_CAMERA; + + return 0; +} + +static int video_g_input(struct file *file, void *fh, unsigned int *input) +{ + *input =3D 0; + + return 0; +} + +static int video_s_input(struct file *file, void *fh, unsigned int input) +{ + return input =3D=3D 0 ? 0 : -EINVAL; +} + +static int video_g_parm(struct file *file, void *priv, + struct v4l2_streamparm *p) +{ + struct stfcamss_video *video =3D video_drvdata(file); + struct video_device *vdev =3D &video->vdev; + struct media_entity *entity; + struct v4l2_subdev *subdev; + struct media_pad *pad; + int ret, is_support =3D 0; + + entity =3D &vdev->entity; + while (1) { + pad =3D &entity->pads[0]; + if (!(pad->flags & MEDIA_PAD_FL_SINK)) + break; + + pad =3D media_pad_remote_pad_first(pad); + if (!pad || !is_media_entity_v4l2_subdev(pad->entity)) + break; + + entity =3D pad->entity; + subdev =3D media_entity_to_v4l2_subdev(entity); + + ret =3D v4l2_g_parm_cap(vdev, subdev, p); + if (ret < 0 && ret !=3D -ENOIOCTLCMD) + break; + if (!ret) + is_support =3D 1; + } + + return is_support ? 0 : ret; +} + +static int video_s_parm(struct file *file, void *priv, + struct v4l2_streamparm *p) +{ + struct stfcamss_video *video =3D video_drvdata(file); + struct video_device *vdev =3D &video->vdev; + struct media_entity *entity; + struct v4l2_subdev *subdev; + struct media_pad *pad; + struct v4l2_streamparm tmp_p; + int ret, is_support =3D 0; + + entity =3D &vdev->entity; + while (1) { + pad =3D &entity->pads[0]; + if (!(pad->flags & MEDIA_PAD_FL_SINK)) + break; + + pad =3D media_pad_remote_pad_first(pad); + if (!pad || !is_media_entity_v4l2_subdev(pad->entity)) + break; + + entity =3D pad->entity; + subdev =3D media_entity_to_v4l2_subdev(entity); + + tmp_p =3D *p; + ret =3D v4l2_s_parm_cap(vdev, subdev, &tmp_p); + if (ret < 0 && ret !=3D -ENOIOCTLCMD) + break; + if (!ret) { + is_support =3D 1; + *p =3D 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 =3D video_drvdata(file); + struct video_device *vdev =3D &video->vdev; + struct media_entity *entity; + struct v4l2_subdev *subdev; + struct media_pad *pad; + struct v4l2_subdev_selection sel =3D { + .which =3D V4L2_SUBDEV_FORMAT_ACTIVE, + .pad =3D video_getcrop_pad_id(video->id), + .target =3D s->target, + .r =3D s->r, + .flags =3D s->flags, + }; + int ret; + + st_debug(ST_VIDEO, "%s, target =3D 0x%x, 0x%x\n", + __func__, sel.target, s->target); + if (s->type !=3D V4L2_BUF_TYPE_VIDEO_CAPTURE && + s->type !=3D V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) + return -EINVAL; + + entity =3D &vdev->entity; + while (1) { + pad =3D &entity->pads[0]; + if (!(pad->flags & MEDIA_PAD_FL_SINK)) + break; + + pad =3D media_pad_remote_pad_first(pad); + if (!pad || !is_media_entity_v4l2_subdev(pad->entity)) + break; + + entity =3D pad->entity; + subdev =3D media_entity_to_v4l2_subdev(entity); + + ret =3D v4l2_subdev_call(subdev, pad, get_selection, NULL, &sel); + if (!ret) { + s->r =3D sel.r; + s->flags =3D sel.flags; + break; + } + if (ret !=3D -ENOIOCTLCMD) + break; + } + + return ret; +} + +static int video_s_selection(struct file *file, void *fh, + struct v4l2_selection *s) +{ + struct stfcamss_video *video =3D video_drvdata(file); + struct video_device *vdev =3D &video->vdev; + struct media_entity *entity; + struct v4l2_subdev *subdev; + struct media_pad *pad; + struct v4l2_subdev_selection sel =3D { + .which =3D V4L2_SUBDEV_FORMAT_ACTIVE, + .pad =3D video_getcrop_pad_id(video->id), + .target =3D s->target, + .r =3D s->r, + .flags =3D s->flags, + }; + struct v4l2_pix_format *format =3D &video->active_fmt.fmt.pix; + int ret; + + st_debug(ST_VIDEO, "%s, target =3D 0x%x, 0x%x\n", + __func__, sel.target, s->target); + if (s->type !=3D V4L2_BUF_TYPE_VIDEO_CAPTURE && + s->type !=3D V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) + return -EINVAL; + + entity =3D &vdev->entity; + while (1) { + pad =3D &entity->pads[0]; + if (!(pad->flags & MEDIA_PAD_FL_SINK)) + break; + + pad =3D media_pad_remote_pad_first(pad); + if (!pad || !is_media_entity_v4l2_subdev(pad->entity)) + break; + + entity =3D pad->entity; + subdev =3D media_entity_to_v4l2_subdev(entity); + + ret =3D v4l2_subdev_call(subdev, pad, set_selection, NULL, &sel); + if (!ret) { + s->r =3D sel.r; + s->flags =3D sel.flags; + format->width =3D s->r.width; + format->height =3D s->r.height; + ret =3D __video_try_fmt(video, &video->active_fmt); + if (ret < 0) + return ret; + break; + } + if (ret !=3D -ENOIOCTLCMD) + break; + } + + st_debug(ST_VIDEO, "ret =3D 0x%x, -EINVAL =3D 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 =3D NULL; + struct media_entity *entity =3D &fh->vdev->entity; + struct media_entity *sensor; + + sensor =3D stfcamss_find_sensor(entity); + if (sensor) + subdev =3D media_entity_to_v4l2_subdev(sensor); + + if (!subdev) + return -ENOTTY; + + fh->ctrl_handler =3D 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 =3D NULL; + struct media_entity *entity =3D &fh->vdev->entity; + struct media_entity *sensor; + + sensor =3D stfcamss_find_sensor(entity); + if (sensor) + subdev =3D media_entity_to_v4l2_subdev(sensor); + + if (!subdev) + return -ENOTTY; + + fh->ctrl_handler =3D subdev->ctrl_handler; + return v4l2_event_unsubscribe(fh, sub); +} + +static const struct v4l2_ioctl_ops stf_vid_vin_ioctl_ops =3D { + .vidioc_querycap =3D video_querycap, + .vidioc_enum_fmt_vid_cap =3D video_enum_fmt, + .vidioc_enum_fmt_vid_out =3D video_enum_fmt, + .vidioc_enum_framesizes =3D video_enum_framesizes, + .vidioc_enum_frameintervals =3D video_enum_frameintervals, + .vidioc_g_fmt_vid_cap =3D video_g_fmt, + .vidioc_s_fmt_vid_cap =3D video_s_fmt, + .vidioc_try_fmt_vid_cap =3D video_try_fmt, + .vidioc_g_fmt_vid_out =3D video_g_fmt, + .vidioc_s_fmt_vid_out =3D video_s_fmt, + .vidioc_try_fmt_vid_out =3D video_try_fmt, + .vidioc_reqbufs =3D vb2_ioctl_reqbufs, + .vidioc_querybuf =3D vb2_ioctl_querybuf, + .vidioc_qbuf =3D vb2_ioctl_qbuf, + .vidioc_expbuf =3D vb2_ioctl_expbuf, + .vidioc_dqbuf =3D vb2_ioctl_dqbuf, + .vidioc_create_bufs =3D vb2_ioctl_create_bufs, + .vidioc_prepare_buf =3D vb2_ioctl_prepare_buf, + .vidioc_streamon =3D vb2_ioctl_streamon, + .vidioc_streamoff =3D vb2_ioctl_streamoff, + .vidioc_enum_input =3D video_enum_input, + .vidioc_g_input =3D video_g_input, + .vidioc_s_input =3D video_s_input, + .vidioc_g_parm =3D video_g_parm, + .vidioc_s_parm =3D video_s_parm, + .vidioc_subscribe_event =3D video_subscribe_event, + .vidioc_unsubscribe_event =3D video_unsubscribe_event, +}; + +static const struct v4l2_ioctl_ops stf_vid_isp_ioctl_ops =3D { + .vidioc_querycap =3D video_querycap, + .vidioc_enum_fmt_vid_cap =3D video_enum_fmt, + .vidioc_enum_fmt_vid_out =3D video_enum_fmt, + .vidioc_enum_framesizes =3D video_enum_framesizes, + .vidioc_enum_frameintervals =3D video_enum_frameintervals, + .vidioc_g_fmt_vid_cap =3D video_g_fmt, + .vidioc_s_fmt_vid_cap =3D video_s_fmt, + .vidioc_try_fmt_vid_cap =3D video_try_fmt, + .vidioc_g_fmt_vid_out =3D video_g_fmt, + .vidioc_s_fmt_vid_out =3D video_s_fmt, + .vidioc_try_fmt_vid_out =3D video_try_fmt, + .vidioc_reqbufs =3D vb2_ioctl_reqbufs, + .vidioc_querybuf =3D vb2_ioctl_querybuf, + .vidioc_qbuf =3D vb2_ioctl_qbuf, + .vidioc_expbuf =3D vb2_ioctl_expbuf, + .vidioc_dqbuf =3D vb2_ioctl_dqbuf, + .vidioc_create_bufs =3D vb2_ioctl_create_bufs, + .vidioc_prepare_buf =3D vb2_ioctl_prepare_buf, + .vidioc_streamon =3D vb2_ioctl_streamon, + .vidioc_streamoff =3D vb2_ioctl_streamoff, + .vidioc_enum_input =3D video_enum_input, + .vidioc_g_input =3D video_g_input, + .vidioc_s_input =3D video_s_input, + .vidioc_g_parm =3D video_g_parm, + .vidioc_s_parm =3D video_s_parm, + .vidioc_s_selection =3D video_s_selection, + .vidioc_g_selection =3D video_g_selection, + .vidioc_subscribe_event =3D video_subscribe_event, + .vidioc_unsubscribe_event =3D video_unsubscribe_event, +}; + +/* -----------------------------------------------------------------------= ------ + * V4L2 file operations + */ + +static int video_open(struct file *file) +{ + struct video_device *vdev =3D video_devdata(file); + struct stfcamss_video *video =3D video_drvdata(file); + struct v4l2_fh *vfh; + int ret; + + mutex_lock(&video->lock); + + vfh =3D kzalloc(sizeof(*vfh), GFP_KERNEL); + if (!vfh) { + ret =3D -ENOMEM; + goto error_alloc; + } + + v4l2_fh_init(vfh, vdev); + v4l2_fh_add(vfh); + + file->private_data =3D vfh; + + ret =3D 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 =3D video_devdata(file); + + vb2_fop_release(file); + v4l2_pipeline_pm_put(&vdev->entity); + file->private_data =3D NULL; + + return 0; +} + +static const struct v4l2_file_operations stf_vid_fops =3D { + .owner =3D THIS_MODULE, + .unlocked_ioctl =3D video_ioctl2, + .open =3D video_open, + .release =3D video_release, + .poll =3D vb2_fop_poll, + .mmap =3D vb2_fop_mmap, + .read =3D vb2_fop_read, +}; + +/* -----------------------------------------------------------------------= ------ + * STFCAMSS video core + */ + +static void stf_video_release(struct video_device *vdev) +{ + struct stfcamss_video *video =3D 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 =3D &video->pad; + int ret; + enum isp_pad_id isp_pad; + + vdev =3D &video->vdev; + + mutex_init(&video->q_lock); + + q =3D &video->vb2_q; + q->drv_priv =3D video; + q->mem_ops =3D &vb2_dma_contig_memops; + q->ops =3D &stf_video_vb2_q_ops; + q->type =3D video->type; + q->io_modes =3D VB2_DMABUF | VB2_MMAP | VB2_READ; + q->timestamp_flags =3D V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC; + q->buf_struct_size =3D sizeof(struct stfcamss_buffer); + q->dev =3D video->stfcamss->dev; + q->lock =3D &video->q_lock; + q->min_buffers_needed =3D STFCAMSS_MIN_BUFFERS; + ret =3D 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 =3D MEDIA_PAD_FL_SINK; + ret =3D 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 =3D stf_vin_map_isp_pad(video->id, STF_ISP_PAD_SRC); + if (video->id =3D=3D VIN_LINE_WR) { + video->formats =3D formats_pix_wr; + video->nformats =3D ARRAY_SIZE(formats_pix_wr); + video->bpl_alignment =3D STFCAMSS_FRAME_WIDTH_ALIGN_8; + vdev->ioctl_ops =3D &stf_vid_vin_ioctl_ops; + } else if (isp_pad =3D=3D STF_ISP_PAD_SRC || + isp_pad =3D=3D STF_ISP_PAD_SRC_SS0 || + isp_pad =3D=3D STF_ISP_PAD_SRC_SS1) { + video->formats =3D formats_pix_isp; + video->nformats =3D ARRAY_SIZE(formats_pix_isp); + video->bpl_alignment =3D STFCAMSS_FRAME_WIDTH_ALIGN_8; + vdev->ioctl_ops =3D &stf_vid_isp_ioctl_ops; + } else { + video->formats =3D formats_raw_isp; + video->nformats =3D ARRAY_SIZE(formats_raw_isp); + video->bpl_alignment =3D STFCAMSS_FRAME_WIDTH_ALIGN_128; + vdev->ioctl_ops =3D &stf_vid_isp_ioctl_ops; + } + + ret =3D 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 =3D &stf_vid_fops; + vdev->device_caps =3D V4L2_CAP_VIDEO_CAPTURE; + vdev->vfl_dir =3D VFL_DIR_RX; + vdev->device_caps |=3D V4L2_CAP_STREAMING | V4L2_CAP_READWRITE; + vdev->release =3D stf_video_release; + vdev->v4l2_dev =3D v4l2_dev; + vdev->queue =3D &video->vb2_q; + vdev->lock =3D &video->lock; + strscpy(vdev->name, name, sizeof(vdev->name)); + + ret =3D 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); +} diff --git a/drivers/media/platform/starfive/stf_video.h b/drivers/media/pl= atform/starfive/stf_video.h new file mode 100644 index 000000000000..b41257cde09a --- /dev/null +++ b/drivers/media/platform/starfive/stf_video.h @@ -0,0 +1,89 @@ +/* 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 +#include +#include +#include +#include +#include + +#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 */ diff --git a/drivers/media/platform/starfive/stf_vin.c b/drivers/media/plat= form/starfive/stf_vin.c new file mode 100644 index 000000000000..38d0e2d74280 --- /dev/null +++ b/drivers/media/platform/starfive/stf_vin.c @@ -0,0 +1,1314 @@ +// SPDX-License-Identifier: GPL-2.0 +/* + * stf_vin.c + * + * StarFive Camera Subsystem - VIN Module + * + * Copyright (C) 2021-2023 StarFive Technology Co., Ltd. + */ +#include + +#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[] =3D { + { 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[] =3D { + { 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[] =3D { + { MEDIA_BUS_FMT_Y12_1X12, 8}, +}; + +static const struct vin_format_table vin_formats_table[] =3D { + /* 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 *outp= ut); +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 =3D NULL; + + switch (line_id) { + case VIN_LINE_WR: + name =3D "wr"; + break; + case VIN_LINE_ISP: + name =3D "isp0"; + break; + case VIN_LINE_ISP_SS0: + name =3D "isp0_ss0"; + break; + case VIN_LINE_ISP_SS1: + name =3D "isp0_ss1"; + break; + case VIN_LINE_ISP_RAW: + name =3D "isp0_raw"; + break; + default: + name =3D "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 =3D line % STF_ISP_LINE_MAX; + if (line_id =3D=3D 0) + line_id =3D STF_ISP_LINE_SRC_RAW; + } else { + line_id =3D 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 =3D=3D VIN_LINE_WR) + pad_id =3D STF_ISP_PAD_SINK; + else if ((line > VIN_LINE_WR) && (line < VIN_LINE_MAX)) + pad_id =3D vin_map_isp_line(line); + else + pad_id =3D def; + + return pad_id; +} + +int stf_vin_subdev_init(struct stfcamss *stfcamss) +{ + struct device *dev =3D stfcamss->dev; + struct stf_vin_dev *vin_dev =3D stfcamss->vin_dev; + int i, ret =3D 0; + + vin_dev->stfcamss =3D stfcamss; + vin_dev->hw_ops =3D &vin_ops; + + vin_dev->isr_ops =3D devm_kzalloc(dev, sizeof(*vin_dev->isr_ops), + GFP_KERNEL); + if (!vin_dev->isr_ops) + return -ENOMEM; + vin_dev->isr_ops->isr_buffer_done =3D vin_buffer_done; + vin_dev->isr_ops->isr_change_buffer =3D vin_change_buffer; + + atomic_set(&vin_dev->ref_count, 0); + + ret =3D 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 =3D 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 =3D 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 =3D 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 =3D 0; + + for (i =3D 0; i < STF_DUMMY_MODULE_NUMS; i++) { + struct dummy_buffer *dummy_buffer =3D &vin_dev->dummy_buffer[i]; + + mutex_init(&dummy_buffer->stream_lock); + dummy_buffer->nums =3D + i =3D=3D 0 ? VIN_DUMMY_BUFFER_NUMS : ISP_DUMMY_BUFFER_NUMS; + dummy_buffer->stream_count =3D 0; + dummy_buffer->buffer =3D + devm_kzalloc(dev, + dummy_buffer->nums * sizeof(struct vin_dummy_buffer), + GFP_KERNEL); + atomic_set(&dummy_buffer->frame_skip, 0); + } + + for (i =3D VIN_LINE_WR; + i < STF_ISP_LINE_MAX + 1; i++) { + struct vin_line *l =3D &vin_dev->line[i]; + + l->video_out.type =3D V4L2_BUF_TYPE_VIDEO_CAPTURE; + l->video_out.stfcamss =3D stfcamss; + l->id =3D i; + l->sdev_type =3D SUBDEV_TYPE_VIN; + l->formats =3D vin_formats_table[i].fmts; + l->nformats =3D vin_formats_table[i].nfmts; + spin_lock_init(&l->output_lock); + + mutex_init(&l->stream_lock); + l->stream_count =3D 0; + mutex_init(&l->power_lock); + l->power_count =3D 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 =3D false; + + while (1) { + pad =3D &entity->pads[0]; + if (!(pad->flags & MEDIA_PAD_FL_SINK)) + return LINK_ERROR; + + pad =3D media_pad_remote_pad_first(pad); + if (!pad || !is_media_entity_v4l2_subdev(pad->entity)) + return LINK_ERROR; + + entity =3D pad->entity; + subdev =3D 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 =3D true; + } else { + return LINK_ERROR; + } + } +} + +static int vin_set_power(struct v4l2_subdev *sd, int on) +{ + struct vin_line *line =3D v4l2_get_subdevdata(sd); + struct stf_vin_dev *vin_dev =3D line_to_vin_dev(line); + struct stfcamss *stfcamss =3D vin_dev->stfcamss; + enum link link; + + mutex_lock(&line->power_lock); + if (on) { + if (line->power_count =3D=3D 0) + vin_init_outputs(line); + line->power_count++; + } else { + if (line->power_count =3D=3D 0) { + st_err(ST_VIN, "line power off on power_count =3D 0\n"); + goto exit_line; + } + line->power_count--; + } +exit_line: + mutex_unlock(&line->power_lock); + + mutex_lock(&vin_dev->power_lock); + link =3D vin_get_link(&sd->entity); + if (link =3D=3D LINK_ERROR) + goto exit; + + if (on) { + if (vin_dev->power_count =3D=3D 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 =3D=3D 0) { + st_err(ST_VIN, "vin_dev power off on power_count=3D0\n"); + goto exit; + } + if (vin_dev->power_count =3D=3D 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 =3D 0; + unsigned int isp_ctrl_skip_frames =3D 0; + struct media_entity *sensor; + struct v4l2_subdev_frame_interval fi =3D { + .pad =3D 0, + }; + + sensor =3D stfcamss_find_sensor(&line->subdev.entity); + if (sensor) { + int fps =3D 0; + struct v4l2_subdev *subdev =3D + media_entity_to_v4l2_subdev(sensor); + + v4l2_subdev_call(subdev, video, g_frame_interval, &fi); + fps =3D fi.interval.denominator; + + if (fps > 0 && fps <=3D 90) + isp_ctrl_skip_frames =3D + 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 <=3D VIN_FRAME_DROP_MIN_VAL) + isp_ctrl_skip_frames =3D VIN_FRAME_DROP_MIN_VAL; + + v4l2_subdev_call(subdev, sensor, g_skip_frames, &frame_skip); + + frame_skip +=3D isp_ctrl_skip_frames; + + if (frame_skip > VIN_FRAME_DROP_MAX_VAL) + frame_skip =3D 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 =3D &line->output; + unsigned long flags; + + spin_lock_irqsave(&line->output_lock, flags); + + output->state =3D VIN_OUTPUT_IDLE; + + output->buf[0] =3D vin_buf_get_pending(output); + + if (!output->buf[0] && output->buf[1]) { + output->buf[0] =3D output->buf[1]; + output->buf[1] =3D NULL; + } + + if (output->buf[0]) + output->state =3D VIN_OUTPUT_SINGLE; + + output->sequence =3D 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 =3D &line->output; + unsigned long flags; + + spin_lock_irqsave(&line->output_lock, flags); + + output->state =3D 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 =3D 0; + + switch (line->id) { + case VIN_LINE_WR: + dummy_module =3D 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 =3D STF_DUMMY_ISP; + break; + default: + dummy_module =3D 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 =3D vin_dev->stfcamss->dev; + struct dummy_buffer *dummy_buffer =3D + &vin_dev->dummy_buffer[dummy_module]; + struct vin_dummy_buffer *buffer =3D NULL; + int ret =3D 0, i; + u32 aligns; + + for (i =3D 0; i < dummy_buffer->nums; i++) { + buffer =3D &vin_dev->dummy_buffer[dummy_module].buffer[i]; + buffer->width =3D fmt->width; + buffer->height =3D fmt->height; + buffer->mcode =3D fmt->code; + if (i =3D=3D STF_VIN_PAD_SINK) { + aligns =3D ALIGN(fmt->width * 4, + STFCAMSS_FRAME_WIDTH_ALIGN_8); + buffer->buffer_size =3D PAGE_ALIGN(aligns * fmt->height); + } else if (i =3D=3D STF_ISP_PAD_SRC || + i =3D=3D STF_ISP_PAD_SRC_SS0 || + i =3D=3D STF_ISP_PAD_SRC_SS1) { + aligns =3D ALIGN(fmt->width, + STFCAMSS_FRAME_WIDTH_ALIGN_8); + buffer->buffer_size =3D + PAGE_ALIGN(aligns * fmt->height * 3 / 2); + } else if (i =3D=3D STF_ISP_PAD_SRC_RAW) { + aligns =3D ALIGN(fmt->width * ISP_RAW_DATA_BITS / 8, + STFCAMSS_FRAME_WIDTH_ALIGN_128); + buffer->buffer_size =3D PAGE_ALIGN(aligns * fmt->height); + } else { + continue; + } + + buffer->vaddr =3D dma_alloc_coherent(dev, + buffer->buffer_size, + &buffer->paddr[0], + GFP_DMA | GFP_KERNEL); + + if (buffer->vaddr) { + if (i =3D=3D STF_ISP_PAD_SRC || + i =3D=3D STF_ISP_PAD_SRC_SS0 || + i =3D=3D STF_ISP_PAD_SRC_SS1) + buffer->paddr[1] =3D + (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_m= odule) +{ + struct device *dev =3D vin_dev->stfcamss->dev; + struct dummy_buffer *dummy_buffer =3D + &vin_dev->dummy_buffer[dummy_module]; + struct vin_dummy_buffer *buffer =3D NULL; + int i; + + for (i =3D 0; i < dummy_buffer->nums; i++) { + buffer =3D &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 =3D line_to_vin_dev(line); + int dummy_module =3D vin_line_to_dummy_module(line); + struct dummy_buffer *dummy_buffer =3D + &vin_dev->dummy_buffer[dummy_module]; + struct vin_dummy_buffer *buffer =3D NULL; + + switch (pad) { + case STF_VIN_PAD_SINK: + if (line->id =3D=3D VIN_LINE_WR) { + buffer =3D &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 =3D &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 =3D &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 =3D &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 =3D &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 =3D &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 =3D &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 =3D &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 =3D &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 =3D v4l2_get_subdevdata(sd); + struct stf_vin_dev *vin_dev =3D line_to_vin_dev(line); + int dummy_module =3D vin_line_to_dummy_module(line); + struct dummy_buffer *dummy_buffer =3D + &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 =3D stfcamss_find_sensor(&line->subdev.entity); + if (!sensor) + return -EINVAL; + + sensor_sd =3D media_entity_to_v4l2_subdev(sensor); + + sensor_fmt.pad =3D 0; + sensor_fmt.which =3D V4L2_SUBDEV_FORMAT_ACTIVE; + ret =3D 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 =3D __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 =3D=3D 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 =3D=3D 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 =3D vin_get_link(&sd->entity); + if (link =3D=3D LINK_ERROR) + goto exit; + + if (enable) { + if (line->stream_count =3D=3D 0) { + vin_dev->hw_ops->vin_stream_set(vin_dev, 1, + sensor_fmt, link); + if (line->id =3D=3D 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 =3D=3D 1) { + if (line->id =3D=3D 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 =3D=3D 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 =3D 0; i < line->nformats; i++) + if (fmt->code =3D=3D line->formats[i].code) + break; + + /* If not found, use UYVY as default */ + if (i >=3D line->nformats) + fmt->code =3D line->formats[0].code; + + fmt->width =3D clamp_t(u32, fmt->width, + STFCAMSS_FRAME_MIN_WIDTH, + STFCAMSS_FRAME_MAX_WIDTH); + fmt->height =3D clamp_t(u32, fmt->height, + STFCAMSS_FRAME_MIN_HEIGHT, + STFCAMSS_FRAME_MAX_HEIGHT); + + fmt->field =3D V4L2_FIELD_NONE; + fmt->colorspace =3D V4L2_COLORSPACE_SRGB; + fmt->flags =3D 0; + break; + + case STF_VIN_PAD_SRC: + /* Set and return a format same as sink pad */ + *fmt =3D *__vin_get_format(line, state, STF_VIN_PAD_SINK, which); + break; + } + + fmt->colorspace =3D 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 =3D v4l2_get_subdevdata(sd); + + if (code->index >=3D line->nformats) + return -EINVAL; + if (code->pad =3D=3D STF_VIN_PAD_SINK) { + code->code =3D line->formats[code->index].code; + } else { + struct v4l2_mbus_framefmt *sink_fmt; + + sink_fmt =3D __vin_get_format(line, state, STF_VIN_PAD_SINK, + code->which); + + code->code =3D sink_fmt->code; + if (!code->code) + return -EINVAL; + } + code->flags =3D 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 =3D v4l2_get_subdevdata(sd); + struct v4l2_mbus_framefmt format; + + if (fse->index !=3D 0) + return -EINVAL; + + format.code =3D fse->code; + format.width =3D 1; + format.height =3D 1; + vin_try_format(line, state, fse->pad, &format, fse->which); + fse->min_width =3D format.width; + fse->min_height =3D format.height; + + if (format.code !=3D fse->code) + return -EINVAL; + + format.code =3D fse->code; + format.width =3D -1; + format.height =3D -1; + vin_try_format(line, state, fse->pad, &format, fse->which); + fse->max_width =3D format.width; + fse->max_height =3D 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 =3D v4l2_get_subdevdata(sd); + struct v4l2_mbus_framefmt *format; + + format =3D __vin_get_format(line, state, fmt->pad, fmt->which); + if (!format) + return -EINVAL; + + fmt->format =3D *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 =3D 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 =3D __vin_get_format(line, state, fmt->pad, fmt->which); + if (!format) + return -EINVAL; + + mutex_lock(&line->stream_lock); + if (line->stream_count) { + fmt->format =3D *format; + mutex_unlock(&line->stream_lock); + goto out; + } else { + vin_try_format(line, state, fmt->pad, &fmt->format, fmt->which); + *format =3D fmt->format; + } + mutex_unlock(&line->stream_lock); + + if (fmt->pad =3D=3D STF_VIN_PAD_SINK) { + /* Propagate the format from sink to source */ + format =3D __vin_get_format(line, state, STF_VIN_PAD_SRC, + fmt->which); + + *format =3D 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 =3D { + .pad =3D STF_VIN_PAD_SINK, + .which =3D fh ? + V4L2_SUBDEV_FORMAT_TRY : V4L2_SUBDEV_FORMAT_ACTIVE, + .format =3D { + .code =3D MEDIA_BUS_FMT_RGB565_2X8_LE, + .width =3D 1920, + .height =3D 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 =3D &line->output; + struct stf_vin_dev *vin_dev =3D 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 =3D 0; + + if (output->buf[0]) { + ping_addr =3D output->buf[0]->addr[0]; + y_addr =3D output->buf[0]->addr[0]; + uv_addr =3D output->buf[0]->addr[1]; + } else { + return; + } + + if (output->buf[1]) + pong_addr =3D output->buf[1]->addr[0]; + else + pong_addr =3D 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 =3D=3D 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 =3D &line->output; + + output->state =3D VIN_OUTPUT_OFF; + output->buf[0] =3D NULL; + output->buf[1] =3D NULL; + output->active_buf =3D 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 =3D NULL; + + if (!list_empty(&output->ready_bufs)) { + buffer =3D 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 *outp= ut) +{ + struct stfcamss_buffer *buffer =3D NULL; + + if (!list_empty(&output->pending_bufs)) { + buffer =3D 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 =3D &line->output; + + switch (output->state) { + case VIN_OUTPUT_CONTINUOUS: + output->state =3D VIN_OUTPUT_SINGLE; + output->active_buf =3D !output->active_buf; + break; + case VIN_OUTPUT_SINGLE: + output->state =3D 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 =3D &line->output; + + switch (output->state) { + case VIN_OUTPUT_CONTINUOUS: + output->active_buf =3D !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] =3D new_buf; + vin_output_init_addrs(line); + output->state =3D 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] =3D output->last_buffer; + output->last_buffer =3D NULL; + } else { + st_err(ST_VIN, "stop state lost lastbuffer!\n"); + } + output->state =3D 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 =3D &line->output; + unsigned long flags; + u64 ts =3D ktime_get_ns(); + + if (output->state =3D=3D VIN_OUTPUT_OFF || + output->state =3D=3D VIN_OUTPUT_RESERVED) + return; + + spin_lock_irqsave(&line->output_lock, flags); + + while ((ready_buf =3D vin_buf_get_ready(output))) { + ready_buf->vb.vb2_buf.timestamp =3D ts; + ready_buf->vb.sequence =3D 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 =3D &line->output; + struct stf_vin_dev *vin_dev =3D line_to_vin_dev(line); + dma_addr_t *new_addr; + unsigned long flags; + u32 active_index; + + if (output->state =3D=3D VIN_OUTPUT_OFF || + output->state =3D=3D VIN_OUTPUT_STOPPING || + output->state =3D=3D VIN_OUTPUT_RESERVED || + output->state =3D=3D VIN_OUTPUT_IDLE) + return; + + spin_lock_irqsave(&line->output_lock, flags); + + active_index =3D output->active_buf; + + ready_buf =3D output->buf[active_index]; + if (!ready_buf) { + st_err_ratelimited(ST_VIN, "Missing ready buf %d %d!\n", + active_index, output->state); + active_index =3D !active_index; + ready_buf =3D 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] =3D vin_buf_get_pending(output); + if (!output->buf[active_index]) { + /* No next buffer - set same address */ + new_addr =3D ready_buf->addr; + vin_buf_update_on_last(line); + } else { + new_addr =3D output->buf[active_index]->addr; + vin_buf_update_on_next(line); + } + + if (output->state =3D=3D VIN_OUTPUT_STOPPING) { + output->last_buffer =3D 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 =3D=3D 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 =3D container_of(vid, struct vin_line, video_out); + struct vin_output *output; + unsigned long flags; + + output =3D &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 =3D container_of(vid, struct vin_line, video_out); + struct vin_output *output =3D &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 =3D NULL; + } + output->buf[0] =3D NULL; + output->buf[1] =3D 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 =3D { + .s_power =3D vin_set_power, +}; + +static const struct v4l2_subdev_video_ops vin_video_ops =3D { + .s_stream =3D vin_set_stream, +}; + +static const struct v4l2_subdev_pad_ops vin_pad_ops =3D { + .enum_mbus_code =3D vin_enum_mbus_code, + .enum_frame_size =3D vin_enum_frame_size, + .get_fmt =3D vin_get_format, + .set_fmt =3D vin_set_format, +}; + +static const struct v4l2_subdev_ops vin_v4l2_ops =3D { + .core =3D &vin_core_ops, + .video =3D &vin_video_ops, + .pad =3D &vin_pad_ops, +}; + +static const struct v4l2_subdev_internal_ops vin_v4l2_internal_ops =3D { + .open =3D vin_init_formats, +}; + +static const struct stfcamss_video_ops stfcamss_vin_video_ops =3D { + .queue_buffer =3D vin_queue_buffer, + .flush_buffers =3D vin_flush_buffers, +}; + +static const struct media_entity_operations vin_media_ops =3D { + .link_setup =3D vin_link_setup, + .link_validate =3D 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 =3D 0; i < STF_ISP_LINE_MAX + 1; i++) { + char name[32]; + char *sub_name =3D vin_get_line_subdevname(i); + + sd =3D &vin_dev->line[i].subdev; + pads =3D vin_dev->line[i].pads; + video_out =3D &vin_dev->line[i].video_out; + video_out->id =3D i; + + v4l2_subdev_init(sd, &vin_v4l2_ops); + sd->internal_ops =3D &vin_v4l2_internal_ops; + sd->flags |=3D 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 =3D 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 =3D MEDIA_PAD_FL_SINK; + pads[STF_VIN_PAD_SRC].flags =3D MEDIA_PAD_FL_SOURCE; + + sd->entity.function =3D + MEDIA_ENT_F_PROC_VIDEO_PIXEL_FORMATTER; + sd->entity.ops =3D &vin_media_ops; + ret =3D 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 =3D 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 =3D &stfcamss_vin_video_ops; + video_out->bpl_alignment =3D 16 * 8; + + snprintf(name, ARRAY_SIZE(name), "%s_%s%d", + sd->name, "video", i); + ret =3D 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 =3D 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 >=3D 0; i--) { + sd =3D &vin_dev->line[i].subdev; + video_out =3D &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 =3D 0; i < STF_DUMMY_MODULE_NUMS; i++) + mutex_destroy(&vin_dev->dummy_buffer[i].stream_lock); + + for (i =3D 0; i < STF_ISP_LINE_MAX + 1; i++) { + sd =3D &vin_dev->line[i].subdev; + video_out =3D &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; +} diff --git a/drivers/media/platform/starfive/stf_vin.h b/drivers/media/plat= form/starfive/stf_vin.h new file mode 100644 index 000000000000..7a0f39e6d2b9 --- /dev/null +++ b/drivers/media/platform/starfive/stf_vin.h @@ -0,0 +1,194 @@ +/* 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 +#include +#include + +#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 =3D -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 =3D -1, + VIN_LINE_WR =3D 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 */ diff --git a/drivers/media/platform/starfive/stf_vin_hw_ops.c b/drivers/med= ia/platform/starfive/stf_vin_hw_ops.c new file mode 100644 index 000000000000..98306c4d4d89 --- /dev/null +++ b/drivers/media/platform/starfive/stf_vin_hw_ops.c @@ -0,0 +1,357 @@ +// 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 =3D priv; + struct stfcamss *stfcamss =3D vin_dev->stfcamss; + struct dummy_buffer *dummy_buffer =3D + &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 =3D 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 =3D priv; + void __iomem *ispbase =3D stf_vin_get_ispbase(vin_dev); + u32 int_status; + + int_status =3D 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 =3D priv; + void __iomem *ispbase =3D stf_vin_get_ispbase(vin_dev); + u32 int_status; + + int_status =3D 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 =3D priv; + struct stf_isp_dev *isp_dev; + void __iomem *ispbase =3D stf_vin_get_ispbase(vin_dev); + u32 int_status, value; + + isp_dev =3D vin_dev->stfcamss->isp_dev; + + int_status =3D reg_read(ispbase, ISP_REG_ISP_CTRL_0); + if (int_status & BIT(27)) { + struct dummy_buffer *dummy_buffer =3D + &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 =3D 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 =3D 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 =3D 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 =3D 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 =3D 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 =3D=3D MEDIA_BUS_FMT_SRGGB10_1X10 || + sensor_fmt.format.code =3D=3D MEDIA_BUS_FMT_SGRBG10_1X10 || + sensor_fmt.format.code =3D=3D MEDIA_BUS_FMT_SGBRG10_1X10 || + sensor_fmt.format.code =3D=3D 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 =3D vin_dev->stfcamss; + unsigned int value =3D 0; + + if (enable) { + value =3D ~(0x1 << 1); + reg_set_bit(stfcamss->syscon_base, SYSCONSAIF_SYSCFG(28), + U0_VIN_CNFG_AXIWR0_INTR_MASK, value); + } else { + /* clear vin interrupt */ + value =3D 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 =3D 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 =3D 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 =3D 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 =3D 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 =3D 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 =3D 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 =3D { + .vin_clk_enable =3D stf_vin_clk_enable, + .vin_clk_disable =3D stf_vin_clk_disable, + .vin_wr_stream_set =3D stf_vin_wr_stream_set, + .vin_wr_irq_enable =3D stf_vin_wr_irq_enable, + .vin_stream_set =3D stf_vin_stream_set, + .vin_wr_set_ping_addr =3D stf_vin_wr_set_ping_addr, + .vin_wr_set_pong_addr =3D stf_vin_wr_set_pong_addr, + .vin_isp_set_yuv_addr =3D stf_vin_isp_set_yuv_addr, + .vin_isp_set_raw_addr =3D stf_vin_isp_set_raw_addr, + .vin_isp_set_ss0_addr =3D stf_vin_isp_set_ss0_addr, + .vin_isp_set_ss1_addr =3D stf_vin_isp_set_ss1_addr, + .vin_wr_irq_handler =3D stf_vin_wr_irq_handler, + .vin_isp_irq_handler =3D stf_vin_isp_irq_handler, + .vin_isp_irq_csi_handler =3D stf_vin_isp_irq_csi_handler, + .vin_isp_irq_csiline_handler =3D stf_vin_isp_irq_csiline_handler, +}; diff --git a/include/uapi/linux/stf_isp_ioctl.h b/include/uapi/linux/stf_is= p_ioctl.h new file mode 100644 index 000000000000..3f302ef235d2 --- /dev/null +++ b/include/uapi/linux/stf_isp_ioctl.h @@ -0,0 +1,127 @@ +/* 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 + +#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 =3D 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 =3D 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 */ --=20 2.34.1