v4l2: starfive v4l2 driver support module load/unload

7 files changed, 92 insertions, 82 deletions

diff --git a/drivers/media/platform/starfive/Kconfig b/drivers/media/platform/starfive/Kconfig
index 49e4b09637bd..de640b98624c 100644
--- a/drivers/media/platform/starfive/Kconfig
+++ b/drivers/media/platform/starfive/Kconfig
@@ -14,38 +14,30 @@ config VIDEO_STARFIVE_VIN
 	  To compile this driver as a module, choose M here: the module
 	  will be called stf-vin.
 
-choice
-	prompt "Image Sensor for VIC board"
-	default VIN_SENSOR_IMX219
-	depends on VIDEO_STARFIVE_VIN
-	optional
-
 config VIN_SENSOR_OV5640
-	bool "VIN SENSOR support OV5640"
+	tristate "VIN SENSOR support OV5640"
 	depends on VIDEO_STARFIVE_VIN
 	select V4L2_FWNODE
 	help
 	  Say Y here if you want to have support for VIN sensor OV5640
 
 config VIN_SENSOR_SC2235
-	bool "VIN SENSOR support SC2235"
+	tristate "VIN SENSOR support SC2235"
 	depends on VIDEO_STARFIVE_VIN
 	select V4L2_FWNODE
 	help
 	  Say Y here if you want to have support for VIN sensor SC2235
 
 config VIN_SENSOR_OV4689
-	bool "VIN SENSOR support OV4689"
+	tristate "VIN SENSOR support OV4689"
 	depends on VIDEO_STARFIVE_VIN
 	select V4L2_FWNODE
 	help
 	  Say Y here if you want to have support for VIN sensor OV4689
 
 config VIN_SENSOR_IMX219
-	bool "VIN SENSOR support imx219"
+	tristate "VIN SENSOR support imx219"
 	depends on VIDEO_STARFIVE_VIN
 	select V4L2_FWNODE
 	help
 	  Say Y here if you want to have support for VIN sensor IMX219
-
-endchoice
diff --git a/drivers/media/platform/starfive/Makefile b/drivers/media/platform/starfive/Makefile
index 5353d3a7de49..a4cb44725c51 100644
--- a/drivers/media/platform/starfive/Makefile
+++ b/drivers/media/platform/starfive/Makefile
@@ -8,16 +8,20 @@ obj-$(CONFIG_VIN_SENSOR_OV5640) += ov5640.o
 obj-$(CONFIG_VIN_SENSOR_SC2235) += sc2235.o
 obj-$(CONFIG_VIN_SENSOR_OV4689) += ov4689_mipi.o
 obj-$(CONFIG_VIN_SENSOR_IMX219) += imx219_mipi.o
-obj-$(CONFIG_VIDEO_STARFIVE_VIN) += stfcamss.o \
-								stf_event.o \
-								stf_dvp.o \
-								stf_csi.o \
-								stf_csiphy.o \
-								stf_isp.o \
-								stf_video.o \
-								stf_vin.o \
-								stf_vin_hw_ops.o \
-								stf_csi_hw_ops.o \
-								stf_csiphy_hw_ops.o \
-								stf_isp_hw_ops.o \
-								stf_dvp_hw_ops.o
+
+starfivecamss-objs += stfcamss.o \
+			stf_event.o \
+			stf_dvp.o \
+			stf_csi.o \
+			stf_csiphy.o \
+			stf_isp.o \
+			stf_video.o \
+			stf_vin.o \
+			stf_vin_hw_ops.o \
+			stf_csi_hw_ops.o \
+			stf_csiphy_hw_ops.o \
+			stf_isp_hw_ops.o \
+			stf_dvp_hw_ops.o \
+			stf_dmabuf.o
+
+obj-$(CONFIG_VIDEO_STARFIVE_VIN) += starfivecamss.o \
diff --git a/drivers/media/platform/starfive/ov4689_mipi.c b/drivers/media/platform/starfive/ov4689_mipi.c
index 64d850dbd136..036aef4ee181 100644
--- a/drivers/media/platform/starfive/ov4689_mipi.c
+++ b/drivers/media/platform/starfive/ov4689_mipi.c
@@ -1401,8 +1401,6 @@ static int ov4689_mod_reg(struct ov4689_dev *sensor, u16 reg, u8 mask, u8 val)
 static int ov4689_set_timings(struct ov4689_dev *sensor,
 			const struct ov4689_mode_info *mode)
 {
-	int ret;
-
 	return 0;
 }
 
@@ -1416,7 +1414,6 @@ static int ov4689_load_regs(struct ov4689_dev *sensor,
 	u8 mask, val;
 	int ret = 0;
 
-	st_info(ST_SENSOR, "%s, mode = 0x%x\n", __func__, mode->id);
 	for (i = 0; i < mode->reg_data_size; ++i, ++regs) {
 		delay_ms = regs->delay_ms;
 		reg_addr = regs->reg_addr;
@@ -1437,6 +1434,7 @@ static int ov4689_load_regs(struct ov4689_dev *sensor,
 	return ov4689_set_timings(sensor, mode);
 }
 
+#if  0
 static int ov4689_get_exposure(struct ov4689_dev *sensor)
 {
 	int exp, ret;
@@ -1457,12 +1455,12 @@ static int ov4689_get_exposure(struct ov4689_dev *sensor)
 
 	return exp >> 4;
 }
+#endif
 
 static int ov4689_set_exposure(struct ov4689_dev *sensor, u32 exposure)
 {
 	int ret;
 
-	st_info(ST_SENSOR, "%s, exposure = 0x%x\n", __func__, exposure);
 	exposure <<= 4;
 
 	ret = ov4689_write_reg(sensor, OV4689_REG_EXPOSURE_LO, exposure & 0xff);
@@ -1493,8 +1491,6 @@ static int ov4689_get_gain(struct ov4689_dev *sensor)
 
 static int ov4689_set_gain(struct ov4689_dev *sensor, int gain)
 {
-	u8 val;
-
 	ov4689_write_reg(sensor, OV4689_REG_GAIN_H, (gain >> 16) & 0x3);
 	ov4689_write_reg(sensor, OV4689_REG_GAIN_M, (gain >> 8) & 0xff);
 	ov4689_write_reg(sensor, OV4689_REG_GAIN_L, gain & 0xff);
@@ -1506,6 +1502,7 @@ static int ov4689_set_stream_mipi(struct ov4689_dev *sensor, bool on)
 	return 0;
 }
 
+#if 0
 static int ov4689_get_sysclk(struct ov4689_dev *sensor)
 {
 	return 0;
@@ -1527,6 +1524,7 @@ static int ov4689_get_hts(struct ov4689_dev *sensor)
 		return ret;
 	return hts;
 }
+#endif
 
 static int ov4689_get_vts(struct ov4689_dev *sensor)
 {
@@ -1539,6 +1537,7 @@ static int ov4689_get_vts(struct ov4689_dev *sensor)
 	return vts;
 }
 
+#if 0
 static int ov4689_set_vts(struct ov4689_dev *sensor, int vts)
 {
 	return ov4689_write_reg16(sensor, OV4689_REG_TIMING_VTS, vts);
@@ -1568,6 +1567,7 @@ static int ov4689_set_binning(struct ov4689_dev *sensor, bool enable)
 {
 	return 0;
 }
+#endif
 
 static const struct ov4689_mode_info *
 ov4689_find_mode(struct ov4689_dev *sensor, enum ov4689_frame_rate fr,
@@ -1701,8 +1701,7 @@ static int ov4689_set_mipi_pclk(struct ov4689_dev *sensor,
 				unsigned long rate)
 {
 	const struct ov4689_mode_info *mode = sensor->current_mode;
-	const struct ov4689_mode_info *orig_mode = sensor->last_mode;
-	u8 mult, val;
+	u8 val;
 	int ret = 0;
 	int fps = ov4689_framerates[sensor->current_fr];
 	u16 htot, val16;
@@ -1715,10 +1714,6 @@ static int ov4689_set_mipi_pclk(struct ov4689_dev *sensor,
 	val16 = val << 8;
 	ret = ov4689_read_reg(sensor, OV4689_TIMING_HTS + 1, &val);
 	val16 |= val;
-	st_info(ST_SENSOR, "fps = %d, max_fps = %d, mode->htot = 0x%x, "
-			"htot = 0x%x, 0x%x = 0x%x\n",
-			fps, mode->max_fps, mode->htot,
-			htot, OV4689_TIMING_HTS, val16);
 	return 0;
 }
 
@@ -1739,7 +1734,6 @@ static int ov4689_set_mode_direct(struct ov4689_dev *sensor,
 static int ov4689_set_mode(struct ov4689_dev *sensor)
 {
 	const struct ov4689_mode_info *mode = sensor->current_mode;
-	const struct ov4689_mode_info *orig_mode = sensor->last_mode;
 	int ret = 0;
 
 	ret = ov4689_set_mode_direct(sensor, mode);
@@ -1948,7 +1942,6 @@ static int ov4689_try_frame_interval(struct ov4689_dev *sensor,
 			rate = i;
 		}
 	}
-	st_info(ST_SENSOR, "best_fps = %d, fps = %d\n", best_fps, fps);
 
 	fi->numerator = 1;
 	fi->denominator = best_fps;
@@ -1959,7 +1952,7 @@ find_mode:
 }
 
 static int ov4689_enum_mbus_code(struct v4l2_subdev *sd,
-				struct v4l2_subdev_pad_config *cfg,
+				struct v4l2_subdev_state *state,
 				struct v4l2_subdev_mbus_code_enum *code)
 {
 	if (code->pad != 0)
@@ -1973,7 +1966,7 @@ static int ov4689_enum_mbus_code(struct v4l2_subdev *sd,
 }
 
 static int ov4689_get_fmt(struct v4l2_subdev *sd,
-			struct v4l2_subdev_pad_config *cfg,
+			struct v4l2_subdev_state *state,
 			struct v4l2_subdev_format *format)
 {
 	struct ov4689_dev *sensor = to_ov4689_dev(sd);
@@ -1985,7 +1978,7 @@ static int ov4689_get_fmt(struct v4l2_subdev *sd,
 	mutex_lock(&sensor->lock);
 
 	if (format->which == V4L2_SUBDEV_FORMAT_TRY)
-		fmt = v4l2_subdev_get_try_format(&sensor->sd, cfg, format->pad);
+		fmt = v4l2_subdev_get_try_format(&sensor->sd, state, format->pad);
 	else
 		fmt = &sensor->fmt;
 
@@ -2003,7 +1996,6 @@ static int ov4689_try_fmt_internal(struct v4l2_subdev *sd,
 {
 	struct ov4689_dev *sensor = to_ov4689_dev(sd);
 	const struct ov4689_mode_info *mode;
-	int i;
 
 	mode = ov4689_find_mode(sensor, fr, fmt->width, fmt->height, true);
 	if (!mode)
@@ -2020,7 +2012,7 @@ static int ov4689_try_fmt_internal(struct v4l2_subdev *sd,
 }
 
 static int ov4689_set_fmt(struct v4l2_subdev *sd,
-			struct v4l2_subdev_pad_config *cfg,
+			struct v4l2_subdev_state *state,
 			struct v4l2_subdev_format *format)
 {
 	struct ov4689_dev *sensor = to_ov4689_dev(sd);
@@ -2044,7 +2036,7 @@ static int ov4689_set_fmt(struct v4l2_subdev *sd,
 		goto out;
 
 	if (format->which == V4L2_SUBDEV_FORMAT_TRY)
-		fmt = v4l2_subdev_get_try_format(sd, cfg, 0);
+		fmt = v4l2_subdev_get_try_format(sd, state, 0);
 	else
 		fmt = &sensor->fmt;
 
@@ -2105,7 +2097,6 @@ static int ov4689_set_ctrl_white_balance(struct ov4689_dev *sensor, int awb)
 		u16 red = (u16)ctrls->red_balance->val;
 		u16 blue = (u16)ctrls->blue_balance->val;
 
-		st_info(ST_SENSOR, "red = 0x%x, blue = 0x%x\n", red, blue);
 		ret = ov4689_write_reg16(sensor, OV4689_REG_AWB_R_GAIN, red);
 		if (ret)
 			return ret;
@@ -2132,7 +2123,6 @@ static int ov4689_set_ctrl_exposure(struct ov4689_dev *sensor,
 		max_exp += ret;
 		ret = 0;
 
-		st_info(ST_SENSOR, "%s, max_exp = 0x%x\n", __func__, max_exp);
 		if (ctrls->exposure->val < max_exp)
 			ret = ov4689_set_exposure(sensor, ctrls->exposure->val);
 	}
@@ -2349,7 +2339,7 @@ free_ctrls:
 }
 
 static int ov4689_enum_frame_size(struct v4l2_subdev *sd,
-				struct v4l2_subdev_pad_config *cfg,
+				struct v4l2_subdev_state *state,
 				struct v4l2_subdev_frame_size_enum *fse)
 {
 	if (fse->pad != 0)
@@ -2369,12 +2359,12 @@ static int ov4689_enum_frame_size(struct v4l2_subdev *sd,
 
 static int ov4689_enum_frame_interval(
 	struct v4l2_subdev *sd,
-	struct v4l2_subdev_pad_config *cfg,
+	struct v4l2_subdev_state *state,
 	struct v4l2_subdev_frame_interval_enum *fie)
 {
 	struct ov4689_dev *sensor = to_ov4689_dev(sd);
+	int ret = 0;
 	struct v4l2_fract tpf;
-	int ret;
 
 	if (fie->pad != 0)
 		return -EINVAL;
@@ -2566,7 +2556,6 @@ static int ov4689_probe(struct i2c_client *client)
 	struct v4l2_mbus_framefmt *fmt;
 	u32 rotation;
 	int ret;
-	u8 chip_id_high, chip_id_low;
 
 	sensor = devm_kzalloc(dev, sizeof(*sensor), GFP_KERNEL);
 	if (!sensor)
diff --git a/drivers/media/platform/starfive/ov5640.c b/drivers/media/platform/starfive/ov5640.c
index dba06a5186fd..36ddf5f9e2ee 100644
--- a/drivers/media/platform/starfive/ov5640.c
+++ b/drivers/media/platform/starfive/ov5640.c
@@ -1124,7 +1124,6 @@ static int ov5640_load_regs(struct ov5640_dev *sensor,
 	u8 mask, val;
 	int ret = 0;
 
-	st_info(ST_SENSOR, "%s, mode = 0x%x\n", __func__, mode->id);
 	for (i = 0; i < mode->reg_data_size; ++i, ++regs) {
 		delay_ms = regs->delay_ms;
 		reg_addr = regs->reg_addr;
@@ -2214,7 +2213,7 @@ find_mode:
 }
 
 static int ov5640_get_fmt(struct v4l2_subdev *sd,
-			  struct v4l2_subdev_pad_config *cfg,
+			  struct v4l2_subdev_state *state,
 			  struct v4l2_subdev_format *format)
 {
 	struct ov5640_dev *sensor = to_ov5640_dev(sd);
@@ -2226,7 +2225,7 @@ static int ov5640_get_fmt(struct v4l2_subdev *sd,
 	mutex_lock(&sensor->lock);
 
 	if (format->which == V4L2_SUBDEV_FORMAT_TRY)
-		fmt = v4l2_subdev_get_try_format(&sensor->sd, cfg,
+		fmt = v4l2_subdev_get_try_format(&sensor->sd, state,
 						 format->pad);
 	else
 		fmt = &sensor->fmt;
@@ -2272,7 +2271,7 @@ static int ov5640_try_fmt_internal(struct v4l2_subdev *sd,
 }
 
 static int ov5640_set_fmt(struct v4l2_subdev *sd,
-			  struct v4l2_subdev_pad_config *cfg,
+			  struct v4l2_subdev_state *state,
 			  struct v4l2_subdev_format *format)
 {
 	struct ov5640_dev *sensor = to_ov5640_dev(sd);
@@ -2296,7 +2295,7 @@ static int ov5640_set_fmt(struct v4l2_subdev *sd,
 		goto out;
 
 	if (format->which == V4L2_SUBDEV_FORMAT_TRY)
-		fmt = v4l2_subdev_get_try_format(sd, cfg, 0);
+		fmt = v4l2_subdev_get_try_format(sd, state, 0);
 	else
 		fmt = &sensor->fmt;
 
@@ -2813,7 +2812,7 @@ free_ctrls:
 }
 
 static int ov5640_enum_frame_size(struct v4l2_subdev *sd,
-				  struct v4l2_subdev_pad_config *cfg,
+				  struct v4l2_subdev_state *state,
 				  struct v4l2_subdev_frame_size_enum *fse)
 {
 	if (fse->pad != 0)
@@ -2833,7 +2832,7 @@ static int ov5640_enum_frame_size(struct v4l2_subdev *sd,
 
 static int ov5640_enum_frame_interval(
 	struct v4l2_subdev *sd,
-	struct v4l2_subdev_pad_config *cfg,
+	struct v4l2_subdev_state *state,
 	struct v4l2_subdev_frame_interval_enum *fie)
 {
 	struct ov5640_dev *sensor = to_ov5640_dev(sd);
@@ -2919,7 +2918,7 @@ out:
 }
 
 static int ov5640_enum_mbus_code(struct v4l2_subdev *sd,
-				 struct v4l2_subdev_pad_config *cfg,
+				 struct v4l2_subdev_state *state,
 				 struct v4l2_subdev_mbus_code_enum *code)
 {
 	if (code->pad != 0)
diff --git a/drivers/media/platform/starfive/sc2235.c b/drivers/media/platform/starfive/sc2235.c
index 263c331ed016..2e0fe90d4760 100644
--- a/drivers/media/platform/starfive/sc2235.c
+++ b/drivers/media/platform/starfive/sc2235.c
@@ -598,7 +598,6 @@ static int sc2235_load_regs(struct sc2235_dev *sensor,
 	u8 mask, val;
 	int ret = 0;
 
-	st_info(ST_SENSOR, "%s, mode = 0x%x\n", __func__, mode->id);
 	for (i = 0; i < mode->reg_data_size; ++i, ++regs) {
 		delay_ms = regs->delay_ms;
 		reg_addr = regs->reg_addr;
@@ -687,6 +686,7 @@ static int sc2235_set_stream_dvp(struct sc2235_dev *sensor, bool on)
 	return 0;
 }
 
+#if 0
 static int sc2235_get_sysclk(struct sc2235_dev *sensor)
 {
 	return 0;
@@ -707,6 +707,7 @@ static int sc2235_get_hts(struct sc2235_dev *sensor)
 		return ret;
 	return hts;
 }
+#endif
 
 static int sc2235_get_vts(struct sc2235_dev *sensor)
 {
@@ -719,6 +720,7 @@ static int sc2235_get_vts(struct sc2235_dev *sensor)
 	return vts;
 }
 
+#if 0
 static int sc2235_set_vts(struct sc2235_dev *sensor, int vts)
 {
 	return sc2235_write_reg16(sensor, SC2235_REG_TIMING_VTS, vts);
@@ -748,6 +750,7 @@ static int sc2235_set_binning(struct sc2235_dev *sensor, bool enable)
 {
 	return 0;
 }
+#endif
 
 static const struct sc2235_mode_info *
 sc2235_find_mode(struct sc2235_dev *sensor, enum sc2235_frame_rate fr,
@@ -800,17 +803,12 @@ static u64 sc2235_calc_pixel_rate(struct sc2235_dev *sensor)
 static int sc2235_set_dvp_pclk(struct sc2235_dev *sensor,
 				unsigned long rate)
 {
-	const struct sc2235_mode_info *mode = sensor->current_mode;
-	const struct sc2235_mode_info *orig_mode = sensor->last_mode;
 	u8 prediv, mult, sysdiv;
 	int ret = 0;
 
 	sc2235_calc_sys_clk(sensor, rate, &prediv, &mult,
 				&sysdiv);
 
-	st_info(ST_SENSOR, "%s, prediv = %d, mult = %d, sysdiv = %d\n",
-			__func__, prediv, mult, sysdiv);
-
 	ret = sc2235_mod_reg(sensor, SC2235_REG_SC_PLL_CTRL0, 0x7f,
 			(sysdiv << 4) | (prediv << 1) | ((mult & 0x20) >> 5));
 	if (ret)
@@ -837,7 +835,6 @@ static int sc2235_set_mode_direct(struct sc2235_dev *sensor,
 static int sc2235_set_mode(struct sc2235_dev *sensor)
 {
 	const struct sc2235_mode_info *mode = sensor->current_mode;
-	const struct sc2235_mode_info *orig_mode = sensor->last_mode;
 	bool auto_gain = sensor->ctrls.auto_gain->val == 1;
 	bool auto_exp =  sensor->ctrls.auto_exp->val == V4L2_EXPOSURE_AUTO;
 	unsigned long rate;
@@ -976,9 +973,7 @@ static void sc2235_set_power_off(struct sc2235_dev *sensor)
 static int sc2235_set_power_dvp(struct sc2235_dev *sensor, bool on)
 {
 	unsigned int flags = sensor->ep.bus.parallel.flags;
-	bool bt656 = sensor->ep.bus_type == V4L2_MBUS_BT656;
 	u8 polarities = 0;
-	int ret;
 
 	/*
 	 * configure parallel port control lines polarity
@@ -1107,7 +1102,7 @@ find_mode:
 }
 
 static int sc2235_get_fmt(struct v4l2_subdev *sd,
-			struct v4l2_subdev_pad_config *cfg,
+			struct v4l2_subdev_state *state,
 			struct v4l2_subdev_format *format)
 {
 	struct sc2235_dev *sensor = to_sc2235_dev(sd);
@@ -1119,7 +1114,7 @@ static int sc2235_get_fmt(struct v4l2_subdev *sd,
 	mutex_lock(&sensor->lock);
 
 	if (format->which == V4L2_SUBDEV_FORMAT_TRY)
-		fmt = v4l2_subdev_get_try_format(&sensor->sd, cfg,
+		fmt = v4l2_subdev_get_try_format(&sensor->sd, state,
 						format->pad);
 	else
 		fmt = &sensor->fmt;
@@ -1165,7 +1160,7 @@ static int sc2235_try_fmt_internal(struct v4l2_subdev *sd,
 }
 
 static int sc2235_set_fmt(struct v4l2_subdev *sd,
-			struct v4l2_subdev_pad_config *cfg,
+			struct v4l2_subdev_state *state,
 			struct v4l2_subdev_format *format)
 {
 	struct sc2235_dev *sensor = to_sc2235_dev(sd);
@@ -1188,7 +1183,7 @@ static int sc2235_set_fmt(struct v4l2_subdev *sd,
 		goto out;
 
 	if (format->which == V4L2_SUBDEV_FORMAT_TRY)
-		fmt = v4l2_subdev_get_try_format(sd, cfg, 0);
+		fmt = v4l2_subdev_get_try_format(sd, state, 0);
 	else
 		fmt = &sensor->fmt;
 
@@ -1513,7 +1508,7 @@ free_ctrls:
 }
 
 static int sc2235_enum_frame_size(struct v4l2_subdev *sd,
-				struct v4l2_subdev_pad_config *cfg,
+				struct v4l2_subdev_state *state,
 				struct v4l2_subdev_frame_size_enum *fse)
 {
 	if (fse->pad != 0)
@@ -1533,7 +1528,7 @@ static int sc2235_enum_frame_size(struct v4l2_subdev *sd,
 
 static int sc2235_enum_frame_interval(
 	struct v4l2_subdev *sd,
-	struct v4l2_subdev_pad_config *cfg,
+	struct v4l2_subdev_state *state,
 	struct v4l2_subdev_frame_interval_enum *fie)
 {
 	struct sc2235_dev *sensor = to_sc2235_dev(sd);
@@ -1619,7 +1614,7 @@ out:
 }
 
 static int sc2235_enum_mbus_code(struct v4l2_subdev *sd,
-				struct v4l2_subdev_pad_config *cfg,
+				struct v4l2_subdev_state *state,
 				struct v4l2_subdev_mbus_code_enum *code)
 {
 	if (code->pad != 0)
diff --git a/drivers/media/platform/starfive/stf_vin_hw_ops.c b/drivers/media/platform/starfive/stf_vin_hw_ops.c
index 4eca44438c25..f8a28e113599 100644
--- a/drivers/media/platform/starfive/stf_vin_hw_ops.c
+++ b/drivers/media/platform/starfive/stf_vin_hw_ops.c
@@ -104,15 +104,18 @@ static int stf_vin_clk_init(struct stf_vin2_dev *vin_dev)
 	}
 
 	for (i = STFRST_VIN_SRC; i <= STFRST_ISP1NOC_AXI; i++) {
-		ret = reset_control_reset(stfcamss->sys_rst[i].rst);
-		if(ret){
-			st_err(ST_VIN, "%s reset rst %d failed\n", __func__, i);
+		st_debug(ST_VIN, "start %s control deassert %s\n", __func__, stfcamss->sys_rst[i].name);
+		// ret = reset_control_reset(stfcamss->sys_rst[i].rst);
+		ret = reset_control_deassert(stfcamss->sys_rst[i].rst);
+		if (ret) {
+			st_err(ST_VIN, "%s control deassert %d failed\n", __func__, i);
 			return ret;
 		}
 	}
 
 	// hold vin resets for sub modules before csi2rx controller get configed
 	for(i = STFRST_SYS_CLK; i <= STFRST_C_ISP1; i++) {
+		st_debug(ST_VIN, "start %s control assert %s\n", __func__, stfcamss->sys_rst[i].name);
 		reset_control_assert(stfcamss->sys_rst[i].rst);
 	}
 
@@ -120,6 +123,7 @@ static int stf_vin_clk_init(struct stf_vin2_dev *vin_dev)
 	// except dphy-rx (follow lunhai's advice)
 	for(i = STFRST_SYS_CLK; i <= STFRST_C_ISP1; i++) {
 		if(i != STFRST_DPHY_HW_RSTN) {
+			st_debug(ST_VIN, "start %s control deassert %s\n", __func__, stfcamss->sys_rst[i].name);
 			reset_control_deassert(stfcamss->sys_rst[i].rst);
 		}
 	}
diff --git a/drivers/media/platform/starfive/stfcamss.c b/drivers/media/platform/starfive/stfcamss.c
index 7aed6a84cf68..84897f1b1950 100644
--- a/drivers/media/platform/starfive/stfcamss.c
+++ b/drivers/media/platform/starfive/stfcamss.c
@@ -1257,15 +1257,19 @@ err_cam_noti_med:
 	media_device_cleanup(&stfcamss->media_dev);
 err_cam_noti:
 	v4l2_async_notifier_cleanup(&stfcamss->notifier);
+	i = stfcamss->nrsts - 1;
 err_cam_rst:
-	for (i = stfcamss->nrsts; i > 0; i--) {
+#if 0
+	for (; i >= 0; i--) {
 		struct stfcamss_rst *reset = &stfcamss->sys_rst[i];
 
 		reset_control_put(reset->rst);
 		st_debug(ST_CAMSS, "put %s reset\n", reset->name);
-	}	
+	}
+#endif
+	i = stfcamss->nclks - 1;
 err_cam_clk:
-	for (i = stfcamss->nclks; i > 0; i--) {
+	for (; i >= 0; i--) {
 		struct stfcamss_clk *clock = &stfcamss->sys_clk[i];
 
 		devm_clk_put(dev, clock->clk);
@@ -1289,11 +1293,34 @@ static int stfcamss_remove(struct platform_device *pdev)
 	stfcamss->debugfs_entry = NULL;
 #endif
 
+	if (stfcamss->media_dev.devnode)
+		media_device_unregister(&stfcamss->media_dev);
 	stfcamss_unregister_subdevices(stfcamss);
 	v4l2_device_unregister(&stfcamss->v4l2_dev);
 	media_device_cleanup(&stfcamss->media_dev);
+	v4l2_async_notifier_unregister(&stfcamss->notifier);
+	v4l2_async_notifier_cleanup(&stfcamss->notifier);
+
+#if 0
+	int i;
+
+	i = stfcamss->nrsts - 1;
+	for (; i >= 0; i--) {
+		struct stfcamss_rst *reset = &stfcamss->sys_rst[i];
 
-	kfree(stfcamss);
+		st_debug(ST_CAMSS, "put %s reset\n", reset->name);
+		reset_control_put(reset->rst);
+	}
+
+	i = stfcamss->nclks - 1;
+	for (; i >= 0; i--) {
+		struct stfcamss_clk *clock = &stfcamss->sys_clk[i];
+
+		st_debug(ST_CAMSS, "put %s clocks\n", clock->name);
+		devm_clk_put(&pdev->dev, clock->clk);
+	}
+#endif
+	// kfree(stfcamss);
 
 	return 0;
 }