From 79e2ce880a356f76fa59e422718aecdda51ab1da Mon Sep 17 00:00:00 2001
From: Sach Lin <sach.lin@rock-chips.com>
Date: Mon, 6 Jun 2022 17:51:41 +0800
Subject: [PATCH] media: spi: rk1608: add crop config.

Signed-off-by: Sach Lin <sach.lin@rock-chips.com>
Change-Id: I34ee3bc8d71348afb42b874cf215a434ebdbcb52
---
 drivers/media/spi/rk1608_dphy.c | 26 ++++++++++++++++++++++----
 drivers/media/spi/rk1608_dphy.h |  2 ++
 2 files changed, 24 insertions(+), 4 deletions(-)

diff --git a/drivers/media/spi/rk1608_dphy.c b/drivers/media/spi/rk1608_dphy.c
index 9b302ba1c522..c38d1516cadd 100644
--- a/drivers/media/spi/rk1608_dphy.c
+++ b/drivers/media/spi/rk1608_dphy.c
@@ -393,20 +393,28 @@ static int rk1608_set_ctrl(struct v4l2_ctrl *ctrl)
 	return ret;
 }
 
+#define CROP_START(SRC, DST) (((SRC) - (DST)) / 2 / 4 * 4)
 static int rk1608_get_selection(struct v4l2_subdev *sd,
 		struct v4l2_subdev_pad_config *cfg,
 		struct v4l2_subdev_selection *sel)
 {
 	struct rk1608_dphy *pdata = to_state(sd);
 	u32 idx = pdata->fmt_inf_idx;
+	u32 width = pdata->fmt_inf[idx].mf.width;
+	u32 height = pdata->fmt_inf[idx].mf.height;
 
 	if (sel->target != V4L2_SEL_TGT_CROP_BOUNDS)
 		return -EINVAL;
 
-	sel->r.left = 0;
-	sel->r.top = 0;
-	sel->r.width = pdata->fmt_inf[idx].mf.width;
-	sel->r.height = pdata->fmt_inf[idx].mf.height;
+	if (pdata->fmt_inf[idx].hcrop && pdata->fmt_inf[idx].vcrop) {
+		width = pdata->fmt_inf[idx].hcrop;
+		height = pdata->fmt_inf[idx].vcrop;
+	}
+
+	sel->r.left = CROP_START(pdata->fmt_inf[idx].mf.width, width);
+	sel->r.top = CROP_START(pdata->fmt_inf[idx].mf.height, height);
+	sel->r.width = width;
+	sel->r.height = height;
 
 	return 0;
 }
@@ -728,6 +736,16 @@ static int rk1608_dphy_dt_property(struct rk1608_dphy *dphy)
 			if (ret)
 				dev_info(dphy->dev, "Can not get outch3-info!");
 
+			ret = of_property_read_u32(node, "hcrop",
+				&dphy->fmt_inf[idx].hcrop);
+			if (ret)
+				dev_warn(dphy->dev, "Can not get hcrop!");
+
+			ret = of_property_read_u32(node, "vcrop",
+				&dphy->fmt_inf[idx].vcrop);
+			if (ret)
+				dev_warn(dphy->dev, "Can not get vcrop!");
+
 			idx++;
 		}
 
diff --git a/drivers/media/spi/rk1608_dphy.h b/drivers/media/spi/rk1608_dphy.h
index e5dc7d38df9b..6d9903e7e1f3 100644
--- a/drivers/media/spi/rk1608_dphy.h
+++ b/drivers/media/spi/rk1608_dphy.h
@@ -24,6 +24,8 @@ struct rk1608_fmt_inf {
 	struct v4l2_mbus_framefmt mf;
 	struct rk1608_chinf in_ch[4];
 	struct rk1608_chinf out_ch[4];
+	u32 hcrop;
+	u32 vcrop;
 };
 
 struct rk1608_dphy {