Sensorhub: Update sensors and firmware

From Samsung SPH-L900 update source.

Change-Id: Ib1dcf851ce5e723661169d7cb4ee8bc8ff647226
Signed-off-by: Curtis Menard <curtis.menard@gmail.com>

18 files changed, 668 insertions, 434 deletions

diff --git a/drivers/sensorhub/accel_lsm330.c b/drivers/sensorhub/accel_lsm330.c
index cbcdba14234..1f236636e0a 100755..100644
--- a/drivers/sensorhub/accel_lsm330.c
+++ b/drivers/sensorhub/accel_lsm330.c
@@ -296,7 +296,11 @@ static struct device_attribute *acc_attrs[] = {
 
 void initialize_accel_factorytest(struct ssp_data *data)
 {
-	struct device *acc_device = NULL;
+	sensors_register(data->acc_device, data, acc_attrs,
+		"accelerometer_sensor");
+}
 
-	sensors_register(acc_device, data, acc_attrs, "accelerometer_sensor");
+void remove_accel_factorytest(struct ssp_data *data)
+{
+	sensors_unregister(data->acc_device, acc_attrs);
 }
diff --git a/drivers/sensorhub/gyro_lsm330.c b/drivers/sensorhub/gyro_lsm330.c
index 58c6b4ba576..0a3a473f107 100755..100644
--- a/drivers/sensorhub/gyro_lsm330.c
+++ b/drivers/sensorhub/gyro_lsm330.c
@@ -131,6 +131,9 @@ static ssize_t gyro_get_temp(struct device *dev,
 	int iDelayCnt = 0, iRet = 0;
 	struct ssp_data *data = dev_get_drvdata(dev);
 
+	if (!(data->uSensorState & (1 << GYROSCOPE_SENSOR)))
+		goto exit;
+
 	data->uFactorydataReady = 0;
 	memset(data->uFactorydata, 0, sizeof(char) * FACTORY_DATA_MAX);
 
@@ -234,6 +237,9 @@ static ssize_t gyro_selftest_dps_store(struct device *dev,
 
 	struct ssp_data *data = dev_get_drvdata(dev);
 
+	if (!(data->uSensorState & (1 << GYROSCOPE_SENSOR)))
+		goto exit;
+
 	sscanf(buf, "%d", &iNewDps);
 
 	if (iNewDps == GYROSCOPE_DPS250)
@@ -306,7 +312,10 @@ static struct device_attribute *gyro_attrs[] = {
 
 void initialize_gyro_factorytest(struct ssp_data *data)
 {
-	struct device *gyro_device = NULL;
+	sensors_register(data->gyro_device, data, gyro_attrs, "gyro_sensor");
+}
 
-	sensors_register(gyro_device, data, gyro_attrs, "gyro_sensor");
+void remove_gyro_factorytest(struct ssp_data *data)
+{
+	sensors_unregister(data->gyro_device, gyro_attrs);
 }
diff --git a/drivers/sensorhub/light_cm36651.c b/drivers/sensorhub/light_cm36651.c
index 6c9d84df5bc..68081e44deb 100755..100644
--- a/drivers/sensorhub/light_cm36651.c
+++ b/drivers/sensorhub/light_cm36651.c
@@ -68,7 +68,10 @@ static struct device_attribute *light_attrs[] = {
 
 void initialize_light_factorytest(struct ssp_data *data)
 {
-	struct device *light_device = NULL;
+	sensors_register(data->light_device, data, light_attrs, "light_sensor");
+}
 
-	sensors_register(light_device, data, light_attrs, "light_sensor");
+void remove_light_factorytest(struct ssp_data *data)
+{
+	sensors_unregister(data->light_device, light_attrs);
 }
diff --git a/drivers/sensorhub/magnetic_ak8963c.c b/drivers/sensorhub/magnetic_ak8963c.c
index 39cdade2f8a..ccd1723bdae 100755..100644
--- a/drivers/sensorhub/magnetic_ak8963c.c
+++ b/drivers/sensorhub/magnetic_ak8963c.c
@@ -21,6 +21,18 @@
 #define VENDOR		"AKM"
 #define CHIP_ID		"AK8963C"
 
+#define GYROSCOPE_DATA_SPEC_MIN		-6500
+#define GYROSCOPE_DATA_SPEC_MAX		6500
+
+#define GYROSCOPE_SELFTEST_X_SPEC_MIN	-200
+#define GYROSCOPE_SELFTEST_X_SPEC_MAX	200
+
+#define GYROSCOPE_SELFTEST_Y_SPEC_MIN	-200
+#define GYROSCOPE_SELFTEST_Y_SPEC_MAX	200
+
+#define GYROSCOPE_SELFTEST_Z_SPEC_MIN	-3200
+#define GYROSCOPE_SELFTEST_Z_SPEC_MAX	-800
+
 static ssize_t magnetic_vendor_show(struct device *dev,
 	struct device_attribute *attr, char *buf)
 {
@@ -50,12 +62,12 @@ static int check_data_spec(struct ssp_data *data)
 		(data->buf[GEOMAGNETIC_SENSOR].y == 0) &&
 		(data->buf[GEOMAGNETIC_SENSOR].z == 0))
 		return FAIL;
-	else if ((data->buf[GEOMAGNETIC_SENSOR].x > 6500) ||
-		(data->buf[GEOMAGNETIC_SENSOR].x < -6500) ||
-		(data->buf[GEOMAGNETIC_SENSOR].y > 6500) ||
-		(data->buf[GEOMAGNETIC_SENSOR].y < -6500) ||
-		(data->buf[GEOMAGNETIC_SENSOR].z > 6500) ||
-		(data->buf[GEOMAGNETIC_SENSOR].z < -6500))
+	else if ((data->buf[GEOMAGNETIC_SENSOR].x > GYROSCOPE_DATA_SPEC_MAX) ||
+		(data->buf[GEOMAGNETIC_SENSOR].x < GYROSCOPE_DATA_SPEC_MIN) ||
+		(data->buf[GEOMAGNETIC_SENSOR].y > GYROSCOPE_DATA_SPEC_MAX) ||
+		(data->buf[GEOMAGNETIC_SENSOR].y < GYROSCOPE_DATA_SPEC_MIN) ||
+		(data->buf[GEOMAGNETIC_SENSOR].z > GYROSCOPE_DATA_SPEC_MAX) ||
+		(data->buf[GEOMAGNETIC_SENSOR].z < GYROSCOPE_DATA_SPEC_MIN))
 		return FAIL;
 	else
 		return SUCCESS;
@@ -67,7 +79,7 @@ static ssize_t adc_data_read(struct device *dev,
 	bool bSuccess = false;
 	u8 chTempbuf[2] = {1, 20};
 	s16 iSensorBuf[3] = {0, };
-	int iRetries = 10;
+	int iRetries = 20;
 	struct ssp_data *data = dev_get_drvdata(dev);
 
 	data->buf[GEOMAGNETIC_SENSOR].x = 0;
@@ -79,7 +91,7 @@ static ssize_t adc_data_read(struct device *dev,
 			chTempbuf, 2);
 
 	do {
-		msleep(60);
+		msleep(50);
 		if (check_data_spec(data) == SUCCESS)
 			break;
 	} while (--iRetries);
@@ -173,22 +185,28 @@ reties:
 
 	pr_info("[SSP] %s: self test x = %d, y = %d, z = %d\n",
 		__func__, iSF_X, iSF_Y, iSF_Z);
-	if ((iSF_X >= -200) && (iSF_X <= 200))
+	if ((iSF_X >= GYROSCOPE_SELFTEST_X_SPEC_MIN)
+		&& (iSF_X <= GYROSCOPE_SELFTEST_X_SPEC_MAX))
 		pr_info("[SSP] x passed self test, expect -200<=x<=200\n");
 	else
 		pr_info("[SSP] x failed self test, expect -200<=x<=200\n");
-	if ((iSF_Y >= -200) && (iSF_Y <= 200))
+	if ((iSF_Y >= GYROSCOPE_SELFTEST_Y_SPEC_MIN)
+		&& (iSF_Y <= GYROSCOPE_SELFTEST_Y_SPEC_MAX))
 		pr_info("[SSP] y passed self test, expect -200<=y<=200\n");
 	else
 		pr_info("[SSP] y failed self test, expect -200<=y<=200\n");
-	if ((iSF_Z >= -3200) && (iSF_Z <= -800))
+	if ((iSF_Z >= GYROSCOPE_SELFTEST_Z_SPEC_MIN)
+		&& (iSF_Z <= GYROSCOPE_SELFTEST_Z_SPEC_MAX))
 		pr_info("[SSP] z passed self test, expect -3200<=z<=-800\n");
 	else
 		pr_info("[SSP] z failed self test, expect -3200<=z<=-800\n");
 
-	if (((iSF_X >= -200) && (iSF_X <= 200)) &&
-		((iSF_Y >= -200) && (iSF_Y <= 200)) &&
-		((iSF_Z >= -3200) && (iSF_Z <= -800)))
+	if ((iSF_X >= GYROSCOPE_SELFTEST_X_SPEC_MIN)
+		&& (iSF_X <= GYROSCOPE_SELFTEST_X_SPEC_MAX)
+		&& (iSF_Y >= GYROSCOPE_SELFTEST_Y_SPEC_MIN)
+		&& (iSF_Y <= GYROSCOPE_SELFTEST_Y_SPEC_MAX)
+		&& (iSF_Z >= GYROSCOPE_SELFTEST_Z_SPEC_MIN)
+		&& (iSF_Z <= GYROSCOPE_SELFTEST_Z_SPEC_MAX))
 		bSelftestPassed = true;
 
 	if ((bSelftestPassed == false) && (iSpecOutReties++ < 5))
@@ -244,7 +262,10 @@ static struct device_attribute *mag_attrs[] = {
 
 void initialize_magnetic_factorytest(struct ssp_data *data)
 {
-	struct device *mag_device = NULL;
+	sensors_register(data->mag_device, data, mag_attrs, "magnetic_sensor");
+}
 
-	sensors_register(mag_device, data, mag_attrs, "magnetic_sensor");
+void remove_magnetic_factorytest(struct ssp_data *data)
+{
+	sensors_unregister(data->mag_device, mag_attrs);
 }
diff --git a/drivers/sensorhub/mcu_at32uc3l0128.c b/drivers/sensorhub/mcu_at32uc3l0128.c
index 216ca8a8301..0f63a8dd70d 100755..100644
--- a/drivers/sensorhub/mcu_at32uc3l0128.c
+++ b/drivers/sensorhub/mcu_at32uc3l0128.c
@@ -44,9 +44,10 @@ ssize_t mcu_update_show(struct device *dev,
 
 	ssp_dbg("[SSP]: %s - mcu binany update!\n", __func__);
 
-	if (data->bBinaryChashed == false) {
+	if (data->bSspShutdown == false) {
 		disable_irq(data->iIrq);
 		disable_irq_wake(data->iIrq);
+		data->bSspShutdown = true;
 	}
 
 	iRet = update_mcu_bin(data);
@@ -84,9 +85,10 @@ ssize_t mcu_update2_show(struct device *dev,
 
 	ssp_dbg("[SSP]: %s - mcu binany update!\n", __func__);
 
-	if (data->bBinaryChashed == false) {
+	if (data->bSspShutdown == false) {
 		disable_irq(data->iIrq);
 		disable_irq_wake(data->iIrq);
+		data->bSspShutdown = true;
 	}
 
 	iRet = update_crashed_mcu_bin(data);
@@ -159,7 +161,7 @@ ssize_t mcu_factorytest_show(struct device *dev,
 	bool bMcuTestSuccessed = false;
 	struct ssp_data *data = dev_get_drvdata(dev);
 
-	if (data->bBinaryChashed == true) {
+	if (data->bSspShutdown == true) {
 		ssp_dbg("[SSP]: %s - MCU Bin is crashed\n", __func__);
 		return sprintf(buf, "NG,NG,NG\n");
 	}
diff --git a/drivers/sensorhub/pressure_bmp182.c b/drivers/sensorhub/pressure_bmp182.c
index d5c390eccb3..d5896f32bc2 100755..100644
--- a/drivers/sensorhub/pressure_bmp182.c
+++ b/drivers/sensorhub/pressure_bmp182.c
@@ -185,8 +185,11 @@ static struct device_attribute *pressure_attrs[] = {
 
 void initialize_pressure_factorytest(struct ssp_data *data)
 {
-	struct device *pressure_device = NULL;
-
-	sensors_register(pressure_device, data, pressure_attrs,
+	sensors_register(data->prs_device, data, pressure_attrs,
 		"barometer_sensor");
 }
+
+void remove_pressure_factorytest(struct ssp_data *data)
+{
+	sensors_unregister(data->prs_device, pressure_attrs);
+}
diff --git a/drivers/sensorhub/prox_cm36651.c b/drivers/sensorhub/prox_cm36651.c
index 2a5d4498703..a7f91a79f37 100755..100644
--- a/drivers/sensorhub/prox_cm36651.c
+++ b/drivers/sensorhub/prox_cm36651.c
@@ -26,6 +26,13 @@
 #define LDI_OTHERS	'0'
 #define LDI_GRAY	'1'
 #define LDI_WHITE	'2'
+
+#define CANCELATION_THRESHOLD		9
+#define DEFAULT_THRESHOLD		13
+#define OTHERS_OCTA_DEFAULT_THRESHOLD	14
+#define WHITE_OCTA_DEFAULT_THRESHOLD	13
+#define GRAY_OCTA_DEFAULT_THRESHOLD	12
+
 /*************************************************************************/
 /* factory Sysfs                                                         */
 /*************************************************************************/
@@ -107,16 +114,16 @@ static void change_proximity_default_threshold(struct ssp_data *data)
 {
 	switch (data->chLcdLdi[1]) {
 	case LDI_GRAY:
-		data->uProxThresh = GRAY_OCTA_DEFAULT_THRESHOLD;
+		data->uProxHiThresh = GRAY_OCTA_DEFAULT_THRESHOLD;
 		break;
 	case LDI_WHITE:
-		data->uProxThresh = WHITE_OCTA_DEFAULT_THRESHOLD;
+		data->uProxHiThresh = WHITE_OCTA_DEFAULT_THRESHOLD;
 		break;
 	case LDI_OTHERS:
-		data->uProxThresh = OTHERS_OCTA_DEFAULT_THRESHOLD;
+		data->uProxHiThresh = OTHERS_OCTA_DEFAULT_THRESHOLD;
 		break;
 	default:
-		data->uProxThresh = DEFAULT_THRESHOLD;
+		data->uProxHiThresh = DEFAULT_THRESHOLD;
 		break;
 	}
 }
@@ -186,17 +193,17 @@ int proximity_open_calibration(struct ssp_data *data)
 		iRet = -EIO;
 	}
 
-	if (data->uProxCanc != 0) /*If there is an offset cal data. */
-		data->uProxThresh = CANCELATION_THRESHOLD;
+	if (data->uProxCanc != 0) /* If there is an offset cal data. */
+		data->uProxHiThresh = CANCELATION_THRESHOLD;
 
 	pr_info("%s: proximity ps_canc = %d, ps_thresh = %d\n",
-		__func__, data->uProxCanc, data->uProxThresh);
+		__func__, data->uProxCanc, data->uProxHiThresh);
 
 	filp_close(cancel_filp, current->files);
 	set_fs(old_fs);
 
 exit:
-	set_proximity_threshold(data);
+	set_proximity_threshold(data, data->uProxHiThresh, data->uProxCanc);
 
 	return iRet;
 }
@@ -208,14 +215,14 @@ static int proximity_store_cancelation(struct ssp_data *data, int iCalCMD)
 	struct file *cancel_filp = NULL;
 
 	if (iCalCMD) {
-		data->uProxThresh = CANCELATION_THRESHOLD;
+		data->uProxHiThresh = CANCELATION_THRESHOLD;
 		data->uProxCanc = get_proximity_rawdata(data);
 	} else {
 		change_proximity_default_threshold(data);
 		data->uProxCanc = 0;
 	}
 
-	set_proximity_threshold(data);
+	set_proximity_threshold(data, data->uProxHiThresh, data->uProxCanc);
 
 	old_fs = get_fs();
 	set_fs(KERNEL_DS);
@@ -248,9 +255,9 @@ static ssize_t proximity_cancel_show(struct device *dev,
 	struct ssp_data *data = dev_get_drvdata(dev);
 
 	ssp_dbg("[SSP]: uProxThresh = %u, uProxCanc = %u\n",
-		data->uProxThresh, data->uProxCanc);
+		data->uProxHiThresh, data->uProxCanc);
 
-	return sprintf(buf, "%u,%u\n", data->uProxCanc, data->uProxThresh);
+	return sprintf(buf, "%u,%u\n", data->uProxCanc, data->uProxHiThresh);
 }
 
 static ssize_t proximity_cancel_store(struct device *dev,
@@ -284,9 +291,9 @@ static ssize_t proximity_thresh_show(struct device *dev,
 {
 	struct ssp_data *data = dev_get_drvdata(dev);
 
-	ssp_dbg("[SSP]: uProxThresh = %u\n", data->uProxThresh);
+	ssp_dbg("[SSP]: uProxThresh = %u\n", data->uProxHiThresh);
 
-	return sprintf(buf, "%u\n", data->uProxThresh);
+	return sprintf(buf, "%u\n", data->uProxHiThresh);
 }
 
 static ssize_t proximity_thresh_store(struct device *dev,
@@ -300,11 +307,11 @@ static ssize_t proximity_thresh_store(struct device *dev,
 	if (iRet < 0)
 		pr_err("[SSP]: %s - kstrtoint failed.", __func__);
 
-	data->uProxThresh = uNewThresh;
-	set_proximity_threshold(data);
+	data->uProxHiThresh = uNewThresh;
+	set_proximity_threshold(data, data->uProxHiThresh, data->uProxCanc);
 
 	ssp_dbg("[SSP]: %s - new prox threshold = 0x%x\n",
-		__func__, data->uProxThresh);
+		__func__, data->uProxHiThresh);
 
 	return size;
 }
@@ -361,7 +368,11 @@ static struct device_attribute *prox_attrs[] = {
 
 void initialize_prox_factorytest(struct ssp_data *data)
 {
-	struct device *prox_device = NULL;
+	sensors_register(data->prox_device, data,
+		prox_attrs, "proximity_sensor");
+}
 
-	sensors_register(prox_device, data, prox_attrs, "proximity_sensor");
+void remove_prox_factorytest(struct ssp_data *data)
+{
+	sensors_unregister(data->prox_device, prox_attrs);
 }
diff --git a/drivers/sensorhub/sensors_core.c b/drivers/sensorhub/sensors_core.c
index 96ffa059178..f968df82610 100644
--- a/drivers/sensorhub/sensors_core.c
+++ b/drivers/sensorhub/sensors_core.c
@@ -55,12 +55,25 @@ int sensors_register(struct device *dev, void * drvdata,
 }
 EXPORT_SYMBOL_GPL(sensors_register);
 
-void sensors_unregister(struct device *dev)
+void sensors_unregister(struct device *dev,
+	struct device_attribute *attributes[])
 {
-	/* TODO : Unregister device */
+	int i;
+
+	for (i = 0; attributes[i] != NULL; i++)
+		device_remove_file(dev, attributes[i]);
 }
 EXPORT_SYMBOL_GPL(sensors_unregister);
 
+void destroy_sensor_class(void)
+{
+	if (sensors_class) {
+		class_destroy(sensors_class);
+		sensors_class = NULL;
+	}
+}
+EXPORT_SYMBOL_GPL(destroy_sensor_class);
+
 static int __init sensors_class_init(void)
 {
 	printk(KERN_INFO "[SENSORS CORE] sensors_class_init\n");
@@ -77,7 +90,10 @@ static int __init sensors_class_init(void)
 
 static void __exit sensors_class_exit(void)
 {
-	class_destroy(sensors_class);
+	if (sensors_class) {
+		class_destroy(sensors_class);
+		sensors_class = NULL;
+	}
 }
 
 /* exported for the APM Power driver, APM emulation */
diff --git a/drivers/sensorhub/ssp.h b/drivers/sensorhub/ssp.h
index 46ff78c01c6..33534f5300a 100644
--- a/drivers/sensorhub/ssp.h
+++ b/drivers/sensorhub/ssp.h
@@ -1,5 +1,5 @@
 /*
- *  Copyright (C) 2011, Samsung Electronics Co. Ltd. All Rights Reserved.
+ *  Copyright (C) 2012, Samsung Electronics Co. Ltd. All Rights Reserved.
  *
  *  This program is free software; you can redistribute it and/or modify
  *  it under the terms of the GNU General Public License as published by
@@ -33,28 +33,19 @@
 #include <linux/timer.h>
 
 #ifdef CONFIG_SENSORS_SSP_SENSORHUB
-#include <linux/completion.h>
-#include <linux/kthread.h>
-#include <linux/list.h>
-#include <linux/spinlock.h>
+#include "ssp_sensorhub.h"
 #endif
 
-#define SSP_DBG		1
-
 #define SUCCESS		1
 #define FAIL		0
 #define ERROR		-1
 
-#define CANCELATION_THRESHOLD	9
-#define DEFAULT_THRESHOLD		13
-#define OTHERS_OCTA_DEFAULT_THRESHOLD	14
-#define WHITE_OCTA_DEFAULT_THRESHOLD	13
-#define GRAY_OCTA_DEFAULT_THRESHOLD	12
-
 #define FACTORY_DATA_MAX	39
+
+#define SSP_DBG		1
+
 #if SSP_DBG
 #define SSP_FUNC_DBG 1
-#define SSP_DATA_DBG 0
 
 #define ssp_dbg(dev, format, ...) do { \
 	printk(KERN_INFO dev, format, ##__VA_ARGS__); \
@@ -71,14 +62,6 @@
 #define func_dbg()
 #endif
 
-#if SSP_DATA_DBG
-#define data_dbg(dev, format, ...) do { \
-	printk(KERN_INFO dev, format, ##__VA_ARGS__); \
-	} while (0)
-#else
-#define data_dbg(dev, format, ...)
-#endif
-
 #define SSP_SW_RESET_TIME	3000
 #define DEFUALT_POLLING_DELAY	(200 * NSEC_PER_MSEC)
 #define PROX_AVG_READ_NUM	80
@@ -159,6 +142,7 @@ enum {
 #define MSG2SSP_SSP_SLEEP	0xC1
 #define MSG2SSP_STS		0xC2	/* Start to Send */
 #define MSG2SSP_RTS		0xC4	/* Ready to Send */
+#define MSG2SSP_STT		0xC8
 #define MSG2SSP_SRM		0xCA	/* Start to Read MSG */
 #define MSG2SSP_SSM		0xCB	/* Start to Send MSG */
 #define MSG2SSP_SSD		0xCE	/* Start to Send Data Type & Length */
@@ -167,14 +151,6 @@ enum {
 #define MSG_ACK					0x80	/* ACK from SSP to AP */
 #define MSG_NAK					0x70	/* NAK from SSP to AP */
 
-#ifdef CONFIG_SENSORS_SSP_SENSORHUB
-#define SUBCMD_GPIOWAKEUP			0X02
-#define SUBCMD_POWEREUP				0X04
-#define MSG2SSP_STT				0xC8
-#define LIBRARY_MAX_NUM				5
-#define LIBRARY_MAX_TRY				32
-#define EVENT_WAIT_COUNT			3
-#endif
 
 /* SSP_INSTRUCTION_CMD */
 enum {
@@ -239,14 +215,6 @@ struct calibraion_data {
 	int z;
 };
 
-#ifdef CONFIG_SENSORS_SSP_SENSORHUB
-struct sensorhub_event {
-	char *library_data;
-	int length;
-	struct list_head list;
-};
-#endif
-
 struct ssp_data {
 	struct input_dev *acc_input_dev;
 	struct input_dev *gyro_input_dev;
@@ -254,6 +222,15 @@ struct ssp_data {
 	struct input_dev *light_input_dev;
 	struct input_dev *prox_input_dev;
 
+	struct device *sen_dev;
+	struct device *mcu_device;
+	struct device *acc_device;
+	struct device *gyro_device;
+	struct device *mag_device;
+	struct device *prs_device;
+	struct device *prox_device;
+	struct device *light_device;
+
 	struct i2c_client *client;
 	struct wake_lock ssp_wake_lock;
 	struct miscdevice akmd_device;
@@ -263,18 +240,18 @@ struct ssp_data {
 	struct calibraion_data accelcal;
 	struct calibraion_data gyrocal;
 	struct sensor_value buf[SENSOR_MAX];
-	struct device *sen_dev;
 
+	bool bSspShutdown;
 	bool bCheckSuspend;
 	bool bDebugEnabled;
 	bool bMcuIRQTestSuccessed;
 	bool bAccelAlert;
 	bool bProximityRawEnabled;
 	bool bBarcodeEnabled;
-	bool bBinaryChashed;
 
 	unsigned char uProxCanc;
-	unsigned char uProxThresh;
+	unsigned char uProxHiThresh;
+	unsigned char uProxLoThresh;
 	unsigned char uFuseRomData[3];
 	unsigned char uFactorydata[FACTORY_DATA_MAX];
 	char *pchLibraryBuf;
@@ -282,16 +259,18 @@ struct ssp_data {
 	int iIrq;
 	int iLibraryLength;
 	int aiCheckStatus[SENSOR_MAX];
-	int iIrqWakeCnt;
 
+	unsigned int uIrqFailCnt;
 	unsigned int uSsdFailCnt;
 	unsigned int uResetCnt;
-	unsigned int uI2cFailCnt;
+	unsigned int uInstFailCnt;
 	unsigned int uTimeOutCnt;
 	unsigned int uIrqCnt;
 	unsigned int uBusyCnt;
+	unsigned int uMissSensorCnt;
+
 	unsigned int uGyroDps;
-	unsigned int uAliveSensorDebug;
+	unsigned int uSensorState;
 	unsigned int uCurFirmRev;
 	unsigned int uFactoryProxAvg[4];
 	unsigned int uFactorydataReady;
@@ -315,21 +294,7 @@ struct ssp_data {
 #endif
 
 #ifdef CONFIG_SENSORS_SSP_SENSORHUB
-	struct input_dev *sensorhub_input_dev;
-	struct miscdevice sensorhub_device;
-	struct wake_lock sensorhub_wake_lock;
-	struct completion transfer_done;
-	struct task_struct *sensorhub_task;
-	struct sensorhub_event events_head;
-	struct sensorhub_event events[LIBRARY_MAX_NUM];
-	struct sensorhub_event *first_event;
-	int event_number;
-	int transfer_try;
-	int transfer_ready;
-	int large_library_length;
-	char *large_library_data;
-	wait_queue_head_t sensorhub_waitqueue;
-	spinlock_t sensorhub_lock;
+	struct ssp_sensorhub_data *hub_data;
 #endif
 };
 
@@ -347,6 +312,13 @@ void initialize_pressure_factorytest(struct ssp_data *);
 void initialize_magnetic_factorytest(struct ssp_data *);
 void initialize_function_pointer(struct ssp_data *);
 void initialize_magnetic(struct ssp_data *);
+void remove_accel_factorytest(struct ssp_data *);
+void remove_gyro_factorytest(struct ssp_data *);
+void remove_prox_factorytest(struct ssp_data *);
+void remove_light_factorytest(struct ssp_data *);
+void remove_pressure_factorytest(struct ssp_data *);
+void remove_magnetic_factorytest(struct ssp_data *);
+void destroy_sensor_class(void);
 int initialize_event_symlink(struct ssp_data *);
 int accel_open_calibration(struct ssp_data *);
 int gyro_open_calibration(struct ssp_data *);
@@ -366,7 +338,7 @@ int get_chipid(struct ssp_data *);
 int get_fuserom_data(struct ssp_data *);
 int set_sensor_position(struct ssp_data *);
 void sync_sensor_state(struct ssp_data *);
-void set_proximity_threshold(struct ssp_data *);
+void set_proximity_threshold(struct ssp_data *, unsigned char, unsigned char);
 void set_proximity_barcode_enable(struct ssp_data *, bool);
 unsigned int get_delay_cmd(u8);
 unsigned int get_msdelay(int64_t);
@@ -391,6 +363,7 @@ void reset_mcu(struct ssp_data *);
 void convert_acc_data(s16 *);
 int sensors_register(struct device *, void *,
 	struct device_attribute*[], char *);
+void sensors_unregister(struct device *, struct device_attribute*[]);
 ssize_t mcu_reset_show(struct device *, struct device_attribute *, char *);
 ssize_t mcu_revision_show(struct device *, struct device_attribute *, char *);
 ssize_t mcu_update_show(struct device *, struct device_attribute *, char *);
@@ -406,12 +379,4 @@ ssize_t mcu_sleep_factorytest_show(struct device *,
 ssize_t mcu_sleep_factorytest_store(struct device *,
 	struct device_attribute *, const char *, size_t);
 
-#ifdef CONFIG_SENSORS_SSP_SENSORHUB
-void ssp_report_sensorhub_notice(struct ssp_data *data, char notice);
-int ssp_handle_sensorhub_data(struct ssp_data *data, char *dataframe,
-				int start, int end);
-int ssp_handle_sensorhub_large_data(struct ssp_data *data, u8 sub_cmd);
-int ssp_initialize_sensorhub(struct ssp_data *data);
-void ssp_remove_sensorhub(struct ssp_data *data);
-#endif
 #endif
diff --git a/drivers/sensorhub/ssp_data.c b/drivers/sensorhub/ssp_data.c
index 3995d710e9c..1be865c7d17 100644
--- a/drivers/sensorhub/ssp_data.c
+++ b/drivers/sensorhub/ssp_data.c
@@ -53,9 +53,6 @@ static void get_3axis_sensordata(char *pchRcvDataFrame, int *iDataIdx,
 	iTemp <<= 8;
 	iTemp += pchRcvDataFrame[(*iDataIdx)++];
 	sensorsdata->z = iTemp;
-
-	data_dbg("x: %d, y: %d, z: %d\n", sensorsdata->x,
-		sensorsdata->y, sensorsdata->z);
 }
 
 static void get_light_sensordata(char *pchRcvDataFrame, int *iDataIdx,
@@ -82,9 +79,6 @@ static void get_light_sensordata(char *pchRcvDataFrame, int *iDataIdx,
 	iTemp <<= 8;
 	iTemp += pchRcvDataFrame[(*iDataIdx)++];
 	sensorsdata->w = iTemp;
-
-	data_dbg("r: %u, g: %u, b: %u, w: %u\n", sensorsdata->r,
-		sensorsdata->g, sensorsdata->b, sensorsdata->w);
 }
 
 static void get_pressure_sensordata(char *pchRcvDataFrame, int *iDataIdx,
@@ -107,9 +101,6 @@ static void get_pressure_sensordata(char *pchRcvDataFrame, int *iDataIdx,
 	iTemp <<= 8;
 	iTemp += (int)pchRcvDataFrame[(*iDataIdx)++];
 	sensorsdata->pressure[1] = (s16)iTemp;
-
-	data_dbg("p : %d, t: %d\n", sensorsdata->pressure[0],
-		sensorsdata->pressure[1]);
 }
 
 static void get_gesture_sensordata(char *pchRcvDataFrame, int *iDataIdx,
@@ -136,10 +127,6 @@ static void get_gesture_sensordata(char *pchRcvDataFrame, int *iDataIdx,
 	iTemp <<= 8;
 	iTemp += pchRcvDataFrame[(*iDataIdx)++];
 	sensorsdata->data[3] = iTemp;
-
-	data_dbg("A: %d, B: %d, C: %d, D: %d\n",
-		sensorsdata->data[0], sensorsdata->data[1],
-		sensorsdata->data[2], sensorsdata->data[3]);
 }
 
 static void get_proximity_sensordata(char *pchRcvDataFrame, int *iDataIdx,
@@ -147,16 +134,12 @@ static void get_proximity_sensordata(char *pchRcvDataFrame, int *iDataIdx,
 {
 	sensorsdata->prox[0] = (u8)pchRcvDataFrame[(*iDataIdx)++];
 	sensorsdata->prox[1] = (u8)pchRcvDataFrame[(*iDataIdx)++];
-
-	data_dbg("prox : %u, %u\n", sensorsdata->prox[0], sensorsdata->prox[1]);
 }
 
 static void get_proximity_rawdata(char *pchRcvDataFrame, int *iDataIdx,
 	struct sensor_value *sensorsdata)
 {
 	sensorsdata->prox[0] = (u8)pchRcvDataFrame[(*iDataIdx)++];
-
-	data_dbg("proxraw : %u\n", sensorsdata->prox[0]);
 }
 
 static void get_factoty_data(struct ssp_data *data, int iSensorData,
diff --git a/drivers/sensorhub/ssp_debug.c b/drivers/sensorhub/ssp_debug.c
index e6471def09f..6c9460634d7 100644
--- a/drivers/sensorhub/ssp_debug.c
+++ b/drivers/sensorhub/ssp_debug.c
@@ -16,18 +16,39 @@
 
 #define SSP_DEBUG_TIMER_SEC		(10 * HZ)
 
+#define LIMIT_RESET_CNT			20
+#define LIMIT_SSD_FAIL_CNT		3
+#define LIMIT_INSTRUCTION_FAIL_CNT	1
+#define LIMIT_IRQ_FAIL_CNT		2
+#define LIMIT_TIMEOUT_CNT		5
+
 /*************************************************************************/
 /* SSP Debug timer function                                              */
 /*************************************************************************/
 
+void print_mcu_debug(char *pchRcvDataFrame, int *pDataIdx)
+{
+	int iLength;
+
+	iLength = pchRcvDataFrame[0];
+	pchRcvDataFrame[iLength] = 0;
+	*pDataIdx = *pDataIdx + iLength + 2;
+
+	ssp_dbg("[SSP]: MSG From MCU - %s\n", pchRcvDataFrame + 1);
+}
+
 void reset_mcu(struct ssp_data *data)
 {
+	data->bSspShutdown = true;
 	disable_irq(data->iIrq);
 	disable_irq_wake(data->iIrq);
 
 	toggle_mcu_reset(data);
 	msleep(SSP_SW_RESET_TIME);
-	initialize_mcu(data);
+	data->bSspShutdown = false;
+
+	if (initialize_mcu(data) < 0)
+		data->bSspShutdown = true;
 
 	sync_sensor_state(data);
 
@@ -44,7 +65,7 @@ void sync_sensor_state(struct ssp_data *data)
 	unsigned char uBuf[2] = {0,};
 	unsigned int uSensorCnt;
 
-	set_proximity_threshold(data);
+	proximity_open_calibration(data);
 
 	udelay(10);
 
@@ -57,8 +78,11 @@ void sync_sensor_state(struct ssp_data *data)
 		}
 	}
 
-	data->uTimeOutCnt = 0;
-	data->uBusyCnt = 0;
+	if (data->bProximityRawEnabled == true) {
+		uBuf[0] = 1;
+		uBuf[1] = 20;
+		send_instruction(data, ADD_SENSOR, PROXIMITY_RAW, uBuf, 2);
+	}
 }
 
 static void print_sensordata(struct ssp_data *data, unsigned int uSensor)
@@ -97,41 +121,44 @@ static void print_sensordata(struct ssp_data *data, unsigned int uSensor)
 	}
 }
 
-void print_mcu_debug(char *pchRcvDataFrame, int *pDataIdx)
-{
-	int iLength;
-
-	iLength = pchRcvDataFrame[0];
-	pchRcvDataFrame[iLength] = 0;
-	*pDataIdx = *pDataIdx + iLength + 2;
-
-	ssp_dbg("[SSP] MSG From MCU : %s\n", pchRcvDataFrame + 1);
-}
-
 static void debug_work_func(struct work_struct *work)
 {
 	unsigned int uSensorCnt;
 	struct ssp_data *data = container_of(work, struct ssp_data, work_debug);
 
-	ssp_dbg("[SSP]: %s(%u) - Sensor state: 0x%x, TO: %u, BC: %u, RC: %u\n",
-		__func__, data->uIrqCnt, data->uAliveSensorDebug,
-		data->uTimeOutCnt, data->uBusyCnt, data->uResetCnt);
-
-	data->uIrqCnt = 0;
+	ssp_dbg("[SSP]: %s(%u) - Sensor state: 0x%x, RC: %u, MS: %u\n",
+		__func__, data->uIrqCnt, data->uSensorState, data->uResetCnt,
+		data->uMissSensorCnt);
 
 	for (uSensorCnt = 0; uSensorCnt < (SENSOR_MAX - 1); uSensorCnt++)
 		if (atomic_read(&data->aSensorEnable) & (1 << uSensorCnt))
 			print_sensordata(data, uSensorCnt);
 
-	if ((data->uSsdFailCnt >= 3) || (data->uI2cFailCnt >= 1)) {
-		if (data->uResetCnt < 20) {
+	if ((atomic_read(&data->aSensorEnable) & 0x4f) && (data->uIrqCnt == 0))
+		data->uIrqFailCnt++;
+	else
+		data->uIrqFailCnt = 0;
+
+	if ((data->uSsdFailCnt >= LIMIT_SSD_FAIL_CNT)
+		|| (data->uInstFailCnt >= LIMIT_INSTRUCTION_FAIL_CNT)
+		|| (data->uIrqFailCnt >= LIMIT_IRQ_FAIL_CNT)
+		|| ((data->uTimeOutCnt + data->uBusyCnt) > LIMIT_TIMEOUT_CNT)) {
+
+		if (data->uResetCnt < LIMIT_RESET_CNT) {
 			reset_mcu(data);
 			data->uResetCnt++;
+		} else {
+			data->bSspShutdown = true;
 		}
 
 		data->uSsdFailCnt = 0;
-		data->uI2cFailCnt = 0;
+		data->uInstFailCnt = 0;
+		data->uTimeOutCnt = 0;
+		data->uBusyCnt = 0;
+		data->uIrqFailCnt = 0;
 	}
+
+	data->uIrqCnt = 0;
 }
 
 static void debug_timer_func(unsigned long ptr)
diff --git a/drivers/sensorhub/ssp_dev.c b/drivers/sensorhub/ssp_dev.c
index 9c955fe440f..64932f13d5b 100644
--- a/drivers/sensorhub/ssp_dev.c
+++ b/drivers/sensorhub/ssp_dev.c
@@ -28,7 +28,6 @@ static irqreturn_t sensordata_irq_thread_fn(int iIrq, void *dev_id)
 {
 	struct ssp_data *data = dev_id;
 
-	data_dbg("%s\n", __func__);
 	select_irq_msg(data);
 	data->uIrqCnt++;
 
@@ -55,23 +54,26 @@ static void initialize_variable(struct ssp_data *data)
 
 	atomic_set(&data->aSensorEnable, 0);
 	data->iLibraryLength = 0;
-	data->uAliveSensorDebug = 0;
+	data->uSensorState = 0;
 	data->uFactorydataReady = 0;
 	data->uFactoryProxAvg[0] = 0;
 
 	data->uResetCnt = 0;
-	data->uI2cFailCnt = 0;
+	data->uInstFailCnt = 0;
 	data->uTimeOutCnt = 0;
 	data->uSsdFailCnt = 0;
 	data->uBusyCnt = 0;
 	data->uIrqCnt = 0;
+	data->uIrqFailCnt = 0;
+	data->uMissSensorCnt = 0;
 
 	data->bCheckSuspend = false;
+	data->bSspShutdown = false;
 	data->bDebugEnabled = false;
 	data->bProximityRawEnabled = false;
 	data->bMcuIRQTestSuccessed = false;
 	data->bBarcodeEnabled = false;
-	data->bBinaryChashed = false;
+	data->bAccelAlert = false;
 
 	data->accelcal.x = 0;
 	data->accelcal.y = 0;
@@ -83,9 +85,18 @@ static void initialize_variable(struct ssp_data *data)
 
 	data->iPressureCal = 0;
 	data->uProxCanc = 0;
-	data->uProxThresh = DEFAULT_THRESHOLD;
+	data->uProxHiThresh = 0;
+	data->uProxLoThresh = 0;
 	data->uGyroDps = GYROSCOPE_DPS500;
 
+	data->mcu_device = NULL;
+	data->acc_device = NULL;
+	data->gyro_device = NULL;
+	data->mag_device = NULL;
+	data->prs_device = NULL;
+	data->prox_device = NULL;
+	data->light_device = NULL;
+
 	initialize_function_pointer(data);
 }
 
@@ -94,14 +105,14 @@ int initialize_mcu(struct ssp_data *data)
 	int iRet = 0;
 
 	iRet = get_chipid(data);
-	pr_info("[SSP] MPU device ID = %d, reading ID = %d\n", DEVICE_ID, iRet);
+	pr_info("[SSP] MCU device ID = %d, reading ID = %d\n", DEVICE_ID, iRet);
 	if (iRet != DEVICE_ID) {
-		if (iRet < 0)
-			pr_err("[SSP]: %s - i2c for reading chip id failed\n",
-			       __func__);
-		else {
-			pr_err("[SSP]: %s - Device identification failed\n",
-			       __func__);
+		if (iRet < 0) {
+			pr_err("[SSP]: %s - MCU is not working : 0x%x\n",
+				__func__, iRet);
+		} else {
+			pr_err("[SSP]: %s - MCU identification failed\n",
+				__func__);
 			iRet = -ENODEV;
 		}
 		return iRet;
@@ -116,11 +127,11 @@ int initialize_mcu(struct ssp_data *data)
 	iRet = get_fuserom_data(data);
 	if (iRet < 0) {
 		pr_err("[SSP]: %s - get_fuserom_data failed\n", __func__);
-		return iRet;
+		return FAIL;
 	}
 
-	data->uAliveSensorDebug = get_sensor_scanning_info(data);
-	if (data->uAliveSensorDebug == 0) {
+	data->uSensorState = get_sensor_scanning_info(data);
+	if (data->uSensorState == 0) {
 		pr_err("[SSP]: %s - get_sensor_scanning_info failed\n",
 			__func__);
 		return FAIL;
@@ -184,14 +195,13 @@ static int ssp_probe(struct i2c_client *client,
 
 	data = kzalloc(sizeof(*data), GFP_KERNEL);
 	if (data == NULL) {
-		pr_err("[SSP]: %s - failed to allocate memory for module data\n",
+		pr_err("[SSP]: %s - failed to allocate memory for data\n",
 			__func__);
 		iRet = -ENOMEM;
 		goto exit;
 	}
 
 	data->client = client;
-	data->client->adapter->timeout = HZ;
 	i2c_set_clientdata(client, data);
 
 	data->wakeup_mcu = pdata->wakeup_mcu;
@@ -246,7 +256,7 @@ static int ssp_probe(struct i2c_client *client,
 
 	iRet = initialize_irq(data);
 	if (iRet < 0) {
-		pr_err("[SSP]: %s - could not create sysfs\n", __func__);
+		pr_err("[SSP]: %s - could not create irq\n", __func__);
 		goto err_setup_irq;
 	}
 
@@ -279,7 +289,7 @@ static int ssp_probe(struct i2c_client *client,
 
 	enable_irq(data->iIrq);
 	enable_irq_wake(data->iIrq);
-	pr_info("[SSP] probe success!\n");
+	pr_info("[SSP]: %s - probe success!\n", __func__);
 
 	enable_debug_timer(data);
 
@@ -313,6 +323,7 @@ static void ssp_shutdown(struct i2c_client *client)
 	struct ssp_data *data = i2c_get_clientdata(client);
 
 	func_dbg();
+	data->bSspShutdown = true;
 
 #ifdef CONFIG_HAS_EARLYSUSPEND
 	unregister_early_suspend(&data->early_suspend);
@@ -325,24 +336,24 @@ static void ssp_shutdown(struct i2c_client *client)
 	free_irq(data->iIrq, data);
 	gpio_free(data->client->irq);
 
-	toggle_mcu_reset(data);
+	remove_sysfs(data);
+	remove_event_symlink(data);
+	remove_input_dev(data);
 
 #ifdef CONFIG_SENSORS_SSP_SENSORHUB
 	ssp_remove_sensorhub(data);
 #endif
-	remove_event_symlink(data);
-	remove_sysfs(data);
-	remove_input_dev(data);
 
 	misc_deregister(&data->akmd_device);
 
 	del_timer_sync(&data->debug_timer);
 	cancel_work_sync(&data->work_debug);
 	destroy_workqueue(data->debug_wq);
-
 	wake_lock_destroy(&data->ssp_wake_lock);
+
+	toggle_mcu_reset(data);
+
 	kfree(data);
-	data = NULL;
 }
 
 #ifdef CONFIG_HAS_EARLYSUSPEND
@@ -385,7 +396,8 @@ static void ssp_late_resume(struct early_suspend *handler)
 		ssp_resume_mode(data);
 #endif
 }
-#else
+
+#else /* CONFIG_HAS_EARLYSUSPEND */
 
 static int ssp_suspend(struct device *dev)
 {
@@ -393,8 +405,7 @@ static int ssp_suspend(struct device *dev)
 	struct ssp_data *data = i2c_get_clientdata(client);
 
 	func_dbg();
-	if (data->bDebugEnabled)
-		disable_debug_timer(data);
+	disable_debug_timer(data);
 
 	if (atomic_read(&data->aSensorEnable) > 0)
 		ssp_sleep_mode(data);
@@ -409,8 +420,7 @@ static int ssp_resume(struct device *dev)
 	struct ssp_data *data = i2c_get_clientdata(client);
 
 	func_dbg();
-	if (data->bDebugEnabled)
-		enable_debug_timer(data);
+	enable_debug_timer(data);
 
 	data->bCheckSuspend = false;
 
@@ -424,7 +434,8 @@ static const struct dev_pm_ops ssp_pm_ops = {
 	.suspend = ssp_suspend,
 	.resume = ssp_resume
 };
-#endif
+
+#endif /* CONFIG_HAS_EARLYSUSPEND */
 
 static const struct i2c_device_id ssp_id[] = {
 	{"ssp", 0},
@@ -460,5 +471,5 @@ module_init(ssp_init);
 module_exit(ssp_exit);
 
 MODULE_DESCRIPTION("ssp driver");
-MODULE_AUTHOR("Kyusung Kim <gs0816.kim@samsung.com>");
+MODULE_AUTHOR("Samsung Electronics");
 MODULE_LICENSE("GPL");
diff --git a/drivers/sensorhub/ssp_firmware.c b/drivers/sensorhub/ssp_firmware.c
index 2bdf8ff20e2..62e5efe4843 100644
--- a/drivers/sensorhub/ssp_firmware.c
+++ b/drivers/sensorhub/ssp_firmware.c
@@ -14,7 +14,7 @@
  */
 #include "ssp.h"
 
-#define SSP_FIRMWARE_REVISION		92600
+#define SSP_FIRMWARE_REVISION		92800
 
 /* Bootload mode cmd */
 #define BL_FW_NAME			"ssp.fw"
@@ -255,9 +255,9 @@ int update_mcu_bin(struct ssp_data *data)
 	data->client->addr = APP_SLAVE_ADDR;
 
 	if (iRet < 0)
-		data->bBinaryChashed = true;
+		data->bSspShutdown = true;
 	else
-		data->bBinaryChashed = false;
+		data->bSspShutdown = false;
 
 	return iRet;
 }
@@ -273,7 +273,7 @@ int update_crashed_mcu_bin(struct ssp_data *data)
 	msleep(SSP_SW_RESET_TIME);
 
 	data->client->addr = APP_SLAVE_ADDR;
-	data->bBinaryChashed = true;
+	data->bSspShutdown = true;
 	return iRet;
 }
 
@@ -291,7 +291,7 @@ void check_fwbl(struct ssp_data *data)
 	} else {
 		data->client->addr = APP_SLAVE_ADDR;
 		data->uCurFirmRev = get_firmware_rev(data);
-		if (data->uCurFirmRev < SSP_FIRMWARE_REVISION) {
+		if (data->uCurFirmRev != SSP_FIRMWARE_REVISION) {
 			pr_info("[SSP] MPU Firm Rev. : Old = %8u, New = %8u\n",
 				data->uCurFirmRev, SSP_FIRMWARE_REVISION);
 			update_mcu_bin(data);
diff --git a/drivers/sensorhub/ssp_i2c.c b/drivers/sensorhub/ssp_i2c.c
index 82d8c5334b8..695ad8dae34 100644
--- a/drivers/sensorhub/ssp_i2c.c
+++ b/drivers/sensorhub/ssp_i2c.c
@@ -14,32 +14,39 @@
  */
 #include "ssp.h"
 
+#define LIMIT_DELAY_CNT		200
+
 int waiting_wakeup_mcu(struct ssp_data *data)
 {
 	int iDelaycnt = 0;
 
-	if (data == NULL)
-		return ERROR;
-
-	while (!data->check_mcu_busy() && (iDelaycnt++ < 200))
+	while (!data->check_mcu_busy() && (iDelaycnt++ < LIMIT_DELAY_CNT)
+		&& (data->bSspShutdown == false))
 		mdelay(5);
 
-	if (iDelaycnt >= 200) {
+	if (iDelaycnt >= LIMIT_DELAY_CNT) {
 		pr_err("[SSP]: %s - MCU Irq Timeout!!\n", __func__);
 		data->uBusyCnt++;
+	} else {
+		data->uBusyCnt = 0;
 	}
 
 	iDelaycnt = 0;
 	data->wakeup_mcu();
-	while (data->check_mcu_ready() && (iDelaycnt++ < 50))
-		udelay(50);
+	while (data->check_mcu_ready() && (iDelaycnt++ < LIMIT_DELAY_CNT)
+		&& (data->bSspShutdown == false))
+		mdelay(5);
 
-	if (iDelaycnt >= 50) {
+	if (iDelaycnt >= LIMIT_DELAY_CNT) {
 		pr_err("[SSP]: %s - MCU Wakeup Timeout!!\n", __func__);
 		data->uTimeOutCnt++;
-		return FAIL;
+	} else {
+		data->uTimeOutCnt = 0;
 	}
 
+	if (data->bSspShutdown == true)
+		return FAIL;
+
 	return SUCCESS;
 }
 
@@ -72,9 +79,9 @@ int ssp_i2c_read(struct ssp_data *data, char *pTxData, u16 uTxLength,
 			do_gettimeofday(&cur_time);
 			iDiffTime = (int)cur_time.tv_sec - iTimeTemp;
 			iTimeTemp = (int)cur_time.tv_sec;
-			if (iTimeTemp >= 4) {
+			if (iDiffTime >= 4) {
 				pr_err("[SSP]: %s - i2c time out %d!\n",
-					__func__, iTimeTemp);
+					__func__, iDiffTime);
 				break;
 			}
 			pr_err("[SSP]: %s - i2c transfer error %d! retry...\n",
@@ -83,7 +90,7 @@ int ssp_i2c_read(struct ssp_data *data, char *pTxData, u16 uTxLength,
 		} else {
 			return SUCCESS;
 		}
-	} while (iRetries--);
+	} while (--iRetries);
 
 	return ERROR;
 }
@@ -92,7 +99,7 @@ int ssp_sleep_mode(struct ssp_data *data)
 {
 	char chRxBuf = 0;
 	char chTxBuf = MSG2SSP_AP_STATUS_SLEEP;
-	int iRet = 0, iRetries = 3;
+	int iRet = 0, iRetries = DEFAULT_RETRIES;
 
 	if (waiting_wakeup_mcu(data) < 0)
 		return ERROR;
@@ -101,7 +108,7 @@ int ssp_sleep_mode(struct ssp_data *data)
 	iRet = ssp_i2c_read(data, &chTxBuf, 1, &chRxBuf, 1, DEFAULT_RETRIES);
 	if (iRet != SUCCESS) {
 		pr_err("[SSP]: %s - MSG2SSP_AP_STATUS_SLEEP CMD fail %d\n",
-				__func__, iRet);
+			__func__, iRet);
 		return ERROR;
 	} else if (chRxBuf != MSG_ACK) {
 		while (iRetries--) {
@@ -115,12 +122,12 @@ int ssp_sleep_mode(struct ssp_data *data)
 		}
 
 		if (iRetries < 0) {
-			data->uI2cFailCnt++;
+			data->uInstFailCnt++;
 			return FAIL;
 		}
 	}
 
-	data->uI2cFailCnt = 0;
+	data->uInstFailCnt = 0;
 	ssp_dbg("[SSP]: %s - MSG2SSP_AP_STATUS_SLEEP CMD\n", __func__);
 
 	return SUCCESS;
@@ -130,7 +137,7 @@ int ssp_resume_mode(struct ssp_data *data)
 {
 	char chRxBuf = 0;
 	char chTxBuf = MSG2SSP_AP_STATUS_WAKEUP;
-	int iRet = 0, iRetries = 3;
+	int iRet = 0, iRetries = DEFAULT_RETRIES;
 
 	if (waiting_wakeup_mcu(data) < 0)
 		return ERROR;
@@ -153,12 +160,12 @@ int ssp_resume_mode(struct ssp_data *data)
 		}
 
 		if (iRetries < 0) {
-			data->uI2cFailCnt++;
+			data->uInstFailCnt++;
 			return FAIL;
 		}
 	}
 
-	data->uI2cFailCnt = 0;
+	data->uInstFailCnt = 0;
 	ssp_dbg("[SSP]: %s - MSG2SSP_AP_STATUS_WAKEUP CMD\n", __func__);
 
 	return SUCCESS;
@@ -169,7 +176,19 @@ int send_instruction(struct ssp_data *data, u8 uInst,
 {
 	char chTxbuf[uLength + 3];
 	char chRxbuf = 0;
-	int iRet = 0, iRetries = 3;
+	int iRet = 0, iRetries = DEFAULT_RETRIES;
+
+	if ((!(data->uSensorState & (1 << uSensorType)))
+		&& (uInst <= CHANGE_DELAY)) {
+		pr_err("[SSP]: %s - Bypass Inst Skip! - %u\n",
+			__func__, uSensorType);
+		return FAIL;
+	} else if ((!((data->uSensorState | 0xf0) & (1 << uSensorType)))
+		&& (uInst == FACTORY_MODE)) {
+		pr_err("[SSP]: %s - Factory Inst Skip! - %u\n",
+			__func__, uSensorType);
+		return FAIL;
+	}
 
 	if (waiting_wakeup_mcu(data) < 0)
 		return ERROR;
@@ -216,6 +235,8 @@ int send_instruction(struct ssp_data *data, u8 uInst,
 			mdelay(10);
 			pr_err("[SSP]: %s - Instruction CMD retry...\n",
 				__func__);
+			if (waiting_wakeup_mcu(data) < 0)
+				return ERROR;
 			iRet = ssp_i2c_read(data, &(chTxbuf[0]),
 				uLength + 3, &chRxbuf, 1, DEFAULT_RETRIES);
 			if ((iRet == SUCCESS) && (chRxbuf == MSG_ACK))
@@ -223,12 +244,12 @@ int send_instruction(struct ssp_data *data, u8 uInst,
 		}
 
 		if (iRetries < 0) {
-			data->uI2cFailCnt++;
+			data->uInstFailCnt++;
 			return FAIL;
 		}
 	}
 
-	data->uI2cFailCnt = 0;
+	data->uInstFailCnt = 0;
 	ssp_dbg("[SSP]: %s - Inst = 0x%x, Sensor Type = 0x%x, "
 		"data = %u, %u\n", __func__, chTxbuf[1], chTxbuf[2],
 		chTxbuf[3], chTxbuf[4]);
@@ -264,23 +285,6 @@ int set_sensor_position(struct ssp_data *data)
 	chTxBuf[3] = CONFIG_SENSORS_SSP_MAGNETOMETER_POSITION;
 	chTxBuf[4] = 0;
 
-#if defined(CONFIG_MACH_T0_EUR_OPEN)
-	if (data->check_ap_rev() == 0x03) {
-		chTxBuf[1] = 7;
-		chTxBuf[2] = 7;
-		chTxBuf[3] = CONFIG_SENSORS_SSP_MAGNETOMETER_POSITION;
-	}
-#endif
-
-#if defined(CONFIG_MACH_T0_USA_SPR) || defined(CONFIG_MACH_T0_USA_USCC)\
-	|| defined(CONFIG_MACH_T0_USA_VZW)
-	if (data->check_ap_rev() <= 0x04) {
-		chTxBuf[1] = 4;
-		chTxBuf[2] = 4;
-		chTxBuf[3] = CONFIG_SENSORS_SSP_MAGNETOMETER_POSITION;
-	}
-#endif
-
 	pr_info("[SSP] Sensor Posision A : %u, G : %u, M: %u, P: %u\n",
 		chTxBuf[1], chTxBuf[2], chTxBuf[3], chTxBuf[4]);
 
@@ -293,18 +297,19 @@ int set_sensor_position(struct ssp_data *data)
 	return iRet;
 }
 
-void set_proximity_threshold(struct ssp_data *data)
+void set_proximity_threshold(struct ssp_data *data,
+	unsigned char uData1, unsigned char uData2)
 {
 	char chTxBuf[3] = { 0, };
 	char chRxBuf = 0;
-	int iRet = 0, iRetries = 3;
+	int iRet = 0, iRetries = DEFAULT_RETRIES;
 
 	if (waiting_wakeup_mcu(data) < 0)
 		return;
 
 	chTxBuf[0] = MSG2SSP_AP_SENSOR_PROXTHRESHOLD;
-	chTxBuf[1] = data->uProxThresh;
-	chTxBuf[2] = data->uProxCanc;
+	chTxBuf[1] = uData1;
+	chTxBuf[2] = uData2;
 
 	iRet = ssp_i2c_read(data, chTxBuf, 3, &chRxBuf, 1, DEFAULT_RETRIES);
 	if (iRet != SUCCESS) {
@@ -323,21 +328,20 @@ void set_proximity_threshold(struct ssp_data *data)
 		}
 
 		if (iRetries < 0) {
-			data->uI2cFailCnt++;
+			data->uInstFailCnt++;
 			return;
 		}
 	}
 
-	data->uI2cFailCnt = 0;
-	pr_info("[SSP] Proximity Threshold : %u, Cancelation : %u\n",
-		data->uProxThresh, data->uProxCanc);
+	data->uInstFailCnt = 0;
+	pr_info("[SSP]: Proximity Threshold - %u, %u\n", uData1, uData2);
 }
 
 void set_proximity_barcode_enable(struct ssp_data *data, bool bEnable)
 {
 	char chTxBuf[2] = { 0, };
 	char chRxBuf = 0;
-	int iRet = 0, iRetries = 3;
+	int iRet = 0, iRetries = DEFAULT_RETRIES;
 
 	if (waiting_wakeup_mcu(data) < 0)
 		return ;
@@ -364,12 +368,12 @@ void set_proximity_barcode_enable(struct ssp_data *data, bool bEnable)
 		}
 
 		if (iRetries < 0) {
-			data->uI2cFailCnt++;
+			data->uInstFailCnt++;
 			return;
 		}
 	}
 
-	data->uI2cFailCnt = 0;
+	data->uInstFailCnt = 0;
 	pr_info("[SSP] Proximity Barcode En : %u\n", bEnable);
 }
 
@@ -446,7 +450,7 @@ int get_fuserom_data(struct ssp_data *data)
 		iRet = ssp_i2c_read(data, chTxBuf, 1, data->pchLibraryBuf,
 			(u16)data->iLibraryLength, DEFAULT_RETRIES);
 		if (iRet != SUCCESS) {
-			pr_err("[SSP]: %s - Fail to recieve SSP data %d\n",
+			pr_err("[SSP]: %s - Fail to receive SSP data %d\n",
 				__func__, iRet);
 			kfree(data->pchLibraryBuf);
 			data->iLibraryLength = 0;
@@ -473,7 +477,7 @@ err_read_fuserom:
 	return FAIL;
 }
 
-static int ssp_recieve_msg(struct ssp_data *data,  u8 uLength)
+static int ssp_receive_msg(struct ssp_data *data,  u8 uLength)
 {
 	char chTxBuf = 0;
 	char *pchRcvDataFrame;	/* SSP-AP Massage data buffer */
@@ -485,7 +489,7 @@ static int ssp_recieve_msg(struct ssp_data *data,  u8 uLength)
 		iRet = ssp_i2c_read(data, &chTxBuf, 1, pchRcvDataFrame,
 				(u16)uLength, 0);
 		if (iRet != SUCCESS) {
-			pr_err("[SSP]: %s - Fail to recieve data %d\n",
+			pr_err("[SSP]: %s - Fail to receive data %d\n",
 				__func__, iRet);
 			kfree(pchRcvDataFrame);
 			return ERROR;
@@ -518,7 +522,7 @@ int select_irq_msg(struct ssp_data *data)
 	} else {
 		if (chRxBuf[0] == MSG2SSP_RTS) {
 			chLength = (u8)chRxBuf[1];
-			ssp_recieve_msg(data, chLength);
+			ssp_receive_msg(data, chLength);
 			data->uSsdFailCnt = 0;
 		}
 #ifdef CONFIG_SENSORS_SSP_SENSORHUB
diff --git a/drivers/sensorhub/ssp_input.c b/drivers/sensorhub/ssp_input.c
index f14f9f4c8f8..a7de8c60836 100644
--- a/drivers/sensorhub/ssp_input.c
+++ b/drivers/sensorhub/ssp_input.c
@@ -50,13 +50,13 @@ void report_gyro_data(struct ssp_data *data, struct sensor_value *gyrodata)
 	data->buf[GYROSCOPE_SENSOR].z = gyrodata->z - data->gyrocal.z;
 
 	if (data->uGyroDps == GYROSCOPE_DPS250)	{
-		lTemp[0] = (long)data->buf[GYROSCOPE_SENSOR].x / 2;
-		lTemp[1] = (long)data->buf[GYROSCOPE_SENSOR].y / 2;
-		lTemp[2] = (long)data->buf[GYROSCOPE_SENSOR].z / 2;
+		lTemp[0] = (long)data->buf[GYROSCOPE_SENSOR].x >> 1;
+		lTemp[1] = (long)data->buf[GYROSCOPE_SENSOR].y >> 1;
+		lTemp[2] = (long)data->buf[GYROSCOPE_SENSOR].z >> 1;
 	} else if (data->uGyroDps == GYROSCOPE_DPS2000)	{
-		lTemp[0] = (long)data->buf[GYROSCOPE_SENSOR].x * 4;
-		lTemp[1] = (long)data->buf[GYROSCOPE_SENSOR].y * 4;
-		lTemp[2] = (long)data->buf[GYROSCOPE_SENSOR].z * 4;
+		lTemp[0] = (long)data->buf[GYROSCOPE_SENSOR].x << 2;
+		lTemp[1] = (long)data->buf[GYROSCOPE_SENSOR].y << 2;
+		lTemp[2] = (long)data->buf[GYROSCOPE_SENSOR].z << 2;
 	} else {
 		lTemp[0] = (long)data->buf[GYROSCOPE_SENSOR].x;
 		lTemp[1] = (long)data->buf[GYROSCOPE_SENSOR].y;
@@ -122,18 +122,6 @@ void report_prox_data(struct ssp_data *data, struct sensor_value *proxdata)
 	data->buf[PROXIMITY_SENSOR].prox[0] = proxdata->prox[0];
 	data->buf[PROXIMITY_SENSOR].prox[1] = proxdata->prox[1];
 
-#if defined(CONFIG_MACH_T0_USA_SPR) || defined(CONFIG_MACH_T0_USA_USCC)\
-	|| defined(CONFIG_MACH_T0_USA_VZW) || defined(CONFIG_MACH_T0_USA_TMO)\
-	|| defined(CONFIG_MACH_T0_USA_ATT)
-	if (data->check_ap_rev() == 0x04)
-		proxdata->prox[0] = 0;
-#endif
-
-#if defined(CONFIG_MACH_T0_USA_TMO)
-	if (data->check_ap_rev() == 0x05)
-		proxdata->prox[0] = 0;
-#endif
-
 	input_report_abs(data->prox_input_dev, ABS_DISTANCE,
 		(!proxdata->prox[0]));
 	input_sync(data->prox_input_dev);
@@ -355,7 +343,7 @@ iRet_pressure_input_unreg_device:
 	input_unregister_device(gyro_input_dev);
 iRet_gyro_input_unreg_device:
 	input_unregister_device(acc_input_dev);
-	return -1;
+	return ERROR;
 
 iRet_acc_input_unreg_device:
 	pr_err("[SSP]: %s - could not register input device\n", __func__);
@@ -370,7 +358,7 @@ iRet_gyro_input_free_device:
 	input_free_device(acc_input_dev);
 iRet_acc_input_free_device:
 	pr_err("[SSP]: %s - could not allocate input device\n", __func__);
-	return -1;
+	return ERROR;
 }
 
 void remove_input_dev(struct ssp_data *data)
diff --git a/drivers/sensorhub/ssp_sensorhub.c b/drivers/sensorhub/ssp_sensorhub.c
index b51d09a0037..bfa80d0845e 100644
--- a/drivers/sensorhub/ssp_sensorhub.c
+++ b/drivers/sensorhub/ssp_sensorhub.c
@@ -15,92 +15,103 @@
 
 #include "ssp.h"
 
-/* sensorhub ioctl command */
-#define SENSORHUB_IOCTL_MAGIC	'S'
-#define IOCTL_READ_CONTEXT_DATA	_IOR(SENSORHUB_IOCTL_MAGIC, 3, char *)
-
+static const struct fast_data {
+	char library_data[3];
+} fast_data_table[] = {
+	{ { 1, 1, 7 } },
+};
 
 static ssize_t ssp_sensorhub_write(struct file *file, const char __user *buf,
 				size_t count, loff_t *pos)
 {
-	struct ssp_data *data = container_of(file->private_data,
-				struct ssp_data, sensorhub_device);
+	struct ssp_sensorhub_data *hub_data
+		= container_of(file->private_data,
+			struct ssp_sensorhub_data, sensorhub_device);
 	int ret = 0;
 	int i;
 	u8 instruction = buf[0];
 
-	if (data == NULL) {
-		pr_err("%s: invalid ssp_data structure", __func__);
-		return -ENOMEM;
-	}
-
 	if (count == 0) {
 		pr_err("%s: library command length err(%d)", __func__, count);
 		return -EINVAL;
 	}
 
 	for (i = 0; i < count; i++)
-		pr_info("%s[%d] = %d", __func__, i, buf[i]);
+		pr_info("%s[%d] = 0x%x", __func__, i, buf[i]);
 
 	if (buf[0] == MSG2SSP_INST_LIBRARY_REMOVE)
 		instruction = REMOVE_LIBRARY;
 	else if (buf[0] == MSG2SSP_INST_LIBRARY_ADD)
 		instruction = ADD_LIBRARY;
 
-	ret = send_instruction(data, instruction,
+	if (hub_data->ssp_data->bSspShutdown) {
+		pr_err("%s: stop sending command(no ssp_data)", __func__);
+		return -ENOMEM;
+	}
+
+	ret = send_instruction(hub_data->ssp_data, instruction,
 		(u8)buf[1], (u8 *)(buf+2), count-2);
-	if (ret < 0)
+	if (ret <= 0)
 		pr_err("%s: send library command err(%d)", __func__, ret);
 
-	return ret;
+	/* i2c transfer fail */
+	if (ret == ERROR)
+		return -EIO;
+	/* no ack from MCU */
+	else if (ret == FAIL)
+		return -EAGAIN;
+	/* success */
+	else
+		return count;
 }
 
 static long ssp_sensorhub_ioctl(struct file *file, unsigned int cmd,
 				unsigned long arg)
 {
 	void __user *argp = (void __user *)arg;
-	struct ssp_data *data = container_of(file->private_data,
-				     struct ssp_data, sensorhub_device);
+	struct ssp_sensorhub_data *hub_data
+		= container_of(file->private_data,
+			struct ssp_sensorhub_data, sensorhub_device);
 	int ret = 0;
 	int i = 0;
 
 	switch (cmd) {
 	case IOCTL_READ_CONTEXT_DATA:
 		/* for receive_msg */
-		if (!data->large_library_length &&
-			data->large_library_data == NULL) {
+		if (!hub_data->large_library_length &&
+			hub_data->large_library_data == NULL) {
 
 			ret = copy_to_user(argp,
-				data->first_event->library_data,
-				data->first_event->length);
+				hub_data->first_event->library_data,
+				hub_data->first_event->length);
 			if (ret < 0) {
 				pr_err("%s: send library data err(%d)",
 					__func__, ret);
-				complete(&data->transfer_done);
+				complete(&hub_data->transfer_done);
 				goto exit;
 			}
 
-			for (i = 0; i < data->first_event->length; i++) {
-				pr_info("%s[%d] = %d", __func__, i,
-					data->first_event->library_data[i]);
+			for (i = 0; i < hub_data->first_event->length; i++) {
+				pr_info("%s[%d] = 0x%x", __func__, i,
+					hub_data->first_event->library_data[i]);
 			}
 
-			data->transfer_try = 0;
-			complete(&data->transfer_done);
+			hub_data->transfer_try = 0;
+			complete(&hub_data->transfer_done);
 
 		/* for receive_large_msg */
 		} else {
 			pr_info("%s: receive_large_msg ioctl", __func__);
-			ret = copy_to_user(argp, data->large_library_data,
-				data->large_library_length);
+			ret = copy_to_user(argp, hub_data->large_library_data,
+				hub_data->large_library_length);
 			if (ret < 0) {
 				pr_err("%s: send large library data err(%d)",
 					__func__, ret);
 				goto exit;
 			}
 
-			kfree(data->large_library_data);
-			data->large_library_length = 0;
+			kfree(hub_data->large_library_data);
+			hub_data->large_library_length = 0;
 		}
 		break;
 
@@ -113,17 +124,19 @@ exit:
 	return ret;
 }
 
-static const struct file_operations ssp_sensorhub_fops = {
+static struct file_operations ssp_sensorhub_fops = {
 	.owner = THIS_MODULE,
 	.open = nonseekable_open,
 	.write = ssp_sensorhub_write,
 	.unlocked_ioctl = ssp_sensorhub_ioctl,
 };
 
-void ssp_report_sensorhub_notice(struct ssp_data *data, char notice)
+void ssp_report_sensorhub_notice(struct ssp_data *ssp_data, char notice)
 {
-	input_report_rel(data->sensorhub_input_dev, REL_RY, notice);
-	input_sync(data->sensorhub_input_dev);
+	struct ssp_sensorhub_data *hub_data = ssp_data->hub_data;
+
+	input_report_rel(hub_data->sensorhub_input_dev, REL_RY, notice);
+	input_sync(hub_data->sensorhub_input_dev);
 
 	if (notice == MSG2SSP_AP_STATUS_WAKEUP)
 		pr_info("%s: wake up", __func__);
@@ -131,22 +144,43 @@ void ssp_report_sensorhub_notice(struct ssp_data *data, char notice)
 		pr_info("%s: sleep", __func__);
 	else if (notice == MSG2SSP_AP_STATUS_RESET)
 		pr_info("%s: reset", __func__);
+	else
+		pr_err("%s: invalid notice", __func__);
 }
 
-static void ssp_report_sensorhub_length(struct ssp_data *data,
+static void ssp_report_sensorhub_length(struct ssp_sensorhub_data *hub_data,
 				int length)
 {
-	input_report_rel(data->sensorhub_input_dev, REL_RX, length);
-	input_sync(data->sensorhub_input_dev);
+	input_report_rel(hub_data->sensorhub_input_dev, REL_RX, length);
+	input_sync(hub_data->sensorhub_input_dev);
 	pr_info("%s = %d", __func__, length);
 }
 
-static int ssp_queue_sensorhub_events(struct ssp_data *data,
+static int ssp_sensorhub_is_fast_data(char *data, int start)
+{
+	int i, j;
+
+	for (i = 0; i < ARRAY_SIZE(fast_data_table); i++) {
+		for (j = 0; j < sizeof(fast_data_table[0]); j++) {
+			if (data[start + j] !=
+				fast_data_table[i].library_data[j])
+				break;
+		}
+		if (j == sizeof(fast_data_table[0]))
+			return i;
+	}
+
+	return -EINVAL;
+}
+
+static int ssp_queue_sensorhub_events(struct ssp_sensorhub_data *hub_data,
 				char *dataframe, int start, int end)
 {
 	struct sensorhub_event *event;
 	int length = end - start;
-	int entries = 0;
+	int event_number = hub_data->event_number;
+	int events = 0;
+	int ret = 0;
 	int i = 0;
 
 	if (length <= 0) {
@@ -155,52 +189,67 @@ static int ssp_queue_sensorhub_events(struct ssp_data *data,
 	}
 
 	/* how many events in the list? */
-	list_for_each_entry(event, &data->events_head.list, list)
-		entries++;
+	spin_lock_bh(&hub_data->sensorhub_lock);
+	list_for_each_entry(event, &hub_data->events_head.list, list)
+		events++;
+	spin_unlock_bh(&hub_data->sensorhub_lock);
 
 	/* drop event if queue is full */
-	if (entries >= LIBRARY_MAX_NUM) {
+	if (events >= LIBRARY_MAX_NUM) {
 		pr_info("%s: queue is full", __func__);
-		data->transfer_ready++;
-		return -ENOMEM;
+		hub_data->transfer_ready++;
+		ret = ssp_sensorhub_is_fast_data(dataframe, start);
+		if (ret >= 0)
+			event_number = LIBRARY_MAX_NUM + ret;
+		else
+			return -ENOMEM;
 	}
 
 	/* allocate memory for new event */
-	if (data->events[data->event_number].library_data != NULL)
-		kfree(data->events[data->event_number].library_data);
+	if (hub_data->events[event_number].library_data != NULL)
+		kfree(hub_data->events[event_number].library_data);
 
-	data->events[data->event_number].library_data
+	hub_data->events[event_number].library_data
 		= kzalloc(length * sizeof(char), GFP_KERNEL);
-	if (data->events[data->event_number].library_data == NULL) {
+	if (hub_data->events[event_number].library_data == NULL) {
 		pr_err("%s: allocate memory for library data err", __func__);
 		return -ENOMEM;
 	}
 
 	/* copy sensorhub event into queue */
 	while (start < end) {
-		data->events[data->event_number].library_data[i++]
+		hub_data->events[event_number].library_data[i++]
 			= dataframe[start++];
-		pr_info("%s[%d] = %d", __func__, i-1,
-			data->events[data->event_number].library_data[i-1]);
+		pr_info("%s[%d] = 0x%x", __func__, i-1,
+			hub_data->events[event_number].library_data[i-1]);
+	}
+	hub_data->events[event_number].length = length;
+
+	if (events <= LIBRARY_MAX_NUM) {
+		/* add new event at the end of queue */
+		spin_lock_bh(&hub_data->sensorhub_lock);
+		list_add_tail(&hub_data->events[event_number].list,
+			&hub_data->events_head.list);
+		if (events++ < LIBRARY_MAX_NUM)
+			hub_data->transfer_ready = 0;
+		spin_unlock_bh(&hub_data->sensorhub_lock);
+
+		/* do not exceed max queue number */
+		if (hub_data->event_number++ >= LIBRARY_MAX_NUM - 1)
+			hub_data->event_number = 0;
+	} else {
+		spin_lock_bh(&hub_data->sensorhub_lock);
+		list_replace(hub_data->events_head.list.prev,
+				&hub_data->events[event_number].list);
+		spin_unlock_bh(&hub_data->sensorhub_lock);
 	}
-	data->events[data->event_number].length = length;
-
-	/* add new sensorhug event at the end of queue */
-	spin_lock_bh(&data->sensorhub_lock);
-	list_add_tail(&data->events[data->event_number].list,
-		&data->events_head.list);
-	data->transfer_ready = 0;
-	spin_unlock_bh(&data->sensorhub_lock);
-	pr_info("%s: total %d entries", __func__, entries + 1);
-
-	/* do not exceed max queue number */
-	if (data->event_number++ >= LIBRARY_MAX_NUM - 1)
-		data->event_number = 0;
 
-	return length;
+	pr_info("%s: total %d events", __func__, events);
+	return events;
 }
 
-static int ssp_receive_large_msg(struct ssp_data *data, u8 sub_cmd)
+static int ssp_receive_large_msg(struct ssp_sensorhub_data *hub_data,
+				u8 sub_cmd)
 {
 	char send_data[2] = { 0, };
 	char receive_data[2] = { 0, };
@@ -216,7 +265,8 @@ static int ssp_receive_large_msg(struct ssp_data *data, u8 sub_cmd)
 	send_data[1] = sub_cmd;
 
 	/* receive_data(msg length) is two byte because msg is large */
-	ret = ssp_i2c_read(data, send_data, 2, receive_data, 2, 0);
+	ret = ssp_i2c_read(hub_data->ssp_data, send_data, 2,
+			receive_data, 2, 0);
 	if (ret < 0) {
 		pr_err("%s: MSG2SSP_STT i2c err(%d)", __func__, ret);
 		return ret;
@@ -236,8 +286,8 @@ static int ssp_receive_large_msg(struct ssp_data *data, u8 sub_cmd)
 	/* receive the first msg data */
 	send_data[0] = MSG2SSP_SRM;
 	large_msg_data = kzalloc((length  * sizeof(char)), GFP_KERNEL);
-	ret = ssp_i2c_read(data, send_data, 1,
-				large_msg_data, length, 0);
+	ret = ssp_i2c_read(hub_data->ssp_data, send_data, 1,
+			large_msg_data, length, 0);
 	if (ret < 0) {
 		pr_err("%s: receive 1st large msg err(%d)", __func__, ret);
 		kfree(large_msg_data);
@@ -245,21 +295,22 @@ static int ssp_receive_large_msg(struct ssp_data *data, u8 sub_cmd)
 	}
 
 	/* empty the previous large library data */
-	if (data->large_library_length != 0)
-		kfree(data->large_library_data);
+	if (hub_data->large_library_length != 0)
+		kfree(hub_data->large_library_data);
 
 	/* large_msg_data[0] of the first msg: total number of large msg
 	 * large_msg_data[1-2] of the first msg: total msg length
 	 * large_msg_data[3-N] of the first msg: the first msg data itself */
 	total_msg_number = large_msg_data[0];
-	data->large_library_length = (int)((unsigned int)large_msg_data[1] << 8)
+	hub_data->large_library_length
+			= (int)((unsigned int)large_msg_data[1] << 8)
 				+ (unsigned int)large_msg_data[2];
-	data->large_library_data
-		= kzalloc((data->large_library_length * sizeof(char)),
+	hub_data->large_library_data
+		= kzalloc((hub_data->large_library_length * sizeof(char)),
 			GFP_KERNEL);
 
 	/* copy the fist msg data into large_library_data */
-	memcpy(data->large_library_data, &large_msg_data[3],
+	memcpy(hub_data->large_library_data, &large_msg_data[3],
 		(length - 3) * sizeof(char));
 	kfree(large_msg_data);
 
@@ -272,7 +323,8 @@ static int ssp_receive_large_msg(struct ssp_data *data, u8 sub_cmd)
 		send_data[1] = 0x81 + msg_number;
 
 		/* receive_data(msg length) is two byte because msg is large */
-		ret = ssp_i2c_read(data, send_data, 2, receive_data, 2, 0);
+		ret = ssp_i2c_read(hub_data->ssp_data, send_data, 2,
+				receive_data, 2, 0);
 		if (ret < 0) {
 			pr_err("%s: MSG2SSP_STT i2c err(%d)",
 					__func__, ret);
@@ -293,8 +345,8 @@ static int ssp_receive_large_msg(struct ssp_data *data, u8 sub_cmd)
 
 		/* receive Nth msg data */
 		send_data[0] = MSG2SSP_SRM;
-		ret = ssp_i2c_read(data, send_data, 1,
-					large_msg_data, length, 0);
+		ret = ssp_i2c_read(hub_data->ssp_data, send_data, 1,
+				large_msg_data, length, 0);
 		if (ret < 0) {
 			pr_err("%s: recieve %dth large msg err(%d)",
 			       __func__, msg_number + 2, ret);
@@ -303,106 +355,124 @@ static int ssp_receive_large_msg(struct ssp_data *data, u8 sub_cmd)
 		}
 
 		/* copy(append) Nth msg data into large_library_data */
-		memcpy(&data->large_library_data[data_locater],
+		memcpy(&hub_data->large_library_data[data_locater],
 			large_msg_data,	length * sizeof(char));
 		data_locater += length;
 		kfree(large_msg_data);
 	}
 
-	return data->large_library_length;
+	return hub_data->large_library_length;
 }
 
 static int ssp_senosrhub_thread_func(void *arg)
 {
-	struct ssp_data *data = (struct ssp_data *)arg;
+	struct ssp_sensorhub_data *hub_data = (struct ssp_sensorhub_data *)arg;
 	struct sensorhub_event *event;
-	int entries = 0;
+	int events = 0;
 	int ret = 0;
 
 	while (!kthread_should_stop()) {
 		/* run if only event queue is not empty */
-		wait_event_interruptible(data->sensorhub_waitqueue,
+		wait_event_interruptible(hub_data->sensorhub_waitqueue,
 				kthread_should_stop() ||
-				!list_empty(&data->events_head.list));
+				!list_empty(&hub_data->events_head.list));
 
-		/* quit while loop and do not report length anymore
+		/* exit thread if kthread should stop */
+		if (unlikely(kthread_should_stop())) {
+			pr_info("%s: kthread_stop()", __func__);
+			break;
+		}
+
+		/* exit thread
 		 * if user does not get data with consecutive trials
 		 */
-		if (data->transfer_try++ >= LIBRARY_MAX_TRY) {
+		if (unlikely(hub_data->transfer_try++ >= LIBRARY_MAX_TRY)) {
 			pr_err("%s: user does not get data", __func__);
 			break;
 		}
 
-		/* first in first out */
-		data->first_event = list_first_entry(&data->events_head.list,
-			struct sensorhub_event, list);
-		if (IS_ERR(data->first_event)) {
-			pr_err("%s: no sensor event entry", __func__);
-			continue;
-		}
-
 		/* report sensorhub event to user */
-		if (data->transfer_ready == 0) {
-			ssp_report_sensorhub_length(data,
-					data->first_event->length);
-			wake_lock_timeout(&data->sensorhub_wake_lock, 5*HZ);
-			data->transfer_ready++;
+		if (hub_data->transfer_ready == 0) {
+			/* first in first out */
+			hub_data->first_event
+				= list_first_entry(&hub_data->events_head.list,
+						struct sensorhub_event, list);
+			if (IS_ERR(hub_data->first_event)) {
+				pr_err("%s: first event err(%ld)", __func__,
+					PTR_ERR(hub_data->first_event));
+				continue;
+			}
+
+			/* report sensorhub event to user */
+			ssp_report_sensorhub_length(hub_data,
+					hub_data->first_event->length);
+			wake_lock_timeout(&hub_data->sensorhub_wake_lock, 5*HZ);
+			hub_data->transfer_ready++;
 		}
 
 		/* wait until user gets data */
-		ret = wait_for_completion_timeout(&data->transfer_done, 3*HZ);
+		ret = wait_for_completion_timeout(&hub_data->transfer_done,
+			3*HZ);
 		if (ret == 0) {
 			pr_err("%s: wait timed out", __func__);
-			data->transfer_ready = 0;
+			hub_data->transfer_ready = 0;
 		} else if (ret < 0) {
 			pr_err("%s: wait_for_completion_timeout err(%d)",
 				__func__, ret);
 		}
 
-		/* delete first entry only if transfer succeed */
-		if (data->transfer_try == 0) {
-			if (!list_empty(&data->events_head.list)) {
-				spin_lock_bh(&data->sensorhub_lock);
-				list_del(&data->first_event->list);
-				data->transfer_ready = 0;
-				spin_unlock_bh(&data->sensorhub_lock);
-				entries = 0;
-				list_for_each_entry(event,
-					&data->events_head.list, list)
-					entries++;
-				pr_info("%s: %d entries remain",
-					__func__, entries);
-			}
+		/* remove first event only if transfer succeed */
+		if (hub_data->transfer_try == 0) {
+			/* remove first event */
+			spin_lock_bh(&hub_data->sensorhub_lock);
+			if (!list_empty(&hub_data->events_head.list))
+				list_del(&hub_data->first_event->list);
+			hub_data->transfer_ready = 0;
+
+			/* how many events in the list? */
+			events = 0;
+			list_for_each_entry(event,
+				&hub_data->events_head.list, list)
+				events++;
+			spin_unlock_bh(&hub_data->sensorhub_lock);
+
+			pr_info("%s: %d events remain", __func__, events);
 			continue;
 		}
 
 		/* throw away extra events */
-		if (data->transfer_ready > EVENT_WAIT_COUNT)
-			data->transfer_ready = 0;
+		if (hub_data->transfer_ready > EVENT_WAIT_COUNT)
+			hub_data->transfer_ready = 0;
 
 		usleep_range(10000, 10000);
 	}
 
+	pr_info("%s: exit", __func__);
 	return ret;
 }
 
-int ssp_handle_sensorhub_data(struct ssp_data *data, char *dataframe,
+int ssp_handle_sensorhub_data(struct ssp_data *ssp_data, char *dataframe,
 				int start, int end)
 {
+	struct ssp_sensorhub_data *hub_data = ssp_data->hub_data;
+
 	/* add new sensorhub event into queue */
-	int ret = ssp_queue_sensorhub_events(data, dataframe, start, end);
-	wake_up(&data->sensorhub_waitqueue);
+	int ret = ssp_queue_sensorhub_events(hub_data, dataframe, start, end);
+	wake_up(&hub_data->sensorhub_waitqueue);
 
 	return ret;
 }
 
-int ssp_handle_sensorhub_large_data(struct ssp_data *data, u8 sub_cmd)
+int ssp_handle_sensorhub_large_data(struct ssp_data *ssp_data, u8 sub_cmd)
 {
+	struct ssp_sensorhub_data *hub_data = ssp_data->hub_data;
+
 	/* receive large size of library data */
-	int ret = ssp_receive_large_msg(data, sub_cmd);
+	int ret = ssp_receive_large_msg(hub_data, sub_cmd);
 	if (ret >= 0) {
-		ssp_report_sensorhub_length(data, data->large_library_length);
-		wake_lock_timeout(&data->sensorhub_wake_lock, 3*HZ);
+		ssp_report_sensorhub_length(hub_data,
+			hub_data->large_library_length);
+		wake_lock_timeout(&hub_data->sensorhub_wake_lock, 3*HZ);
 	} else {
 		pr_err("%s: ssp_receive_large_msg err(%d)", __func__, ret);
 	}
@@ -410,77 +480,93 @@ int ssp_handle_sensorhub_large_data(struct ssp_data *data, u8 sub_cmd)
 	return ret;
 }
 
-int ssp_initialize_sensorhub(struct ssp_data *data)
+int ssp_initialize_sensorhub(struct ssp_data *ssp_data)
 {
+	struct ssp_sensorhub_data *hub_data;
 	int ret;
 
+	hub_data = kzalloc(sizeof(*hub_data), GFP_KERNEL);
+	if (!hub_data) {
+		pr_err("%s: failed to allocate memory for sensorhub data",
+			__func__);
+		return -ENOMEM;
+	}
+	hub_data->ssp_data = ssp_data;
+	ssp_data->hub_data = hub_data;
+
 	/* allocate sensorhub input devices */
-	data->sensorhub_input_dev = input_allocate_device();
-	if (!data->sensorhub_input_dev) {
+	hub_data->sensorhub_input_dev = input_allocate_device();
+	if (!hub_data->sensorhub_input_dev) {
 		pr_err("%s: allocate sensorhub input devices err", __func__);
 		ret = -ENOMEM;
 		goto err_input_allocate_device_sensorhub;
 	}
 
-	wake_lock_init(&data->sensorhub_wake_lock, WAKE_LOCK_SUSPEND,
+	wake_lock_init(&hub_data->sensorhub_wake_lock, WAKE_LOCK_SUSPEND,
 			"sensorhub_wake_lock");
-	INIT_LIST_HEAD(&data->events_head.list);
-	init_waitqueue_head(&data->sensorhub_waitqueue);
-	init_completion(&data->transfer_done);
-	spin_lock_init(&data->sensorhub_lock);
+	INIT_LIST_HEAD(&hub_data->events_head.list);
+	init_waitqueue_head(&hub_data->sensorhub_waitqueue);
+	init_completion(&hub_data->transfer_done);
+	spin_lock_init(&hub_data->sensorhub_lock);
 
-	ret = input_register_device(data->sensorhub_input_dev);
+	ret = input_register_device(hub_data->sensorhub_input_dev);
 	if (ret < 0) {
 		pr_err("%s: could not register sensorhub input device(%d)",
 			__func__, ret);
-		input_free_device(data->sensorhub_input_dev);
+		input_free_device(hub_data->sensorhub_input_dev);
 		goto err_input_register_device_sensorhub;
 	}
 
-	data->sensorhub_input_dev->name = "ssp_context";
-	input_set_drvdata(data->sensorhub_input_dev, data);
-	input_set_capability(data->sensorhub_input_dev, EV_REL, REL_RX);
-	input_set_capability(data->sensorhub_input_dev, EV_REL, REL_RY);
+	hub_data->sensorhub_input_dev->name = "ssp_context";
+	input_set_drvdata(hub_data->sensorhub_input_dev, hub_data);
+	input_set_capability(hub_data->sensorhub_input_dev, EV_REL, REL_RX);
+	input_set_capability(hub_data->sensorhub_input_dev, EV_REL, REL_RY);
 
 	/* create sensorhub device node */
-	data->sensorhub_device.minor = MISC_DYNAMIC_MINOR;
-	data->sensorhub_device.name = "ssp_sensorhub";
-	data->sensorhub_device.fops = &ssp_sensorhub_fops;
+	hub_data->sensorhub_device.minor = MISC_DYNAMIC_MINOR;
+	hub_data->sensorhub_device.name = "ssp_sensorhub";
+	hub_data->sensorhub_device.fops = &ssp_sensorhub_fops;
 
-	ret = misc_register(&data->sensorhub_device);
+	ret = misc_register(&hub_data->sensorhub_device);
 	if (ret < 0) {
 		pr_err("%s: misc_register() failed", __func__);
 		goto err_misc_register;
 	}
 
-	data->sensorhub_task = kthread_run(ssp_senosrhub_thread_func,
-					(void *)data, "ssp_sensorhub_task");
-	if (IS_ERR(data->sensorhub_task)) {
-		ret = PTR_ERR(data->sensorhub_task);
+	hub_data->sensorhub_task = kthread_run(ssp_senosrhub_thread_func,
+					(void *)hub_data, "ssp_sensorhub_task");
+	if (IS_ERR(hub_data->sensorhub_task)) {
+		ret = PTR_ERR(hub_data->sensorhub_task);
 		goto err_kthread_create;
 	}
 
 	return 0;
 
 err_kthread_create:
-	misc_deregister(&data->sensorhub_device);
+	misc_deregister(&hub_data->sensorhub_device);
 err_misc_register:
-	input_unregister_device(data->sensorhub_input_dev);
+	input_unregister_device(hub_data->sensorhub_input_dev);
 err_input_register_device_sensorhub:
-	complete_all(&data->transfer_done);
-	wake_lock_destroy(&data->sensorhub_wake_lock);
+	complete_all(&hub_data->transfer_done);
+	wake_lock_destroy(&hub_data->sensorhub_wake_lock);
 err_input_allocate_device_sensorhub:
+	kfree(hub_data);
 	return ret;
 }
 
-void ssp_remove_sensorhub(struct ssp_data *data)
+void ssp_remove_sensorhub(struct ssp_data *ssp_data)
 {
-	complete_all(&data->transfer_done);
-	if (data->sensorhub_task)
-		kthread_stop(data->sensorhub_task);
-	misc_deregister(&data->sensorhub_device);
-	input_unregister_device(data->sensorhub_input_dev);
-	wake_lock_destroy(&data->sensorhub_wake_lock);
+	struct ssp_sensorhub_data *hub_data = ssp_data->hub_data;
+
+	ssp_sensorhub_fops.write = NULL;
+	ssp_sensorhub_fops.unlocked_ioctl = NULL;
+	misc_deregister(&hub_data->sensorhub_device);
+	input_unregister_device(hub_data->sensorhub_input_dev);
+	wake_lock_destroy(&hub_data->sensorhub_wake_lock);
+	complete_all(&hub_data->transfer_done);
+	if (hub_data->sensorhub_task)
+		kthread_stop(hub_data->sensorhub_task);
+	kfree(hub_data);
 }
 
 MODULE_DESCRIPTION("Samsung Sensor Platform(SSP) sensorhub driver");
diff --git a/drivers/sensorhub/ssp_sensorhub.h b/drivers/sensorhub/ssp_sensorhub.h
new file mode 100644
index 00000000000..4ececf6b6ec
--- /dev/null
+++ b/drivers/sensorhub/ssp_sensorhub.h
@@ -0,0 +1,68 @@
+/*
+ *  Copyright (C) 2012, Samsung Electronics Co. Ltd. All Rights Reserved.
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ */
+
+#ifndef __SSP_SENSORHUB__
+#define __SSP_SENSORHUB__
+
+#include <linux/completion.h>
+#include <linux/kthread.h>
+#include <linux/list.h>
+#include <linux/spinlock.h>
+#include "ssp.h"
+
+#define SUBCMD_GPIOWAKEUP	0X02
+#define SUBCMD_POWEREUP		0X04
+#define LIBRARY_MAX_NUM		5
+#define LIBRARY_MAX_TRY		32
+#define EVENT_WAIT_COUNT	3
+
+/* sensorhub ioctl command */
+#define SENSORHUB_IOCTL_MAGIC	'S'
+#define IOCTL_READ_CONTEXT_DATA	_IOR(SENSORHUB_IOCTL_MAGIC, 3, char *)
+
+
+struct sensorhub_event {
+	char *library_data;
+	int length;
+	struct list_head list;
+};
+
+struct ssp_sensorhub_data {
+	struct ssp_data *ssp_data;
+	struct input_dev *sensorhub_input_dev;
+	struct miscdevice sensorhub_device;
+	struct wake_lock sensorhub_wake_lock;
+	struct completion transfer_done;
+	struct task_struct *sensorhub_task;
+	struct sensorhub_event events_head;
+	struct sensorhub_event events[LIBRARY_MAX_NUM + 1];
+	struct sensorhub_event *first_event;
+	int event_number;
+	int transfer_try;
+	int transfer_ready;
+	int large_library_length;
+	char *large_library_data;
+	wait_queue_head_t sensorhub_waitqueue;
+	spinlock_t sensorhub_lock;
+};
+
+void ssp_report_sensorhub_notice(struct ssp_data *data, char notice);
+int ssp_handle_sensorhub_data(struct ssp_data *data, char *dataframe,
+				int start, int end);
+int ssp_handle_sensorhub_large_data(struct ssp_data *data, u8 sub_cmd);
+int ssp_initialize_sensorhub(struct ssp_data *data);
+void ssp_remove_sensorhub(struct ssp_data *data);
+
+#endif
diff --git a/drivers/sensorhub/ssp_sysfs.c b/drivers/sensorhub/ssp_sysfs.c
index e4021a7075c..0ce5ff96698 100644
--- a/drivers/sensorhub/ssp_sysfs.c
+++ b/drivers/sensorhub/ssp_sysfs.c
@@ -46,8 +46,14 @@ static void change_sensor_delay(struct ssp_data *data,
 	int iSensorType, int64_t dNewDelay)
 {
 	u8 uBuf[2];
+	unsigned int uNewEnable = 0;
 	int64_t dTempDelay = data->adDelayBuf[iSensorType];
 
+	if (!(atomic_read(&data->aSensorEnable) & (1 << iSensorType))) {
+		data->aiCheckStatus[iSensorType] = NO_SENSOR_STATE;
+		return;
+	}
+
 	data->adDelayBuf[iSensorType] = dNewDelay;
 
 	if (iSensorType == ORIENTATION_SENSOR)
@@ -60,7 +66,18 @@ static void change_sensor_delay(struct ssp_data *data,
 
 		uBuf[1] = (u8)get_msdelay(dNewDelay);
 		uBuf[0] = (u8)get_delay_cmd(uBuf[1]);
-		send_instruction(data, ADD_SENSOR, iSensorType, uBuf, 2);
+
+		if (send_instruction(data, ADD_SENSOR, iSensorType, uBuf, 2)
+			!= SUCCESS) {
+			uNewEnable =
+				(unsigned int)atomic_read(&data->aSensorEnable)
+				& (~(unsigned int)(1 << iSensorType));
+			atomic_set(&data->aSensorEnable, uNewEnable);
+
+			data->aiCheckStatus[iSensorType] = NO_SENSOR_STATE;
+			data->uMissSensorCnt++;
+			break;
+		}
 
 		data->aiCheckStatus[iSensorType] = RUNNING_SENSOR_STATE;
 
@@ -73,7 +90,8 @@ static void change_sensor_delay(struct ssp_data *data,
 		}
 		break;
 	case RUNNING_SENSOR_STATE:
-		if (dTempDelay == data->adDelayBuf[iSensorType])
+		if (get_msdelay(dTempDelay)
+			== get_msdelay(data->adDelayBuf[iSensorType]))
 			break;
 
 		ssp_dbg("[SSP]: %s - Change %u, New = %lldns\n",
@@ -82,6 +100,7 @@ static void change_sensor_delay(struct ssp_data *data,
 		uBuf[1] = (u8)get_msdelay(dNewDelay);
 		uBuf[0] = (u8)get_delay_cmd(uBuf[1]);
 		send_instruction(data, CHANGE_DELAY, iSensorType, uBuf, 2);
+
 		break;
 	default:
 		data->aiCheckStatus[iSensorType] = ADD_SENSOR_STATE;
@@ -150,10 +169,12 @@ static int ssp_remove_sensor(struct ssp_data *data,
 			data->bDebugEnabled = false;
 	}
 
-	uBuf[1] = (u8)get_msdelay(dSensorDelay);
-	uBuf[0] = (u8)get_delay_cmd(uBuf[1]);
+	if (atomic_read(&data->aSensorEnable) & (1 << uChangedSensor)) {
+		uBuf[1] = (u8)get_msdelay(dSensorDelay);
+		uBuf[0] = (u8)get_delay_cmd(uBuf[1]);
 
-	send_instruction(data, REMOVE_SENSOR, uChangedSensor, uBuf, 2);
+		send_instruction(data, REMOVE_SENSOR, uChangedSensor, uBuf, 2);
+	}
 	data->aiCheckStatus[uChangedSensor] = NO_SENSOR_STATE;
 	return 0;
 }
@@ -413,9 +434,12 @@ static struct device_attribute *mcu_attrs[] = {
 
 static void initialize_mcu_factorytest(struct ssp_data *data)
 {
-	struct device *mcu_device = NULL;
+	sensors_register(data->mcu_device, data, mcu_attrs, "ssp_sensor");
+}
 
-	sensors_register(mcu_device, data, mcu_attrs, "ssp_sensor");
+static void remove_mcu_factorytest(struct ssp_data *data)
+{
+	sensors_unregister(data->mcu_device, mcu_attrs);
 }
 
 int initialize_sysfs(struct ssp_data *data)
@@ -478,4 +502,13 @@ void remove_sysfs(struct ssp_data *data)
 		&dev_attr_light_poll_delay);
 	device_remove_file(&data->prox_input_dev->dev,
 		&dev_attr_prox_poll_delay);
+
+	remove_accel_factorytest(data);
+	remove_gyro_factorytest(data);
+	remove_prox_factorytest(data);
+	remove_light_factorytest(data);
+	remove_pressure_factorytest(data);
+	remove_magnetic_factorytest(data);
+	remove_mcu_factorytest(data);
+	destroy_sensor_class();
 }