From 88d467400f311f71aaa65c57d765e42bdcb41907 Mon Sep 17 00:00:00 2001
From: Ondrej Jirman <megi@xff.cz>
Date: Thu, 9 Jun 2022 01:04:18 +0200
Subject: [PATCH 324/463] media: i2c: imx258: Add debug register access

This is useful during development.

Signed-off-by: Ondrej Jirman <megi@xff.cz>
---
 drivers/media/i2c/imx258.c | 48 ++++++++++++++++++++++++++++++++++++++
 1 file changed, 48 insertions(+)

diff --git a/drivers/media/i2c/imx258.c b/drivers/media/i2c/imx258.c
index fea9d049aeb4c..5adcfc98a11f4 100644
--- a/drivers/media/i2c/imx258.c
+++ b/drivers/media/i2c/imx258.c
@@ -1194,6 +1194,53 @@ static int imx258_identify_module(struct imx258 *imx258)
 	return 0;
 }
 
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static int imx258_g_register(struct v4l2_subdev *sd,
+			     struct v4l2_dbg_register *reg)
+{
+	struct imx258 *imx258 = to_imx258(sd);
+	u64 val = 0;
+	int ret;
+
+	if (reg->reg > 0xffff)
+		return -EINVAL;
+
+	reg->size = 1;
+
+	mutex_lock(&imx258->mutex);
+	ret = cci_read(imx258->regmap, CCI_REG8(reg->reg), &val, NULL);
+	mutex_unlock(&imx258->mutex);
+	if (ret)
+		return -EIO;
+
+	reg->val = val;
+	return 0;
+}
+
+static int imx258_s_register(struct v4l2_subdev *sd,
+			     const struct v4l2_dbg_register *reg)
+{
+	struct imx258 *imx258 = to_imx258(sd);
+	int ret;
+
+	if (reg->reg > 0xffff || reg->val > 0xff)
+		return -EINVAL;
+
+	mutex_lock(&imx258->mutex);
+	ret = cci_write(imx258->regmap, CCI_REG8(reg->reg), reg->val, NULL);
+	mutex_unlock(&imx258->mutex);
+
+	return ret;
+}
+#endif
+
+static const struct v4l2_subdev_core_ops imx258_core_ops = {
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.g_register = imx258_g_register,
+	.s_register = imx258_s_register,
+#endif
+};
+
 static const struct v4l2_subdev_video_ops imx258_video_ops = {
 	.s_stream = imx258_set_stream,
 };
@@ -1207,6 +1254,7 @@ static const struct v4l2_subdev_pad_ops imx258_pad_ops = {
 };
 
 static const struct v4l2_subdev_ops imx258_subdev_ops = {
+	.core = &imx258_core_ops,
 	.video = &imx258_video_ops,
 	.pad = &imx258_pad_ops,
 };
-- 
2.46.0

