diff --git a/drivers/media/i2c/d4xx.c b/drivers/media/i2c/d4xx.c
index a2da995a5e18..2c8c6e36595d 100644
--- a/drivers/media/i2c/d4xx.c
+++ b/drivers/media/i2c/d4xx.c
@@ -328,7 +328,6 @@ struct ds5_ctrls {
 	struct v4l2_ctrl_handler handler_depth;
 	struct v4l2_ctrl_handler handler_rgb;
 	struct v4l2_ctrl_handler handler_y8;
-	struct v4l2_ctrl_handler handler_imu;
 	struct {
 		struct v4l2_ctrl *log;
 		struct v4l2_ctrl *fw_version;
@@ -675,6 +674,8 @@ static u8 d4xx_set_sub_stream[] = {
 	0, 0, 0, 0, 0, 0
 };
 
+static int ds5_mux_s_stream_vc(struct ds5 *state, u16 vc_id, u16 on);
+
 /* Pad ops */
 
 static const u16 ds5_default_framerate = 30;
@@ -1137,6 +1138,9 @@ static int ds5_sensor_enum_frame_interval(struct v4l2_subdev *sd,
 	const struct ds5_resolution *res;
 	unsigned int i;
 
+	//if (fie->pad)
+	//	return -EINVAL;
+
 	for (i = 0, fmt = sensor->formats; i < sensor->n_formats; i++, fmt++)
 		if (fie->code == fmt->mbus_code)
 			break;
@@ -1171,6 +1175,8 @@ static int ds5_sensor_get_fmt(struct v4l2_subdev *sd,
 	struct ds5_sensor *sensor = container_of(sd, struct ds5_sensor, sd);
 	struct ds5 *state = v4l2_get_subdevdata(sd);
 
+	//fmt->pad = sensor->mux_pad;
+
 	if (fmt->pad)
 		return -EINVAL;
 
@@ -1375,6 +1381,7 @@ static int ds5_sensor_set_fmt(struct v4l2_subdev *sd,
 #endif
 }
 
+#if 0 /* function not called */
 static int ds5_configure(struct ds5 *state)
 {
 	struct ds5_sensor *sensor;
@@ -1382,7 +1389,7 @@ static int ds5_configure(struct ds5 *state)
 	u16 dt_addr, md_addr, override_addr, fps_addr, width_addr, height_addr;
 	int ret;
 
-	if (state->is_depth) {
+	// if (state->is_depth) {
 		sensor = &state->depth.sensor;
 		dt_addr = DS5_DEPTH_STREAM_DT;
 		md_addr = DS5_DEPTH_STREAM_MD;
@@ -1390,9 +1397,44 @@ static int ds5_configure(struct ds5 *state)
 		fps_addr = DS5_DEPTH_FPS;
 		width_addr = DS5_DEPTH_RES_WIDTH;
 		height_addr = DS5_DEPTH_RES_HEIGHT;
-		md_fmt = 0x12;
+		// TODO: read VC from device tree
 		vc_id = 0;
-	} else if (state->is_rgb) {
+		md_fmt = 0x12;
+
+	fmt = sensor->streaming ? sensor->config.format->data_type : 0;
+
+	/*
+	 * Set depth stream Z16 data type as 0x31
+	 * Set IR stream Y8I data type as 0x32
+	 */
+	if (fmt != 0)
+		ret = ds5_write(state, dt_addr, 0x31);
+	if (ret < 0)
+		return ret;
+
+	ret = ds5_write(state, md_addr, (vc_id << 8) | md_fmt);
+	if (ret < 0)
+		return ret;
+
+	if (override_addr != 0) {
+		ret = ds5_write(state, override_addr, fmt);
+		if (ret < 0)
+			return ret;
+	}
+
+	ret = ds5_write(state, fps_addr, sensor->config.framerate);
+	if (ret < 0)
+		return ret;
+
+	ret = ds5_write(state, width_addr, sensor->config.resolution->width);
+	if (ret < 0)
+		return ret;
+
+	ret = ds5_write(state, height_addr, sensor->config.resolution->height);
+	if (ret < 0)
+		return ret;
+
+	// } else if (state->is_rgb) {
 		sensor = &state->rgb.sensor;
 		dt_addr = DS5_RGB_STREAM_DT;
 		md_addr = DS5_RGB_STREAM_MD;
@@ -1400,9 +1442,42 @@ static int ds5_configure(struct ds5 *state)
 		fps_addr = DS5_RGB_FPS;
 		width_addr = DS5_RGB_RES_WIDTH;
 		height_addr = DS5_RGB_RES_HEIGHT;
-		md_fmt = 0x12;
 		vc_id = 1;
-	} else if (state->is_y8) {
+		md_fmt = 0x12;
+
+	fmt = sensor->streaming ? sensor->config.format->data_type : 0;
+
+	/*
+	 * Set depth stream Z16 data type as 0x31
+	 * Set IR stream Y8I data type as 0x32
+	 */
+	ret = ds5_write(state, dt_addr, fmt);
+	if (ret < 0)
+		return ret;
+
+	ret = ds5_write(state, md_addr, (vc_id << 8) | md_fmt);
+	if (ret < 0)
+		return ret;
+
+	if (override_addr != 0) {
+		ret = ds5_write(state, override_addr, fmt);
+		if (ret < 0)
+			return ret;
+	}
+
+	ret = ds5_write(state, fps_addr, sensor->config.framerate);
+	if (ret < 0)
+		return ret;
+
+	ret = ds5_write(state, width_addr, sensor->config.resolution->width);
+	if (ret < 0)
+		return ret;
+
+	ret = ds5_write(state, height_addr, sensor->config.resolution->height);
+	if (ret < 0)
+		return ret;
+
+	// } else if (state->is_y8) {
 		sensor = &state->motion_t.sensor;
 		dt_addr = DS5_IR_STREAM_DT;
 		md_addr = DS5_IR_STREAM_MD;
@@ -1410,9 +1485,44 @@ static int ds5_configure(struct ds5 *state)
 		fps_addr = DS5_IR_FPS;
 		width_addr = DS5_IR_RES_WIDTH;
 		height_addr = DS5_IR_RES_HEIGHT;
-		md_fmt = 0x12;
 		vc_id = 2;
-	} else if (state->is_imu) {
+		md_fmt = 0x12;
+
+	fmt = sensor->streaming ? sensor->config.format->data_type : 0;
+
+	/*
+	 * Set depth stream Z16 data type as 0x31
+	 * Set IR stream Y8I data type as 0x32
+	 */
+	if (fmt != 0 &&
+		 sensor->config.format->data_type == 0x1e)
+		ret = ds5_write(state, dt_addr, 0x32);
+	if (ret < 0)
+		return ret;
+
+	ret = ds5_write(state, md_addr, (vc_id << 8) | md_fmt);
+	if (ret < 0)
+		return ret;
+
+	if (override_addr != 0) {
+		ret = ds5_write(state, override_addr, fmt);
+		if (ret < 0)
+			return ret;
+	}
+
+	ret = ds5_write(state, fps_addr, sensor->config.framerate);
+	if (ret < 0)
+		return ret;
+
+	ret = ds5_write(state, width_addr, sensor->config.resolution->width);
+	if (ret < 0)
+		return ret;
+
+	ret = ds5_write(state, height_addr, sensor->config.resolution->height);
+	if (ret < 0)
+		return ret;
+
+	// } else if (state->is_imu) {
 		sensor = &state->imu.sensor;
 		dt_addr = DS5_IMU_STREAM_DT;
 		md_addr = DS5_IMU_STREAM_MD;
@@ -1420,26 +1530,72 @@ static int ds5_configure(struct ds5 *state)
 		fps_addr = DS5_IMU_FPS;
 		width_addr = DS5_IMU_RES_WIDTH;
 		height_addr = DS5_IMU_RES_HEIGHT;
-		md_fmt = 0x0;
 		vc_id = 3;
-	} else {
-		return -EINVAL;
+		md_fmt = 0x0;
+
+	fmt = sensor->streaming ? sensor->config.format->data_type : 0;
+
+	/*
+	 * Set depth stream Z16 data type as 0x31
+	 * Set IR stream Y8I data type as 0x32
+	 */
+	ret = ds5_write(state, dt_addr, fmt);
+	if (ret < 0)
+		return ret;
+
+	ret = ds5_write(state, md_addr, (vc_id << 8) | md_fmt);
+	if (ret < 0)
+		return ret;
+
+	if (override_addr != 0) {
+		ret = ds5_write(state, override_addr, fmt);
+		if (ret < 0)
+			return ret;
 	}
 
+	ret = ds5_write(state, fps_addr, sensor->config.framerate);
+	if (ret < 0)
+		return ret;
 
-	fmt = sensor->streaming ? sensor->config.format->data_type : 0;
+	ret = ds5_write(state, width_addr, sensor->config.resolution->width);
+	if (ret < 0)
+		return ret;
+
+	ret = ds5_write(state, height_addr, sensor->config.resolution->height);
+	if (ret < 0)
+		return ret;
+
+	return 0;
+}
+#endif
+
+static int ds5_configure_depth(struct ds5 *state)
+{
+	struct ds5_sensor *sensor;
+	u16 fmt, md_fmt, vc_id;
+	u16 dt_addr, md_addr, override_addr, fps_addr, width_addr, height_addr;
+	int ret = -1;
+
+	// if (state->is_depth) {
+		sensor = &state->depth.sensor;
+		dt_addr = DS5_DEPTH_STREAM_DT;
+		md_addr = DS5_DEPTH_STREAM_MD;
+		override_addr = DS5_DEPTH_OVERRIDE;
+		fps_addr = DS5_DEPTH_FPS;
+		width_addr = DS5_DEPTH_RES_WIDTH;
+		height_addr = DS5_DEPTH_RES_HEIGHT;
+		// TODO: read VC from device tree
+		vc_id = 0;
+		md_fmt = 0x12;
+
+	fmt = 0x1e;
 
 	/*
 	 * Set depth stream Z16 data type as 0x31
 	 * Set IR stream Y8I data type as 0x32
 	 */
-	if (state->is_depth && fmt != 0)
+	//if (fmt != 0)
 		ret = ds5_write(state, dt_addr, 0x31);
-	else if (state->is_y8 && fmt != 0 &&
-		 sensor->config.format->data_type == 0x1E)
-		ret = ds5_write(state, dt_addr, 0x32);
-	else
-		ret = ds5_write(state, dt_addr, fmt);
 	if (ret < 0)
 		return ret;
 
@@ -1447,7 +1603,117 @@ static int ds5_configure(struct ds5 *state)
 	if (ret < 0)
 		return ret;
 
-	if (!sensor->streaming)
+	if (override_addr != 0) {
+		ret = ds5_write(state, override_addr, fmt);
+		if (ret < 0)
+			return ret;
+	}
+
+	ret = ds5_write(state, fps_addr, sensor->config.framerate);
+	if (ret < 0)
+		return ret;
+
+	ret = ds5_write(state, width_addr, sensor->config.resolution->width);
+	if (ret < 0)
+		return ret;
+
+	ret = ds5_write(state, height_addr, sensor->config.resolution->height);
+	if (ret < 0)
+		return ret;
+
+	return 0;
+}
+static int ds5_configure_rgb(struct ds5 *state)
+{
+	struct ds5_sensor *sensor;
+	u16 fmt, md_fmt, vc_id;
+	u16 dt_addr, md_addr, override_addr, fps_addr, width_addr, height_addr;
+	int ret = -1;
+
+	// } else if (state->is_rgb) {
+		sensor = &state->rgb.sensor;
+		dt_addr = DS5_RGB_STREAM_DT;
+		md_addr = DS5_RGB_STREAM_MD;
+		override_addr = 0;
+		fps_addr = DS5_RGB_FPS;
+		width_addr = DS5_RGB_RES_WIDTH;
+		height_addr = DS5_RGB_RES_HEIGHT;
+		vc_id = 1;
+		md_fmt = 0x12;
+
+	fmt = sensor->streaming ? sensor->config.format->data_type : 0;
+
+	/*
+	 * Set depth stream Z16 data type as 0x31
+	 * Set IR stream Y8I data type as 0x32
+	 */
+	dev_info(NULL, "%s, fmt 0x%08x, data_type 0x%08x\n", __func__, fmt, sensor->config.format->data_type);
+//	ret = ds5_write(state, dt_addr, fmt);
+	ret = ds5_write(state, dt_addr, 0x1e);
+	if (ret < 0)
+		return ret;
+
+	ret = ds5_write(state, md_addr, (vc_id << 8) | md_fmt);
+	if (ret < 0)
+		return ret;
+
+	if (override_addr != 0) {
+		ret = ds5_write(state, override_addr, fmt);
+		if (ret < 0)
+			return ret;
+	}
+
+	ret = ds5_write(state, fps_addr, sensor->config.framerate);
+	if (ret < 0)
+		return ret;
+
+	//ret = ds5_write(state, width_addr, 480);
+	ret = ds5_write(state, width_addr, sensor->config.resolution->width);
+	if (ret < 0)
+		return ret;
+
+	//ret = ds5_write(state, height_addr, 270);
+	ret = ds5_write(state, height_addr, sensor->config.resolution->height);
+	if (ret < 0)
+		return ret;
+
+	return 0;
+}
+
+static int ds5_configure_ir(struct ds5 *state)
+{
+	struct ds5_sensor *sensor;
+	u16 fmt, md_fmt, vc_id;
+	u16 dt_addr, md_addr, override_addr, fps_addr, width_addr, height_addr;
+	int ret = -1;
+
+	// } else if (state->is_y8) {
+		sensor = &state->motion_t.sensor;
+		dt_addr = DS5_IR_STREAM_DT;
+		md_addr = DS5_IR_STREAM_MD;
+		override_addr = DS5_IR_OVERRIDE;
+		fps_addr = DS5_IR_FPS;
+		width_addr = DS5_IR_RES_WIDTH;
+		height_addr = DS5_IR_RES_HEIGHT;
+		vc_id = 2;
+		md_fmt = 0x12;
+
+	fmt = sensor->config.format->data_type;
+
+	/*
+	 * Set depth stream Z16 data type as 0x31
+	 * Set IR stream Y8I data type as 0x32
+	 */
+	if (sensor->config.format->data_type == 0x1e)
+		ret = ds5_write(state, dt_addr, 0x32);
+	else if (sensor->config.format->data_type == 0x2a)
+		ret = ds5_write(state, dt_addr, 0x2a);
+
+	if (ret < 0)
+		return ret;
+
+	ret = ds5_write(state, md_addr, (vc_id << 8) | md_fmt);
+	if (ret < 0)
 		return ret;
 
 	if (override_addr != 0) {
@@ -1471,6 +1737,46 @@ static int ds5_configure(struct ds5 *state)
 	return 0;
 }
 
+static int ds5_configure_imu(struct ds5 *state)
+{
+	struct ds5_sensor *sensor;
+	u16 fmt, md_fmt, vc_id;
+	u16 dt_addr, md_addr, fps_addr, width_addr, height_addr;
+	int ret = -1;
+
+	sensor = &state->imu.sensor;
+	dt_addr = DS5_IMU_STREAM_DT;
+	md_addr = DS5_IMU_STREAM_MD;
+	fps_addr = DS5_IMU_FPS;
+	width_addr = DS5_IMU_RES_WIDTH;
+	height_addr = DS5_IMU_RES_HEIGHT;
+	vc_id = 3;
+	md_fmt = 0;
+
+	fmt = sensor->config.format->data_type;
+	ret = ds5_write(state, dt_addr, fmt);
+	if (ret < 0)
+		return ret;
+
+	ret = ds5_write(state, md_addr, (vc_id << 8) | md_fmt);
+	if (ret < 0)
+		return ret;
+
+	ret = ds5_write(state, fps_addr, sensor->config.framerate);
+	if (ret < 0)
+		return ret;
+
+	ret = ds5_write(state, width_addr, sensor->config.resolution->width);
+	if (ret < 0)
+		return ret;
+
+	ret = ds5_write(state, height_addr, sensor->config.resolution->height);
+	if (ret < 0)
+		return ret;
+
+	return 0;
+}
+
 static const struct v4l2_subdev_pad_ops ds5_depth_pad_ops = {
 	.enum_mbus_code		= ds5_sensor_enum_mbus_code,
 	.enum_frame_size	= ds5_sensor_enum_frame_size,
@@ -1615,7 +1921,7 @@ static int ds5_hw_set_auto_exposure(struct ds5 *state, u32 base, s32 val)
  */
 static int ds5_hw_set_exposure(struct ds5 *state, u32 base, s32 val)
 {
-	int ret;
+	int ret = -1;
 
 	if (val < 1)
 		val = 1;
@@ -1629,6 +1935,8 @@ static int ds5_hw_set_exposure(struct ds5 *state, u32 base, s32 val)
 	 *	Color: 1 is 100 us
 	 *	Depth: 1 is 1 us
 	 */
+//	if (!state->is_rgb)
+//		val *= 100;
 
 	ret = ds5_write(state, base | DS5_MANUAL_EXPOSURE_MSB, (u16)(val >> 16));
 	if (!ret)
@@ -1791,7 +2099,7 @@ static int ds5_get_hwmc(struct ds5 *state, unsigned char *data)
 static int ds5_set_calibration_data(struct ds5 *state,
 		struct hwm_cmd *cmd, u16 length)
 {
-	int ret;
+	int ret = -1;
 	int retries = 10;
 	u16 status = 2;
 
@@ -1814,8 +2122,6 @@ static int ds5_set_calibration_data(struct ds5 *state,
 	return -EINVAL;
 }
 
-static int ds5_mux_s_stream(struct v4l2_subdev *sd, int on);
-
 static int ds5_s_state(struct ds5 *state, int vc)
 {
 	int ret = 0;
@@ -1852,7 +2158,6 @@ static int ds5_s_state(struct ds5 *state, int vc)
 		ret = -EINVAL;
 		break;
 	}
-	ds5_set_state_last_set(state);
 	return ret;
 }
 
@@ -1891,13 +2196,6 @@ static int ds5_s_ctrl(struct v4l2_ctrl *ctrl)
 			state->is_y8 = 1;
 			state->is_imu = 0;
 		break;
-		case DS5_MUX_PAD_IMU_A:
-			state = container_of(ctrl->handler, struct ds5, ctrls.handler_imu);
-			state->is_rgb = 0;
-			state->is_depth = 0;
-			state->is_y8 = 0;
-			state->is_imu = 1;
-		break;
 		default:
 			state->is_rgb = 0;
 			state->is_depth = 0;
@@ -1910,6 +2208,8 @@ static int ds5_s_ctrl(struct v4l2_ctrl *ctrl)
 
 	if (state->is_rgb)
 		base = DS5_RGB_CONTROL_BASE;
+	else if (state->is_imu)
+		return ret;
 
 	v4l2_dbg(3, 1, sd, "ctrl: %s, value: %d\n", ctrl->name, ctrl->val);
 	dev_dbg(&state->client->dev, "%s(): %s - ctrl: %s, value: %d\n",
@@ -2166,18 +2466,17 @@ static int ds5_s_ctrl(struct v4l2_ctrl *ctrl)
 		dev_info(&state->client->dev, "V4L2_CID_IPU_SET_SUB_STREAM %x\n", val);
 		vc_id = (val >> 8) & 0x00FF;
 		on = val & 0x00FF;
-		if (vc_id < DS5_MUX_PAD_COUNT)
+		if (on == 0xff) {
 			ret = ds5_s_state(state, vc_id);
-		if (on == 0xff)
 			break;
+		}
 		if (vc_id > NR_OF_DS5_STREAMS - 1)
 			dev_err(&state->client->dev, "invalid vc %d\n", vc_id);
 		else
 			d4xx_set_sub_stream[vc_id] = on;
-#ifndef CONFIG_VIDEO_D4XX_SERDES
-		ret = ds5_mux_s_stream(sd, on);
-#endif
-		ret = 0;
+
+		ret = ds5_mux_s_stream_vc(state, vc_id, on);
+
 		break;
 	}
 
@@ -2190,7 +2489,7 @@ static int ds5_get_calibration_data(struct ds5 *state, enum table_id id,
 		unsigned char *table, unsigned int length)
 {
 	struct hwm_cmd *cmd;
-	int ret;
+	int ret = -1;
 	int retries = 3;
 	u16 status = 2;
 	u16 table_length;
@@ -2236,7 +2535,7 @@ static int ds5_get_calibration_data(struct ds5 *state, enum table_id id,
 static int ds5_gvd(struct ds5 *state, unsigned char *data)
 {
 	struct hwm_cmd cmd;
-	int ret;
+	int ret = -1;
 	u16 length = 0;
 	u16 status = 2;
 	u8 retries = 3;
@@ -2301,13 +2600,6 @@ static int ds5_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
 			state->is_y8 = 1;
 			state->is_imu = 0;
 		break;
-		case DS5_MUX_PAD_IMU_A:
-			state = container_of(ctrl->handler, struct ds5, ctrls.handler_imu);
-			state->is_rgb = 0;
-			state->is_depth = 0;
-			state->is_y8 = 0;
-			state->is_imu = 1;
-		break;
 		default:
 			state->is_rgb = 0;
 			state->is_depth = 0;
@@ -2486,7 +2778,6 @@ static int ds5_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
 				"%s(): V4L2_CID_IPU_QUERY_SUB_STREAM sensor->mux_pad:%d vc:[%d]\n",
 				__func__, sensor->mux_pad, vc_id);
 			*ctrl->p_new.p_s32 = pad_to_substream[sensor->mux_pad];
-			state->mux.last_set = sensor;
 			} else {
 				/* we are in DS5 MUX case */
 				*ctrl->p_new.p_s32 = -1;
@@ -2787,7 +3078,7 @@ static int ds5_ctrl_init(struct ds5 *state, int sid)
 	struct ds5_ctrls *ctrls = &state->ctrls;
 	struct v4l2_ctrl_handler *hdl = &ctrls->handler;
 	struct v4l2_subdev *sd = &state->mux.sd.subdev;
-	int ret;
+	int ret = -1;
 	struct ds5_sensor *sensor = NULL;
 
 	switch (sid) {
@@ -2803,10 +3094,6 @@ static int ds5_ctrl_init(struct ds5 *state, int sid)
 		hdl = &ctrls->handler_y8;
 		sensor = &state->motion_t.sensor;
 		break;
-	case 3:
-		hdl = &ctrls->handler_imu;
-		sensor = &state->imu.sensor;
-		break;
 	default:
 		hdl = &ctrls->handler;
 		sensor = NULL;
@@ -2845,21 +3132,21 @@ static int ds5_ctrl_init(struct ds5 *state, int sid)
 		ctrls->gain->flags =
 				V4L2_CTRL_FLAG_VOLATILE | V4L2_CTRL_FLAG_EXECUTE_ON_WRITE;
 	}
-	if (sid >= 0 && sid < 3) {
-
-		ctrls->auto_exp = v4l2_ctrl_new_std_menu(hdl, ops,
-				V4L2_CID_EXPOSURE_AUTO,
-				V4L2_EXPOSURE_APERTURE_PRIORITY,
-				~((1 << V4L2_EXPOSURE_MANUAL) |
-						(1 << V4L2_EXPOSURE_APERTURE_PRIORITY)),
-						V4L2_EXPOSURE_APERTURE_PRIORITY);
-
-		if (ctrls->auto_exp) {
-			ctrls->auto_exp->flags |=
-					V4L2_CTRL_FLAG_VOLATILE | V4L2_CTRL_FLAG_EXECUTE_ON_WRITE;
-			ctrls->auto_exp->priv = sensor;
-		}
+if (sid >= 0) {
+
+	ctrls->auto_exp = v4l2_ctrl_new_std_menu(hdl, ops,
+			V4L2_CID_EXPOSURE_AUTO,
+			V4L2_EXPOSURE_APERTURE_PRIORITY,
+			~((1 << V4L2_EXPOSURE_MANUAL) |
+					(1 << V4L2_EXPOSURE_APERTURE_PRIORITY)),
+					V4L2_EXPOSURE_APERTURE_PRIORITY);
+
+	if (ctrls->auto_exp) {
+		ctrls->auto_exp->flags |=
+				V4L2_CTRL_FLAG_VOLATILE | V4L2_CTRL_FLAG_EXECUTE_ON_WRITE;
+		ctrls->auto_exp->priv = sensor;
 	}
+}
 	/* Exposure time: V4L2_CID_EXPOSURE_ABSOLUTE default unit: 100 us. */
 	if (sid == 0 || sid == 2) {
 		ctrls->exposure = v4l2_ctrl_new_std(hdl, ops,
@@ -2906,35 +3193,31 @@ static int ds5_ctrl_init(struct ds5 *state, int sid)
 	// }
 
 	// Add these after v4l2_ctrl_handler_setup so they won't be set up
-	if (sid >= 0 && sid < 3) {
-		ctrls->log = v4l2_ctrl_new_custom(hdl, &ds5_ctrl_log, sensor);
-		ctrls->fw_version = v4l2_ctrl_new_custom(hdl, &ds5_ctrl_fw_version, sensor);
-		ctrls->gvd = v4l2_ctrl_new_custom(hdl, &ds5_ctrl_gvd, sensor);
-		ctrls->get_depth_calib =
-				v4l2_ctrl_new_custom(hdl, &ds5_ctrl_get_depth_calib, sensor);
-		ctrls->set_depth_calib =
-				v4l2_ctrl_new_custom(hdl, &ds5_ctrl_set_depth_calib, sensor);
-		ctrls->get_coeff_calib =
-				v4l2_ctrl_new_custom(hdl, &ds5_ctrl_get_coeff_calib, sensor);
-		ctrls->set_coeff_calib =
-				v4l2_ctrl_new_custom(hdl, &ds5_ctrl_set_coeff_calib, sensor);
-		ctrls->ae_roi_get = v4l2_ctrl_new_custom(hdl, &ds5_ctrl_ae_roi_get, sensor);
-		ctrls->ae_roi_set = v4l2_ctrl_new_custom(hdl, &ds5_ctrl_ae_roi_set, sensor);
-		ctrls->ae_setpoint_get =
-				v4l2_ctrl_new_custom(hdl, &ds5_ctrl_ae_setpoint_get, sensor);
-		ctrls->ae_setpoint_set =
-				v4l2_ctrl_new_custom(hdl, &ds5_ctrl_ae_setpoint_set, sensor);
-		ctrls->erb = v4l2_ctrl_new_custom(hdl, &ds5_ctrl_erb, sensor);
-		ctrls->ewb = v4l2_ctrl_new_custom(hdl, &ds5_ctrl_ewb, sensor);
-		ctrls->hwmc = v4l2_ctrl_new_custom(hdl, &ds5_ctrl_hwmc, sensor);
-		v4l2_ctrl_new_custom(hdl, &ds5_ctrl_hwmc_rw, sensor);
-	}
-	// DEPTH custom
+if (sid >= 0) {
+	ctrls->log = v4l2_ctrl_new_custom(hdl, &ds5_ctrl_log, sensor);
+	ctrls->fw_version = v4l2_ctrl_new_custom(hdl, &ds5_ctrl_fw_version, sensor);
+	ctrls->gvd = v4l2_ctrl_new_custom(hdl, &ds5_ctrl_gvd, sensor);
+	ctrls->get_depth_calib =
+			v4l2_ctrl_new_custom(hdl, &ds5_ctrl_get_depth_calib, sensor);
+	ctrls->set_depth_calib =
+			v4l2_ctrl_new_custom(hdl, &ds5_ctrl_set_depth_calib, sensor);
+	ctrls->get_coeff_calib =
+			v4l2_ctrl_new_custom(hdl, &ds5_ctrl_get_coeff_calib, sensor);
+	ctrls->set_coeff_calib =
+			v4l2_ctrl_new_custom(hdl, &ds5_ctrl_set_coeff_calib, sensor);
+	ctrls->ae_roi_get = v4l2_ctrl_new_custom(hdl, &ds5_ctrl_ae_roi_get, sensor);
+	ctrls->ae_roi_set = v4l2_ctrl_new_custom(hdl, &ds5_ctrl_ae_roi_set, sensor);
+	ctrls->ae_setpoint_get =
+			v4l2_ctrl_new_custom(hdl, &ds5_ctrl_ae_setpoint_get, sensor);
+	ctrls->ae_setpoint_set =
+			v4l2_ctrl_new_custom(hdl, &ds5_ctrl_ae_setpoint_set, sensor);
+	ctrls->erb = v4l2_ctrl_new_custom(hdl, &ds5_ctrl_erb, sensor);
+	ctrls->ewb = v4l2_ctrl_new_custom(hdl, &ds5_ctrl_ewb, sensor);
+	ctrls->hwmc = v4l2_ctrl_new_custom(hdl, &ds5_ctrl_hwmc, sensor);
+	v4l2_ctrl_new_custom(hdl, &ds5_ctrl_hwmc_rw, sensor);
+}
 	if (sid == 0)
 		v4l2_ctrl_new_custom(hdl, &ds5_ctrl_pwm, sensor);
-	// IMU custom
-	if (sid == 3)
-		ctrls->fw_version = v4l2_ctrl_new_custom(hdl, &ds5_ctrl_fw_version, sensor);
 
 	switch (sid) {
 	case 0:
@@ -2955,12 +3238,6 @@ static int ds5_ctrl_init(struct ds5 *state, int sid)
 			"%s():%d set ctrl_handler pad:%d\n",
 			__func__, __LINE__, state->motion_t.sensor.mux_pad);
 		break;
-	case 3:
-		state->imu.sensor.sd.ctrl_handler = hdl;
-		dev_info(state->imu.sensor.sd.dev,
-			"%s():%d set ctrl_handler pad:%d\n",
-			__func__, __LINE__, state->imu.sensor.mux_pad);
-		break;
 	default:
 		state->mux.sd.subdev.ctrl_handler = hdl;
 		dev_info(state->mux.sd.subdev.dev,
@@ -3003,7 +3280,7 @@ static int ds5_sensor_register(struct ds5 *state, struct ds5_sensor *sensor)
 {
 	struct v4l2_subdev *sd = &sensor->sd;
 	struct media_entity *entity = &sensor->sd.entity;
-	int ret;
+	int ret = -1;
 
 	// FIXME: is async needed?
 	ret = v4l2_device_register_subdev(state->mux.sd.subdev.v4l2_dev, sd);
@@ -3078,7 +3355,7 @@ static int ds5_mux_enum_mbus_code(struct v4l2_subdev *sd,
 	struct ds5 *state = container_of(sd, struct ds5, mux.sd.subdev);
 	struct v4l2_subdev_mbus_code_enum tmp = *mce;
 	struct v4l2_subdev *remote_sd;
-	int ret;
+	int ret = -1;
 
 	dev_info(&state->client->dev, "%s(): %s \n", __func__, sd->name);
 	switch (mce->pad) {
@@ -3152,7 +3429,7 @@ static int ds5_mux_enum_frame_size(struct v4l2_subdev *sd,
 	struct v4l2_subdev_frame_size_enum tmp = *fse;
 	struct v4l2_subdev *remote_sd;
 	u32 pad = fse->pad;
-	int ret;
+	int ret = -1;
 
 	tmp.pad = 0;
 
@@ -3220,7 +3497,7 @@ static int ds5_mux_enum_frame_interval(struct v4l2_subdev *sd,
 	struct v4l2_subdev_frame_interval_enum tmp = *fie;
 	struct v4l2_subdev *remote_sd;
 	u32 pad = fie->pad;
-	int ret;
+	int ret = -1;
 
 	tmp.pad = 0;
 
@@ -3295,9 +3572,7 @@ static int ds5_mux_set_fmt(struct v4l2_subdev *sd,
 	// u32 pad = fmt->pad;
 	int ret = 0;
 	int substream = -1;
-	if (pad != DS5_MUX_PAD_EXTERNAL)
-		ds5_s_state(state, pad - 1);
-	sensor = state->mux.last_set;
+
 	switch (pad) {
 	case DS5_MUX_PAD_DEPTH_A:
 	case DS5_MUX_PAD_MOTION_T_A:
@@ -3354,10 +3629,8 @@ static int ds5_mux_get_fmt(struct v4l2_subdev *sd,
 	int ret = 0;
 	struct ds5_sensor *sensor = state->mux.last_set;
 	u32 pad = sensor->mux_pad;
-	if (pad != DS5_MUX_PAD_EXTERNAL)
-		ds5_s_state(state, pad - 1);
-	sensor = state->mux.last_set;
-	dev_info(sd->dev, "%s(): %u %s %p\n", __func__, pad, ds5_get_sensor_name(state), state->mux.last_set);
+
+	dev_info(sd->dev, "%s(): %u %p\n", __func__, pad, state->mux.last_set);
 
 	switch (pad) {
 	case DS5_MUX_PAD_DEPTH_A:
@@ -3442,59 +3715,204 @@ static int ds5_mux_s_frame_interval(struct v4l2_subdev *sd,
 static int ds5_mux_s_stream(struct v4l2_subdev *sd, int on)
 {
 	struct ds5 *state = container_of(sd, struct ds5, mux.sd.subdev);
-	u16 streaming, status;
+	u16 streaming_depth, streaming_rgb, streaming_y8, rate, depth_status, rgb_status, y8_status;
 	int ret = 0;
+	u16 streaming, status;
+	u16 config_status_base, stream_status_base, stream_id;
 	unsigned int i = 0;
-	int restore_val = 0;
-	u16 config_status_base, stream_status_base, stream_id, vc_id;
+	u16 err_status;
+	u16 tmp;
+
+	dev_info(&state->client->dev, "%s(): %s on = %d\n", __func__, state->mux.last_set->sd.name, on);
+
+	state->mux.last_set->streaming = on;
+
+	// TODO: remove, workaround for FW crash in start
+	msleep_range(100);
+
+	if (!on) {
+		ds5_read(state, 0x1004, &streaming_depth);
+		ds5_read(state, 0x4800, &depth_status);
+		ds5_read(state, 0x4802, &rgb_status);
+
+		msleep_range(100);
+		ret = ds5_write(state, DS5_START_STOP_STREAM,
+				DS5_STREAM_STOP | DS5_STREAM_DEPTH);
+		msleep_range(100);
+		ret = ds5_write(state, DS5_START_STOP_STREAM,
+				DS5_STREAM_STOP | DS5_STREAM_RGB);
+		msleep_range(100);
+		ret = ds5_write(state, DS5_START_STOP_STREAM,
+				DS5_STREAM_STOP | DS5_STREAM_IMU);
+		msleep_range(100);
+		ret = ds5_write(state, DS5_START_STOP_STREAM,
+				DS5_STREAM_STOP | DS5_STREAM_IR);
+
+		return 0;
+	}
+
+	msleep_range(100);
+	if (on)
+		ret = ds5_configure_depth(state);
+	msleep_range(100);
+	ds5_write(state, 0x1000,  on ? 0x200 : 0x100);
 
-	if (state->is_depth) {
 		config_status_base = DS5_DEPTH_CONFIG_STATUS;
 		stream_status_base = DS5_DEPTH_STREAM_STATUS;
-		stream_id = DS5_STREAM_DEPTH;
-		vc_id = 0;
-	} else if (state->is_rgb) {
+		// check streaming status from FW
+		for (i = 0; i < DS5_START_MAX_COUNT; i++) {
+			ds5_read(state, stream_status_base, &streaming);
+			ds5_read(state, config_status_base, &status);
+			if ((status & DS5_STATUS_STREAMING) &&
+			    streaming == DS5_STREAM_STREAMING)
+				break;
+
+			msleep_range(DS5_START_POLL_TIME);
+		}
+
+		if (i == DS5_START_MAX_COUNT) {
+			dev_err(&state->client->dev,
+				"start depth streaming failed, exit on timeout\n");
+		}
+
+	msleep_range(100);
+	if (on)
+		ret = ds5_configure_rgb(state);
+	msleep_range(100);
+	/* RGB */
+	ds5_write(state, 0x1000,  on ? 0x201 : 0x101);
+
 		config_status_base = DS5_RGB_CONFIG_STATUS;
 		stream_status_base = DS5_RGB_STREAM_STATUS;
-		stream_id = DS5_STREAM_RGB;
-		vc_id = 1;
-	} else if (state->is_y8) {
+		// check streaming status from FW
+		for (i = 0; i < DS5_START_MAX_COUNT; i++) {
+			ds5_read(state, stream_status_base, &streaming);
+			ds5_read(state, config_status_base, &status);
+			if ((status & DS5_STATUS_STREAMING) &&
+			    streaming == DS5_STREAM_STREAMING)
+				break;
+
+			msleep_range(DS5_START_POLL_TIME);
+		}
+
+		if (i == DS5_START_MAX_COUNT) {
+			dev_err(&state->client->dev,
+				"start RGB streaming failed, exit on timeout\n");
+		}
+
+	/* IR */
+	msleep_range(100);
+	if (on)
+		ret = ds5_configure_ir(state);
+	msleep_range(100);
+	ds5_write(state, 0x1000,  on ? 0x204 : 0x104);
+
 		config_status_base = DS5_IR_CONFIG_STATUS;
 		stream_status_base = DS5_IR_STREAM_STATUS;
 		stream_id = DS5_STREAM_IR;
-		vc_id = 2;
-	} else if (state->is_imu) {
-		config_status_base = DS5_IMU_CONFIG_STATUS;
-		stream_status_base = DS5_IMU_STREAM_STATUS;
-		stream_id = DS5_STREAM_IMU;
-		vc_id = 3;
-	} else {
-		return -EINVAL;
-	}
 
-	dev_warn(&state->client->dev, "s_stream for stream %s, on = %d\n",
-			state->mux.last_set->sd.name, on);
+		// check streaming status from FW
+		for (i = 0; i < DS5_START_MAX_COUNT; i++) {
+			ds5_read(state, stream_status_base, &streaming);
+			ds5_read(state, config_status_base, &status);
+			if ((status & DS5_STATUS_STREAMING) &&
+			    streaming == DS5_STREAM_STREAMING)
+				break;
+
+			msleep_range(DS5_START_POLL_TIME);
+		}
+
+		if (DS5_START_MAX_COUNT == i) {
+			dev_err(&state->client->dev,
+				"start imu streaming failed, exit on timeout\n");
+		}
+
+	// TODO: this read seems to cause FW crash, need to debug
+	//ds5_read(state, 0x402, &rate);
+	rate = 0;
+
+	ds5_read(state, 0x401a, &err_status);
+	ds5_read(state, 0x1004, &streaming_depth);
+	ds5_read(state, 0x1008, &streaming_rgb);
+	ds5_read(state, 0x1010, &streaming_y8);
+	ds5_read(state, 0x4800, &depth_status);
+	ds5_read(state, 0x4802, &rgb_status);
+	ds5_read(state, 0x4808, &y8_status);
+	ds5_read(state, 0x4002, &tmp);
+	ds5_read(state, 0x4022, &tmp);
+	ds5_read(state, 0x4082, &tmp);
+
+	//ds5_write(state, 0x4002,  0x100 );
+	//ds5_write(state, 0x4022,  0x0 );
+	//ds5_read(state, 0x4002, &tmp);
+	//ds5_read(state, 0x4022, &tmp);
+	msleep_range(DS5_START_POLL_TIME*50);
+
+	dev_info(&state->client->dev, "%s(): streaming %x-%x-%x depth status 0x%04x, rgb status 0x%04x, rate %u\n", __func__,
+		 streaming_depth, streaming_rgb, streaming_y8, depth_status, rgb_status, rate);
+
+	return ret;
+}
+
+static int ds5_mux_s_stream_vc(struct ds5 *state, u16 vc_id, u16 on)
+{
+	u16 streaming_depth, streaming_rgb, streaming_y8, rate, depth_status, rgb_status, y8_status;
+	int ret = 0;
+	u16 streaming, status;
+	u16 config_status_base, stream_status_base, stream_id;
+	unsigned int i = 0;
+	u16 err_status;
+	u16 tmp;
+
+	dev_info(&state->client->dev, "%s(): %s on = %d\n", __func__, state->mux.last_set->sd.name, on);
 
-	restore_val = state->mux.last_set->streaming;
 	state->mux.last_set->streaming = on;
 
-	if (on) {
+	// TODO: remove, workaround for FW crash in start
+	msleep_range(100);
 
-		ret = ds5_configure(state);
-		if (ret)
-			goto restore_s_state;
+	if (!on) {
+		ds5_read(state, 0x1004, &streaming_depth);
+		ds5_read(state, 0x4800, &depth_status);
+		ds5_read(state, 0x4802, &rgb_status);
 
-		ret = ds5_write(state, DS5_START_STOP_STREAM,
-				DS5_STREAM_START | stream_id);
-		if (ret < 0)
-			goto restore_s_state;
+		if ((vc_id == DS5_MUX_PAD_DEPTH_A - 1) || (vc_id == DS5_MUX_PAD_DEPTH_B - 1)) {
+			msleep_range(100);
+			ret = ds5_write(state, DS5_START_STOP_STREAM,
+					DS5_STREAM_STOP | DS5_STREAM_DEPTH);
+		}
+		if ((vc_id == DS5_MUX_PAD_RGB_A - 1) || (vc_id == DS5_MUX_PAD_RGB_B - 1)) {
+			msleep_range(100);
+			ret = ds5_write(state, DS5_START_STOP_STREAM,
+					DS5_STREAM_STOP | DS5_STREAM_RGB);
+		}
+		if ((vc_id == DS5_MUX_PAD_IMU_A - 1) || (vc_id == DS5_MUX_PAD_IMU_B - 1)) {
+			msleep_range(100);
+			ret = ds5_write(state, DS5_START_STOP_STREAM,
+					DS5_STREAM_STOP | DS5_STREAM_IMU);
+		}
+		if ((vc_id == DS5_MUX_PAD_MOTION_T_A - 1) || (vc_id == DS5_MUX_PAD_MOTION_T_B - 1)) {
+			msleep_range(100);
+			ret = ds5_write(state, DS5_START_STOP_STREAM,
+					DS5_STREAM_STOP | DS5_STREAM_IR);
+		}
+		return 0;
+	}
 
+	msleep_range(100);
+	if ((on) && ((vc_id == DS5_MUX_PAD_DEPTH_A - 1) || (vc_id == DS5_MUX_PAD_DEPTH_B - 1))) {
+		ret = ds5_configure_depth(state);
+	msleep_range(100);
+	ds5_write(state, 0x1000,  on ? 0x200 : 0x100);
+
+		config_status_base = DS5_DEPTH_CONFIG_STATUS;
+		stream_status_base = DS5_DEPTH_STREAM_STATUS;
 		// check streaming status from FW
 		for (i = 0; i < DS5_START_MAX_COUNT; i++) {
 			ds5_read(state, stream_status_base, &streaming);
 			ds5_read(state, config_status_base, &status);
 			if ((status & DS5_STATUS_STREAMING) &&
-					streaming == DS5_STREAM_STREAMING)
+			    streaming == DS5_STREAM_STREAMING)
 				break;
 
 			msleep_range(DS5_START_POLL_TIME);
@@ -3502,43 +3920,120 @@ static int ds5_mux_s_stream(struct v4l2_subdev *sd, int on)
 
 		if (DS5_START_MAX_COUNT == i) {
 			dev_err(&state->client->dev,
-				"start streaming failed, exit on timeout\n");
-			/* notify fw */
-			ret = ds5_write(state, DS5_START_STOP_STREAM,
-					DS5_STREAM_STOP | stream_id);
-			ret = -EAGAIN;
-			goto restore_s_state;
-		} else {
-			dev_dbg(&state->client->dev, "started after %dms\n",
-				i * DS5_START_POLL_TIME);
+				"start depth streaming failed, exit on timeout\n");
+		}
+	}
+
+	if ((on) && ((vc_id == DS5_MUX_PAD_RGB_A - 1) || (vc_id == DS5_MUX_PAD_RGB_B - 1))) {
+	msleep_range(100);
+	if (on)
+		ret = ds5_configure_rgb(state);
+	msleep_range(100);
+	/* RGB */
+	ds5_write(state, 0x1000,  on ? 0x201 : 0x101);
+
+		config_status_base = DS5_RGB_CONFIG_STATUS;
+		stream_status_base = DS5_RGB_STREAM_STATUS;
+		// check streaming status from FW
+		for (i = 0; i < DS5_START_MAX_COUNT; i++) {
+			ds5_read(state, stream_status_base, &streaming);
+			ds5_read(state, config_status_base, &status);
+			if ((status & DS5_STATUS_STREAMING) &&
+			    streaming == DS5_STREAM_STREAMING)
+				break;
+
+			msleep_range(DS5_START_POLL_TIME);
+		}
+
+		if (DS5_START_MAX_COUNT == i) {
+			dev_err(&state->client->dev,
+					"start RGB streaming failed, exit on timeout\n");
+		}
+	}
+
+	if ((on) && ((vc_id == DS5_MUX_PAD_MOTION_T_A - 1) || (vc_id == DS5_MUX_PAD_MOTION_T_B - 1))) {
+	msleep_range(100);
+	if (on)
+		ret = ds5_configure_ir(state);
+	msleep_range(100);
+	ds5_write(state, 0x1000,  on ? 0x204 : 0x104);
+
+		config_status_base = DS5_IR_CONFIG_STATUS;
+		stream_status_base = DS5_IR_STREAM_STATUS;
+		stream_id = DS5_STREAM_IR;
+
+		// check streaming status from FW
+		for (i = 0; i < DS5_START_MAX_COUNT; i++) {
+			ds5_read(state, stream_status_base, &streaming);
+			ds5_read(state, config_status_base, &status);
+			if ((status & DS5_STATUS_STREAMING) &&
+			    streaming == DS5_STREAM_STREAMING)
+				break;
+
+			msleep_range(DS5_START_POLL_TIME);
 		}
-	} else {
-		ret = ds5_write(state, DS5_START_STOP_STREAM,
-				DS5_STREAM_STOP | stream_id);
-		if (ret < 0)
-			goto restore_s_state;
 
+		if (DS5_START_MAX_COUNT == i) {
+			dev_err(&state->client->dev,
+				"start imu streaming failed, exit on timeout\n");
+		}
 	}
 
-	ds5_read(state, config_status_base, &status);
-	ds5_read(state, stream_status_base, &streaming);
-	dev_info(&state->client->dev,
-			"%s %s, stream_status 0x%x:%x, config_status 0x%x:%x\n",
-			ds5_get_sensor_name(state),
-			(on)?"START":"STOP",
-			stream_status_base, streaming,
-			config_status_base, status);
+	if ((on) && ((vc_id == DS5_MUX_PAD_IMU_A - 1) ||
+	    (vc_id == DS5_MUX_PAD_IMU_B - 1))) {
+		msleep_range(100);
 
-	return ret;
+		if (on)
+			ret = ds5_configure_imu(state);
+
+		msleep_range(100);
+		ds5_write(state, 0x1000,  on ? 0x202 : 0x102);
+
+		config_status_base = DS5_IMU_CONFIG_STATUS;
+		stream_status_base = DS5_IMU_STREAM_STATUS;
+		stream_id = DS5_STREAM_IMU;
+
+		// check streaming status from FW
+		for (i = 0; i < DS5_START_MAX_COUNT; i++) {
+			ds5_read(state, stream_status_base, &streaming);
+			ds5_read(state, config_status_base, &status);
+
+			if ((status & DS5_STATUS_STREAMING) &&
+			    (streaming == DS5_STREAM_STREAMING))
+				break;
 
-restore_s_state:
+			msleep_range(DS5_START_POLL_TIME);
+		}
 
-	ds5_read(state, config_status_base, &status);
-	dev_err(&state->client->dev,
-			"%s stream toggle failed! %x status 0x%04x\n",
-			ds5_get_sensor_name(state) ,restore_val, status);
+		if (i == DS5_START_MAX_COUNT) {
+			dev_err(&state->client->dev,
+				"start imu streaming failed, exit on timeout\n");
+		}
+	}
 
-	state->mux.last_set->streaming = restore_val;
+	// TODO: this read seems to cause FW crash, need to debug
+	//ds5_read(state, 0x402, &rate);
+	rate = 0;
+	ds5_read(state, 0x401a, &err_status);
+
+	ds5_read(state, 0x1004, &streaming_depth);
+	ds5_read(state, 0x1008, &streaming_rgb);
+	ds5_read(state, 0x1010, &streaming_y8);
+	ds5_read(state, 0x4800, &depth_status);
+	ds5_read(state, 0x4802, &rgb_status);
+	ds5_read(state, 0x4808, &y8_status);
+	ds5_read(state, 0x4002, &tmp);
+	ds5_read(state, 0x4022, &tmp);
+	ds5_read(state, 0x4082, &tmp);
+
+	//ds5_write(state, 0x4002,  0x100 );
+	//ds5_write(state, 0x4022,  0x0 );
+	//ds5_read(state, 0x4002, &tmp);
+	//ds5_read(state, 0x4022, &tmp);
+	msleep_range(DS5_START_POLL_TIME*50);
+
+	dev_info(&state->client->dev, "%s(): streaming %x-%x-%x depth status 0x%04x, rgb status 0x%04x, rate %u\n", __func__,
+		 streaming_depth, streaming_rgb, streaming_y8, depth_status, rgb_status, rate);
 
 	return ret;
 }
@@ -3811,14 +4306,17 @@ static int ds5_mux_init(struct i2c_client *c, struct ds5 *state)
 		return ret;
 	/*set for y8*/
 	ret = ds5_ctrl_init(state, 2);
-	if (ret < 0)
-		return ret;
-	/*set for imu*/
-	ret = ds5_ctrl_init(state, 3);
 	if (ret < 0)
 		return ret;
 
-	ds5_set_state_last_set(state);
+	if (state->is_depth)
+		state->mux.last_set = &state->depth.sensor;
+	else if (state->is_rgb)
+		state->mux.last_set = &state->rgb.sensor;
+	else if (state->is_y8)
+		state->mux.last_set = &state->motion_t.sensor;
+	else
+		state->mux.last_set = &state->imu.sensor;
 
 #ifdef CONFIG_TEGRA_CAMERA_PLATFORM
 	state->mux.sd.dev = &c->dev;
@@ -4138,20 +4636,16 @@ static ssize_t ds5_dfu_device_read(struct file *flip,
 		char __user *buffer, size_t len, loff_t *offset)
 {
 	struct ds5 *state = flip->private_data;
-	u16 fw_ver, fw_build;
-	char msg[32];
+	u16 fw_ver;
+	char msg[20];
 	int ret = 0;
 
 	if (mutex_lock_interruptible(&state->lock))
 		return -ERESTARTSYS;
-	ret |= ds5_read(state, DS5_FW_VERSION, &fw_ver);
-	ret |= ds5_read(state, DS5_FW_BUILD, &fw_build);
+	ret = ds5_read(state, DS5_FW_VERSION, &fw_ver);
 	if (ret < 0)
 		goto e_dfu_read_failed;
-	snprintf(msg, sizeof(msg) ,"DFU info: \tver:  %d.%d.%d.%d\n",
-			(fw_ver >> 8) & 0xff, fw_ver & 0xff,
-			(fw_build >> 8) & 0xff, fw_build & 0xff);
-
+	snprintf(msg, sizeof(msg), "DFU info: \tver: (0x%x)\n", fw_ver);
 	if (copy_to_user(buffer, msg, strlen(msg)))
 		ret = -EFAULT;
 	else {
@@ -4181,8 +4675,7 @@ static ssize_t ds5_dfu_device_write(struct file *flip,
 					__func__, ret);
 			goto dfu_write_error;
 		}
-	/*fallthrough - procceed to recovery*/
-	__attribute__((__fallthrough__));
+	/*no break - proceed to recovery*/
 	case DS5_DFU_RECOVERY:
 		ret = ds5_dfu_detach(state);
 		if (ret < 0) {
@@ -4192,8 +4685,8 @@ static ssize_t ds5_dfu_device_write(struct file *flip,
 		}
 		state->dfu_dev.dfu_state_flag = DS5_DFU_IN_PROGRESS;
 		state->dfu_dev.init_v4l_f = 1;
-	/*fallthrough - procceed to download*/
-	__attribute__((__fallthrough__));
+
+	/*no break - proceed to download*/
 	case DS5_DFU_IN_PROGRESS: {
 		unsigned int dfu_full_blocks = len / DFU_BLOCK_SIZE;
 		unsigned int dfu_part_blocks = len % DFU_BLOCK_SIZE;
@@ -4244,11 +4737,8 @@ static ssize_t ds5_dfu_device_write(struct file *flip,
 	return len;
 
 dfu_write_error:
+	//TODO: Reset device here
 	state->dfu_dev.dfu_state_flag = DS5_DFU_ERROR;
-	// Reset DFU device to IDLE states
-	ret = ds5_write(state, 0x5010, 0x0);
-	if (!ret)
-		state->dfu_dev.dfu_state_flag = DS5_DFU_IDLE;
 	mutex_unlock(&state->lock);
 	return ret;
 };
@@ -4257,6 +4747,8 @@ static int ds5_dfu_device_open(struct inode *inode, struct file *file)
 {
 	struct ds5 *state = container_of(inode->i_cdev, struct ds5,
 			dfu_dev.ds5_cdev);
+	struct i2c_adapter *parent = i2c_parent_is_i2c_adapter(
+			state->client->adapter);
 
 	if (state->dfu_dev.device_open_count)
 		return -EBUSY;
@@ -4356,14 +4848,12 @@ static int ds5_chrdev_init(struct i2c_client *c, struct ds5 *state)
 {
 	struct cdev *ds5_cdev = &state->dfu_dev.ds5_cdev;
 	struct class **ds5_class = &state->dfu_dev.ds5_class;
-	struct d4xx_pdata *pdata = c->dev.platform_data;
-
 	struct device *chr_dev;
-	char dev_name[sizeof(DS5_DRIVER_NAME_DFU) + 8];
+	char dev_name[sizeof(DS5_DRIVER_NAME_DFU) + 5];
 	dev_t *dev_num = &c->dev.devt;
 	int ret;
 
-	dev_dbg(&c->dev, "%s()\n", __func__);
+	dev_info(&c->dev, "%s()\n", __func__);
 	/* Request the kernel for N_MINOR devices */
 	ret = alloc_chrdev_region(dev_num, 0, 1, DS5_DRIVER_NAME_DFU);
 	if (ret < 0)
@@ -4388,8 +4878,8 @@ static int ds5_chrdev_init(struct i2c_client *c, struct ds5 *state)
 	/* Build up the current device number. To be used further */
 	*dev_num = MKDEV(MAJOR(*dev_num), MINOR(*dev_num));
 	/* Create a device node for this device. */
-	snprintf(dev_name, sizeof(dev_name), "%s-%c",
-			DS5_DRIVER_NAME_DFU, pdata->suffix);
+	snprintf(dev_name, sizeof(dev_name), "%s%d",
+			DS5_DRIVER_NAME_DFU, MAJOR(*dev_num));
 	chr_dev = device_create(*ds5_class, NULL, *dev_num, NULL, dev_name);
 	if (IS_ERR(chr_dev)) {
 		ret = PTR_ERR(chr_dev);
@@ -4406,9 +4896,7 @@ static int ds5_chrdev_remove(struct ds5 *state)
 {
 	struct class **ds5_class = &state->dfu_dev.ds5_class;
 	dev_t *dev_num = &state->client->dev.devt;
-	if (!ds5_class) {
-		return 0;
-	}
+
 	dev_dbg(&state->client->dev, "%s()\n", __func__);
 	unregister_chrdev_region(*dev_num, 1);
 	device_destroy(*ds5_class, *dev_num);
@@ -4639,16 +5127,16 @@ static int ds5_i2c_addr_setting(struct i2c_client *c, struct ds5 *state)
 	}
 
 	c->addr = 0x48;
-	max9296_write_8(state, 0x0010, 0x23);
+	max9296_write_8(state, 0x0010, 0x40);
 	c->addr = 0x4a;
-	max9296_write_8(state, 0x0010, 0x23);
+	max9296_write_8(state, 0x0010, 0x40);
 	c->addr = 0x68;
-	max9296_write_8(state, 0x0010, 0x23);
+	max9296_write_8(state, 0x0010, 0x40);
 	c->addr = 0x6c;
-	max9296_write_8(state, 0x0010, 0x23);
+	max9296_write_8(state, 0x0010, 0x40);
 
 	c->addr = 0x6c;
-	max9296_write_8(state, 0x0010, 0x22);
+	max9296_write_8(state, 0x0010, 0x32);
 	msleep_range(1000);
 
 	c->addr = 0x40;
@@ -4660,7 +5148,7 @@ static int ds5_i2c_addr_setting(struct i2c_client *c, struct ds5 *state)
 	max9295_write_8(state, 0x0045, 0x20);
 
 	c->addr = 0x68;
-	max9296_write_8(state, 0x0010, 0x22);
+	max9296_write_8(state, 0x0010, 0x32);
 	msleep_range(1000);
 
 	c->addr = 0x40;
@@ -4670,7 +5158,7 @@ static int ds5_i2c_addr_setting(struct i2c_client *c, struct ds5 *state)
 	max9295_write_8(state, 0x0045, 0x20);
 
 	c->addr = 0x4a;
-	max9296_write_8(state, 0x0010, 0x22);
+	max9296_write_8(state, 0x0010, 0x32);
 	msleep_range(1000);
 
 	c->addr = 0x40;
@@ -4680,7 +5168,7 @@ static int ds5_i2c_addr_setting(struct i2c_client *c, struct ds5 *state)
 	max9295_write_8(state, 0x0045, 0x20);
 
 	c->addr = 0x48;
-	max9296_write_8(state, 0x0010, 0x22);
+	max9296_write_8(state, 0x0010, 0x32);
 	msleep_range(1000);
 
 	c->addr = 0x40;
@@ -4751,47 +5239,14 @@ static int ds5_probe(struct i2c_client *c, const struct i2c_device_id *id)
 		}
 	}
 
-	// Verify communication
-	retry = 10;
-	do {
-	ret = ds5_read(state, 0x5020, &rec_state);
-	} while (retry-- && ret < 0);
-	if (ret < 0) {
-		dev_err(&c->dev,
-			"%s(): cannot communicate with D4XX: %d on addr: 0x%x\n",
-			__func__, ret, c->addr);
+	ret = ds5_chrdev_init(c, state);
+	if (ret < 0)
 		goto e_regulator;
-	}
-
-	state->is_depth = 0;
-	state->is_y8 = 0;
-	state->is_rgb = 0;
-	state->is_imu = 0;
-#ifdef CONFIG_OF
-	ret = of_property_read_string(c->dev.of_node, "cam-type", &str);
-	if (!ret && !strncmp(str, "Depth", strlen("Depth"))) {
-		state->is_depth = 1;
-	}
-	if (!ret && !strncmp(str, "Y8", strlen("Y8"))) {
-		state->is_y8 = 1;
-	}
-	if (!ret && !strncmp(str, "RGB", strlen("RGB"))) {
-		state->is_rgb = 1;
-	}
-	if (!ret && !strncmp(str, "IMU", strlen("IMU"))) {
-		state->is_imu = 1;
-	}
-#else
-	state->is_depth = 1;
-#endif
-	/* create DFU chardev once */
-	if (state->is_depth) {
-		ret = ds5_chrdev_init(c, state);
-		if (ret < 0)
-			goto e_regulator;
-	}
 
+	retry = 100;
+	do {
 	ret = ds5_read(state, 0x5020, &rec_state);
+	} while (retry-- && ret < 0);
 	if (ret < 0) {
 		dev_err(&c->dev, "%s(): cannot communicate with D4XX: %d\n",
 				__func__, ret);
@@ -4804,6 +5259,22 @@ static int ds5_probe(struct i2c_client *c, const struct i2c_device_id *id)
 		return 0;
 	}
 
+	state->is_depth = 0;
+	state->is_y8 = 0;
+	state->is_rgb = 0;
+	state->is_imu = 0;
+
+	err = of_property_read_string(c->dev.of_node, "cam-type",
+			&str);
+	if (!err && !strncmp(str, "Depth", strlen("Depth")))
+		state->is_depth = 1;
+	if (!err && !strncmp(str, "Y8", strlen("Y8")))
+		state->is_y8 = 1;
+	if (!err && !strncmp(str, "RGB", strlen("RGB")))
+		state->is_rgb = 1;
+	if (!err && !strncmp(str, "IMU", strlen("IMU")))
+		state->is_imu = 1;
+
 	ds5_read_with_check(state, DS5_FW_VERSION, &state->fw_version);
 	ds5_read_with_check(state, DS5_FW_BUILD, &state->fw_build);
 
@@ -4835,8 +5306,7 @@ static int ds5_probe(struct i2c_client *c, const struct i2c_device_id *id)
 	return 0;
 
 e_chardev:
-	if(state->dfu_dev.ds5_class)
-		ds5_chrdev_remove(state);
+	ds5_chrdev_remove(state);
 e_regulator:
 	if (state->vcc)
 		regulator_disable(state->vcc);
@@ -4852,10 +5322,7 @@ static int ds5_remove(struct i2c_client *c)
 	if (state->vcc)
 		regulator_disable(state->vcc);
 //	gpio_free(state->pwdn_gpio);
-	if (state->is_depth) {
-		ds5_chrdev_remove(state);
-	}
-
+	ds5_chrdev_remove(state);
 	if (state->dfu_dev.dfu_state_flag != DS5_DFU_RECOVERY) {
 #ifdef CONFIG_SYSFS
 		sysfs_remove_group(&c->dev.kobj, &ds5_attr_group);
diff --git a/drivers/media/i2c/hm11b1.c b/drivers/media/i2c/hm11b1.c
index b4d51bc11728..6b1f892ac949 100644
--- a/drivers/media/i2c/hm11b1.c
+++ b/drivers/media/i2c/hm11b1.c
@@ -1053,7 +1053,11 @@ static int hm11b1_identify_module(struct hm11b1 *hm11b1)
 	return 0;
 }
 
+#if LINUX_VERSION_CODE < KERNEL_VERSION(6, 1, 0)
 static int hm11b1_remove(struct i2c_client *client)
+#else
+static void hm11b1_remove(struct i2c_client *client)
+#endif
 {
 	struct v4l2_subdev *sd = i2c_get_clientdata(client);
 	struct hm11b1 *hm11b1 = to_hm11b1(sd);
@@ -1064,7 +1068,9 @@ static int hm11b1_remove(struct i2c_client *client)
 	pm_runtime_disable(&client->dev);
 	mutex_destroy(&hm11b1->mutex);
 
+#if LINUX_VERSION_CODE < KERNEL_VERSION(6, 1, 0)
 	return 0;
+#endif
 }
 
 #if IS_ENABLED(CONFIG_INTEL_SKL_INT3472)
diff --git a/drivers/media/i2c/lt6911uxc.c b/drivers/media/i2c/lt6911uxc.c
index d6c31bcd07b4..015e86a440c3 100644
--- a/drivers/media/i2c/lt6911uxc.c
+++ b/drivers/media/i2c/lt6911uxc.c
@@ -561,6 +561,8 @@ static u64 __maybe_unused get_hblank(struct lt6911uxc_state *lt6911uxc)
 	return hblank;
 }
 
+static int lt6911uxc_set_stream(struct v4l2_subdev *sd, int enable);
+
 static int lt6911uxc_set_ctrl(struct v4l2_ctrl *ctrl)
 {
 	struct lt6911uxc_state *lt6911uxc = container_of(ctrl->handler,
@@ -568,6 +570,17 @@ static int lt6911uxc_set_ctrl(struct v4l2_ctrl *ctrl)
 	struct i2c_client *client = v4l2_get_subdevdata(&lt6911uxc->sd);
 	s64 exposure_max;
 	int ret = 0;
+	u32 val;
+
+	/* Set streaming when ipu set sub_stream */
+	if (ctrl->id == V4L2_CID_IPU_SET_SUB_STREAM) {
+		val = (*ctrl->p_new.p_s64 & 0xFFFF);
+		dev_info(&client->dev, "V4L2_CID_IPU_SET_SUB_STREAM %x\n", val);
+		mutex_unlock(&lt6911uxc->mutex);
+		ret = lt6911uxc_set_stream(&lt6911uxc->sd, val & 0x00FF);
+		mutex_lock(&lt6911uxc->mutex);
+		return ret;
+	}
 
 	/* Propagate change of current control to all related controls */
 	if (ctrl->id == V4L2_CID_VBLANK) {
diff --git a/drivers/media/i2c/ov01a10.c b/drivers/media/i2c/ov01a10.c
index 7212ccd108d2..768c71cd272c 100644
--- a/drivers/media/i2c/ov01a10.c
+++ b/drivers/media/i2c/ov01a10.c
@@ -1,5 +1,5 @@
 // SPDX-License-Identifier: GPL-2.0
-// Copyright (c) 2020-2021 Intel Corporation.
+// Copyright (c) 2020-2022 Intel Corporation.
 
 #include <asm/unaligned.h>
 #include <linux/acpi.h>
@@ -837,7 +837,11 @@ static int ov01a10_identify_module(struct ov01a10 *ov01a10)
 	return 0;
 }
 
+#if LINUX_VERSION_CODE < KERNEL_VERSION(6, 1, 0)
 static int ov01a10_remove(struct i2c_client *client)
+#else
+static void ov01a10_remove(struct i2c_client *client)
+#endif
 {
 	struct v4l2_subdev *sd = i2c_get_clientdata(client);
 	struct ov01a10 *ov01a10 = to_ov01a10(sd);
@@ -848,7 +852,9 @@ static int ov01a10_remove(struct i2c_client *client)
 	pm_runtime_disable(&client->dev);
 	mutex_destroy(&ov01a10->mutex);
 
+#if LINUX_VERSION_CODE < KERNEL_VERSION(6, 1, 0)
 	return 0;
+#endif
 }
 
 static int ov01a10_probe(struct i2c_client *client)
diff --git a/drivers/media/i2c/ov01a1s.c b/drivers/media/i2c/ov01a1s.c
index d604ab61311e..8ce5d04c678d 100644
--- a/drivers/media/i2c/ov01a1s.c
+++ b/drivers/media/i2c/ov01a1s.c
@@ -881,7 +881,11 @@ static int ov01a1s_identify_module(struct ov01a1s *ov01a1s)
 	return 0;
 }
 
+#if LINUX_VERSION_CODE < KERNEL_VERSION(6, 1, 0)
 static int ov01a1s_remove(struct i2c_client *client)
+#else
+static void ov01a1s_remove(struct i2c_client *client)
+#endif
 {
 	struct v4l2_subdev *sd = i2c_get_clientdata(client);
 	struct ov01a1s *ov01a1s = to_ov01a1s(sd);
@@ -892,7 +896,9 @@ static int ov01a1s_remove(struct i2c_client *client)
 	pm_runtime_disable(&client->dev);
 	mutex_destroy(&ov01a1s->mutex);
 
+#if LINUX_VERSION_CODE < KERNEL_VERSION(6, 1, 0)
 	return 0;
+#endif
 }
 
 #if IS_ENABLED(CONFIG_INTEL_SKL_INT3472)
diff --git a/drivers/media/i2c/ov02c10.c b/drivers/media/i2c/ov02c10.c
index 94bf9a6fb592..a6bea5ef3286 100644
--- a/drivers/media/i2c/ov02c10.c
+++ b/drivers/media/i2c/ov02c10.c
@@ -1221,7 +1221,11 @@ static int ov02c10_identify_module(struct ov02c10 *ov02c10)
 	return 0;
 }
 
+#if LINUX_VERSION_CODE < KERNEL_VERSION(6, 1, 0)
 static int ov02c10_remove(struct i2c_client *client)
+#else
+static void ov02c10_remove(struct i2c_client *client)
+#endif
 {
 	struct v4l2_subdev *sd = i2c_get_clientdata(client);
 	struct ov02c10 *ov02c10 = to_ov02c10(sd);
@@ -1232,7 +1236,9 @@ static int ov02c10_remove(struct i2c_client *client)
 	pm_runtime_disable(&client->dev);
 	mutex_destroy(&ov02c10->mutex);
 
+#if LINUX_VERSION_CODE < KERNEL_VERSION(6, 1, 0)
 	return 0;
+#endif
 }
 
 static int ov02c10_probe(struct i2c_client *client)
diff --git a/drivers/media/i2c/ov13858_intel.c b/drivers/media/i2c/ov13858_intel.c
index 11bf93018bab..43885cf3156a 100644
--- a/drivers/media/i2c/ov13858_intel.c
+++ b/drivers/media/i2c/ov13858_intel.c
@@ -5,6 +5,7 @@
 #include <linux/i2c.h>
 #include <linux/module.h>
 #include <linux/pm_runtime.h>
+#include <linux/version.h>
 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-fwnode.h>
@@ -2013,7 +2014,11 @@ static int ov13858_probe(struct i2c_client *client,
 	return ret;
 }
 
+#if LINUX_VERSION_CODE < KERNEL_VERSION(6, 1, 0)
 static int ov13858_remove(struct i2c_client *client)
+#else
+static void ov13858_remove(struct i2c_client *client)
+#endif
 {
 	struct v4l2_subdev *sd = i2c_get_clientdata(client);
 	struct ov13858 *ov13858 = to_ov13858(sd);
@@ -2024,7 +2029,9 @@ static int ov13858_remove(struct i2c_client *client)
 
 	pm_runtime_disable(&client->dev);
 
+#if LINUX_VERSION_CODE < KERNEL_VERSION(6, 1, 0)
 	return 0;
+#endif
 }
 
 static const struct i2c_device_id ov13858_id_table[] = {
diff --git a/drivers/media/i2c/ov8856.c b/drivers/media/i2c/ov8856.c
index 4a19ef0060a6..07b4d16e7cc7 100644
--- a/drivers/media/i2c/ov8856.c
+++ b/drivers/media/i2c/ov8856.c
@@ -1,5 +1,5 @@
 // SPDX-License-Identifier: GPL-2.0
-// Copyright (c) 2021 Intel Corporation.
+// Copyright (c) 2021-2022 Intel Corporation.
 
 #include <asm/unaligned.h>
 #include <linux/acpi.h>
@@ -1197,7 +1197,11 @@ static int ov8856_identify_module(struct ov8856 *ov8856)
 	return 0;
 }
 
+#if LINUX_VERSION_CODE < KERNEL_VERSION(6, 1, 0)
 static int ov8856_remove(struct i2c_client *client)
+#else
+static void ov8856_remove(struct i2c_client *client)
+#endif
 {
 	struct v4l2_subdev *sd = i2c_get_clientdata(client);
 	struct ov8856 *ov8856 = to_ov8856(sd);
@@ -1208,7 +1212,9 @@ static int ov8856_remove(struct i2c_client *client)
 	pm_runtime_disable(&client->dev);
 	mutex_destroy(&ov8856->mutex);
 
+#if LINUX_VERSION_CODE < KERNEL_VERSION(6, 1, 0)
 	return 0;
+#endif
 }
 
 static int ov8856_probe(struct i2c_client *client)
diff --git a/drivers/media/pci/intel/ipu-isys-csi2-be-soc.c b/drivers/media/pci/intel/ipu-isys-csi2-be-soc.c
index 83e587721e39..92fb5ecce2fb 100644
--- a/drivers/media/pci/intel/ipu-isys-csi2-be-soc.c
+++ b/drivers/media/pci/intel/ipu-isys-csi2-be-soc.c
@@ -25,7 +25,6 @@ static const u32 csi2_be_soc_supported_codes_pad[] = {
 	MEDIA_BUS_FMT_RGB888_1X24,
 	MEDIA_BUS_FMT_UYVY8_1X16,
 	MEDIA_BUS_FMT_YUYV8_1X16,
-	MEDIA_BUS_FMT_VYUY8_1X16,
 	MEDIA_BUS_FMT_SBGGR12_1X12,
 	MEDIA_BUS_FMT_SGBRG12_1X12,
 	MEDIA_BUS_FMT_SGRBG12_1X12,
@@ -109,9 +108,9 @@ __subdev_link_validate(struct v4l2_subdev *sd, struct media_link *link,
 		       struct v4l2_subdev_format *source_fmt,
 		       struct v4l2_subdev_format *sink_fmt)
 {
-	struct ipu_isys_pipeline *ip = container_of(sd->entity.pipe,
-						    struct ipu_isys_pipeline,
-						    pipe);
+	struct ipu_isys_pipeline *ip =
+		container_of(media_entity_pipeline(&sd->entity),
+			     struct ipu_isys_pipeline, pipe);
 
 	ip->csi2_be_soc = to_ipu_isys_csi2_be_soc(sd);
 	return ipu_isys_subdev_link_validate(sd, link, source_fmt, sink_fmt);
diff --git a/drivers/media/pci/intel/ipu-isys-csi2-be.c b/drivers/media/pci/intel/ipu-isys-csi2-be.c
index 78bbc6e983bf..1b885ef21e5b 100644
--- a/drivers/media/pci/intel/ipu-isys-csi2-be.c
+++ b/drivers/media/pci/intel/ipu-isys-csi2-be.c
@@ -73,9 +73,9 @@ static int __subdev_link_validate(struct v4l2_subdev *sd,
 				  struct v4l2_subdev_format *source_fmt,
 				  struct v4l2_subdev_format *sink_fmt)
 {
-	struct ipu_isys_pipeline *ip = container_of(sd->entity.pipe,
-						    struct ipu_isys_pipeline,
-						    pipe);
+	struct ipu_isys_pipeline *ip =
+		container_of(media_entity_pipeline(&sd->entity),
+			     struct ipu_isys_pipeline, pipe);
 
 	ip->csi2_be = to_ipu_isys_csi2_be(sd);
 	return ipu_isys_subdev_link_validate(sd, link, source_fmt, sink_fmt);
diff --git a/drivers/media/pci/intel/ipu-isys-csi2.c b/drivers/media/pci/intel/ipu-isys-csi2.c
index 979f6bf4400e..0582388ca04d 100644
--- a/drivers/media/pci/intel/ipu-isys-csi2.c
+++ b/drivers/media/pci/intel/ipu-isys-csi2.c
@@ -25,7 +25,6 @@ static const u32 csi2_supported_codes_pad_sink[] = {
 	MEDIA_BUS_FMT_RGB888_1X24,
 	MEDIA_BUS_FMT_UYVY8_1X16,
 	MEDIA_BUS_FMT_YUYV8_1X16,
-	MEDIA_BUS_FMT_VYUY8_1X16,
 	MEDIA_BUS_FMT_YUYV10_1X20,
 	MEDIA_BUS_FMT_SBGGR10_1X10,
 	MEDIA_BUS_FMT_SGBRG10_1X10,
@@ -53,7 +52,6 @@ static const u32 csi2_supported_codes_pad_source[] = {
 	MEDIA_BUS_FMT_RGB888_1X24,
 	MEDIA_BUS_FMT_UYVY8_1X16,
 	MEDIA_BUS_FMT_YUYV8_1X16,
-	MEDIA_BUS_FMT_VYUY8_1X16,
 	MEDIA_BUS_FMT_YUYV10_1X20,
 	MEDIA_BUS_FMT_SBGGR10_1X10,
 	MEDIA_BUS_FMT_SGBRG10_1X10,
@@ -80,9 +78,9 @@ static struct v4l2_subdev_internal_ops csi2_sd_internal_ops = {
 #if LINUX_VERSION_CODE >= KERNEL_VERSION(5, 10, 255)
 int ipu_isys_csi2_get_link_freq(struct ipu_isys_csi2 *csi2, s64 *link_freq)
 {
-	struct ipu_isys_pipeline *pipe = container_of(csi2->asd.sd.entity.pipe,
-						      struct ipu_isys_pipeline,
-						      pipe);
+	struct ipu_isys_pipeline *pipe =
+		container_of(media_entity_pipeline(&csi2->asd.sd.entity),
+			     struct ipu_isys_pipeline, pipe);
 	struct v4l2_subdev *ext_sd =
 		media_entity_to_v4l2_subdev(pipe->external->entity);
 	struct device *dev = &csi2->isys->adev->dev;
@@ -111,9 +109,9 @@ int ipu_isys_csi2_get_link_freq(struct ipu_isys_csi2 *csi2, s64 *link_freq)
 #else
 int ipu_isys_csi2_get_link_freq(struct ipu_isys_csi2 *csi2, __s64 *link_freq)
 {
-	struct ipu_isys_pipeline *pipe = container_of(csi2->asd.sd.entity.pipe,
-						      struct ipu_isys_pipeline,
-						      pipe);
+	struct ipu_isys_pipeline *pipe =
+		container_of(media_entity_pipeline(&csi2->asd.sd.entity),
+			     struct ipu_isys_pipeline, pipe);
 	struct v4l2_subdev *ext_sd =
 	    media_entity_to_v4l2_subdev(pipe->external->entity);
 	struct v4l2_ext_control c = {.id = V4L2_CID_LINK_FREQ, };
@@ -261,9 +259,8 @@ ipu_isys_csi2_calc_timing(struct ipu_isys_csi2 *csi2,
 static int set_stream(struct v4l2_subdev *sd, int enable)
 {
 	struct ipu_isys_csi2 *csi2 = to_ipu_isys_csi2(sd);
-	struct ipu_isys_pipeline *ip = container_of(sd->entity.pipe,
-						    struct ipu_isys_pipeline,
-						    pipe);
+	struct ipu_isys_pipeline *ip =
+		to_ipu_isys_pipeline(media_entity_pipeline(&sd->entity));
 	struct ipu_isys_csi2_config *cfg;
 	struct v4l2_subdev *ext_sd;
 	struct ipu_isys_csi2_timing timing = {0};
@@ -329,21 +326,23 @@ static void csi2_capture_done(struct ipu_isys_pipeline *ip,
 
 static int csi2_link_validate(struct media_link *link)
 {
+	struct media_pipeline *media_pipe;
 	struct ipu_isys_csi2 *csi2;
 	struct ipu_isys_pipeline *ip;
 	int rval;
 
-	if (!link->sink->entity ||
-	    !link->sink->entity->pipe || !link->source->entity)
+	if (!link->sink->entity || !link->source->entity)
+		return -EINVAL;
+	media_pipe = media_entity_pipeline(link->sink->entity);
+	if (!media_pipe)
 		return -EINVAL;
 	csi2 =
 	    to_ipu_isys_csi2(media_entity_to_v4l2_subdev(link->sink->entity));
-	ip = to_ipu_isys_pipeline(link->sink->entity->pipe);
+
+	ip = to_ipu_isys_pipeline(media_pipe);
 	csi2->receiver_errors = 0;
 	ip->csi2 = csi2;
-	ipu_isys_video_add_capture_done(to_ipu_isys_pipeline
-					(link->sink->entity->pipe),
-					csi2_capture_done);
+	ipu_isys_video_add_capture_done(ip, csi2_capture_done);
 
 	rval = v4l2_subdev_link_validate(link);
 	if (rval)
@@ -410,9 +409,8 @@ static int __subdev_link_validate(struct v4l2_subdev *sd,
 				  struct v4l2_subdev_format *source_fmt,
 				  struct v4l2_subdev_format *sink_fmt)
 {
-	struct ipu_isys_pipeline *ip = container_of(sd->entity.pipe,
-						    struct ipu_isys_pipeline,
-						    pipe);
+	struct ipu_isys_pipeline *ip =
+		to_ipu_isys_pipeline(media_entity_pipeline(&sd->entity));
 
 	if (source_fmt->format.field == V4L2_FIELD_ALTERNATE)
 		ip->interlaced = true;
diff --git a/drivers/media/pci/intel/ipu-isys-queue.c b/drivers/media/pci/intel/ipu-isys-queue.c
index bb8a017f7a7d..9b5a043fe8f0 100644
--- a/drivers/media/pci/intel/ipu-isys-queue.c
+++ b/drivers/media/pci/intel/ipu-isys-queue.c
@@ -529,13 +529,14 @@ static int ipu_isys_stream_start(struct ipu_isys_pipeline *ip,
 	return rval;
 }
 
-static void __buf_queue(struct vb2_buffer *vb, bool force)
+static void buf_queue(struct vb2_buffer *vb)
 {
 	struct ipu_isys_queue *aq = vb2_queue_to_ipu_isys_queue(vb->vb2_queue);
 	struct ipu_isys_video *av = ipu_isys_queue_to_video(aq);
 	struct ipu_isys_buffer *ib = vb2_buffer_to_ipu_isys_buffer(vb);
-	struct ipu_isys_pipeline *ip =
-	    to_ipu_isys_pipeline(av->vdev.entity.pipe);
+	struct media_pipeline *media_pipe =
+		media_entity_pipeline(&av->vdev.entity);
+	struct ipu_isys_pipeline *ip = to_ipu_isys_pipeline(media_pipe);
 	struct ipu_isys_buffer_list bl;
 
 	struct ipu_fw_isys_frame_buff_set_abi *buf = NULL;
@@ -578,12 +579,12 @@ static void __buf_queue(struct vb2_buffer *vb, bool force)
 	mutex_lock(&av->mutex);
 
 	/* ip may be cleared in ipu reset */
-	ip = to_ipu_isys_pipeline(av->vdev.entity.pipe);
+	ip = to_ipu_isys_pipeline(media_entity_pipeline(&av->vdev.entity));
 	pipe_av = container_of(ip, struct ipu_isys_video, ip);
 	if (ib->req)
 		return;
 
-	if (!pipe_av || !vb->vb2_queue->streaming) {
+	if (!pipe_av || !media_pipe || !vb->vb2_queue->start_streaming_called) {
 		dev_info(&av->isys->adev->dev,
 			"no pipe or streaming, adding to incoming\n");
 		return;
@@ -592,7 +593,7 @@ static void __buf_queue(struct vb2_buffer *vb, bool force)
 	mutex_unlock(&av->mutex);
 	mutex_lock(&pipe_av->mutex);
 
-	if (!force && ip->nr_streaming != ip->nr_queues) {
+	if (ip->nr_streaming != ip->nr_queues) {
 		dev_info(&av->isys->adev->dev,
 			"not streaming yet, adding to incoming\n");
 		goto out;
@@ -661,11 +662,6 @@ static void __buf_queue(struct vb2_buffer *vb, bool force)
 	mutex_lock(&av->mutex);
 }
 
-static void buf_queue(struct vb2_buffer *vb)
-{
-	__buf_queue(vb, false);
-}
-
 int ipu_isys_link_fmt_validate(struct ipu_isys_queue *aq)
 {
 	struct ipu_isys_video *av = ipu_isys_queue_to_video(aq);
@@ -809,7 +805,7 @@ static int __start_streaming(struct vb2_queue *q, unsigned int count)
 
 	mutex_lock(&av->isys->stream_mutex);
 
-	first = !av->vdev.entity.pipe;
+	first = !media_entity_pipeline(&av->vdev.entity);
 
 	if (first) {
 		rval = ipu_isys_video_prepare_streaming(av, 1);
@@ -831,7 +827,7 @@ static int __start_streaming(struct vb2_queue *q, unsigned int count)
 		goto out_unprepare_streaming;
 	}
 
-	ip = to_ipu_isys_pipeline(av->vdev.entity.pipe);
+	ip = to_ipu_isys_pipeline(media_entity_pipeline(&av->vdev.entity));
 	pipe_av = container_of(ip, struct ipu_isys_video, ip);
 	if (pipe_av != av) {
 		mutex_unlock(&av->mutex);
@@ -1124,7 +1120,7 @@ static void stop_streaming(struct vb2_queue *q)
 	struct ipu_isys_queue *aq = vb2_queue_to_ipu_isys_queue(q);
 	struct ipu_isys_video *av = ipu_isys_queue_to_video(aq);
 	struct ipu_isys_pipeline *ip =
-	    to_ipu_isys_pipeline(av->vdev.entity.pipe);
+		to_ipu_isys_pipeline(media_entity_pipeline(&av->vdev.entity));
 	struct ipu_isys_video *pipe_av =
 	    container_of(ip, struct ipu_isys_video, ip);
 
@@ -1254,7 +1250,7 @@ ipu_isys_buf_calc_sequence_time(struct ipu_isys_buffer *ib,
 	struct ipu_isys_video *av = ipu_isys_queue_to_video(aq);
 	struct device *dev = &av->isys->adev->dev;
 	struct ipu_isys_pipeline *ip =
-	    to_ipu_isys_pipeline(av->vdev.entity.pipe);
+		to_ipu_isys_pipeline(media_entity_pipeline(&av->vdev.entity));
 	u64 ns;
 	u32 sequence;
 
diff --git a/drivers/media/pci/intel/ipu-isys-subdev.c b/drivers/media/pci/intel/ipu-isys-subdev.c
index 6a99a7f57849..43c6ee0706c1 100644
--- a/drivers/media/pci/intel/ipu-isys-subdev.c
+++ b/drivers/media/pci/intel/ipu-isys-subdev.c
@@ -23,7 +23,6 @@ unsigned int ipu_isys_mbus_code_to_bpp(u32 code)
 	case MEDIA_BUS_FMT_RGB565_1X16:
 	case MEDIA_BUS_FMT_UYVY8_1X16:
 	case MEDIA_BUS_FMT_YUYV8_1X16:
-	case MEDIA_BUS_FMT_VYUY8_1X16:
 		return 16;
 	case MEDIA_BUS_FMT_SBGGR12_1X12:
 	case MEDIA_BUS_FMT_SGBRG12_1X12:
@@ -35,7 +34,6 @@ unsigned int ipu_isys_mbus_code_to_bpp(u32 code)
 	case MEDIA_BUS_FMT_SGRBG10_1X10:
 	case MEDIA_BUS_FMT_SRGGB10_1X10:
 		return 10;
-	case MEDIA_BUS_FMT_Y8_1X8:
 	case MEDIA_BUS_FMT_SBGGR8_1X8:
 	case MEDIA_BUS_FMT_SGBRG8_1X8:
 	case MEDIA_BUS_FMT_SGRBG8_1X8:
@@ -62,7 +60,6 @@ unsigned int ipu_isys_mbus_code_to_mipi(u32 code)
 		return IPU_ISYS_MIPI_CSI2_TYPE_YUV422_10;
 	case MEDIA_BUS_FMT_UYVY8_1X16:
 	case MEDIA_BUS_FMT_YUYV8_1X16:
-	case MEDIA_BUS_FMT_VYUY8_1X16:
 		return IPU_ISYS_MIPI_CSI2_TYPE_YUV422_8;
 	case MEDIA_BUS_FMT_SBGGR12_1X12:
 	case MEDIA_BUS_FMT_SGBRG12_1X12:
@@ -75,7 +72,6 @@ unsigned int ipu_isys_mbus_code_to_mipi(u32 code)
 	case MEDIA_BUS_FMT_SGRBG10_1X10:
 	case MEDIA_BUS_FMT_SRGGB10_1X10:
 		return IPU_ISYS_MIPI_CSI2_TYPE_RAW10;
-	case MEDIA_BUS_FMT_Y8_1X8:
 	case MEDIA_BUS_FMT_SBGGR8_1X8:
 	case MEDIA_BUS_FMT_SGBRG8_1X8:
 	case MEDIA_BUS_FMT_SGRBG8_1X8:
@@ -697,9 +693,8 @@ int ipu_isys_subdev_link_validate(struct v4l2_subdev *sd,
 {
 	struct v4l2_subdev *source_sd =
 	    media_entity_to_v4l2_subdev(link->source->entity);
-	struct ipu_isys_pipeline *ip = container_of(sd->entity.pipe,
-						    struct ipu_isys_pipeline,
-						    pipe);
+	struct ipu_isys_pipeline *ip =
+		to_ipu_isys_pipeline(media_entity_pipeline(&sd->entity));
 	struct ipu_isys_subdev *asd = to_ipu_isys_subdev(sd);
 
 	if (!source_sd)
diff --git a/drivers/media/pci/intel/ipu-isys-video.c b/drivers/media/pci/intel/ipu-isys-video.c
index f966932847a3..faf6e47985f7 100644
--- a/drivers/media/pci/intel/ipu-isys-video.c
+++ b/drivers/media/pci/intel/ipu-isys-video.c
@@ -626,14 +626,16 @@ static bool is_external(struct ipu_isys_video *av, struct media_entity *entity)
 static int link_validate(struct media_link *link)
 {
 	struct ipu_isys_video *av =
-	    container_of(link->sink, struct ipu_isys_video, pad);
+		container_of(link->sink, struct ipu_isys_video, pad);
 	/* All sub-devices connected to a video node are ours. */
 	struct ipu_isys_pipeline *ip =
-		to_ipu_isys_pipeline(av->vdev.entity.pipe);
+		to_ipu_isys_pipeline(media_entity_pipeline(&av->vdev.entity));
 	struct v4l2_subdev *sd;
 
+	WARN_ON(!ip);
 	if (!link->source->entity)
 		return -EINVAL;
+
 	sd = media_entity_to_v4l2_subdev(link->source->entity);
 	if (is_external(av, link->source->entity)) {
 #if LINUX_VERSION_CODE < KERNEL_VERSION(6, 0, 0)
@@ -672,7 +674,7 @@ static void put_stream_opened(struct ipu_isys_video *av)
 static int get_stream_handle(struct ipu_isys_video *av)
 {
 	struct ipu_isys_pipeline *ip =
-	    to_ipu_isys_pipeline(av->vdev.entity.pipe);
+		to_ipu_isys_pipeline(media_entity_pipeline(&av->vdev.entity));
 	unsigned int stream_handle;
 	unsigned long flags;
 
@@ -694,7 +696,7 @@ static int get_stream_handle(struct ipu_isys_video *av)
 static void put_stream_handle(struct ipu_isys_video *av)
 {
 	struct ipu_isys_pipeline *ip =
-	    to_ipu_isys_pipeline(av->vdev.entity.pipe);
+		to_ipu_isys_pipeline(media_entity_pipeline(&av->vdev.entity));
 	unsigned long flags;
 
 	spin_lock_irqsave(&av->isys->lock, flags);
@@ -910,7 +912,7 @@ ipu_isys_prepare_fw_cfg_default(struct ipu_isys_video *av,
 				struct ipu_fw_isys_stream_cfg_data_abi *cfg)
 {
 	struct ipu_isys_pipeline *ip =
-	    to_ipu_isys_pipeline(av->vdev.entity.pipe);
+		to_ipu_isys_pipeline(media_entity_pipeline(&av->vdev.entity));
 	struct ipu_isys_queue *aq = &av->aq;
 	struct ipu_fw_isys_output_pin_info_abi *pin_info;
 	struct ipu_isys *isys = av->isys;
@@ -1110,7 +1112,7 @@ static int start_stream_firmware(struct ipu_isys_video *av,
 				 struct ipu_isys_buffer_list *bl)
 {
 	struct ipu_isys_pipeline *ip =
-	    to_ipu_isys_pipeline(av->vdev.entity.pipe);
+		to_ipu_isys_pipeline(media_entity_pipeline(&av->vdev.entity));
 	struct device *dev = &av->isys->adev->dev;
 	struct v4l2_subdev_selection sel_fmt = {
 		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
@@ -1336,7 +1338,7 @@ static int start_stream_firmware(struct ipu_isys_video *av,
 static void stop_streaming_firmware(struct ipu_isys_video *av)
 {
 	struct ipu_isys_pipeline *ip =
-	    to_ipu_isys_pipeline(av->vdev.entity.pipe);
+		to_ipu_isys_pipeline(media_entity_pipeline(&av->vdev.entity));
 	struct device *dev = &av->isys->adev->dev;
 	int rval, tout;
 	enum ipu_fw_isys_send_type send_type =
@@ -1365,7 +1367,7 @@ static void stop_streaming_firmware(struct ipu_isys_video *av)
 static void close_streaming_firmware(struct ipu_isys_video *av)
 {
 	struct ipu_isys_pipeline *ip =
-	    to_ipu_isys_pipeline(av->vdev.entity.pipe);
+		to_ipu_isys_pipeline(media_entity_pipeline(&av->vdev.entity));
 	struct device *dev = &av->isys->adev->dev;
 	int rval, tout;
 
@@ -1523,6 +1525,10 @@ static int media_pipeline_walk_by_vc(struct ipu_isys_video *av,
 	struct media_pad *source_pad = media_pad_remote_pad_first(&av->pad);
 #endif
 	unsigned int pad_id;
+#if LINUX_VERSION_CODE < KERNEL_VERSION(5, 18, 0)
+	int previous_stream_count = 0;
+	struct media_entity *entity_enum = entity;
+#endif
 
 	if (!source_pad) {
 		dev_err(entity->graph_obj.mdev->dev, "no remote pad found\n");
@@ -1543,6 +1549,14 @@ static int media_pipeline_walk_by_vc(struct ipu_isys_video *av,
 			goto error_graph_walk_start;
 	}
 
+#if LINUX_VERSION_CODE < KERNEL_VERSION(5, 18, 0)
+	media_graph_walk_start(&pipe->graph, entity_enum);
+	while ((entity_enum = media_graph_walk_next(graph))) {
+		if (entity_enum->stream_count > previous_stream_count)
+			previous_stream_count = entity_enum->stream_count;
+	}
+#endif
+
 	media_graph_walk_start(&pipe->graph, entity);
 	while ((entity = media_graph_walk_next(graph))) {
 		DECLARE_BITMAP(active, MEDIA_ENTITY_MAX_PADS);
@@ -1552,7 +1566,7 @@ static int media_pipeline_walk_by_vc(struct ipu_isys_video *av,
 			entity->name);
 
 #if LINUX_VERSION_CODE < KERNEL_VERSION(5, 18, 0)
-		entity->stream_count++;
+		entity->stream_count = previous_stream_count + 1;
 #endif
 
 		if (entity->pipe && entity->pipe == pipe) {
@@ -1719,6 +1733,7 @@ int ipu_isys_video_prepare_streaming(struct ipu_isys_video *av,
 	struct media_entity_graph graph;
 #endif
 	struct media_entity *entity;
+	struct media_pipeline *media_pipe;
 	struct media_device *mdev = &av->isys->media_dev;
 	int rval;
 	unsigned int i;
@@ -1726,12 +1741,17 @@ int ipu_isys_video_prepare_streaming(struct ipu_isys_video *av,
 	dev_dbg(dev, "prepare stream: %d\n", state);
 
 	if (!state) {
-		ip = to_ipu_isys_pipeline(av->vdev.entity.pipe);
+		media_pipe = media_entity_pipeline(&av->vdev.entity);
+		ip = to_ipu_isys_pipeline(media_pipe);
 
 		if (ip->interlaced && isys->short_packet_source ==
 		    IPU_ISYS_SHORT_PACKET_FROM_RECEIVER)
 			short_packet_queue_destroy(ip);
+#if LINUX_VERSION_CODE < KERNEL_VERSION(6, 1, 0)
 		media_pipeline_stop(&av->vdev.entity);
+#else
+		media_pipeline_stop(av->vdev.entity.pads);
+#endif
 		media_entity_enum_cleanup(&ip->entity_enum);
 		return 0;
 	}
@@ -1805,7 +1825,11 @@ int ipu_isys_video_prepare_streaming(struct ipu_isys_video *av,
 	return 0;
 
 out_pipeline_stop:
+#if LINUX_VERSION_CODE < KERNEL_VERSION(6, 1, 0)
 	media_pipeline_stop(&av->vdev.entity);
+#else
+	media_pipeline_stop(av->vdev.entity.pads);
+#endif
 
 out_enum_cleanup:
 	media_entity_enum_cleanup(&ip->entity_enum);
@@ -1828,7 +1852,7 @@ int ipu_isys_video_set_streaming(struct ipu_isys_video *av,
 
 	struct media_entity *entity, *entity2;
 	struct ipu_isys_pipeline *ip =
-	    to_ipu_isys_pipeline(av->vdev.entity.pipe);
+		to_ipu_isys_pipeline(media_entity_pipeline(&av->vdev.entity));
 	struct v4l2_subdev *sd, *esd;
 	int rval = 0;
 	struct v4l2_ext_control c = {.id = V4L2_CID_IPU_SET_SUB_STREAM, };
diff --git a/drivers/media/pci/intel/ipu-psys.c b/drivers/media/pci/intel/ipu-psys.c
index f804b3bc7a4a..ca2830e5dc7e 100644
--- a/drivers/media/pci/intel/ipu-psys.c
+++ b/drivers/media/pci/intel/ipu-psys.c
@@ -1842,6 +1842,6 @@ MODULE_AUTHOR("Zaikuo Wang <zaikuo.wang@intel.com>");
 MODULE_AUTHOR("Yunliang Ding <yunliang.ding@intel.com>");
 MODULE_LICENSE("GPL");
 MODULE_DESCRIPTION("Intel ipu processing system driver");
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(5, 15, 0) || LINUX_VERSION_CODE == KERNEL_VERSION(5, 15, 71)
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(5, 16, 0) || LINUX_VERSION_CODE == KERNEL_VERSION(5, 15, 71)
 MODULE_IMPORT_NS(DMA_BUF);
 #endif
diff --git a/drivers/media/pci/intel/ipu.c b/drivers/media/pci/intel/ipu.c
index 72304a0f7063..3521154cd8b0 100644
--- a/drivers/media/pci/intel/ipu.c
+++ b/drivers/media/pci/intel/ipu.c
@@ -543,11 +543,14 @@ static int ipu_pci_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 		isp->cpd_fw_name = IPU6SE_FIRMWARE_NAME;
 		break;
 	case IPU6EP_ADL_P_PCI_ID:
-	case IPU6EP_ADL_N_PCI_ID:
 	case IPU6EP_RPL_P_PCI_ID:
 		ipu_ver = IPU_VER_6EP;
 		isp->cpd_fw_name = is_es ? IPU6EPES_FIRMWARE_NAME : IPU6EP_FIRMWARE_NAME;
 		break;
+	case IPU6EP_ADL_N_PCI_ID:
+		ipu_ver = IPU_VER_6EP;
+		isp->cpd_fw_name = IPU6EPADLN_FIRMWARE_NAME;
+		break;
 	case IPU6EP_MTL_PCI_ID:
 		ipu_ver = IPU_VER_6EP_MTL;
 		isp->cpd_fw_name = IPU6EPMTL_FIRMWARE_NAME;
diff --git a/drivers/media/pci/intel/ipu.h b/drivers/media/pci/intel/ipu.h
index 744b71489845..3bb4069620d6 100644
--- a/drivers/media/pci/intel/ipu.h
+++ b/drivers/media/pci/intel/ipu.h
@@ -115,4 +115,13 @@ int request_cpd_fw(const struct firmware **firmware_p, const char *name,
 extern enum ipu_version ipu_ver;
 void ipu_internal_pdata_init(void);
 
+#if LINUX_VERSION_CODE < KERNEL_VERSION(6, 1, 0)
+#include <media/media-entity.h>
+/* Helpers for building against various kernel versions */
+static inline struct media_pipeline *media_entity_pipeline(struct media_entity *entity)
+{
+	return entity->pipe;
+}
+#endif
+
 #endif /* IPU_H */
diff --git a/drivers/media/pci/intel/ipu6/ipu-platform.h b/drivers/media/pci/intel/ipu6/ipu-platform.h
index 86b6e12b2bb4..0ec02ff805ba 100644
--- a/drivers/media/pci/intel/ipu6/ipu-platform.h
+++ b/drivers/media/pci/intel/ipu6/ipu-platform.h
@@ -11,6 +11,7 @@
 #define IPU6EPES_FIRMWARE_NAME		"intel/ipu6epes_fw.bin"
 #define IPU6_FIRMWARE_NAME		"intel/ipu6_fw.bin"
 #define IPU6EPMTL_FIRMWARE_NAME		"intel/ipu6epmtl_fw.bin"
+#define IPU6EPADLN_FIRMWARE_NAME	"intel/ipu6epadln_fw.bin"
 
 #if IS_ENABLED(CONFIG_VIDEO_INTEL_IPU_USE_PLATFORMDATA) \
 	&& IS_ENABLED(CONFIG_VIDEO_INTEL_IPU_PDATA_DYNAMIC_LOADING)
diff --git a/drivers/media/pci/intel/ipu6/ipu6-isys-csi2.c b/drivers/media/pci/intel/ipu6/ipu6-isys-csi2.c
index c5eb58adbb34..7707e169f1d9 100644
--- a/drivers/media/pci/intel/ipu6/ipu6-isys-csi2.c
+++ b/drivers/media/pci/intel/ipu6/ipu6-isys-csi2.c
@@ -452,9 +452,9 @@ int ipu_isys_csi2_set_stream(struct v4l2_subdev *sd,
 {
 	struct ipu_isys_csi2 *csi2 = to_ipu_isys_csi2(sd);
 	struct ipu_isys *isys = csi2->isys;
-	struct ipu_isys_pipeline *ip = container_of(sd->entity.pipe,
-						    struct ipu_isys_pipeline,
-						    pipe);
+	struct ipu_isys_pipeline *ip =
+		container_of(media_entity_pipeline(&sd->entity),
+			     struct ipu_isys_pipeline, pipe);
 	struct ipu_isys_csi2_config *cfg =
 		v4l2_get_subdev_hostdata(media_entity_to_v4l2_subdev
 					 (ip->external->entity));
diff --git a/drivers/media/platform/intel/ipu6-acpi-pdata.c b/drivers/media/platform/intel/ipu6-acpi-pdata.c
index f4c53b35eb88..192ce9181232 100644
--- a/drivers/media/platform/intel/ipu6-acpi-pdata.c
+++ b/drivers/media/platform/intel/ipu6-acpi-pdata.c
@@ -704,7 +704,7 @@ void set_serdes_info(struct device *dev, char *sensor_name, const char *serdes_n
 		serdes_info.gpio_powerup_seq = 0;
 
 	if (!strcmp(sensor_name, IMX390_NAME))
-		serdes_info.phy_i2c_addr = IMX390_D3RCM_I2C_ADDRESS;
+		serdes_info.phy_i2c_addr = IMX390_D3CM_I2C_ADDRESS;
 	else
 		serdes_info.phy_i2c_addr = 0;
 }
diff --git a/include/media/imx390.h b/include/media/imx390.h
index 18988eea9ba5..be95249fbe96 100644
--- a/include/media/imx390.h
+++ b/include/media/imx390.h
@@ -8,7 +8,7 @@
 
 #define IMX390_NAME "imx390"
 
-#define IMX390_D3RCM_I2C_ADDRESS (0x21 << 1)
+#define IMX390_D3CM_I2C_ADDRESS (0x21 << 1)
 
 struct imx390_platform_data {
 	unsigned int port;