diff --git a/drivers/media/video/ov5640.c b/drivers/media/video/ov5640.c
index 8f50b98b9f2b..e66f28da30c1 100755
--- a/drivers/media/video/ov5640.c
+++ b/drivers/media/video/ov5640.c
@@ -3465,16 +3465,17 @@ static int sensor_probe(struct i2c_client *client,
         icd->ops = NULL;
         i2c_set_clientdata(client, NULL);
         kfree(sensor);
+		sensor = NULL;
+    } else {
+		#if CONFIG_SENSOR_Focus
+		sensor->sensor_wq = create_workqueue(SENSOR_NAME_STRING( wq));
+		if (sensor->sensor_wq == NULL)
+			SENSOR_TR("%s workqueue create fail!", SENSOR_NAME_STRING( wq));
+		mutex_init(&sensor->wq_lock);
+		sensor->sensor_wk.state = sensor_work_ready;
+		#endif
     }
 
-	#if CONFIG_SENSOR_Focus
-	sensor->sensor_wq = create_workqueue(SENSOR_NAME_STRING( wq));
-	if (sensor->sensor_wq == NULL)
-		SENSOR_TR("%s workqueue create fail!", SENSOR_NAME_STRING( wq));
-	mutex_init(&sensor->wq_lock);
-	sensor->sensor_wk.state = sensor_work_ready;
-	#endif
-
     SENSOR_DG("\n%s..%s..%d  ret = %x \n",__FUNCTION__,__FILE__,__LINE__,ret);
     return ret;
 }
@@ -3495,7 +3496,7 @@ static int sensor_remove(struct i2c_client *client)
     i2c_set_clientdata(client, NULL);
     client->driver = NULL;
     kfree(sensor);
-
+	sensor = NULL;
     return 0;
 }
 
diff --git a/drivers/media/video/ov5642.c b/drivers/media/video/ov5642.c
index ba3c14443534..af71249126d9 100755
--- a/drivers/media/video/ov5642.c
+++ b/drivers/media/video/ov5642.c
@@ -5437,20 +5437,21 @@ static int sensor_probe(struct i2c_client *client,
 	#endif
 
     ret = sensor_video_probe(icd, client);
-    if (ret) {
+    if (ret < 0) {
         icd->ops = NULL;
         i2c_set_clientdata(client, NULL);
         kfree(sensor);
+		sensor = NULL;
+    } else {
+		#if CONFIG_SENSOR_Focus
+		sensor->sensor_wq = create_workqueue(SENSOR_NAME_STRING( wq));
+		if (sensor->sensor_wq == NULL)
+			SENSOR_TR("%s workqueue create fail!", SENSOR_NAME_STRING( wq));
+		mutex_init(&sensor->wq_lock);
+		sensor->sensor_wk.state = sensor_work_ready;
+		#endif
     }
 
-	#if CONFIG_SENSOR_Focus
-	sensor->sensor_wq = create_workqueue(SENSOR_NAME_STRING( wq));
-	if (sensor->sensor_wq == NULL)
-		SENSOR_TR("%s workqueue create fail!", SENSOR_NAME_STRING( wq));
-	mutex_init(&sensor->wq_lock);
-	sensor->sensor_wk.state = sensor_work_ready;
-	#endif
-
     SENSOR_DG("\n%s..%s..%d  ret = %x \n",__FUNCTION__,__FILE__,__LINE__,ret);
     return ret;
 }
@@ -5471,7 +5472,7 @@ static int sensor_remove(struct i2c_client *client)
     i2c_set_clientdata(client, NULL);
     client->driver = NULL;
     kfree(sensor);
-
+	sensor = NULL;
     return 0;
 }