diff --git a/drivers/cam_sensor_module/cam_sensor/cam_sensor_core.c b/drivers/cam_sensor_module/cam_sensor/cam_sensor_core.c
index d8db350e033d..83578c81c00b 100644
--- a/drivers/cam_sensor_module/cam_sensor/cam_sensor_core.c
+++ b/drivers/cam_sensor_module/cam_sensor/cam_sensor_core.c
@@ -444,6 +444,14 @@ int32_t cam_sensor_update_slave_info(struct cam_cmd_probe *probe_info,
 
 	s_ctrl->sensor_probe_addr_type =  probe_info->addr_type;
 	s_ctrl->sensor_probe_data_type =  probe_info->data_type;
+
+	s_ctrl->probe_sub_device       =  probe_info->probe_sub_device;
+	s_ctrl->sub_device_addr        =  probe_info->sub_device_addr;
+	s_ctrl->sub_device_data_type   =  probe_info->sub_device_data_type;
+	s_ctrl->sub_device_addr_type   =  probe_info->sub_device_addr_type;
+	s_ctrl->sub_device_id_addr     =  probe_info->sub_device_id_addr;
+	s_ctrl->expected_sub_device_id =  probe_info->expected_sub_device_id;
+
 	CAM_DBG(CAM_SENSOR,
 		"Sensor Addr: 0x%x sensor_id: 0x%x sensor_mask: 0x%x sensor_pipeline_delay:0x%x",
 		s_ctrl->sensordata->slave_info.sensor_id_reg_addr,
@@ -703,6 +711,54 @@ int cam_sensor_match_id(struct cam_sensor_ctrl_t *s_ctrl)
 	return rc;
 }
 
+int cam_sensor_match_sub_device_id(struct cam_sensor_ctrl_t *s_ctrl)
+{
+	int rc = 0;
+	uint32_t sub_device_id = 0;
+	uint16_t sensor_address = 0;
+
+	/* if hal doesn't config ProbeSubDevice parameter in sensor xml, return success immediately */
+	if (!s_ctrl->probe_sub_device) {
+		return 0;
+	}
+
+	/* save sensor i2c address */
+	sensor_address = s_ctrl->io_master_info.cci_client->sid;
+
+	/* set sub-device i2c address */
+	if (s_ctrl->sub_device_addr) {
+		s_ctrl->io_master_info.cci_client->sid = s_ctrl->sub_device_addr >> 1;
+	}
+
+	rc = camera_io_dev_read(
+		&(s_ctrl->io_master_info),
+		s_ctrl->sub_device_id_addr,
+		&sub_device_id,
+		s_ctrl->sub_device_addr_type,
+		s_ctrl->sub_device_data_type);
+
+	CAM_INFO(CAM_SENSOR, "Read sub device id: 0x%x expected sub device id 0x%x:",
+		sub_device_id, s_ctrl->expected_sub_device_id);
+
+	if (sub_device_id == s_ctrl->expected_sub_device_id) {
+		CAM_INFO(CAM_SENSOR,
+			"Probe sub device success,slot:%d,sub_device_addr:0x%x,sub_device_id:0x%x",
+			s_ctrl->soc_info.index,
+			s_ctrl->sub_device_addr,
+			s_ctrl->expected_sub_device_id);
+		rc = 0;
+	}
+	else {
+		CAM_ERR(CAM_SENSOR, "Probe sub device fail");
+		rc = -EINVAL;
+	}
+
+	/* restore sensor i2c address */
+	s_ctrl->io_master_info.cci_client->sid = sensor_address;
+
+	return rc;
+}
+
 int32_t cam_sensor_driver_cmd(struct cam_sensor_ctrl_t *s_ctrl,
 	void *arg)
 {
@@ -785,6 +841,14 @@ int32_t cam_sensor_driver_cmd(struct cam_sensor_ctrl_t *s_ctrl,
 			goto free_power_settings;
 		}
 
+		/* Match sub-device ID */
+		rc = cam_sensor_match_sub_device_id(s_ctrl);
+		if (rc < 0) {
+			cam_sensor_power_down(s_ctrl);
+			msleep(20);
+			goto free_power_settings;
+		}
+
 		CAM_INFO(CAM_SENSOR,
 			"Probe success,slot:%d,slave_addr:0x%x,sensor_id:0x%x",
 			s_ctrl->soc_info.index,
diff --git a/drivers/cam_sensor_module/cam_sensor/cam_sensor_dev.h b/drivers/cam_sensor_module/cam_sensor/cam_sensor_dev.h
index 8586ce51c87f..c132b0a3846d 100644
--- a/drivers/cam_sensor_module/cam_sensor/cam_sensor_dev.h
+++ b/drivers/cam_sensor_module/cam_sensor/cam_sensor_dev.h
@@ -111,6 +111,12 @@ struct cam_sensor_ctrl_t {
 	bool bob_pwm_switch;
 	uint32_t last_flush_req;
 	uint16_t pipeline_delay;
+	uint8_t   probe_sub_device;
+	uint16_t  sub_device_addr;
+	uint8_t   sub_device_data_type;
+	uint8_t   sub_device_addr_type;
+	uint16_t  sub_device_id_addr;
+	uint16_t  expected_sub_device_id;
 };
 
 /**
diff --git a/include/uapi/camera/media/cam_sensor.h b/include/uapi/camera/media/cam_sensor.h
index a4195326ea6c..49cb7da7cdfd 100644
--- a/include/uapi/camera/media/cam_sensor.h
+++ b/include/uapi/camera/media/cam_sensor.h
@@ -172,6 +172,12 @@ struct cam_cmd_probe {
 	__u32    data_mask;
 	__u16    camera_id;
 	__u16    reserved;
+	__u8   probe_sub_device;
+	__u32  sub_device_addr;
+	__u8   sub_device_data_type;
+	__u8   sub_device_addr_type;
+	__u32  sub_device_id_addr;
+	__u32  expected_sub_device_id;
 } __attribute__((packed));
 
 /**