From 74c23276a45e3f219e30b5a20087c6e18348427f Mon Sep 17 00:00:00 2001
From: Tim Zimmermann <tim@linux4.de>
Date: Mon, 1 Aug 2022 07:45:38 +0200
Subject: [PATCH] video: fbdev: exynos: Support new multi res ioctls

Change-Id: I2abfb0fa1a54139e66f30621b5c05f4c06daed49
---
 drivers/video/fbdev/exynos/dpu20/decon.h      | 14 +++++
 drivers/video/fbdev/exynos/dpu20/decon_core.c | 61 +++++++++++++++++++
 2 files changed, 75 insertions(+)

diff --git a/drivers/video/fbdev/exynos/dpu20/decon.h b/drivers/video/fbdev/exynos/dpu20/decon.h
index 656a5826b057..aa4dc272a033 100644
--- a/drivers/video/fbdev/exynos/dpu20/decon.h
+++ b/drivers/video/fbdev/exynos/dpu20/decon.h
@@ -580,6 +580,15 @@ struct dpu_size_info {
 	u32 h_out;
 };
 
+struct decon_display_mode {
+	uint32_t index;
+	uint32_t width;
+	uint32_t height;
+	uint32_t mm_width;
+	uint32_t mm_height;
+	uint32_t fps;
+};
+
 /**
  * Display Subsystem event management status.
  *
@@ -1942,6 +1951,11 @@ int _decon_enable(struct decon_device *decon, enum decon_state state);
 #define EXYNOS_GET_COLOR_MODE		_IOW('F', 601, struct decon_color_mode_info)
 #define EXYNOS_SET_COLOR_MODE		_IOW('F', 602, __u32)
 
+/* Display mode */
+#define EXYNOS_GET_DISPLAY_MODE_NUM	_IOW('F', 700, u32)
+#define EXYNOS_GET_DISPLAY_MODE		_IOW('F', 701, struct decon_display_mode)
+#define EXYNOS_SET_DISPLAY_MODE		_IOW('F', 702, struct decon_display_mode)
+
 /* EDID data */
 #define EXYNOS_GET_EDID		_IOW('F', 800, struct decon_edid_data)
 
diff --git a/drivers/video/fbdev/exynos/dpu20/decon_core.c b/drivers/video/fbdev/exynos/dpu20/decon_core.c
index c947c829cdf7..41594a39ae87 100644
--- a/drivers/video/fbdev/exynos/dpu20/decon_core.c
+++ b/drivers/video/fbdev/exynos/dpu20/decon_core.c
@@ -3238,6 +3238,8 @@ static int decon_ioctl(struct fb_info *info, unsigned int cmd,
 	struct dpp_restrictions_info __user *argp_res;
 	struct decon_color_mode_info cm_info;
 	struct decon_edid_data edid_data;
+	struct decon_display_mode dm_info;
+	struct decon_reg_data decon_regs;
 	u32 color_mode;
 	int ret = 0;
 	u32 crtc;
@@ -3247,6 +3249,7 @@ static int decon_ioctl(struct fb_info *info, unsigned int cmd,
 	u32 pwr;
 	int i;
 	u32 cm_num;
+	u32 dm_num;
 
 	decon_hiber_block_exit(decon);
 	switch (cmd) {
@@ -3581,6 +3584,64 @@ static int decon_ioctl(struct fb_info *info, unsigned int cmd,
 		}
 		break;
 
+	case EXYNOS_GET_DISPLAY_MODE_NUM:
+		dm_num = mres_info->mres_number;
+		if (copy_to_user((u32 __user *)arg, &dm_num, sizeof(u32)))
+                        ret = -EFAULT;
+
+		break;
+
+	case EXYNOS_GET_DISPLAY_MODE:
+		if (copy_from_user(&dm_info,
+				   (struct decon_display_mode __user *)arg,
+				   sizeof(struct decon_display_mode))) {
+			ret = -EFAULT;
+			break;
+		}
+
+		if (dm_info.index < mres_info->mres_number) {
+			dm_info.width = mres_info->res_info[dm_info.index].width;
+			dm_info.height = mres_info->res_info[dm_info.index].height;
+			dm_info.mm_width = lcd_info->width;
+			dm_info.mm_height = lcd_info->height;
+			dm_info.fps = lcd_info->fps;
+		} else {
+			ret = -EINVAL;
+			break;
+		}
+
+		if (copy_to_user((struct decon_display_mode __user *)arg,
+				&dm_info,
+				sizeof(struct decon_display_mode))) {
+			ret = -EFAULT;
+			break;
+		}
+
+		break;
+
+	case EXYNOS_SET_DISPLAY_MODE:
+		if (copy_from_user(&dm_info,
+				   (struct decon_display_mode __user *)arg,
+				   sizeof(struct decon_display_mode))) {
+			ret = -EFAULT;
+			break;
+		}
+
+		if (dm_info.index < mres_info->mres_number) {
+			if (!IS_DECON_OFF_STATE(decon)) {
+				memset(&decon_regs, 0, sizeof(struct decon_reg_data));
+				decon_regs.mres_update = true;
+				decon_regs.lcd_width = mres_info->res_info[dm_info.index].width;
+				decon_regs.lcd_height = mres_info->res_info[dm_info.index].height;
+				decon_regs.mres_idx = dm_info.index;
+				dpu_set_mres_config(decon, &decon_regs);
+			}
+		} else {
+			ret = -EINVAL;
+			break;
+		}
+
+		break;
 
 	default:
 		decon_err("DECON:ERR:%s:invalid command : 0x%x\n", __func__, cmd);