first commit

77 files changed, 62890 insertions, 0 deletions

diff --git a/drivers/misc/mediatek/accelerometer/KXTJ2_1009-new/Makefile b/drivers/misc/mediatek/accelerometer/KXTJ2_1009-new/Makefile
new file mode 100644
index 000000000..e355a0d40
--- /dev/null
+++ b/drivers/misc/mediatek/accelerometer/KXTJ2_1009-new/Makefile
@@ -0,0 +1,4 @@
+include $(srctree)/drivers/misc/mediatek/Makefile.custom
+
+obj-y	:=  kxtj2_1009.o
+
diff --git a/drivers/misc/mediatek/accelerometer/KXTJ2_1009-new/kxtj2_1009.c b/drivers/misc/mediatek/accelerometer/KXTJ2_1009-new/kxtj2_1009.c
new file mode 100644
index 000000000..c9284f786
--- /dev/null
+++ b/drivers/misc/mediatek/accelerometer/KXTJ2_1009-new/kxtj2_1009.c
@@ -0,0 +1,2599 @@
+/* KXTJ2_1009 motion sensor driver
+ *
+ *
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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.
+ *
+ */
+
+#include <linux/interrupt.h>
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include <linux/irq.h>
+#include <linux/miscdevice.h>
+#include <asm/uaccess.h>
+#include <linux/delay.h>
+#include <linux/input.h>
+#include <linux/workqueue.h>
+#include <linux/kobject.h>
+#include <linux/earlysuspend.h>
+#include <linux/platform_device.h>
+#include <asm/atomic.h>
+
+#include <mach/mt_typedefs.h>
+#include <mach/mt_gpio.h>
+#include <mach/mt_pm_ldo.h>
+
+#include <accel.h>
+#include <linux/batch.h>
+#ifdef CUSTOM_KERNEL_SENSORHUB
+#include <SCP_sensorHub.h>
+#endif//#ifdef CUSTOM_KERNEL_SENSORHUB
+
+#define POWER_NONE_MACRO MT65XX_POWER_NONE
+
+
+#include <cust_acc.h>
+#include <linux/hwmsensor.h>
+#include <linux/hwmsen_dev.h>
+#include <linux/sensors_io.h>
+#include "kxtj2_1009.h"
+#include <linux/hwmsen_helper.h>
+/*----------------------------------------------------------------------------*/
+#define I2C_DRIVERID_KXTJ2_1009 150
+/*----------------------------------------------------------------------------*/
+#define DEBUG 1
+/*----------------------------------------------------------------------------*/
+//#define CONFIG_KXTJ2_1009_LOWPASS   /*apply low pass filter on output*/       
+#define SW_CALIBRATION
+//#define USE_EARLY_SUSPEND
+/*----------------------------------------------------------------------------*/
+#define KXTJ2_1009_AXIS_X          0
+#define KXTJ2_1009_AXIS_Y          1
+#define KXTJ2_1009_AXIS_Z          2
+#define KXTJ2_1009_DATA_LEN        6
+#define KXTJ2_1009_DEV_NAME        "KXTJ2_1009"
+/*----------------------------------------------------------------------------*/
+
+/*----------------------------------------------------------------------------*/
+static const struct i2c_device_id kxtj2_1009_i2c_id[] = {{KXTJ2_1009_DEV_NAME,0},{}};
+static struct i2c_board_info __initdata i2c_kxtj2_1009={ I2C_BOARD_INFO(KXTJ2_1009_DEV_NAME, (KXTJ2_1009_I2C_SLAVE_ADDR>>1))};
+/*the adapter id will be available in customization*/
+//static unsigned short kxtj2_1009_force[] = {0x00, KXTJ2_1009_I2C_SLAVE_ADDR, I2C_CLIENT_END, I2C_CLIENT_END};
+//static const unsigned short *const kxtj2_1009_forces[] = { kxtj2_1009_force, NULL };
+//static struct i2c_client_address_data kxtj2_1009_addr_data = { .forces = kxtj2_1009_forces,};
+
+/*----------------------------------------------------------------------------*/
+static int kxtj2_1009_i2c_probe(struct i2c_client *client, const struct i2c_device_id *id); 
+static int kxtj2_1009_i2c_remove(struct i2c_client *client);
+//static int kxtj2_1009_i2c_detect(struct i2c_client *client, int kind, struct i2c_board_info *info);
+static int kxtj2_1009_suspend(struct i2c_client *client, pm_message_t msg);
+static int kxtj2_1009_resume(struct i2c_client *client);
+
+static int kxtj2_1009_local_init(void);
+static int  kxtj2_1009_remove(void);
+
+#ifdef CUSTOM_KERNEL_SENSORHUB
+static int kxtj2_1009_setup_irq(void);
+#endif//#ifdef CUSTOM_KERNEL_SENSORHUB
+
+/*----------------------------------------------------------------------------*/
+typedef enum {
+    ADX_TRC_FILTER  = 0x01,
+    ADX_TRC_RAWDATA = 0x02,
+    ADX_TRC_IOCTL   = 0x04,
+    ADX_TRC_CALI	= 0X08,
+    ADX_TRC_INFO	= 0X10,
+} ADX_TRC;
+/*----------------------------------------------------------------------------*/
+struct scale_factor{
+    u8  whole;
+    u8  fraction;
+};
+/*----------------------------------------------------------------------------*/
+struct data_resolution {
+    struct scale_factor scalefactor;
+    int                 sensitivity;
+};
+/*----------------------------------------------------------------------------*/
+#define C_MAX_FIR_LENGTH (32)
+/*----------------------------------------------------------------------------*/
+struct data_filter {
+    s16 raw[C_MAX_FIR_LENGTH][KXTJ2_1009_AXES_NUM];
+    int sum[KXTJ2_1009_AXES_NUM];
+    int num;
+    int idx;
+};
+/*----------------------------------------------------------------------------*/
+struct kxtj2_1009_i2c_data {
+    struct i2c_client *client;
+    struct acc_hw *hw;
+    struct hwmsen_convert   cvt;
+#ifdef CUSTOM_KERNEL_SENSORHUB
+    struct work_struct	irq_work;
+#endif//#ifdef CUSTOM_KERNEL_SENSORHUB
+    
+    /*misc*/
+    struct data_resolution *reso;
+    atomic_t                trace;
+    atomic_t                suspend;
+    atomic_t                selftest;
+	atomic_t				filter;
+    s16                     cali_sw[KXTJ2_1009_AXES_NUM+1];
+
+    /*data*/
+    s8                      offset[KXTJ2_1009_AXES_NUM+1];  /*+1: for 4-byte alignment*/
+    s16                     data[KXTJ2_1009_AXES_NUM+1];
+
+#ifdef CUSTOM_KERNEL_SENSORHUB
+	int 					SCP_init_done;
+#endif//#ifdef CUSTOM_KERNEL_SENSORHUB
+
+#if defined(CONFIG_KXTJ2_1009_LOWPASS)
+    atomic_t                firlen;
+    atomic_t                fir_en;
+    struct data_filter      fir;
+#endif 
+    /*early suspend*/
+#if defined(CONFIG_HAS_EARLYSUSPEND) && defined(USE_EARLY_SUSPEND)
+    struct early_suspend    early_drv;
+#endif     
+};
+/*----------------------------------------------------------------------------*/
+static struct i2c_driver kxtj2_1009_i2c_driver = {
+    .driver = {
+//        .owner          = THIS_MODULE,
+        .name           = KXTJ2_1009_DEV_NAME,
+    },
+	.probe      		= kxtj2_1009_i2c_probe,
+	.remove    			= kxtj2_1009_i2c_remove,
+//	.detect				= kxtj2_1009_i2c_detect,
+#if !defined(CONFIG_HAS_EARLYSUSPEND) || !defined(USE_EARLY_SUSPEND)
+    .suspend            = kxtj2_1009_suspend,
+    .resume             = kxtj2_1009_resume,
+#endif
+	.id_table = kxtj2_1009_i2c_id,
+//	.address_data = &kxtj2_1009_addr_data,
+};
+
+/*----------------------------------------------------------------------------*/
+static struct i2c_client *kxtj2_1009_i2c_client = NULL;
+static struct kxtj2_1009_i2c_data *obj_i2c_data = NULL;
+static bool sensor_power = true;
+static GSENSOR_VECTOR3D gsensor_gain;
+static char selftestRes[8]= {0}; 
+static DEFINE_MUTEX(kxtj2_1009_mutex);
+static bool enable_status = false;
+
+static int kxtj2_1009_init_flag =-1; // 0<==>OK -1 <==> fail
+
+static struct acc_init_info kxtj2_1009_init_info = {
+		.name = "kxtj2_1009",
+		.init = kxtj2_1009_local_init,
+		.uninit = kxtj2_1009_remove,
+	
+};
+
+/*----------------------------------------------------------------------------*/
+#define GSE_TAG                  "[Gsensor] "
+#define GSE_FUN(f)               printk( GSE_TAG"%s\n", __FUNCTION__)
+#define GSE_ERR(fmt, args...)    printk(KERN_ERR GSE_TAG"%s %d : "fmt, __FUNCTION__, __LINE__, ##args)
+#define GSE_LOG(fmt, args...)    printk( GSE_TAG fmt, ##args)
+/*----------------------------------------------------------------------------*/
+static struct data_resolution kxtj2_1009_data_resolution[1] = {
+ /* combination by {FULL_RES,RANGE}*/
+    {{ 0, 9}, 1024}, // dataformat +/-2g  in 12-bit resolution;  { 3, 9} = 3.9 = (2*2*1000)/(2^12);  256 = (2^12)/(2*2)          
+};
+/*----------------------------------------------------------------------------*/
+static struct data_resolution kxtj2_1009_offset_resolution = {{15, 6}, 64};
+/*----------------------------------------------------------------------------*/
+static int KXTJ2_1009_SetPowerMode(struct i2c_client *client, bool enable);
+/*--------------------KXTJ2_1009 power control function----------------------------------*/
+static void KXTJ2_1009_power(struct acc_hw *hw, unsigned int on) 
+{
+	static unsigned int power_on = 0;
+
+	if(hw->power_id != POWER_NONE_MACRO)		// have externel LDO
+	{        
+		GSE_LOG("power %s\n", on ? "on" : "off");
+		if(power_on == on)	// power status not change
+		{
+			GSE_LOG("ignore power control: %d\n", on);
+		}
+		else if(on)	// power on
+		{
+			if(!hwPowerOn(hw->power_id, hw->power_vol, "KXTJ2_1009"))
+			{
+				GSE_ERR("power on fails!!\n");
+			}
+		}
+		else	// power off
+		{
+			if (!hwPowerDown(hw->power_id, "KXTJ2_1009"))
+			{
+				GSE_ERR("power off fail!!\n");
+			}			  
+		}
+	}
+	power_on = on;    
+}
+/*----------------------------------------------------------------------------*/
+
+/*----------------------------------------------------------------------------*/
+static int KXTJ2_1009_SetDataResolution(struct kxtj2_1009_i2c_data *obj)
+{
+	int err;
+	u8  databuf[2];
+	bool cur_sensor_power = sensor_power;
+
+	KXTJ2_1009_SetPowerMode(obj->client, false);
+
+	if(hwmsen_read_block(obj->client, KXTJ2_1009_REG_DATA_RESOLUTION, databuf, 0x01))
+	{
+		printk("kxtj2_1009 read Dataformat failt \n");
+		return KXTJ2_1009_ERR_I2C;
+	}
+
+	databuf[0] &= ~KXTJ2_1009_RANGE_DATA_RESOLUTION_MASK;
+	databuf[0] |= KXTJ2_1009_RANGE_DATA_RESOLUTION_MASK;//12bit
+	databuf[1] = databuf[0];
+	databuf[0] = KXTJ2_1009_REG_DATA_RESOLUTION;
+
+
+	err = i2c_master_send(obj->client, databuf, 0x2);
+
+	if(err <= 0)
+	{
+		return KXTJ2_1009_ERR_I2C;
+	}
+
+	KXTJ2_1009_SetPowerMode(obj->client, cur_sensor_power/*true*/);
+
+	//kxtj2_1009_data_resolution[0] has been set when initialize: +/-2g  in 8-bit resolution:  15.6 mg/LSB*/   
+	obj->reso = &kxtj2_1009_data_resolution[0];
+
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int KXTJ2_1009_ReadData(struct i2c_client *client, s16 data[KXTJ2_1009_AXES_NUM])
+{
+	struct kxtj2_1009_i2c_data *priv = i2c_get_clientdata(client);        
+    int err = 0;
+#if 0//ifdef CUSTOM_KERNEL_SENSORHUB
+    SCP_SENSOR_HUB_DATA req;
+    int len;
+#else//#ifdef CUSTOM_KERNEL_SENSORHUB
+	u8 addr = KXTJ2_1009_REG_DATAX0;
+	u8 buf[KXTJ2_1009_DATA_LEN] = {0};
+	int i;
+#endif//#ifdef CUSTOM_KERNEL_SENSORHUB
+
+#if 0//ifdef CUSTOM_KERNEL_SENSORHUB
+    req.get_data_req.sensorType = ID_ACCELEROMETER;
+    req.get_data_req.action = SENSOR_HUB_GET_DATA;
+    len = sizeof(req.get_data_req);
+    err = SCP_sensorHub_req_send(&req, &len, 1);
+    if (err)
+    {
+        GSE_ERR("SCP_sensorHub_req_send!\n");
+        return err;
+    }
+
+    if (ID_ACCELEROMETER != req.get_data_rsp.sensorType ||
+        SENSOR_HUB_GET_DATA != req.get_data_rsp.action ||
+        0 != req.get_data_rsp.errCode)
+    {
+        GSE_ERR("error : %d\n", req.get_data_rsp.errCode);
+        return req.get_data_rsp.errCode;
+    }
+
+    len -= offsetof(SCP_SENSOR_HUB_GET_DATA_RSP, int8_Data);
+
+    if (6 == len)
+    {
+        data[KXTJ2_1009_AXIS_X] = req.get_data_rsp.int16_Data[0];
+        data[KXTJ2_1009_AXIS_Y] = req.get_data_rsp.int16_Data[1];
+        data[KXTJ2_1009_AXIS_Z] = req.get_data_rsp.int16_Data[2];
+    }
+    else if (3 == len)
+    {
+        data[KXTJ2_1009_AXIS_X] = req.get_data_rsp.int8_Data[0];
+        data[KXTJ2_1009_AXIS_Y] = req.get_data_rsp.int8_Data[1];
+        data[KXTJ2_1009_AXIS_Z] = req.get_data_rsp.int8_Data[2];
+    }
+    else
+    {
+        GSE_ERR("data length fail : %d\n", len);
+    }
+
+    if(atomic_read(&priv->trace) & ADX_TRC_RAWDATA)
+	{
+        //show data
+	}
+#else//#ifdef CUSTOM_KERNEL_SENSORHUB
+	if(NULL == client)
+	{
+		err = -EINVAL;
+	}
+	else if((err = hwmsen_read_block(client, addr, buf, 0x06)) != 0)
+	{
+		GSE_ERR("error: %d\n", err);
+	}
+	else
+	{
+		data[KXTJ2_1009_AXIS_X] = (s16)((buf[KXTJ2_1009_AXIS_X*2] >> 4) |
+		         (buf[KXTJ2_1009_AXIS_X*2+1] << 4));
+		data[KXTJ2_1009_AXIS_Y] = (s16)((buf[KXTJ2_1009_AXIS_Y*2] >> 4) |
+		         (buf[KXTJ2_1009_AXIS_Y*2+1] << 4));
+		data[KXTJ2_1009_AXIS_Z] = (s16)((buf[KXTJ2_1009_AXIS_Z*2] >> 4) |
+		         (buf[KXTJ2_1009_AXIS_Z*2+1] << 4));
+
+		for(i=0;i<3;i++)				
+		{								//because the data is store in binary complement number formation in computer system
+			if ( data[i] == 0x0800 )	//so we want to calculate actual number here
+				data[i]= -2048;			//10bit resolution, 512= 2^(12-1)
+			else if ( data[i] & 0x0800 )//transfor format
+			{							//printk("data 0 step %x \n",data[i]);
+				data[i] -= 0x1; 		//printk("data 1 step %x \n",data[i]);
+				data[i] = ~data[i]; 	//printk("data 2 step %x \n",data[i]);
+				data[i] &= 0x07ff;		//printk("data 3 step %x \n\n",data[i]);
+				data[i] = -data[i]; 	
+			}
+		}	
+
+
+		if(atomic_read(&priv->trace) & ADX_TRC_RAWDATA)
+		{
+			GSE_LOG("[%08X %08X %08X] => [%5d %5d %5d]\n", data[KXTJ2_1009_AXIS_X], data[KXTJ2_1009_AXIS_Y], data[KXTJ2_1009_AXIS_Z],
+		                               data[KXTJ2_1009_AXIS_X], data[KXTJ2_1009_AXIS_Y], data[KXTJ2_1009_AXIS_Z]);
+		}
+#ifdef CONFIG_KXTJ2_1009_LOWPASS
+		if(atomic_read(&priv->filter))
+		{
+			if(atomic_read(&priv->fir_en) && !atomic_read(&priv->suspend))
+			{
+				int idx, firlen = atomic_read(&priv->firlen);   
+				if(priv->fir.num < firlen)
+				{                
+					priv->fir.raw[priv->fir.num][KXTJ2_1009_AXIS_X] = data[KXTJ2_1009_AXIS_X];
+					priv->fir.raw[priv->fir.num][KXTJ2_1009_AXIS_Y] = data[KXTJ2_1009_AXIS_Y];
+					priv->fir.raw[priv->fir.num][KXTJ2_1009_AXIS_Z] = data[KXTJ2_1009_AXIS_Z];
+					priv->fir.sum[KXTJ2_1009_AXIS_X] += data[KXTJ2_1009_AXIS_X];
+					priv->fir.sum[KXTJ2_1009_AXIS_Y] += data[KXTJ2_1009IK_AXIS_Y];
+					priv->fir.sum[KXTJ2_1009_AXIS_Z] += data[KXTJ2_1009_AXIS_Z];
+					if(atomic_read(&priv->trace) & ADX_TRC_FILTER)
+					{
+						GSE_LOG("add [%2d] [%5d %5d %5d] => [%5d %5d %5d]\n", priv->fir.num,
+							priv->fir.raw[priv->fir.num][KXTJ2_1009_AXIS_X], priv->fir.raw[priv->fir.num][KXTJ2_1009_AXIS_Y], priv->fir.raw[priv->fir.num][KXTJ2_1009_AXIS_Z],
+							priv->fir.sum[KXTJ2_1009_AXIS_X], priv->fir.sum[KXTJ2_1009_AXIS_Y], priv->fir.sum[KXTJ2_1009_AXIS_Z]);
+					}
+					priv->fir.num++;
+					priv->fir.idx++;
+				}
+				else
+				{
+					idx = priv->fir.idx % firlen;
+					priv->fir.sum[KXTJ2_1009_AXIS_X] -= priv->fir.raw[idx][KXTJ2_1009_AXIS_X];
+					priv->fir.sum[KXTJ2_1009_AXIS_Y] -= priv->fir.raw[idx][KXTJ2_1009_AXIS_Y];
+					priv->fir.sum[KXTJ2_1009_AXIS_Z] -= priv->fir.raw[idx][KXTJ2_1009_AXIS_Z];
+					priv->fir.raw[idx][KXTJ2_1009_AXIS_X] = data[KXTJ2_1009_AXIS_X];
+					priv->fir.raw[idx][KXTJ2_1009_AXIS_Y] = data[KXTJ2_1009_AXIS_Y];
+					priv->fir.raw[idx][KXTJ2_1009_AXIS_Z] = data[KXTJ2_1009_AXIS_Z];
+					priv->fir.sum[KXTJ2_1009_AXIS_X] += data[KXTJ2_1009_AXIS_X];
+					priv->fir.sum[KXTJ2_1009_AXIS_Y] += data[KXTJ2_1009_AXIS_Y];
+					priv->fir.sum[KXTJ2_1009_AXIS_Z] += data[KXTJ2_1009_AXIS_Z];
+					priv->fir.idx++;
+					data[KXTJ2_1009_AXIS_X] = priv->fir.sum[KXTJ2_1009_AXIS_X]/firlen;
+					data[KXTJ2_1009_AXIS_Y] = priv->fir.sum[KXTJ2_1009_AXIS_Y]/firlen;
+					data[KXTJ2_1009_AXIS_Z] = priv->fir.sum[KXTJ2_1009_AXIS_Z]/firlen;
+					if(atomic_read(&priv->trace) & ADX_TRC_FILTER)
+					{
+						GSE_LOG("add [%2d] [%5d %5d %5d] => [%5d %5d %5d] : [%5d %5d %5d]\n", idx,
+						priv->fir.raw[idx][KXTJ2_1009_AXIS_X], priv->fir.raw[idx][KXTJ2_1009_AXIS_Y], priv->fir.raw[idx][KXTJ2_1009_AXIS_Z],
+						priv->fir.sum[KXTJ2_1009_AXIS_X], priv->fir.sum[KXTJ2_1009_AXIS_Y], priv->fir.sum[KXTJ2_1009_AXIS_Z],
+						data[KXTJ2_1009_AXIS_X], data[KXTJ2_1009_AXIS_Y], data[KXTJ2_1009_AXIS_Z]);
+					}
+				}
+			}
+		}	
+#endif         
+	}
+#endif//#ifdef CUSTOM_KERNEL_SENSORHUB
+	return err;
+}
+/*----------------------------------------------------------------------------*/
+static int KXTJ2_1009_ReadOffset(struct i2c_client *client, s8 ofs[KXTJ2_1009_AXES_NUM])
+{    
+	int err = 0;
+
+	ofs[1]=ofs[2]=ofs[0]=0x00;
+
+	printk("offesx=%x, y=%x, z=%x",ofs[0],ofs[1],ofs[2]);
+	
+	return err;    
+}
+/*----------------------------------------------------------------------------*/
+static int KXTJ2_1009_ResetCalibration(struct i2c_client *client)
+{
+	struct kxtj2_1009_i2c_data *obj = i2c_get_clientdata(client);
+	int err = 0;
+#ifdef CUSTOM_KERNEL_SENSORHUB
+    SCP_SENSOR_HUB_DATA data;
+    KXTJ2_1009_CUST_DATA *pCustData;
+    unsigned int len;
+
+	if(0 != obj->SCP_init_done)
+	{
+	    pCustData = (KXTJ2_1009_CUST_DATA *)&data.set_cust_req.custData;
+
+	    data.set_cust_req.sensorType = ID_ACCELEROMETER;
+	    data.set_cust_req.action = SENSOR_HUB_SET_CUST;
+	    pCustData->resetCali.action = KXTJ2_1009_CUST_ACTION_RESET_CALI;
+	    len = offsetof(SCP_SENSOR_HUB_SET_CUST_REQ, custData) + sizeof(pCustData->resetCali);
+	    SCP_sensorHub_req_send(&data, &len, 1);
+	}
+#endif
+
+	memset(obj->cali_sw, 0x00, sizeof(obj->cali_sw));
+	memset(obj->offset, 0x00, sizeof(obj->offset));
+	return err;    
+}
+/*----------------------------------------------------------------------------*/
+static int KXTJ2_1009_ReadCalibration(struct i2c_client *client, int dat[KXTJ2_1009_AXES_NUM])
+{
+    struct kxtj2_1009_i2c_data *obj = i2c_get_clientdata(client);
+    int mul;
+
+	#ifdef SW_CALIBRATION
+		mul = 0;//only SW Calibration, disable HW Calibration
+	#else
+	    if ((err = KXTJ2_1009_ReadOffset(client, obj->offset))) {
+        GSE_ERR("read offset fail, %d\n", err);
+        return err;
+    	}    
+    	mul = obj->reso->sensitivity/kxtj2_1009_offset_resolution.sensitivity;
+	#endif
+
+    dat[obj->cvt.map[KXTJ2_1009_AXIS_X]] = obj->cvt.sign[KXTJ2_1009_AXIS_X]*(obj->offset[KXTJ2_1009_AXIS_X]*mul + obj->cali_sw[KXTJ2_1009_AXIS_X]);
+    dat[obj->cvt.map[KXTJ2_1009_AXIS_Y]] = obj->cvt.sign[KXTJ2_1009_AXIS_Y]*(obj->offset[KXTJ2_1009_AXIS_Y]*mul + obj->cali_sw[KXTJ2_1009_AXIS_Y]);
+    dat[obj->cvt.map[KXTJ2_1009_AXIS_Z]] = obj->cvt.sign[KXTJ2_1009_AXIS_Z]*(obj->offset[KXTJ2_1009_AXIS_Z]*mul + obj->cali_sw[KXTJ2_1009_AXIS_Z]);                        
+                                       
+    return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int KXTJ2_1009_ReadCalibrationEx(struct i2c_client *client, int act[KXTJ2_1009_AXES_NUM], int raw[KXTJ2_1009_AXES_NUM])
+{  
+	/*raw: the raw calibration data; act: the actual calibration data*/
+	struct kxtj2_1009_i2c_data *obj = i2c_get_clientdata(client);
+    #ifdef SW_CALIBRATION
+    #else
+	int err;
+    #endif
+	int mul;
+
+ 
+
+	#ifdef SW_CALIBRATION
+		mul = 0;//only SW Calibration, disable HW Calibration
+	#else
+		if(err = KXTJ2_1009_ReadOffset(client, obj->offset))
+		{
+			GSE_ERR("read offset fail, %d\n", err);
+			return err;
+		}   
+		mul = obj->reso->sensitivity/kxtj2_1009_offset_resolution.sensitivity;
+	#endif
+	
+	raw[KXTJ2_1009_AXIS_X] = obj->offset[KXTJ2_1009_AXIS_X]*mul + obj->cali_sw[KXTJ2_1009_AXIS_X];
+	raw[KXTJ2_1009_AXIS_Y] = obj->offset[KXTJ2_1009_AXIS_Y]*mul + obj->cali_sw[KXTJ2_1009_AXIS_Y];
+	raw[KXTJ2_1009_AXIS_Z] = obj->offset[KXTJ2_1009_AXIS_Z]*mul + obj->cali_sw[KXTJ2_1009_AXIS_Z];
+
+	act[obj->cvt.map[KXTJ2_1009_AXIS_X]] = obj->cvt.sign[KXTJ2_1009_AXIS_X]*raw[KXTJ2_1009_AXIS_X];
+	act[obj->cvt.map[KXTJ2_1009_AXIS_Y]] = obj->cvt.sign[KXTJ2_1009_AXIS_Y]*raw[KXTJ2_1009_AXIS_Y];
+	act[obj->cvt.map[KXTJ2_1009_AXIS_Z]] = obj->cvt.sign[KXTJ2_1009_AXIS_Z]*raw[KXTJ2_1009_AXIS_Z];                        
+	                       
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int KXTJ2_1009_WriteCalibration(struct i2c_client *client, int dat[KXTJ2_1009_AXES_NUM])
+{
+	struct kxtj2_1009_i2c_data *obj = i2c_get_clientdata(client);
+	int err;
+	int cali[KXTJ2_1009_AXES_NUM], raw[KXTJ2_1009_AXES_NUM];
+#ifdef CUSTOM_KERNEL_SENSORHUB
+    SCP_SENSOR_HUB_DATA data;
+    KXTJ2_1009_CUST_DATA *pCustData;
+    unsigned int len;
+#endif
+#ifdef SW_CALIBRATION
+#else
+	int lsb = kxtj2_1009_offset_resolution.sensitivity;
+	int divisor = obj->reso->sensitivity/lsb;
+#endif
+
+	if(0 != (err = KXTJ2_1009_ReadCalibrationEx(client, cali, raw)))	/*offset will be updated in obj->offset*/
+	{ 
+		GSE_ERR("read offset fail, %d\n", err);
+		return err;
+	}
+
+	GSE_LOG("OLDOFF: (%+3d %+3d %+3d): (%+3d %+3d %+3d) / (%+3d %+3d %+3d)\n", 
+		raw[KXTJ2_1009_AXIS_X], raw[KXTJ2_1009_AXIS_Y], raw[KXTJ2_1009_AXIS_Z],
+		obj->offset[KXTJ2_1009_AXIS_X], obj->offset[KXTJ2_1009_AXIS_Y], obj->offset[KXTJ2_1009_AXIS_Z],
+		obj->cali_sw[KXTJ2_1009_AXIS_X], obj->cali_sw[KXTJ2_1009_AXIS_Y], obj->cali_sw[KXTJ2_1009_AXIS_Z]);
+
+#ifdef CUSTOM_KERNEL_SENSORHUB
+    pCustData = (KXTJ2_1009_CUST_DATA *)data.set_cust_req.custData;
+    data.set_cust_req.sensorType = ID_ACCELEROMETER;
+    data.set_cust_req.action = SENSOR_HUB_SET_CUST;
+    pCustData->setCali.action = KXTJ2_1009_CUST_ACTION_SET_CALI;
+    pCustData->setCali.data[KXTJ2_1009_AXIS_X] = dat[KXTJ2_1009_AXIS_X];
+    pCustData->setCali.data[KXTJ2_1009_AXIS_Y] = dat[KXTJ2_1009_AXIS_Y];
+    pCustData->setCali.data[KXTJ2_1009_AXIS_Z] = dat[KXTJ2_1009_AXIS_Z];
+    len = offsetof(SCP_SENSOR_HUB_SET_CUST_REQ, custData) + sizeof(pCustData->setCali);
+    SCP_sensorHub_req_send(&data, &len, 1);
+#endif
+
+	/*calculate the real offset expected by caller*/
+	cali[KXTJ2_1009_AXIS_X] += dat[KXTJ2_1009_AXIS_X];
+	cali[KXTJ2_1009_AXIS_Y] += dat[KXTJ2_1009_AXIS_Y];
+	cali[KXTJ2_1009_AXIS_Z] += dat[KXTJ2_1009_AXIS_Z];
+
+	GSE_LOG("UPDATE: (%+3d %+3d %+3d)\n", 
+		dat[KXTJ2_1009_AXIS_X], dat[KXTJ2_1009_AXIS_Y], dat[KXTJ2_1009_AXIS_Z]);
+
+#ifdef SW_CALIBRATION
+	obj->cali_sw[KXTJ2_1009_AXIS_X] = obj->cvt.sign[KXTJ2_1009_AXIS_X]*(cali[obj->cvt.map[KXTJ2_1009_AXIS_X]]);
+	obj->cali_sw[KXTJ2_1009_AXIS_Y] = obj->cvt.sign[KXTJ2_1009_AXIS_Y]*(cali[obj->cvt.map[KXTJ2_1009_AXIS_Y]]);
+	obj->cali_sw[KXTJ2_1009_AXIS_Z] = obj->cvt.sign[KXTJ2_1009_AXIS_Z]*(cali[obj->cvt.map[KXTJ2_1009_AXIS_Z]]);	
+#else
+	obj->offset[KXTJ2_1009_AXIS_X] = (s8)(obj->cvt.sign[KXTJ2_1009_AXIS_X]*(cali[obj->cvt.map[KXTJ2_1009_AXIS_X]])/(divisor));
+	obj->offset[KXTJ2_1009_AXIS_Y] = (s8)(obj->cvt.sign[KXTJ2_1009_AXIS_Y]*(cali[obj->cvt.map[KXTJ2_1009_AXIS_Y]])/(divisor));
+	obj->offset[KXTJ2_1009_AXIS_Z] = (s8)(obj->cvt.sign[KXTJ2_1009_AXIS_Z]*(cali[obj->cvt.map[KXTJ2_1009_AXIS_Z]])/(divisor));
+
+	/*convert software calibration using standard calibration*/
+	obj->cali_sw[KXTJ2_1009_AXIS_X] = obj->cvt.sign[KXTJ2_1009_AXIS_X]*(cali[obj->cvt.map[KXTJ2_1009_AXIS_X]])%(divisor);
+	obj->cali_sw[KXTJ2_1009_AXIS_Y] = obj->cvt.sign[KXTJ2_1009_AXIS_Y]*(cali[obj->cvt.map[KXTJ2_1009_AXIS_Y]])%(divisor);
+	obj->cali_sw[KXTJ2_1009_AXIS_Z] = obj->cvt.sign[KXTJ2_1009_AXIS_Z]*(cali[obj->cvt.map[KXTJ2_1009_AXIS_Z]])%(divisor);
+
+	GSE_LOG("NEWOFF: (%+3d %+3d %+3d): (%+3d %+3d %+3d) / (%+3d %+3d %+3d)\n", 
+		obj->offset[KXTJ2_1009_AXIS_X]*divisor + obj->cali_sw[KXTJ2_1009_AXIS_X], 
+		obj->offset[KXTJ2_1009_AXIS_Y]*divisor + obj->cali_sw[KXTJ2_1009_AXIS_Y], 
+		obj->offset[KXTJ2_1009_AXIS_Z]*divisor + obj->cali_sw[KXTJ2_1009_AXIS_Z], 
+		obj->offset[KXTJ2_1009_AXIS_X], obj->offset[KXTJ2_1009_AXIS_Y], obj->offset[KXTJ2_1009_AXIS_Z],
+		obj->cali_sw[KXTJ2_1009_AXIS_X], obj->cali_sw[KXTJ2_1009_AXIS_Y], obj->cali_sw[KXTJ2_1009_AXIS_Z]);
+
+	if(err = hwmsen_write_block(obj->client, KXTJ2_1009_REG_OFSX, obj->offset, KXTJ2_1009_AXES_NUM))
+	{
+		GSE_ERR("write offset fail: %d\n", err);
+		return err;
+	}
+#endif
+
+	return err;
+}
+/*----------------------------------------------------------------------------*/
+static int KXTJ2_1009_CheckDeviceID(struct i2c_client *client)
+{
+	u8 databuf[10];    
+	int res = 0;
+
+	memset(databuf, 0, sizeof(u8)*10);    
+	databuf[0] = KXTJ2_1009_REG_DEVID;   
+
+	res = i2c_master_send(client, databuf, 0x1);
+	if(res <= 0)
+	{
+		goto exit_KXTJ2_1009_CheckDeviceID;
+	}
+	
+	udelay(500);
+
+	databuf[0] = 0x0;        
+	res = i2c_master_recv(client, databuf, 0x01);
+	if(res <= 0)
+	{
+		goto exit_KXTJ2_1009_CheckDeviceID;
+	}
+	
+
+	if(false)
+	{
+		printk("KXTJ2_1009_CheckDeviceID 0x%x failt!\n ", databuf[0]);
+		return KXTJ2_1009_ERR_IDENTIFICATION;
+	}
+	else
+	{
+		printk("KXTJ2_1009_CheckDeviceID 0x%x pass!\n ", databuf[0]);
+	}
+	
+	exit_KXTJ2_1009_CheckDeviceID:
+	if (res <= 0)
+	{
+		return KXTJ2_1009_ERR_I2C;
+	}
+	
+	return KXTJ2_1009_SUCCESS;
+}
+/*----------------------------------------------------------------------------*/
+#ifdef CUSTOM_KERNEL_SENSORHUB
+static int KXTJ2_1009_SCP_SetPowerMode(bool enable)
+{
+	int res = 0;
+    SCP_SENSOR_HUB_DATA req;
+    int len;
+	
+	if(enable == sensor_power)
+	{
+		GSE_LOG("Sensor power status is newest!\n");
+		return KXTJ2_1009_SUCCESS;
+	}
+
+    req.activate_req.sensorType = ID_ACCELEROMETER;
+    req.activate_req.action = SENSOR_HUB_ACTIVATE;
+    req.activate_req.enable = enable;
+    len = sizeof(req.activate_req);
+    res = SCP_sensorHub_req_send(&req, &len, 1);
+    if (res)
+    {
+        GSE_ERR("SCP_sensorHub_req_send!\n");
+        return res;
+    }
+
+	GSE_LOG("KXTJ2_1009_SetPowerMode %d!\n ",enable);
+
+
+	sensor_power = enable;
+
+	mdelay(5);
+	
+	return KXTJ2_1009_SUCCESS;    
+}
+#endif
+/*----------------------------------------------------------------------------*/
+static int KXTJ2_1009_SetPowerMode(struct i2c_client *client, bool enable)
+{
+	int res = 0;
+    u8 databuf[2];
+	u8 addr = KXTJ2_1009_REG_POWER_CTL;
+	
+	if(enable == sensor_power)
+	{
+		GSE_LOG("Sensor power status is newest!\n");
+		return KXTJ2_1009_SUCCESS;
+	}
+
+	if(hwmsen_read_block(client, addr, databuf, 0x01))
+	{
+		GSE_ERR("read power ctl register err!\n");
+		return KXTJ2_1009_ERR_I2C;
+	}
+
+	
+	if(enable == TRUE)
+	{
+		databuf[0] |= KXTJ2_1009_MEASURE_MODE;
+	}
+	else
+	{
+		databuf[0] &= ~KXTJ2_1009_MEASURE_MODE;
+	}
+	databuf[1] = databuf[0];
+	databuf[0] = KXTJ2_1009_REG_POWER_CTL;
+	
+
+	res = i2c_master_send(client, databuf, 0x2);
+
+	if(res <= 0)
+	{
+		return KXTJ2_1009_ERR_I2C;
+	}
+
+	GSE_LOG("KXTJ2_1009_SetPowerMode %d!\n ",enable);
+
+
+	sensor_power = enable;
+
+	mdelay(5);
+	
+	return KXTJ2_1009_SUCCESS;    
+}
+/*----------------------------------------------------------------------------*/
+static int KXTJ2_1009_SetDataFormat(struct i2c_client *client, u8 dataformat)
+{
+	struct kxtj2_1009_i2c_data *obj = i2c_get_clientdata(client);
+	u8 databuf[10];    
+	int res = 0;
+	bool cur_sensor_power = sensor_power;
+
+	memset(databuf, 0, sizeof(u8)*10);  
+
+	KXTJ2_1009_SetPowerMode(client, false);
+
+	if(hwmsen_read_block(client, KXTJ2_1009_REG_DATA_FORMAT, databuf, 0x01))
+	{
+		printk("kxtj2_1009 read Dataformat failt \n");
+		return KXTJ2_1009_ERR_I2C;
+	}
+
+	databuf[0] &= ~KXTJ2_1009_RANGE_MASK;
+	databuf[0] |= dataformat;
+	databuf[1] = databuf[0];
+	databuf[0] = KXTJ2_1009_REG_DATA_FORMAT;
+
+
+	res = i2c_master_send(client, databuf, 0x2);
+
+	if(res <= 0)
+	{
+		return KXTJ2_1009_ERR_I2C;
+	}
+
+	KXTJ2_1009_SetPowerMode(client, cur_sensor_power/*true*/);
+	
+	printk("KXTJ2_1009_SetDataFormat OK! \n");
+	
+
+	return KXTJ2_1009_SetDataResolution(obj);    
+}
+/*----------------------------------------------------------------------------*/
+static int KXTJ2_1009_SetBWRate(struct i2c_client *client, u8 bwrate)
+{
+	u8 databuf[10];    
+	int res = 0;
+	bool cur_sensor_power = sensor_power;
+
+	memset(databuf, 0, sizeof(u8)*10);    
+
+	
+	KXTJ2_1009_SetPowerMode(client, false);
+
+	if(hwmsen_read_block(client, KXTJ2_1009_REG_BW_RATE, databuf, 0x01))
+	{
+		printk("kxtj2_1009 read rate failt \n");
+		return KXTJ2_1009_ERR_I2C;
+	}
+
+	databuf[0] &= 0xf0;
+	databuf[0] |= bwrate;
+	databuf[1] = databuf[0];
+	databuf[0] = KXTJ2_1009_REG_BW_RATE;
+
+
+	res = i2c_master_send(client, databuf, 0x2);
+
+	if(res <= 0)
+	{
+		return KXTJ2_1009_ERR_I2C;
+	}
+
+	
+	KXTJ2_1009_SetPowerMode(client, cur_sensor_power/*true*/);
+	printk("KXTJ2_1009_SetBWRate OK! \n");
+	
+	return KXTJ2_1009_SUCCESS;    
+}
+/*----------------------------------------------------------------------------*/
+static int KXTJ2_1009_SetIntEnable(struct i2c_client *client, u8 intenable)
+{
+	u8 databuf[10];    
+	int res = 0;
+
+	memset(databuf, 0, sizeof(u8)*10);    
+	databuf[0] = KXTJ2_1009_REG_INT_ENABLE;    
+	databuf[1] = 0x00;
+
+	res = i2c_master_send(client, databuf, 0x2);
+
+	if(res <= 0)
+	{
+		return KXTJ2_1009_ERR_I2C;
+	}
+	
+	return KXTJ2_1009_SUCCESS;    
+}
+/*----------------------------------------------------------------------------*/
+static int kxtj2_1009_init_client(struct i2c_client *client, int reset_cali)
+{
+	struct kxtj2_1009_i2c_data *obj = i2c_get_clientdata(client);
+	int res = 0;
+
+	res = KXTJ2_1009_CheckDeviceID(client); 
+	if(res != KXTJ2_1009_SUCCESS)
+	{
+		return res;
+	}	
+
+	res = KXTJ2_1009_SetPowerMode(client, enable_status/*false*/);
+	if(res != KXTJ2_1009_SUCCESS)
+	{
+		return res;
+	}
+	
+
+	res = KXTJ2_1009_SetBWRate(client, KXTJ2_1009_BW_100HZ);
+	if(res != KXTJ2_1009_SUCCESS ) //0x2C->BW=100Hz
+	{
+		return res;
+	}
+
+	res = KXTJ2_1009_SetDataFormat(client, KXTJ2_1009_RANGE_2G);
+	if(res != KXTJ2_1009_SUCCESS) //0x2C->BW=100Hz
+	{
+		return res;
+	}
+
+	gsensor_gain.x = gsensor_gain.y = gsensor_gain.z = obj->reso->sensitivity;
+
+#ifdef CUSTOM_KERNEL_SENSORHUB
+    res = kxtj2_1009_setup_irq();
+    if(res != KXTJ2_1009_SUCCESS)
+	{
+		return res;
+	}
+#endif//#ifdef CUSTOM_KERNEL_SENSORHUB
+
+	res = KXTJ2_1009_SetIntEnable(client, 0x00);        
+	if(res != KXTJ2_1009_SUCCESS)//0x2E->0x80
+	{
+		return res;
+	}
+
+	if(0 != reset_cali)
+	{ 
+		/*reset calibration only in power on*/
+		res = KXTJ2_1009_ResetCalibration(client);
+		if(res != KXTJ2_1009_SUCCESS)
+		{
+			return res;
+		}
+	}
+	printk("kxtj2_1009_init_client OK!\n");
+#ifdef CONFIG_KXTJ2_1009_LOWPASS
+	memset(&obj->fir, 0x00, sizeof(obj->fir));  
+#endif
+
+	return KXTJ2_1009_SUCCESS;
+}
+/*----------------------------------------------------------------------------*/
+static int KXTJ2_1009_ReadChipInfo(struct i2c_client *client, char *buf, int bufsize)
+{
+	u8 databuf[10];    
+
+	memset(databuf, 0, sizeof(u8)*10);
+
+	if((NULL == buf)||(bufsize<=30))
+	{
+		return -1;
+	}
+	
+	if(NULL == client)
+	{
+		*buf = 0;
+		return -2;
+	}
+
+	sprintf(buf, "KXTJ2_1009 Chip");
+	return 0;
+}
+
+/*Kionix Auto-Cali Start*/
+#define KIONIX_AUTO_CAL     //Setup AUTO-Cali parameter
+#ifdef KIONIX_AUTO_CAL
+//#define DEBUG_MSG_CAL
+#define Sensitivity_def      1024	//	
+#define Detection_range   200 	// Follow KXTJ2 SPEC Offset Range define
+#define Stable_range        50     	// Stable iteration
+#define BUF_RANGE_Limit 10 	
+static int BUF_RANGE = BUF_RANGE_Limit;			
+static int temp_zbuf[50]={0};
+static int temp_zsum = 0; // 1024 * BUF_RANGE ;
+static int Z_AVG[2] = {Sensitivity_def,Sensitivity_def} ;
+static int Wave_Max,Wave_Min;
+#endif
+/*Kionix Auto-Cali End*/
+
+/*----------------------------------------------------------------------------*/
+static int KXTJ2_1009_ReadSensorData(struct i2c_client *client, char *buf, int bufsize)
+{
+	struct kxtj2_1009_i2c_data *obj = (struct kxtj2_1009_i2c_data*)i2c_get_clientdata(client);
+	u8 databuf[20];
+	int acc[KXTJ2_1009_AXES_NUM];
+	int res = 0;
+/*Kionix Auto-Cali Start*/
+#ifdef KIONIX_AUTO_CAL
+    s16 raw[3];
+    int k;
+#endif    
+/*Kionix Auto-Cali End*/
+
+	memset(databuf, 0, sizeof(u8)*10);
+
+	if(NULL == buf)
+	{
+		return -1;
+	}
+	if(NULL == client)
+	{
+		*buf = 0;
+		return -2;
+	}
+
+	if (atomic_read(&obj->suspend))
+	{
+		return 0;
+	}
+	/*if(sensor_power == FALSE)
+	{
+		res = KXTJ2_1009_SetPowerMode(client, true);
+		if(res)
+		{
+			GSE_ERR("Power on kxtj2_1009 error %d!\n", res);
+		}
+	}*/
+
+	if(0 != (res = KXTJ2_1009_ReadData(client, obj->data)))
+	{        
+		GSE_ERR("I2C error: ret value=%d", res);
+		return -3;
+	}
+	else
+	{
+#if 0//ifdef CUSTOM_KERNEL_SENSORHUB
+        acc[KXTJ2_1009_AXIS_X] = obj->data[KXTJ2_1009_AXIS_X];
+		acc[KXTJ2_1009_AXIS_Y] = obj->data[KXTJ2_1009_AXIS_Y];
+		acc[KXTJ2_1009_AXIS_Z] = obj->data[KXTJ2_1009_AXIS_Z];		
+#else
+
+/*Kionix Auto-Cali Start*/
+#ifdef KIONIX_AUTO_CAL
+        raw[0]=obj->data[KXTJ2_1009_AXIS_X];
+        raw[1]=obj->data[KXTJ2_1009_AXIS_Y];
+        raw[2]=obj->data[KXTJ2_1009_AXIS_Z];
+
+        if(     (abs(raw[0]) < Detection_range)  
+            &&  (abs(raw[1]) < Detection_range) 
+            &&  (abs((abs(raw[2])- Sensitivity_def))  < ((Detection_range)+ 308)))
+        {
+
+            #ifdef DEBUG_MSG_CAL
+            printk("+++KXTJ2 Calibration Raw Data,%d,%d,%d\n",raw[0],raw[1],raw[2]);
+            #endif
+            temp_zsum = 0;
+            Wave_Max =-4095;
+            Wave_Min = 4095;
+            
+            // BUF_RANGE = 1000 / acc_data.delay; **************************88 
+            //BUF_RANGE = 1000 / acceld->poll_interval ; 
+            
+            if ( BUF_RANGE > BUF_RANGE_Limit ) BUF_RANGE = BUF_RANGE_Limit; 
+                
+            //k printk("KXTJ2 Buffer Range =%d\n",BUF_RANGE);
+            
+            for (k=0; k < BUF_RANGE-1; k++) {
+                temp_zbuf[k] = temp_zbuf[k+1];
+                if (temp_zbuf[k] == 0) temp_zbuf[k] = Sensitivity_def ;
+                temp_zsum += temp_zbuf[k];
+                if (temp_zbuf[k] > Wave_Max) Wave_Max = temp_zbuf[k];
+                if (temp_zbuf[k] < Wave_Min) Wave_Min = temp_zbuf[k];
+            }
+
+            temp_zbuf[k] = raw[2]; // k=BUF_RANGE-1, update Z raw to bubber
+            temp_zsum += temp_zbuf[k];
+            if (temp_zbuf[k] > Wave_Max) Wave_Max = temp_zbuf[k];
+            if (temp_zbuf[k] < Wave_Min) Wave_Min = temp_zbuf[k];      
+            if (Wave_Max-Wave_Min < Stable_range )
+            {
+                
+                if ( temp_zsum > 0)
+                {
+                    Z_AVG[0] = temp_zsum / BUF_RANGE;
+                    //k
+    		        #ifdef DEBUG_MSG_CAL
+                    printk("+++ Z_AVG=%d\n ", Z_AVG[0]);
+                    #endif
+                }
+                else 
+                {
+                    Z_AVG[1] = temp_zsum / BUF_RANGE;
+                    //k 
+		            #ifdef DEBUG_MSG_CAL
+                    printk("--- Z_AVG=%d\n ", Z_AVG[1]);
+                    #endif
+                }
+                // printk("KXTJ2 start Z compensation Z_AVG Max Min,%d,%d,%d\n",(temp_zsum / BUF_RANGE),Wave_Max,Wave_Min);
+            }
+        }
+        else if(abs((abs(raw[2])- Sensitivity_def))  > ((Detection_range)+ 154))
+        {
+            #ifdef DEBUG_MSG_CAL
+            printk("KXTJ2 out of SPEC Raw Data,%d,%d,%d\n",raw[0],raw[1],raw[2]);
+            #endif
+        }
+        //else
+        //{
+        //    printk("KXTJ2 not in horizontal X=%d, Y=%d\n", raw[0], raw[1]);
+        //}
+
+        if ( raw[2] >=0) 
+            raw[2] = raw[2] * 1024 / abs(Z_AVG[0]); // Gain Compensation
+        else 
+            raw[2] = raw[2] * 1024 / abs(Z_AVG[1]); // Gain Compensation
+                
+        //k
+        #ifdef DEBUG_MSG_CAL
+        //printk("---KXTJ2 Calibration Raw Data,%d,%d,%d==> Z+=%d  Z-=%d \n",raw[0],raw[1],raw[2],Z_AVG[0],Z_AVG[1]);
+        printk("---After Cali,X=%d,Y=%d,Z=%d \n",raw[0],raw[1],raw[2]);
+        #endif
+        obj->data[KXTJ2_1009_AXIS_X]=raw[0];
+        obj->data[KXTJ2_1009_AXIS_Y]=raw[1];
+        obj->data[KXTJ2_1009_AXIS_Z]=raw[2];
+#endif
+/*Kionix Auto-Cali End*/
+
+		//printk("raw data x=%d, y=%d, z=%d \n",obj->data[KXTJ2_1009_AXIS_X],obj->data[KXTJ2_1009_AXIS_Y],obj->data[KXTJ2_1009_AXIS_Z]);
+		obj->data[KXTJ2_1009_AXIS_X] += obj->cali_sw[KXTJ2_1009_AXIS_X];
+		obj->data[KXTJ2_1009_AXIS_Y] += obj->cali_sw[KXTJ2_1009_AXIS_Y];
+		obj->data[KXTJ2_1009_AXIS_Z] += obj->cali_sw[KXTJ2_1009_AXIS_Z];
+		
+		//printk("cali_sw x=%d, y=%d, z=%d \n",obj->cali_sw[KXTJ2_1009_AXIS_X],obj->cali_sw[KXTJ2_1009_AXIS_Y],obj->cali_sw[KXTJ2_1009_AXIS_Z]);
+		
+		/*remap coordinate*/
+		acc[obj->cvt.map[KXTJ2_1009_AXIS_X]] = obj->cvt.sign[KXTJ2_1009_AXIS_X]*obj->data[KXTJ2_1009_AXIS_X];
+		acc[obj->cvt.map[KXTJ2_1009_AXIS_Y]] = obj->cvt.sign[KXTJ2_1009_AXIS_Y]*obj->data[KXTJ2_1009_AXIS_Y];
+		acc[obj->cvt.map[KXTJ2_1009_AXIS_Z]] = obj->cvt.sign[KXTJ2_1009_AXIS_Z]*obj->data[KXTJ2_1009_AXIS_Z];
+		//printk("cvt x=%d, y=%d, z=%d \n",obj->cvt.sign[KXTJ2_1009_AXIS_X],obj->cvt.sign[KXTJ2_1009_AXIS_Y],obj->cvt.sign[KXTJ2_1009_AXIS_Z]);
+
+
+		//GSE_LOG("Mapped gsensor data: %d, %d, %d!\n", acc[KXTJ2_1009_AXIS_X], acc[KXTJ2_1009_AXIS_Y], acc[KXTJ2_1009_AXIS_Z]);
+
+		//Out put the mg
+		//printk("mg acc=%d, GRAVITY=%d, sensityvity=%d \n",acc[KXTJ2_1009_AXIS_X],GRAVITY_EARTH_1000,obj->reso->sensitivity);
+		acc[KXTJ2_1009_AXIS_X] = acc[KXTJ2_1009_AXIS_X] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+		acc[KXTJ2_1009_AXIS_Y] = acc[KXTJ2_1009_AXIS_Y] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+		acc[KXTJ2_1009_AXIS_Z] = acc[KXTJ2_1009_AXIS_Z] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;		
+#endif	
+
+		sprintf(buf, "%04x %04x %04x", acc[KXTJ2_1009_AXIS_X], acc[KXTJ2_1009_AXIS_Y], acc[KXTJ2_1009_AXIS_Z]);
+		if(atomic_read(&obj->trace) & ADX_TRC_IOCTL)
+		{
+			GSE_LOG("gsensor data: %s!\n", buf);
+		}
+	}
+	
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int KXTJ2_1009_ReadRawData(struct i2c_client *client, char *buf)
+{
+	struct kxtj2_1009_i2c_data *obj = (struct kxtj2_1009_i2c_data*)i2c_get_clientdata(client);
+	int res = 0;
+
+	if (!buf || !client)
+	{
+		return EINVAL;
+	}
+	
+	if(0 != (res = KXTJ2_1009_ReadData(client, obj->data)))
+	{        
+		GSE_ERR("I2C error: ret value=%d", res);
+		return EIO;
+	}
+	else
+	{
+		sprintf(buf, "KXTJ2_1009_ReadRawData %04x %04x %04x", obj->data[KXTJ2_1009_AXIS_X], 
+			obj->data[KXTJ2_1009_AXIS_Y], obj->data[KXTJ2_1009_AXIS_Z]);
+	
+	}
+	
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int KXTJ2_1009_InitSelfTest(struct i2c_client *client)
+{
+	int res = 0;
+	u8  data,result;
+	
+	res = hwmsen_read_byte(client, KXTJ2_1009_REG_CTL_REG3, &data);
+	if(res != KXTJ2_1009_SUCCESS)
+	{
+		return res;
+	}
+//enable selftest bit
+	res = hwmsen_write_byte(client, KXTJ2_1009_REG_CTL_REG3,  KXTJ2_1009_SELF_TEST|data);
+	if(res != KXTJ2_1009_SUCCESS) //0x2C->BW=100Hz
+	{
+		return res;
+	}
+//step 1
+	res = hwmsen_read_byte(client, KXTJ2_1009_DCST_RESP, &result);
+	if(res != KXTJ2_1009_SUCCESS)
+	{
+		return res;
+	}
+	printk("step1: result = %x",result);
+	if(result != 0xaa)
+		return -EINVAL;
+
+//step 2
+	res = hwmsen_write_byte(client, KXTJ2_1009_REG_CTL_REG3,  KXTJ2_1009_SELF_TEST|data);
+	if(res != KXTJ2_1009_SUCCESS) //0x2C->BW=100Hz
+	{
+		return res;
+	}
+//step 3
+	res = hwmsen_read_byte(client, KXTJ2_1009_DCST_RESP, &result);
+	if(res != KXTJ2_1009_SUCCESS)
+	{
+		return res;
+	}
+	printk("step3: result = %x",result);
+	if(result != 0xAA)
+		return -EINVAL;
+		
+//step 4
+	res = hwmsen_read_byte(client, KXTJ2_1009_DCST_RESP, &result);
+	if(res != KXTJ2_1009_SUCCESS)
+	{
+		return res;
+	}
+	printk("step4: result = %x",result);
+	if(result != 0x55)
+		return -EINVAL;
+	else
+		return KXTJ2_1009_SUCCESS;
+}
+/*----------------------------------------------------------------------------*/
+#if 0
+static int KXTJ2_1009_JudgeTestResult(struct i2c_client *client, s32 prv[KXTJ2_1009_AXES_NUM], s32 nxt[KXTJ2_1009_AXES_NUM])
+{
+
+    int res=0;
+	u8 test_result=0;
+    if(0 != (res = hwmsen_read_byte(client, 0x0c, &test_result)))
+        return res;
+
+	printk("test_result = %x \n",test_result);
+    if ( test_result != 0xaa ) 
+	{
+        GSE_ERR("KXTJ2_1009_JudgeTestResult failt\n");
+        res = -EINVAL;
+    }
+    return res;
+}
+#endif
+/*----------------------------------------------------------------------------*/
+static ssize_t show_chipinfo_value(struct device_driver *ddri, char *buf)
+{
+	struct i2c_client *client = kxtj2_1009_i2c_client;
+	char strbuf[KXTJ2_1009_BUFSIZE];
+	if(NULL == client)
+	{
+		GSE_ERR("i2c client is null!!\n");
+		return 0;
+	}
+	
+	KXTJ2_1009_ReadChipInfo(client, strbuf, KXTJ2_1009_BUFSIZE);
+	return snprintf(buf, PAGE_SIZE, "%s\n", strbuf);        
+}
+#if 0
+static ssize_t gsensor_init(struct device_driver *ddri, char *buf, size_t count)
+	{
+		struct i2c_client *client = kxtj2_1009_i2c_client;
+		char strbuf[KXTJ2_1009_BUFSIZE];
+		
+		if(NULL == client)
+		{
+			GSE_ERR("i2c client is null!!\n");
+			return 0;
+		}
+		kxtj2_1009_init_client(client, 1);
+		return snprintf(buf, PAGE_SIZE, "%s\n", strbuf);			
+	}
+#endif
+/*----------------------------------------------------------------------------*/
+static ssize_t show_sensordata_value(struct device_driver *ddri, char *buf)
+{
+	struct i2c_client *client = kxtj2_1009_i2c_client;
+	char strbuf[KXTJ2_1009_BUFSIZE];
+	
+	if(NULL == client)
+	{
+		GSE_ERR("i2c client is null!!\n");
+		return 0;
+	}
+	KXTJ2_1009_ReadSensorData(client, strbuf, KXTJ2_1009_BUFSIZE);
+	//KXTJ2_1009_ReadRawData(client, strbuf);
+	return snprintf(buf, PAGE_SIZE, "%s\n", strbuf);            
+}
+#if 0
+static ssize_t show_sensorrawdata_value(struct device_driver *ddri, char *buf, size_t count)
+	{
+		struct i2c_client *client = kxtj2_1009_i2c_client;
+		char strbuf[KXTJ2_1009_BUFSIZE];
+		
+		if(NULL == client)
+		{
+			GSE_ERR("i2c client is null!!\n");
+			return 0;
+		}
+		//KXTJ2_1009_ReadSensorData(client, strbuf, KXTJ2_1009_BUFSIZE);
+		KXTJ2_1009_ReadRawData(client, strbuf);
+		return snprintf(buf, PAGE_SIZE, "%s\n", strbuf);			
+	}
+#endif
+/*----------------------------------------------------------------------------*/
+static ssize_t show_cali_value(struct device_driver *ddri, char *buf)
+{
+	struct i2c_client *client = kxtj2_1009_i2c_client;
+	struct kxtj2_1009_i2c_data *obj;
+	int err, len = 0, mul;
+	int tmp[KXTJ2_1009_AXES_NUM];
+
+	if(NULL == client)
+	{
+		GSE_ERR("i2c client is null!!\n");
+		return 0;
+	}
+
+	obj = i2c_get_clientdata(client);
+
+
+
+	if(0 != (err = KXTJ2_1009_ReadOffset(client, obj->offset)))
+	{
+		return -EINVAL;
+	}
+	else if(0 != (err = KXTJ2_1009_ReadCalibration(client, tmp)))
+	{
+		return -EINVAL;
+	}
+	else
+	{    
+		mul = obj->reso->sensitivity/kxtj2_1009_offset_resolution.sensitivity;
+		len += snprintf(buf+len, PAGE_SIZE-len, "[HW ][%d] (%+3d, %+3d, %+3d) : (0x%02X, 0x%02X, 0x%02X)\n", mul,                        
+			obj->offset[KXTJ2_1009_AXIS_X], obj->offset[KXTJ2_1009_AXIS_Y], obj->offset[KXTJ2_1009_AXIS_Z],
+			obj->offset[KXTJ2_1009_AXIS_X], obj->offset[KXTJ2_1009_AXIS_Y], obj->offset[KXTJ2_1009_AXIS_Z]);
+		len += snprintf(buf+len, PAGE_SIZE-len, "[SW ][%d] (%+3d, %+3d, %+3d)\n", 1, 
+			obj->cali_sw[KXTJ2_1009_AXIS_X], obj->cali_sw[KXTJ2_1009_AXIS_Y], obj->cali_sw[KXTJ2_1009_AXIS_Z]);
+
+		len += snprintf(buf+len, PAGE_SIZE-len, "[ALL]    (%+3d, %+3d, %+3d) : (%+3d, %+3d, %+3d)\n", 
+			obj->offset[KXTJ2_1009_AXIS_X]*mul + obj->cali_sw[KXTJ2_1009_AXIS_X],
+			obj->offset[KXTJ2_1009_AXIS_Y]*mul + obj->cali_sw[KXTJ2_1009_AXIS_Y],
+			obj->offset[KXTJ2_1009_AXIS_Z]*mul + obj->cali_sw[KXTJ2_1009_AXIS_Z],
+			tmp[KXTJ2_1009_AXIS_X], tmp[KXTJ2_1009_AXIS_Y], tmp[KXTJ2_1009_AXIS_Z]);
+		
+		return len;
+    }
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t store_cali_value(struct device_driver *ddri, const char *buf, size_t count)
+{
+	struct i2c_client *client = kxtj2_1009_i2c_client;  
+	int err, x, y, z;
+	int dat[KXTJ2_1009_AXES_NUM];
+
+	if(!strncmp(buf, "rst", 3))
+	{
+		if(0 != (err = KXTJ2_1009_ResetCalibration(client)))
+		{
+			GSE_ERR("reset offset err = %d\n", err);
+		}	
+	}
+	else if(3 == sscanf(buf, "0x%02X 0x%02X 0x%02X", &x, &y, &z))
+	{
+		dat[KXTJ2_1009_AXIS_X] = x;
+		dat[KXTJ2_1009_AXIS_Y] = y;
+		dat[KXTJ2_1009_AXIS_Z] = z;
+		if(0 != (err = KXTJ2_1009_WriteCalibration(client, dat)))
+		{
+			GSE_ERR("write calibration err = %d\n", err);
+		}		
+	}
+	else
+	{
+		GSE_ERR("invalid format\n");
+	}
+	
+	return count;
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t show_self_value(struct device_driver *ddri, char *buf)
+{
+	struct i2c_client *client = kxtj2_1009_i2c_client;
+
+	if(NULL == client)
+	{
+		GSE_ERR("i2c client is null!!\n");
+		return 0;
+	}
+
+    return snprintf(buf, 8, "%s\n", selftestRes);
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t store_self_value(struct device_driver *ddri, const char *buf, size_t count)
+{   /*write anything to this register will trigger the process*/
+	struct item{
+	s16 raw[KXTJ2_1009_AXES_NUM];
+	};
+	
+	struct i2c_client *client = kxtj2_1009_i2c_client;  
+	int res, num;
+	struct item *prv = NULL, *nxt = NULL;
+	u8 data;
+
+	if(1 != sscanf(buf, "%d", &num))
+	{
+		GSE_ERR("parse number fail\n");
+		return count;
+	}
+	else if(num == 0)
+	{
+		GSE_ERR("invalid data count\n");
+		return count;
+	}
+
+	prv = kzalloc(sizeof(*prv) * num, GFP_KERNEL);
+	nxt = kzalloc(sizeof(*nxt) * num, GFP_KERNEL);
+	if (!prv || !nxt)
+	{
+		goto exit;
+	}
+
+
+	GSE_LOG("NORMAL:\n");
+	KXTJ2_1009_SetPowerMode(client,true); 
+
+	/*initial setting for self test*/
+	if(!KXTJ2_1009_InitSelfTest(client))
+	{
+		GSE_LOG("SELFTEST : PASS\n");
+		strcpy(selftestRes,"y");
+	}	
+	else
+	{
+		GSE_LOG("SELFTEST : FAIL\n");		
+		strcpy(selftestRes,"n");
+	}
+
+	res = hwmsen_read_byte(client, KXTJ2_1009_REG_CTL_REG3, &data);
+	if(res != KXTJ2_1009_SUCCESS)
+	{
+		return res;
+	}
+
+	res = hwmsen_write_byte(client, KXTJ2_1009_REG_CTL_REG3,  ~KXTJ2_1009_SELF_TEST&data);
+	if(res != KXTJ2_1009_SUCCESS) //0x2C->BW=100Hz
+	{
+		return res;
+	}
+	
+	exit:
+	/*restore the setting*/    
+	kxtj2_1009_init_client(client, 0);
+	kfree(prv);
+	kfree(nxt);
+	return count;
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t show_selftest_value(struct device_driver *ddri, char *buf)
+{
+	struct i2c_client *client = kxtj2_1009_i2c_client;
+	struct kxtj2_1009_i2c_data *obj;
+
+	if(NULL == client)
+	{
+		GSE_ERR("i2c client is null!!\n");
+		return 0;
+	}
+
+	obj = i2c_get_clientdata(client);
+	return snprintf(buf, PAGE_SIZE, "%d\n", atomic_read(&obj->selftest));
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t store_selftest_value(struct device_driver *ddri, const char *buf, size_t count)
+{
+	struct kxtj2_1009_i2c_data *obj = obj_i2c_data;
+	int tmp;
+
+	if(NULL == obj)
+	{
+		GSE_ERR("i2c data obj is null!!\n");
+		return 0;
+	}
+	
+	
+	if(1 == sscanf(buf, "%d", &tmp))
+	{        
+		if(atomic_read(&obj->selftest) && !tmp)
+		{
+			/*enable -> disable*/
+			kxtj2_1009_init_client(obj->client, 0);
+		}
+		else if(!atomic_read(&obj->selftest) && tmp)
+		{
+			/*disable -> enable*/
+			KXTJ2_1009_InitSelfTest(obj->client);            
+		}
+		
+		GSE_LOG("selftest: %d => %d\n", atomic_read(&obj->selftest), tmp);
+		atomic_set(&obj->selftest, tmp); 
+	}
+	else
+	{ 
+		GSE_ERR("invalid content: '%s', length = %d\n", buf, (int)count);   
+	}
+	return count;
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t show_firlen_value(struct device_driver *ddri, char *buf)
+{
+#ifdef CONFIG_KXTJ2_1009_LOWPASS
+	struct i2c_client *client = kxtj2_1009_i2c_client;
+	struct kxtj2_1009_i2c_data *obj = i2c_get_clientdata(client);
+	if(atomic_read(&obj->firlen))
+	{
+		int idx, len = atomic_read(&obj->firlen);
+		GSE_LOG("len = %2d, idx = %2d\n", obj->fir.num, obj->fir.idx);
+
+		for(idx = 0; idx < len; idx++)
+		{
+			GSE_LOG("[%5d %5d %5d]\n", obj->fir.raw[idx][KXTJ2_1009_AXIS_X], obj->fir.raw[idx][KXTJ2_1009_AXIS_Y], obj->fir.raw[idx][KXTJ2_1009_AXIS_Z]);
+		}
+		
+		GSE_LOG("sum = [%5d %5d %5d]\n", obj->fir.sum[KXTJ2_1009_AXIS_X], obj->fir.sum[KXTJ2_1009_AXIS_Y], obj->fir.sum[KXTJ2_1009_AXIS_Z]);
+		GSE_LOG("avg = [%5d %5d %5d]\n", obj->fir.sum[KXTJ2_1009_AXIS_X]/len, obj->fir.sum[KXTJ2_1009_AXIS_Y]/len, obj->fir.sum[KXTJ2_1009_AXIS_Z]/len);
+	}
+	return snprintf(buf, PAGE_SIZE, "%d\n", atomic_read(&obj->firlen));
+#else
+	return snprintf(buf, PAGE_SIZE, "not support\n");
+#endif
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t store_firlen_value(struct device_driver *ddri, const char *buf, size_t count)
+{
+#ifdef CONFIG_KXTJ2_1009_LOWPASS
+	struct i2c_client *client = kxtj2_1009_i2c_client;  
+	struct kxtj2_1009_i2c_data *obj = i2c_get_clientdata(client);
+	int firlen;
+
+	if(1 != sscanf(buf, "%d", &firlen))
+	{
+		GSE_ERR("invallid format\n");
+	}
+	else if(firlen > C_MAX_FIR_LENGTH)
+	{
+		GSE_ERR("exceeds maximum filter length\n");
+	}
+	else
+	{ 
+		atomic_set(&obj->firlen, firlen);
+		if(NULL == firlen)
+		{
+			atomic_set(&obj->fir_en, 0);
+		}
+		else
+		{
+			memset(&obj->fir, 0x00, sizeof(obj->fir));
+			atomic_set(&obj->fir_en, 1);
+		}
+	}
+#endif    
+	return count;
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t show_trace_value(struct device_driver *ddri, char *buf)
+{
+	ssize_t res;
+	struct kxtj2_1009_i2c_data *obj = obj_i2c_data;
+	if (obj == NULL)
+	{
+		GSE_ERR("i2c_data obj is null!!\n");
+		return 0;
+	}
+	
+	res = snprintf(buf, PAGE_SIZE, "0x%04X\n", atomic_read(&obj->trace));     
+	return res;    
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t store_trace_value(struct device_driver *ddri, const char *buf, size_t count)
+{
+	struct kxtj2_1009_i2c_data *obj = obj_i2c_data;
+	int trace;
+	if (obj == NULL)
+	{
+		GSE_ERR("i2c_data obj is null!!\n");
+		return 0;
+	}
+	
+	if(1 == sscanf(buf, "0x%x", &trace))
+	{
+		atomic_set(&obj->trace, trace);
+	}	
+	else
+	{
+		GSE_ERR("invalid content: '%s', length = %d\n", buf, (int)count);
+	}
+	
+	return count;    
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t show_status_value(struct device_driver *ddri, char *buf)
+{
+	ssize_t len = 0;    
+	struct kxtj2_1009_i2c_data *obj = obj_i2c_data;
+	if (obj == NULL)
+	{
+		GSE_ERR("i2c_data obj is null!!\n");
+		return 0;
+	}	
+	
+	if(obj->hw)
+	{
+		len += snprintf(buf+len, PAGE_SIZE-len, "CUST: %d %d (%d %d)\n", 
+	            obj->hw->i2c_num, obj->hw->direction, obj->hw->power_id, obj->hw->power_vol);   
+	}
+	else
+	{
+		len += snprintf(buf+len, PAGE_SIZE-len, "CUST: NULL\n");
+	}
+	return len;    
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t show_power_status_value(struct device_driver *ddri, char *buf)
+{
+	u8 databuf[2];    
+	u8 addr = KXTJ2_1009_REG_POWER_CTL;
+	if(hwmsen_read_block(kxtj2_1009_i2c_client, addr, databuf, 0x01))
+	{
+		GSE_ERR("read power ctl register err!\n");
+		return KXTJ2_1009_ERR_I2C;
+	}
+    
+	if(sensor_power)
+		printk("G sensor is in work mode, sensor_power = %d\n", sensor_power);
+	else
+		printk("G sensor is in standby mode, sensor_power = %d\n", sensor_power);
+
+	return snprintf(buf, PAGE_SIZE, "%x\n", databuf[0]);
+}
+
+/*----------------------------------------------------------------------------*/
+static DRIVER_ATTR(chipinfo,   S_IWUSR | S_IRUGO, show_chipinfo_value,      NULL);
+static DRIVER_ATTR(sensordata, S_IWUSR | S_IRUGO, show_sensordata_value,    NULL);
+static DRIVER_ATTR(cali,       S_IWUSR | S_IRUGO, show_cali_value,          store_cali_value);
+static DRIVER_ATTR(selftest, S_IWUSR | S_IRUGO, show_self_value,  store_self_value);
+static DRIVER_ATTR(self,   S_IWUSR | S_IRUGO, show_selftest_value,      store_selftest_value);
+static DRIVER_ATTR(firlen,     S_IWUSR | S_IRUGO, show_firlen_value,        store_firlen_value);
+static DRIVER_ATTR(trace,      S_IWUSR | S_IRUGO, show_trace_value,         store_trace_value);
+static DRIVER_ATTR(status,               S_IRUGO, show_status_value,        NULL);
+static DRIVER_ATTR(powerstatus,               S_IRUGO, show_power_status_value,        NULL);
+
+
+/*----------------------------------------------------------------------------*/
+static u8 i2c_dev_reg =0 ;
+
+static ssize_t show_register(struct device_driver *pdri, char *buf)
+{
+	printk("i2c_dev_reg is 0x%2x \n", i2c_dev_reg);
+
+	return 0;
+}
+
+static ssize_t store_register(struct device_driver *ddri, const char *buf, size_t count)
+{
+	i2c_dev_reg = simple_strtoul(buf, NULL, 16);
+	printk("set i2c_dev_reg = 0x%2x \n", i2c_dev_reg);
+
+	return 0;
+}
+static ssize_t store_register_value(struct device_driver *ddri, const char *buf, size_t count)
+{
+	struct kxtj2_1009_i2c_data *obj = obj_i2c_data;
+	u8 databuf[2];  
+	unsigned long input_value;
+	int res;
+	
+	memset(databuf, 0, sizeof(u8)*2);    
+
+	input_value = simple_strtoul(buf, NULL, 16);
+	printk("input_value = 0x%2x \n", (unsigned int)input_value);
+
+	if(NULL == obj)
+	{
+		GSE_ERR("i2c data obj is null!!\n");
+		return 0;
+	}
+
+	databuf[0] = i2c_dev_reg;
+	databuf[1] = input_value;
+	printk("databuf[0]=0x%2x  databuf[1]=0x%2x \n", databuf[0],databuf[1]);
+
+	res = i2c_master_send(obj->client, databuf, 0x2);
+
+	if(res <= 0)
+	{
+		return KXTJ2_1009_ERR_I2C;
+	}
+	return 0;
+	
+}
+
+static ssize_t show_register_value(struct device_driver *ddri, char *buf)
+{
+		struct kxtj2_1009_i2c_data *obj = obj_i2c_data;
+		u8 databuf[1];	
+		
+		memset(databuf, 0, sizeof(u8)*1);	 
+	
+		if(NULL == obj)
+		{
+			GSE_ERR("i2c data obj is null!!\n");
+			return 0;
+		}
+		
+		if(hwmsen_read_block(obj->client, i2c_dev_reg, databuf, 0x01))
+		{
+			GSE_ERR("read power ctl register err!\n");
+			return KXTJ2_1009_ERR_I2C;
+		}
+
+		printk("i2c_dev_reg=0x%2x  data=0x%2x \n", i2c_dev_reg,databuf[0]);
+	
+		return 0;
+		
+}
+
+
+static DRIVER_ATTR(i2c,      S_IWUSR | S_IRUGO, show_register_value,         store_register_value);
+static DRIVER_ATTR(register,      S_IWUSR | S_IRUGO, show_register,         store_register);
+
+
+/*----------------------------------------------------------------------------*/
+static struct driver_attribute *kxtj2_1009_attr_list[] = {
+	&driver_attr_chipinfo,     /*chip information*/
+	&driver_attr_sensordata,   /*dump sensor data*/
+	&driver_attr_cali,         /*show calibration data*/
+	&driver_attr_self,         /*self test demo*/
+	&driver_attr_selftest,     /*self control: 0: disable, 1: enable*/
+	&driver_attr_firlen,       /*filter length: 0: disable, others: enable*/
+	&driver_attr_trace,        /*trace log*/
+	&driver_attr_status,
+	&driver_attr_powerstatus,
+	&driver_attr_register,
+	&driver_attr_i2c,
+};
+/*----------------------------------------------------------------------------*/
+static int kxtj2_1009_create_attr(struct device_driver *driver) 
+{
+	int idx, err = 0;
+	int num = (int)(sizeof(kxtj2_1009_attr_list)/sizeof(kxtj2_1009_attr_list[0]));
+	if (driver == NULL)
+	{
+		return -EINVAL;
+	}
+
+	for(idx = 0; idx < num; idx++)
+	{
+		if(0 != (err = driver_create_file(driver, kxtj2_1009_attr_list[idx])))
+		{            
+			GSE_ERR("driver_create_file (%s) = %d\n", kxtj2_1009_attr_list[idx]->attr.name, err);
+			break;
+		}
+	}    
+	return err;
+}
+/*----------------------------------------------------------------------------*/
+static int kxtj2_1009_delete_attr(struct device_driver *driver)
+{
+	int idx ,err = 0;
+	int num = (int)(sizeof(kxtj2_1009_attr_list)/sizeof(kxtj2_1009_attr_list[0]));
+
+	if(driver == NULL)
+	{
+		return -EINVAL;
+	}
+	
+
+	for(idx = 0; idx < num; idx++)
+	{
+		driver_remove_file(driver, kxtj2_1009_attr_list[idx]);
+	}
+	
+
+	return err;
+}
+/****************************************************************************** 
+ * Function Configuration
+******************************************************************************/
+/*----------------------------------------------------------------------------*/
+#ifdef CUSTOM_KERNEL_SENSORHUB
+static void kxtj2_1009_irq_work(struct work_struct *work)
+{
+    struct kxtj2_1009_i2c_data *obj = obj_i2c_data;
+    struct scp_acc_hw scp_hw;
+    KXTJ2_1009_CUST_DATA *p_cust_data;
+    SCP_SENSOR_HUB_DATA data;
+    int max_cust_data_size_per_packet;
+    int i;
+    uint sizeOfCustData;
+    uint len;
+    char *p = (char *)&scp_hw;
+
+    GSE_FUN();
+
+    scp_hw.i2c_num = obj->hw->i2c_num;
+    scp_hw.direction = obj->hw->direction;
+    scp_hw.power_id = obj->hw->power_id;
+    scp_hw.power_vol = obj->hw->power_vol;
+    scp_hw.firlen = obj->hw->firlen;
+    memcpy(scp_hw.i2c_addr, obj->hw->i2c_addr, sizeof(obj->hw->i2c_addr));
+    scp_hw.power_vio_id = obj->hw->power_vio_id;
+    scp_hw.power_vio_vol = obj->hw->power_vio_vol;
+    scp_hw.is_batch_supported = obj->hw->is_batch_supported;
+
+    p_cust_data = (KXTJ2_1009_CUST_DATA *)data.set_cust_req.custData;
+    sizeOfCustData = sizeof(scp_hw);
+    max_cust_data_size_per_packet = sizeof(data.set_cust_req.custData) - offsetof(KXTJ2_1009_SET_CUST, data);
+    
+    for (i=0;sizeOfCustData>0;i++)
+    {
+        data.set_cust_req.sensorType = ID_ACCELEROMETER;
+        data.set_cust_req.action = SENSOR_HUB_SET_CUST;
+        p_cust_data->setCust.action = KXTJ2_1009_CUST_ACTION_SET_CUST;
+        p_cust_data->setCust.part = i;
+        if (sizeOfCustData > max_cust_data_size_per_packet)
+        {
+            len = max_cust_data_size_per_packet;
+        }
+        else
+        {
+            len = sizeOfCustData;
+        }
+
+        memcpy(p_cust_data->setCust.data, p, len);
+        sizeOfCustData -= len;
+        p += len;
+        
+        len += offsetof(SCP_SENSOR_HUB_SET_CUST_REQ, custData) + offsetof(KXTJ2_1009_SET_CUST, data);
+        SCP_sensorHub_req_send(&data, &len, 1);
+    }
+
+	//KXTJ2_1009_ResetCalibration
+	p_cust_data = (KXTJ2_1009_CUST_DATA *)&data.set_cust_req.custData;
+	
+	data.set_cust_req.sensorType = ID_ACCELEROMETER;
+	data.set_cust_req.action = SENSOR_HUB_SET_CUST;
+	p_cust_data->resetCali.action = KXTJ2_1009_CUST_ACTION_RESET_CALI;
+	len = offsetof(SCP_SENSOR_HUB_SET_CUST_REQ, custData) + sizeof(p_cust_data->resetCali);
+	SCP_sensorHub_req_send(&data, &len, 1);
+
+    obj->SCP_init_done = 1;
+}
+/*----------------------------------------------------------------------------*/
+static int kxtj2_1009_irq_handler(void* data, uint len)
+{
+	struct kxtj2_1009_i2c_data *obj = obj_i2c_data;
+    SCP_SENSOR_HUB_DATA_P rsp = (SCP_SENSOR_HUB_DATA_P)data;
+    
+	if(!obj)
+	{
+		return -1;
+	}
+
+    switch(rsp->rsp.action)
+    {
+        case SENSOR_HUB_NOTIFY:
+            switch(rsp->notify_rsp.event)
+            {
+                case SCP_INIT_DONE:
+                    schedule_work(&obj->irq_work);
+                    break;
+                default:
+                    GSE_ERR("Error sensor hub notify");
+                    break;
+            }
+            break;
+        default:
+            GSE_ERR("Error sensor hub action");
+            break;
+    }
+
+    return 0;
+}
+
+static int kxtj2_1009_setup_irq()
+{
+    int err = 0;
+
+    err = SCP_sensorHub_rsp_registration(ID_ACCELEROMETER, kxtj2_1009_irq_handler);
+    
+	return err;
+}
+#endif//#ifdef CUSTOM_KERNEL_SENSORHUB
+/*----------------------------------------------------------------------------*/
+static int kxtj2_1009_open(struct inode *inode, struct file *file)
+{
+	file->private_data = kxtj2_1009_i2c_client;
+
+	if(file->private_data == NULL)
+	{
+		GSE_ERR("null pointer!!\n");
+		return -EINVAL;
+	}
+	return nonseekable_open(inode, file);
+}
+/*----------------------------------------------------------------------------*/
+static int kxtj2_1009_release(struct inode *inode, struct file *file)
+{
+	file->private_data = NULL;
+	return 0;
+}
+
+#ifdef CONFIG_COMPAT
+static long kxtj2_1009_compat_ioctl(struct file *file, unsigned int cmd,
+       unsigned long arg)
+{
+    long err = 0;
+
+	void __user *arg32 = compat_ptr(arg);
+	
+	if (!file->f_op || !file->f_op->unlocked_ioctl)
+		return -ENOTTY;
+	
+    switch (cmd)
+    {
+        case COMPAT_GSENSOR_IOCTL_READ_SENSORDATA:
+            if (arg32 == NULL)
+            {
+                err = -EINVAL;
+                break;    
+            }
+		
+		    err = file->f_op->unlocked_ioctl(file, GSENSOR_IOCTL_READ_SENSORDATA, arg32);
+		    if (err){
+		        GSE_ERR("GSENSOR_IOCTL_READ_SENSORDATA unlocked_ioctl failed.");
+		        return err;
+		    }
+        break;
+
+        default:
+            GSE_ERR("unknown IOCTL: 0x%08x\n", cmd);
+            err = -ENOIOCTLCMD;
+        break;
+
+    }
+
+    return err;
+}
+#endif
+
+/*----------------------------------------------------------------------------*/
+//static int kxtj2_1009_ioctl(struct inode *inode, struct file *file, unsigned int cmd,
+//       unsigned long arg)
+static long kxtj2_1009_unlocked_ioctl(struct file *file, unsigned int cmd,unsigned long arg)
+{
+	struct i2c_client *client = (struct i2c_client*)file->private_data;
+	struct kxtj2_1009_i2c_data *obj = (struct kxtj2_1009_i2c_data*)i2c_get_clientdata(client);	
+	char strbuf[KXTJ2_1009_BUFSIZE];
+	void __user *data;
+	SENSOR_DATA sensor_data;
+	long err = 0;
+	int cali[3];
+
+	//GSE_FUN(f);
+	if(_IOC_DIR(cmd) & _IOC_READ)
+	{
+		err = !access_ok(VERIFY_WRITE, (void __user *)arg, _IOC_SIZE(cmd));
+	}
+	else if(_IOC_DIR(cmd) & _IOC_WRITE)
+	{
+		err = !access_ok(VERIFY_READ, (void __user *)arg, _IOC_SIZE(cmd));
+	}
+
+	if(err)
+	{
+		GSE_ERR("access error: %08X, (%2d, %2d)\n", cmd, _IOC_DIR(cmd), _IOC_SIZE(cmd));
+		return -EFAULT;
+	}
+
+	switch(cmd)
+	{
+		case GSENSOR_IOCTL_INIT:
+			kxtj2_1009_init_client(client, 0);			
+			break;
+
+		case GSENSOR_IOCTL_READ_CHIPINFO:
+			data = (void __user *) arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}
+			
+			KXTJ2_1009_ReadChipInfo(client, strbuf, KXTJ2_1009_BUFSIZE);
+			if(copy_to_user(data, strbuf, strlen(strbuf)+1))
+			{
+				err = -EFAULT;
+				break;
+			}				 
+			break;	  
+
+		case GSENSOR_IOCTL_READ_SENSORDATA:
+			data = (void __user *) arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}
+			KXTJ2_1009_SetPowerMode(obj->client, true);
+			KXTJ2_1009_ReadSensorData(client, strbuf, KXTJ2_1009_BUFSIZE);
+			if(copy_to_user(data, strbuf, strlen(strbuf)+1))
+			{
+				err = -EFAULT;
+				break;	  
+			}				 
+			break;
+
+		case GSENSOR_IOCTL_READ_GAIN:
+			data = (void __user *) arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}			
+			
+			if(copy_to_user(data, &gsensor_gain, sizeof(GSENSOR_VECTOR3D)))
+			{
+				err = -EFAULT;
+				break;
+			}				 
+			break;
+
+		case GSENSOR_IOCTL_READ_RAW_DATA:
+			data = (void __user *) arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}
+			KXTJ2_1009_ReadRawData(client, strbuf);
+			if(copy_to_user(data, &strbuf, strlen(strbuf)+1))
+			{
+				err = -EFAULT;
+				break;	  
+			}
+			break;	  
+
+		case GSENSOR_IOCTL_SET_CALI:
+			data = (void __user*)arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}
+			if(copy_from_user(&sensor_data, data, sizeof(sensor_data)))
+			{
+				err = -EFAULT;
+				break;	  
+			}
+			if(atomic_read(&obj->suspend))
+			{
+				GSE_ERR("Perform calibration in suspend state!!\n");
+				err = -EINVAL;
+			}
+			else
+			{
+				cali[KXTJ2_1009_AXIS_X] = sensor_data.x * obj->reso->sensitivity / GRAVITY_EARTH_1000;
+				cali[KXTJ2_1009_AXIS_Y] = sensor_data.y * obj->reso->sensitivity / GRAVITY_EARTH_1000;
+				cali[KXTJ2_1009_AXIS_Z] = sensor_data.z * obj->reso->sensitivity / GRAVITY_EARTH_1000;			  
+				err = KXTJ2_1009_WriteCalibration(client, cali);			 
+			}
+			break;
+
+		case GSENSOR_IOCTL_CLR_CALI:
+			err = KXTJ2_1009_ResetCalibration(client);
+			break;
+
+		case GSENSOR_IOCTL_GET_CALI:
+			data = (void __user*)arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}
+			if(0 != (err = KXTJ2_1009_ReadCalibration(client, cali)))
+			{
+				break;
+			}
+			
+			sensor_data.x = cali[KXTJ2_1009_AXIS_X] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+			sensor_data.y = cali[KXTJ2_1009_AXIS_Y] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+			sensor_data.z = cali[KXTJ2_1009_AXIS_Z] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+			if(copy_to_user(data, &sensor_data, sizeof(sensor_data)))
+			{
+				err = -EFAULT;
+				break;
+			}		
+			break;
+		
+
+		default:
+			GSE_ERR("unknown IOCTL: 0x%08x\n", cmd);
+			err = -ENOIOCTLCMD;
+			break;
+			
+	}
+
+	return err;
+}
+ 
+
+/*----------------------------------------------------------------------------*/
+static struct file_operations kxtj2_1009_fops = {
+	.owner = THIS_MODULE,
+	.open = kxtj2_1009_open,
+	.release = kxtj2_1009_release,
+	.unlocked_ioctl = kxtj2_1009_unlocked_ioctl,
+	#ifdef CONFIG_COMPAT
+	.compat_ioctl = kxtj2_1009_compat_ioctl,
+	#endif
+	//.ioctl = kxtj2_1009_ioctl,
+};
+/*----------------------------------------------------------------------------*/
+static struct miscdevice kxtj2_1009_device = {
+	.minor = MISC_DYNAMIC_MINOR,
+	.name = "gsensor",
+	.fops = &kxtj2_1009_fops,
+};
+/*----------------------------------------------------------------------------*/
+#if !defined(CONFIG_HAS_EARLYSUSPEND) || !defined(USE_EARLY_SUSPEND)
+/*----------------------------------------------------------------------------*/
+static int kxtj2_1009_suspend(struct i2c_client *client, pm_message_t msg) 
+{
+	struct kxtj2_1009_i2c_data *obj = i2c_get_clientdata(client);    
+	int err = 0;
+	GSE_FUN();    
+
+	if(msg.event == PM_EVENT_SUSPEND)
+	{   
+		if(obj == NULL)
+		{
+			GSE_ERR("null pointer!!\n");
+			return -EINVAL;
+		}
+        mutex_lock(&kxtj2_1009_mutex);
+		atomic_set(&obj->suspend, 1);
+#ifdef CUSTOM_KERNEL_SENSORHUB
+		if(0 != (err = KXTJ2_1009_SCP_SetPowerMode(false)))
+#else
+		if(0 != (err = KXTJ2_1009_SetPowerMode(obj->client,false)))
+#endif
+		{
+			GSE_ERR("write power control fail!!\n");
+			mutex_unlock(&kxtj2_1009_mutex);
+			return -1;
+		}
+        mutex_unlock(&kxtj2_1009_mutex);
+
+		//sensor_power = false;      
+#ifndef CUSTOM_KERNEL_SENSORHUB
+		KXTJ2_1009_power(obj->hw, 0);
+#endif
+	}
+	return err;
+}
+/*----------------------------------------------------------------------------*/
+static int kxtj2_1009_resume(struct i2c_client *client)
+{
+	struct kxtj2_1009_i2c_data *obj = i2c_get_clientdata(client);        
+	int err;
+	GSE_FUN();
+
+	if(obj == NULL)
+	{
+		GSE_ERR("null pointer!!\n");
+		return -EINVAL;
+	}
+
+#ifndef CUSTOM_KERNEL_SENSORHUB
+	KXTJ2_1009_power(obj->hw, 1);
+#endif
+    mutex_lock(&kxtj2_1009_mutex);
+#ifdef CUSTOM_KERNEL_SENSORHUB
+	if(0 != (err = KXTJ2_1009_SCP_SetPowerMode(enable_status)))
+#else
+	if(0 != (err = kxtj2_1009_init_client(client, 0)))
+#endif
+	{
+		GSE_ERR("initialize client fail!!\n");
+		mutex_unlock(&kxtj2_1009_mutex);
+		return err;        
+	}
+	atomic_set(&obj->suspend, 0);
+    mutex_unlock(&kxtj2_1009_mutex);
+
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+#else //!defined(CONFIG_HAS_EARLYSUSPEND) || !defined(USE_EARLY_SUSPEND)
+/*----------------------------------------------------------------------------*/
+static void kxtj2_1009_early_suspend(struct early_suspend *h) 
+{
+	struct kxtj2_1009_i2c_data *obj = container_of(h, struct kxtj2_1009_i2c_data, early_drv);   
+	int err;
+	GSE_FUN();    
+
+	if(obj == NULL)
+	{
+		GSE_ERR("null pointer!!\n");
+		return;
+	}
+	mutex_lock(&kxtj2_1009_mutex);
+	atomic_set(&obj->suspend, 1);
+#ifdef CUSTOM_KERNEL_SENSORHUB
+	if(err = KXTJ2_1009_SCP_SetPowerMode(false))
+#else
+	if(err = KXTJ2_1009_SetPowerMode(obj->client, false))
+#endif
+	{
+		GSE_ERR("write power control fail!!\n");
+		mutex_unlock(&kxtj2_1009_mutex);
+		return;
+	}
+	mutex_unlock(&kxtj2_1009_mutex);
+
+	//sensor_power = false;
+#ifndef CUSTOM_KERNEL_SENSORHUB	
+	KXTJ2_1009_power(obj->hw, 0);
+#endif
+}
+/*----------------------------------------------------------------------------*/
+static void kxtj2_1009_late_resume(struct early_suspend *h)
+{
+	struct kxtj2_1009_i2c_data *obj = container_of(h, struct kxtj2_1009_i2c_data, early_drv);         
+	int err;
+	GSE_FUN();
+
+	if(obj == NULL)
+	{
+		GSE_ERR("null pointer!!\n");
+		return;
+	}
+
+#ifndef CUSTOM_KERNEL_SENSORHUB
+	KXTJ2_1009_power(obj->hw, 1);
+#endif
+	mutex_lock(&kxtj2_1009_mutex);
+#ifdef CUSTOM_KERNEL_SENSORHUB
+	if(err = KXTJ2_1009_SCP_SetPowerMode(enable_status))
+#else
+	if(err = kxtj2_1009_init_client(obj->client, 0))
+#endif
+	{
+		GSE_ERR("initialize client fail!!\n");
+		mutex_unlock(&kxtj2_1009_mutex);
+		return;        
+	}
+	atomic_set(&obj->suspend, 0); 
+	mutex_unlock(&kxtj2_1009_mutex);
+}
+/*----------------------------------------------------------------------------*/
+#endif //!defined(CONFIG_HAS_EARLYSUSPEND) || !defined(USE_EARLY_SUSPEND)
+/*----------------------------------------------------------------------------*/
+
+// if use  this typ of enable , Gsensor should report inputEvent(x, y, z ,stats, div) to HAL
+static int kxtj2_1009_open_report_data(int open)
+{
+	//should queuq work to report event if  is_report_input_direct=true
+	return 0;
+}
+
+// if use  this typ of enable , Gsensor only enabled but not report inputEvent to HAL
+
+static int kxtj2_1009_enable_nodata(int en)
+{
+    int err = 0;
+
+    mutex_lock(&kxtj2_1009_mutex);
+	if(((en == 0) && (sensor_power == false)) ||((en == 1) && (sensor_power == true)))
+	{
+		enable_status = sensor_power;
+		GSE_LOG("Gsensor device have updated!\n");
+	}
+	else
+	{
+		enable_status = !sensor_power;
+		if (atomic_read(&obj_i2c_data->suspend) == 0)
+		{
+#ifdef CUSTOM_KERNEL_SENSORHUB
+            err = KXTJ2_1009_SCP_SetPowerMode(enable_status);
+#else//#ifdef CUSTOM_KERNEL_SENSORHUB
+			err = KXTJ2_1009_SetPowerMode(obj_i2c_data->client, enable_status);
+#endif
+			GSE_LOG("Gsensor not in suspend KXTJ2_1009_SetPowerMode!, enable_status = %d\n",enable_status);
+		}
+		else
+		{
+			GSE_LOG("Gsensor in suspend and can not enable or disable!enable_status = %d\n",enable_status);
+		}
+	}
+	mutex_unlock(&kxtj2_1009_mutex);
+
+    if(err != KXTJ2_1009_SUCCESS)
+	{
+		printk("kxtj2_1009_enable_nodata fail!\n");
+		return -1;
+	}
+
+    printk("kxtj2_1009_enable_nodata OK!\n");
+	return 0;
+}
+
+static int kxtj2_1009_set_delay(u64 ns)
+{
+    int err = 0;
+    int value;
+#ifdef CUSTOM_KERNEL_SENSORHUB
+    SCP_SENSOR_HUB_DATA req;
+    int len;
+#else//#ifdef CUSTOM_KERNEL_SENSORHUB
+	int sample_delay;
+#endif//#ifdef CUSTOM_KERNEL_SENSORHUB
+
+    value = (int)ns/1000/1000;
+
+#ifdef CUSTOM_KERNEL_SENSORHUB
+    req.set_delay_req.sensorType = ID_ACCELEROMETER;
+    req.set_delay_req.action = SENSOR_HUB_SET_DELAY;
+    req.set_delay_req.delay = value;
+    len = sizeof(req.activate_req);
+    err = SCP_sensorHub_req_send(&req, &len, 1);
+    if (err)
+    {
+        GSE_ERR("SCP_sensorHub_req_send!\n");
+        return err;
+    }
+#else//#ifdef CUSTOM_KERNEL_SENSORHUB    
+	if(value <= 5)
+	{
+		sample_delay = KXTJ2_1009_BW_200HZ;
+	}
+	else if(value <= 10)
+	{
+		sample_delay = KXTJ2_1009_BW_100HZ;
+	}
+	else
+	{
+		sample_delay = KXTJ2_1009_BW_50HZ;
+	}
+
+	mutex_lock(&kxtj2_1009_mutex);
+	err = KXTJ2_1009_SetBWRate(obj_i2c_data->client, sample_delay);
+	mutex_unlock(&kxtj2_1009_mutex);
+	if(err != KXTJ2_1009_SUCCESS ) //0x2C->BW=100Hz
+	{
+		GSE_ERR("Set delay parameter error!\n");
+        return -1;
+	}
+
+	if(value >= 50)
+	{
+		atomic_set(&obj_i2c_data->filter, 0);
+	}
+	else
+	{	
+	#if defined(CONFIG_KXTJ2_1009_LOWPASS)
+		priv->fir.num = 0;
+		priv->fir.idx = 0;
+		priv->fir.sum[KXTJ2_1009_AXIS_X] = 0;
+		priv->fir.sum[KXTJ2_1009_AXIS_Y] = 0;
+		priv->fir.sum[KXTJ2_1009_AXIS_Z] = 0;
+		atomic_set(&priv->filter, 1);
+	#endif
+	}
+#endif//#ifdef CUSTOM_KERNEL_SENSORHUB
+    
+    GSE_LOG("kxtj2_1009_set_delay (%d)\n",value);
+
+	return 0;
+}
+
+static int kxtj2_1009_set_batch(int flags, int64_t period_ns, int64_t timeout)
+{
+    int err = 0;
+
+#ifdef CUSTOM_KERNEL_SENSORHUB
+    uint32_t period_ms;
+    uint32_t timeout_ms;
+    SCP_SENSOR_HUB_DATA req;
+    int len;
+
+    period_ms = period_ns/1000000;
+    timeout_ms = timeout/1000000;
+
+    req.batch_req.sensorType = ID_ACCELEROMETER;
+    req.batch_req.action = SENSOR_HUB_BATCH;
+    req.batch_req.flag = flags;
+    req.batch_req.period_ms = period_ms;
+    req.batch_req.timeout_ms = timeout_ms;
+
+    len = sizeof(req.batch_req);
+
+    err = SCP_sensorHub_req_send(&req, &len, 1);
+    if (err)
+    {
+        GSE_ERR("SCP_sensorHub_req_send!\n");
+        return err;
+    }
+#endif
+
+    return err;
+}
+
+static int kxtj2_1009_get_data(int* x ,int* y,int* z, int* status)
+{
+#ifdef CUSTOM_KERNEL_SENSORHUB
+		SCP_SENSOR_HUB_DATA req;
+		int len;
+		int err = 0;
+#else
+	char buff[KXTJ2_1009_BUFSIZE];
+#endif
+
+#ifdef CUSTOM_KERNEL_SENSORHUB
+		req.get_data_req.sensorType = ID_ACCELEROMETER;
+		req.get_data_req.action = SENSOR_HUB_GET_DATA;
+		len = sizeof(req.get_data_req);
+		err = SCP_sensorHub_req_send(&req, &len, 1);
+		if (err)
+		{
+			GSE_ERR("SCP_sensorHub_req_send!\n");
+			return err;
+		}
+
+		if (ID_ACCELEROMETER != req.get_data_rsp.sensorType ||
+			SENSOR_HUB_GET_DATA != req.get_data_rsp.action ||
+			0 != req.get_data_rsp.errCode)
+		{
+			GSE_ERR("error : %d\n", req.get_data_rsp.errCode);
+			return req.get_data_rsp.errCode;
+		}
+
+		//sscanf(buff, "%x %x %x", req.get_data_rsp.int16_Data[0], req.get_data_rsp.int16_Data[1], req.get_data_rsp.int16_Data[2]);
+		*x = (int)req.get_data_rsp.int16_Data[0]*GRAVITY_EARTH_1000/1000;
+		*y = (int)req.get_data_rsp.int16_Data[1]*GRAVITY_EARTH_1000/1000;
+		*z = (int)req.get_data_rsp.int16_Data[2]*GRAVITY_EARTH_1000/1000;
+		GSE_ERR("x = %d, y = %d, z = %d\n", *x, *y, *z);
+		*status = SENSOR_STATUS_ACCURACY_MEDIUM;
+
+		if(atomic_read(&obj_i2c_data->trace) & ADX_TRC_RAWDATA)
+		{
+			//show data
+		}	
+#else
+    mutex_lock(&kxtj2_1009_mutex);
+	KXTJ2_1009_ReadSensorData(obj_i2c_data->client, buff, KXTJ2_1009_BUFSIZE);
+	mutex_unlock(&kxtj2_1009_mutex);
+	sscanf(buff, "%x %x %x", x, y, z);				
+	*status = SENSOR_STATUS_ACCURACY_MEDIUM;
+#endif
+	return 0;
+}
+
+/*----------------------------------------------------------------------------*/
+static int kxtj2_1009_i2c_probe(struct i2c_client *client, const struct i2c_device_id *id)
+{
+	struct i2c_client *new_client;
+	struct kxtj2_1009_i2c_data *obj;
+	struct acc_control_path ctl={0};
+    struct acc_data_path data={0};
+	int err = 0;
+	GSE_FUN();
+
+	if(!(obj = kzalloc(sizeof(*obj), GFP_KERNEL)))
+	{
+		err = -ENOMEM;
+		goto exit;
+	}
+	
+	memset(obj, 0, sizeof(struct kxtj2_1009_i2c_data));
+
+	obj->hw = get_cust_acc_hw();
+	
+	if(0 != (err = hwmsen_get_convert(obj->hw->direction, &obj->cvt)))
+	{
+		GSE_ERR("invalid direction: %d\n", obj->hw->direction);
+		goto exit;
+	}
+
+#ifdef CUSTOM_KERNEL_SENSORHUB
+    INIT_WORK(&obj->irq_work, kxtj2_1009_irq_work);
+#endif//#ifdef CUSTOM_KERNEL_SENSORHUB
+
+	obj_i2c_data = obj;
+	obj->client = client;
+	new_client = obj->client;
+	i2c_set_clientdata(new_client,obj);
+	
+	atomic_set(&obj->trace, 0);
+	atomic_set(&obj->suspend, 0);
+
+#ifdef CUSTOM_KERNEL_SENSORHUB
+    obj->SCP_init_done = 0;
+#endif//#ifdef CUSTOM_KERNEL_SENSORHUB
+
+#ifdef CONFIG_KXTJ2_1009_LOWPASS
+	if(obj->hw->firlen > C_MAX_FIR_LENGTH)
+	{
+		atomic_set(&obj->firlen, C_MAX_FIR_LENGTH);
+	}	
+	else
+	{
+		atomic_set(&obj->firlen, obj->hw->firlen);
+	}
+	
+	if(atomic_read(&obj->firlen) > 0)
+	{
+		atomic_set(&obj->fir_en, 1);
+	}
+	
+#endif
+
+	kxtj2_1009_i2c_client = new_client;	
+
+	if(0 != (err = kxtj2_1009_init_client(new_client, 1)))
+	{
+		goto exit_init_failed;
+	}
+	
+
+	if(0 != (err = misc_register(&kxtj2_1009_device)))
+	{
+		GSE_ERR("kxtj2_1009_device register failed\n");
+		goto exit_misc_device_register_failed;
+	}
+
+	if(0 != (err = kxtj2_1009_create_attr(&kxtj2_1009_init_info.platform_diver_addr->driver)))
+	{
+		GSE_ERR("create attribute err = %d\n", err);
+		goto exit_create_attr_failed;
+	}
+
+	ctl.open_report_data= kxtj2_1009_open_report_data;
+	ctl.enable_nodata = kxtj2_1009_enable_nodata;
+	ctl.set_delay  = kxtj2_1009_set_delay;
+    //ctl.batch = kxtj2_1009_set_batch;
+	ctl.is_report_input_direct = false;
+#ifdef CUSTOM_KERNEL_SENSORHUB
+    ctl.is_support_batch = obj->hw->is_batch_supported;
+#else
+    ctl.is_support_batch = false;
+#endif
+
+	err = acc_register_control_path(&ctl);
+	if(err)
+	{
+	 	GSE_ERR("register acc control path err\n");
+		goto exit_create_attr_failed;
+	}
+
+	data.get_data = kxtj2_1009_get_data;
+	data.vender_div = 1000;
+	err = acc_register_data_path(&data);
+	if(err)
+	{
+	 	GSE_ERR("register acc data path err\n");
+		goto exit_create_attr_failed;
+	}
+	err = batch_register_support_info(ID_ACCELEROMETER,ctl.is_support_batch, 102, 0); //divisor is 1000/9.8
+    if(err)
+    {
+        GSE_ERR("register gsensor batch support err = %d\n", err);
+        goto exit_create_attr_failed;
+    }
+
+#if defined(CONFIG_HAS_EARLYSUSPEND) && defined(USE_EARLY_SUSPEND)
+	obj->early_drv.level    = EARLY_SUSPEND_LEVEL_DISABLE_FB - 1,
+	obj->early_drv.suspend  = kxtj2_1009_early_suspend,
+	obj->early_drv.resume   = kxtj2_1009_late_resume,    
+	register_early_suspend(&obj->early_drv);
+#endif 
+
+    kxtj2_1009_init_flag =0;
+	GSE_LOG("%s: OK\n", __func__);    
+	return 0;
+
+	exit_create_attr_failed:
+	misc_deregister(&kxtj2_1009_device);
+	exit_misc_device_register_failed:
+	exit_init_failed:
+	//i2c_detach_client(new_client);
+	exit_kfree:
+	kfree(obj);
+	exit:
+	GSE_ERR("%s: err = %d\n", __func__, err);
+    kxtj2_1009_init_flag =-1;
+	return err;
+}
+
+/*----------------------------------------------------------------------------*/
+static int kxtj2_1009_i2c_remove(struct i2c_client *client)
+{
+	int err = 0;	
+	
+	if(0 != (err = kxtj2_1009_delete_attr(&(kxtj2_1009_init_info.platform_diver_addr->driver))))
+	{
+		GSE_ERR("kxtj2_1009_delete_attr fail: %d\n", err);
+	}
+	
+	if(0 != (err = misc_deregister(&kxtj2_1009_device)))
+	{
+		GSE_ERR("misc_deregister fail: %d\n", err);
+	}
+
+	kxtj2_1009_i2c_client = NULL;
+	i2c_unregister_device(client);
+	kfree(i2c_get_clientdata(client));
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int kxtj2_1009_remove(void)
+{
+    struct acc_hw *hw = get_cust_acc_hw();
+
+    GSE_FUN();    
+    KXTJ2_1009_power(hw, 0);    
+    i2c_del_driver(&kxtj2_1009_i2c_driver);
+    return 0;
+}
+
+static int  kxtj2_1009_local_init(void)
+{
+    struct acc_hw *hw = get_cust_acc_hw();
+	//printk("fwq loccal init+++\n");
+
+	KXTJ2_1009_power(hw, 1);
+	if(i2c_add_driver(&kxtj2_1009_i2c_driver))
+	{
+		GSE_ERR("add driver error\n");
+		return -1;
+	}
+	if(-1 == kxtj2_1009_init_flag)
+	{
+	   return -1;
+	}
+	//printk("fwq loccal init---\n");
+	return 0;
+}
+
+/*----------------------------------------------------------------------------*/
+static int __init kxtj2_1009_init(void)
+{
+	struct acc_hw *hw = get_cust_acc_hw();
+
+    GSE_FUN();
+	GSE_LOG("%s: i2c_number=%d\n", __func__,hw->i2c_num);
+	i2c_register_board_info(hw->i2c_num, &i2c_kxtj2_1009, 1);
+	acc_driver_add(&kxtj2_1009_init_info);
+	return 0;    
+}
+/*----------------------------------------------------------------------------*/
+static void __exit kxtj2_1009_exit(void)
+{
+	GSE_FUN();
+}
+/*----------------------------------------------------------------------------*/
+module_init(kxtj2_1009_init);
+module_exit(kxtj2_1009_exit);
+/*----------------------------------------------------------------------------*/
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("KXTJ2_1009 I2C driver");
+MODULE_AUTHOR("Dexiang.Liu@mediatek.com");
diff --git a/drivers/misc/mediatek/accelerometer/KXTJ2_1009-new/kxtj2_1009.h b/drivers/misc/mediatek/accelerometer/KXTJ2_1009-new/kxtj2_1009.h
new file mode 100644
index 000000000..fa417223e
--- /dev/null
+++ b/drivers/misc/mediatek/accelerometer/KXTJ2_1009-new/kxtj2_1009.h
@@ -0,0 +1,104 @@
+/* linux/drivers/hwmon/adxl345.c
+ *
+ * (C) Copyright 2008 
+ * MediaTek <www.mediatek.com>
+ *
+ * KXTJ2_1009 driver for MT6575
+ *
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA  KXTIK
+ */
+#ifndef KXTJ2_1009_H
+#define KXTJ2_1009_H
+	 
+#include <linux/ioctl.h>
+	 
+#define KXTJ2_1009_I2C_SLAVE_ADDR		0x1C
+	 
+ /* KXTJ2_1009 Register Map  (Please refer to KXTJ2_1009 Specifications) */
+#define KXTJ2_1009_REG_DEVID			0x0F
+#define	KXTJ2_1009_REG_BW_RATE			0x21
+#define KXTJ2_1009_REG_POWER_CTL		0x1B
+#define KXTJ2_1009_REG_CTL_REG3		0x1D
+#define KXTJ2_1009_DCST_RESP			0x0C
+#define KXTJ2_1009_REG_DATA_FORMAT		0x1B
+#define KXTJ2_1009_REG_DATA_RESOLUTION		0x1B
+#define KXTJ2_1009_RANGE_DATA_RESOLUTION_MASK	0x40
+#define KXTJ2_1009_REG_DATAX0			0x06	 
+#define KXTJ2_1009_FIXED_DEVID			0x09	 
+#define KXTJ2_1009_BW_200HZ				0x05
+#define KXTJ2_1009_BW_100HZ				0x04
+#define KXTJ2_1009_BW_50HZ				0x03	 
+#define KXTJ2_1009_MEASURE_MODE		0x80		 
+#define KXTJ2_1009_RANGE_MASK		0x18
+#define KXTJ2_1009_RANGE_2G			0x00
+#define KXTJ2_1009_RANGE_4G			0x08
+#define KXTJ2_1009_RANGE_8G			0x10
+#define KXTJ2_1009_REG_INT_ENABLE	0x1E
+
+#define KXTJ2_1009_SELF_TEST           0x10
+	 	 
+	 
+#define KXTJ2_1009_SUCCESS						0
+#define KXTJ2_1009_ERR_I2C						-1
+#define KXTJ2_1009_ERR_STATUS					-3
+#define KXTJ2_1009_ERR_SETUP_FAILURE				-4
+#define KXTJ2_1009_ERR_GETGSENSORDATA			-5
+#define KXTJ2_1009_ERR_IDENTIFICATION			-6
+	 
+	 
+	 
+#define KXTJ2_1009_BUFSIZE				256
+	 
+#define KXTJ2_1009_AXES_NUM        3
+
+/*----------------------------------------------------------------------------*/
+typedef enum{
+    KXTJ2_1009_CUST_ACTION_SET_CUST = 1,
+    KXTJ2_1009_CUST_ACTION_SET_CALI,
+    KXTJ2_1009_CUST_ACTION_RESET_CALI
+}CUST_ACTION;
+/*----------------------------------------------------------------------------*/
+typedef struct
+{
+    uint16_t    action;
+}KXTJ2_1009_CUST;
+/*----------------------------------------------------------------------------*/
+typedef struct
+{
+    uint16_t    action;
+    uint16_t    part;
+    int32_t     data[0];
+}KXTJ2_1009_SET_CUST;
+/*----------------------------------------------------------------------------*/
+typedef struct
+{
+    uint16_t    action;
+    int32_t     data[KXTJ2_1009_AXES_NUM];
+}KXTJ2_1009_SET_CALI;
+/*----------------------------------------------------------------------------*/
+typedef KXTJ2_1009_CUST KXTJ2_1009_RESET_CALI;
+/*----------------------------------------------------------------------------*/
+typedef union
+{
+    uint32_t                data[10];
+    KXTJ2_1009_CUST         cust;
+    KXTJ2_1009_SET_CUST     setCust;
+    KXTJ2_1009_SET_CALI     setCali;
+    KXTJ2_1009_RESET_CALI   resetCali;
+}KXTJ2_1009_CUST_DATA;
+/*----------------------------------------------------------------------------*/
+
+#endif
+
diff --git a/drivers/misc/mediatek/accelerometer/KXTJ2_1009/Makefile b/drivers/misc/mediatek/accelerometer/KXTJ2_1009/Makefile
new file mode 100755
index 000000000..e355a0d40
--- /dev/null
+++ b/drivers/misc/mediatek/accelerometer/KXTJ2_1009/Makefile
@@ -0,0 +1,4 @@
+include $(srctree)/drivers/misc/mediatek/Makefile.custom
+
+obj-y	:=  kxtj2_1009.o
+
diff --git a/drivers/misc/mediatek/accelerometer/KXTJ2_1009/kxtj2_1009.c b/drivers/misc/mediatek/accelerometer/KXTJ2_1009/kxtj2_1009.c
new file mode 100644
index 000000000..7b01d0ed4
--- /dev/null
+++ b/drivers/misc/mediatek/accelerometer/KXTJ2_1009/kxtj2_1009.c
@@ -0,0 +1,2070 @@
+/* KXTJ2_1009 motion sensor driver
+ *
+ *
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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.
+ *
+ */
+
+#include <linux/interrupt.h>
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include <linux/irq.h>
+#include <linux/miscdevice.h>
+#include <asm/uaccess.h>
+#include <linux/delay.h>
+#include <linux/input.h>
+#include <linux/workqueue.h>
+#include <linux/kobject.h>
+#include <linux/earlysuspend.h>
+#include <linux/platform_device.h>
+#include <asm/atomic.h>
+
+#include <mach/mt_typedefs.h>
+#include <mach/mt_gpio.h>
+#include <mach/mt_pm_ldo.h>
+
+#define POWER_NONE_MACRO MT65XX_POWER_NONE
+
+
+#include <cust_acc.h>
+#include <linux/hwmsensor.h>
+#include <linux/hwmsen_dev.h>
+#include <linux/sensors_io.h>
+#include "kxtj2_1009.h"
+#include <linux/hwmsen_helper.h>
+/*----------------------------------------------------------------------------*/
+#define I2C_DRIVERID_KXTJ2_1009 150
+/*----------------------------------------------------------------------------*/
+#define DEBUG 1
+/*----------------------------------------------------------------------------*/
+//#define CONFIG_KXTJ2_1009_LOWPASS   /*apply low pass filter on output*/       
+#define SW_CALIBRATION
+//#define USE_EARLY_SUSPEND
+/*----------------------------------------------------------------------------*/
+#define KXTJ2_1009_AXIS_X          0
+#define KXTJ2_1009_AXIS_Y          1
+#define KXTJ2_1009_AXIS_Z          2
+#define KXTJ2_1009_AXES_NUM        3
+#define KXTJ2_1009_DATA_LEN        6
+#define KXTJ2_1009_DEV_NAME        "KXTJ2_1009"
+/*----------------------------------------------------------------------------*/
+
+/*----------------------------------------------------------------------------*/
+static const struct i2c_device_id kxtj2_1009_i2c_id[] = {{KXTJ2_1009_DEV_NAME,0},{}};
+static struct i2c_board_info __initdata i2c_kxtj2_1009={ I2C_BOARD_INFO(KXTJ2_1009_DEV_NAME, (KXTJ2_1009_I2C_SLAVE_ADDR>>1))};
+/*the adapter id will be available in customization*/
+//static unsigned short kxtj2_1009_force[] = {0x00, KXTJ2_1009_I2C_SLAVE_ADDR, I2C_CLIENT_END, I2C_CLIENT_END};
+//static const unsigned short *const kxtj2_1009_forces[] = { kxtj2_1009_force, NULL };
+//static struct i2c_client_address_data kxtj2_1009_addr_data = { .forces = kxtj2_1009_forces,};
+
+/*----------------------------------------------------------------------------*/
+static int kxtj2_1009_i2c_probe(struct i2c_client *client, const struct i2c_device_id *id); 
+static int kxtj2_1009_i2c_remove(struct i2c_client *client);
+static int kxtj2_1009_i2c_detect(struct i2c_client *client/*, int kind*/, struct i2c_board_info *info);
+static int kxtj2_1009_suspend(struct i2c_client *client, pm_message_t msg);
+static int kxtj2_1009_resume(struct i2c_client *client);
+
+/*----------------------------------------------------------------------------*/
+typedef enum {
+    ADX_TRC_FILTER  = 0x01,
+    ADX_TRC_RAWDATA = 0x02,
+    ADX_TRC_IOCTL   = 0x04,
+    ADX_TRC_CALI	= 0X08,
+    ADX_TRC_INFO	= 0X10,
+} ADX_TRC;
+/*----------------------------------------------------------------------------*/
+struct scale_factor{
+    u8  whole;
+    u8  fraction;
+};
+/*----------------------------------------------------------------------------*/
+struct data_resolution {
+    struct scale_factor scalefactor;
+    int                 sensitivity;
+};
+/*----------------------------------------------------------------------------*/
+#define C_MAX_FIR_LENGTH (32)
+/*----------------------------------------------------------------------------*/
+struct data_filter {
+    s16 raw[C_MAX_FIR_LENGTH][KXTJ2_1009_AXES_NUM];
+    int sum[KXTJ2_1009_AXES_NUM];
+    int num;
+    int idx;
+};
+/*----------------------------------------------------------------------------*/
+struct kxtj2_1009_i2c_data {
+    struct i2c_client *client;
+    struct acc_hw *hw;
+    struct hwmsen_convert   cvt;
+    
+    /*misc*/
+    struct data_resolution *reso;
+    atomic_t                trace;
+    atomic_t                suspend;
+    atomic_t                selftest;
+	atomic_t				filter;
+    s16                     cali_sw[KXTJ2_1009_AXES_NUM+1];
+
+    /*data*/
+    s8                      offset[KXTJ2_1009_AXES_NUM+1];  /*+1: for 4-byte alignment*/
+    s16                     data[KXTJ2_1009_AXES_NUM+1];
+
+#if defined(CONFIG_KXTJ2_1009_LOWPASS)
+    atomic_t                firlen;
+    atomic_t                fir_en;
+    struct data_filter      fir;
+#endif 
+    /*early suspend*/
+#if defined(CONFIG_HAS_EARLYSUSPEND) && defined(USE_EARLY_SUSPEND)
+    struct early_suspend    early_drv;
+#endif     
+};
+/*----------------------------------------------------------------------------*/
+static struct i2c_driver kxtj2_1009_i2c_driver = {
+    .driver = {
+//        .owner          = THIS_MODULE,
+        .name           = KXTJ2_1009_DEV_NAME,
+    },
+	.probe      		= kxtj2_1009_i2c_probe,
+	.remove    			= kxtj2_1009_i2c_remove,
+	.detect				= kxtj2_1009_i2c_detect,
+#if !defined(CONFIG_HAS_EARLYSUSPEND) || !defined(USE_EARLY_SUSPEND)
+    .suspend            = kxtj2_1009_suspend,
+    .resume             = kxtj2_1009_resume,
+#endif
+	.id_table = kxtj2_1009_i2c_id,
+//	.address_data = &kxtj2_1009_addr_data,
+};
+
+/*----------------------------------------------------------------------------*/
+static struct i2c_client *kxtj2_1009_i2c_client = NULL;
+static struct platform_driver kxtj2_1009_gsensor_driver;
+static struct kxtj2_1009_i2c_data *obj_i2c_data = NULL;
+static bool sensor_power = true;
+static GSENSOR_VECTOR3D gsensor_gain;
+static char selftestRes[8]= {0}; 
+static DEFINE_MUTEX(kxtj2_1009_mutex);
+static bool enable_status = false;
+
+
+/*----------------------------------------------------------------------------*/
+#define GSE_TAG                  "[Gsensor] "
+#define GSE_FUN(f)               printk( GSE_TAG"%s\n", __FUNCTION__)
+#define GSE_ERR(fmt, args...)    printk(KERN_ERR GSE_TAG"%s %d : "fmt, __FUNCTION__, __LINE__, ##args)
+#define GSE_LOG(fmt, args...)    printk( GSE_TAG fmt, ##args)
+/*----------------------------------------------------------------------------*/
+static struct data_resolution kxtj2_1009_data_resolution[1] = {
+ /* combination by {FULL_RES,RANGE}*/
+    {{ 0, 9}, 1024}, // dataformat +/-2g  in 12-bit resolution;  { 3, 9} = 3.9 = (2*2*1000)/(2^12);  256 = (2^12)/(2*2)          
+};
+/*----------------------------------------------------------------------------*/
+static struct data_resolution kxtj2_1009_offset_resolution = {{15, 6}, 64};
+/*----------------------------------------------------------------------------*/
+static int KXTJ2_1009_SetPowerMode(struct i2c_client *client, bool enable);
+/*--------------------KXTJ2_1009 power control function----------------------------------*/
+static void KXTJ2_1009_power(struct acc_hw *hw, unsigned int on) 
+{
+	static unsigned int power_on = 0;
+
+	if(hw->power_id != POWER_NONE_MACRO)		// have externel LDO
+	{        
+		GSE_LOG("power %s\n", on ? "on" : "off");
+		if(power_on == on)	// power status not change
+		{
+			GSE_LOG("ignore power control: %d\n", on);
+		}
+		else if(on)	// power on
+		{
+			if(!hwPowerOn(hw->power_id, hw->power_vol, "KXTJ2_1009"))
+			{
+				GSE_ERR("power on fails!!\n");
+			}
+		}
+		else	// power off
+		{
+			if (!hwPowerDown(hw->power_id, "KXTJ2_1009"))
+			{
+				GSE_ERR("power off fail!!\n");
+			}			  
+		}
+	}
+	power_on = on;    
+}
+/*----------------------------------------------------------------------------*/
+
+/*----------------------------------------------------------------------------*/
+static int KXTJ2_1009_SetDataResolution(struct kxtj2_1009_i2c_data *obj)
+{
+	int err;
+	u8  databuf[2];
+	bool cur_sensor_power = sensor_power;
+
+	KXTJ2_1009_SetPowerMode(obj->client, false);
+
+	if(hwmsen_read_block(obj->client, KXTJ2_1009_REG_DATA_RESOLUTION, databuf, 0x01))
+	{
+		printk("kxtj2_1009 read Dataformat failt \n");
+		return KXTJ2_1009_ERR_I2C;
+	}
+
+	databuf[0] &= ~KXTJ2_1009_RANGE_DATA_RESOLUTION_MASK;
+	databuf[0] |= KXTJ2_1009_RANGE_DATA_RESOLUTION_MASK;//12bit
+	databuf[1] = databuf[0];
+	databuf[0] = KXTJ2_1009_REG_DATA_RESOLUTION;
+
+
+	err = i2c_master_send(obj->client, databuf, 0x2);
+
+	if(err <= 0)
+	{
+		return KXTJ2_1009_ERR_I2C;
+	}
+
+	KXTJ2_1009_SetPowerMode(obj->client, cur_sensor_power/*true*/);
+
+	//kxtj2_1009_data_resolution[0] has been set when initialize: +/-2g  in 8-bit resolution:  15.6 mg/LSB*/   
+	obj->reso = &kxtj2_1009_data_resolution[0];
+
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int KXTJ2_1009_ReadData(struct i2c_client *client, s16 data[KXTJ2_1009_AXES_NUM])
+{
+	struct kxtj2_1009_i2c_data *priv = i2c_get_clientdata(client);        
+	u8 addr = KXTJ2_1009_REG_DATAX0;
+	u8 buf[KXTJ2_1009_DATA_LEN] = {0};
+	int err = 0;
+	int i;
+
+	if(NULL == client)
+	{
+		err = -EINVAL;
+	}
+	else if((err = hwmsen_read_block(client, addr, buf, 0x06)) != 0)
+	{
+		GSE_ERR("error: %d\n", err);
+	}
+	else
+	{
+		data[KXTJ2_1009_AXIS_X] = (s16)((buf[KXTJ2_1009_AXIS_X*2] >> 4) |
+		         (buf[KXTJ2_1009_AXIS_X*2+1] << 4));
+		data[KXTJ2_1009_AXIS_Y] = (s16)((buf[KXTJ2_1009_AXIS_Y*2] >> 4) |
+		         (buf[KXTJ2_1009_AXIS_Y*2+1] << 4));
+		data[KXTJ2_1009_AXIS_Z] = (s16)((buf[KXTJ2_1009_AXIS_Z*2] >> 4) |
+		         (buf[KXTJ2_1009_AXIS_Z*2+1] << 4));
+
+		for(i=0;i<3;i++)				
+		{								//because the data is store in binary complement number formation in computer system
+			if ( data[i] == 0x0800 )	//so we want to calculate actual number here
+				data[i]= -2048;			//10bit resolution, 512= 2^(12-1)
+			else if ( data[i] & 0x0800 )//transfor format
+			{							//printk("data 0 step %x \n",data[i]);
+				data[i] -= 0x1; 		//printk("data 1 step %x \n",data[i]);
+				data[i] = ~data[i]; 	//printk("data 2 step %x \n",data[i]);
+				data[i] &= 0x07ff;		//printk("data 3 step %x \n\n",data[i]);
+				data[i] = -data[i]; 	
+			}
+		}	
+
+
+		if(atomic_read(&priv->trace) & ADX_TRC_RAWDATA)
+		{
+			GSE_LOG("[%08X %08X %08X] => [%5d %5d %5d]\n", data[KXTJ2_1009_AXIS_X], data[KXTJ2_1009_AXIS_Y], data[KXTJ2_1009_AXIS_Z],
+		                               data[KXTJ2_1009_AXIS_X], data[KXTJ2_1009_AXIS_Y], data[KXTJ2_1009_AXIS_Z]);
+		}
+#ifdef CONFIG_KXTJ2_1009_LOWPASS
+		if(atomic_read(&priv->filter))
+		{
+			if(atomic_read(&priv->fir_en) && !atomic_read(&priv->suspend))
+			{
+				int idx, firlen = atomic_read(&priv->firlen);   
+				if(priv->fir.num < firlen)
+				{                
+					priv->fir.raw[priv->fir.num][KXTJ2_1009_AXIS_X] = data[KXTJ2_1009_AXIS_X];
+					priv->fir.raw[priv->fir.num][KXTJ2_1009_AXIS_Y] = data[KXTJ2_1009_AXIS_Y];
+					priv->fir.raw[priv->fir.num][KXTJ2_1009_AXIS_Z] = data[KXTJ2_1009_AXIS_Z];
+					priv->fir.sum[KXTJ2_1009_AXIS_X] += data[KXTJ2_1009_AXIS_X];
+					priv->fir.sum[KXTJ2_1009_AXIS_Y] += data[KXTJ2_1009IK_AXIS_Y];
+					priv->fir.sum[KXTJ2_1009_AXIS_Z] += data[KXTJ2_1009_AXIS_Z];
+					if(atomic_read(&priv->trace) & ADX_TRC_FILTER)
+					{
+						GSE_LOG("add [%2d] [%5d %5d %5d] => [%5d %5d %5d]\n", priv->fir.num,
+							priv->fir.raw[priv->fir.num][KXTJ2_1009_AXIS_X], priv->fir.raw[priv->fir.num][KXTJ2_1009_AXIS_Y], priv->fir.raw[priv->fir.num][KXTJ2_1009_AXIS_Z],
+							priv->fir.sum[KXTJ2_1009_AXIS_X], priv->fir.sum[KXTJ2_1009_AXIS_Y], priv->fir.sum[KXTJ2_1009_AXIS_Z]);
+					}
+					priv->fir.num++;
+					priv->fir.idx++;
+				}
+				else
+				{
+					idx = priv->fir.idx % firlen;
+					priv->fir.sum[KXTJ2_1009_AXIS_X] -= priv->fir.raw[idx][KXTJ2_1009_AXIS_X];
+					priv->fir.sum[KXTJ2_1009_AXIS_Y] -= priv->fir.raw[idx][KXTJ2_1009_AXIS_Y];
+					priv->fir.sum[KXTJ2_1009_AXIS_Z] -= priv->fir.raw[idx][KXTJ2_1009_AXIS_Z];
+					priv->fir.raw[idx][KXTJ2_1009_AXIS_X] = data[KXTJ2_1009_AXIS_X];
+					priv->fir.raw[idx][KXTJ2_1009_AXIS_Y] = data[KXTJ2_1009_AXIS_Y];
+					priv->fir.raw[idx][KXTJ2_1009_AXIS_Z] = data[KXTJ2_1009_AXIS_Z];
+					priv->fir.sum[KXTJ2_1009_AXIS_X] += data[KXTJ2_1009_AXIS_X];
+					priv->fir.sum[KXTJ2_1009_AXIS_Y] += data[KXTJ2_1009_AXIS_Y];
+					priv->fir.sum[KXTJ2_1009_AXIS_Z] += data[KXTJ2_1009_AXIS_Z];
+					priv->fir.idx++;
+					data[KXTJ2_1009_AXIS_X] = priv->fir.sum[KXTJ2_1009_AXIS_X]/firlen;
+					data[KXTJ2_1009_AXIS_Y] = priv->fir.sum[KXTJ2_1009_AXIS_Y]/firlen;
+					data[KXTJ2_1009_AXIS_Z] = priv->fir.sum[KXTJ2_1009_AXIS_Z]/firlen;
+					if(atomic_read(&priv->trace) & ADX_TRC_FILTER)
+					{
+						GSE_LOG("add [%2d] [%5d %5d %5d] => [%5d %5d %5d] : [%5d %5d %5d]\n", idx,
+						priv->fir.raw[idx][KXTJ2_1009_AXIS_X], priv->fir.raw[idx][KXTJ2_1009_AXIS_Y], priv->fir.raw[idx][KXTJ2_1009_AXIS_Z],
+						priv->fir.sum[KXTJ2_1009_AXIS_X], priv->fir.sum[KXTJ2_1009_AXIS_Y], priv->fir.sum[KXTJ2_1009_AXIS_Z],
+						data[KXTJ2_1009_AXIS_X], data[KXTJ2_1009_AXIS_Y], data[KXTJ2_1009_AXIS_Z]);
+					}
+				}
+			}
+		}	
+#endif         
+	}
+	return err;
+}
+/*----------------------------------------------------------------------------*/
+static int KXTJ2_1009_ReadOffset(struct i2c_client *client, s8 ofs[KXTJ2_1009_AXES_NUM])
+{    
+	int err = 0;
+
+	ofs[1]=ofs[2]=ofs[0]=0x00;
+
+	printk("offesx=%x, y=%x, z=%x",ofs[0],ofs[1],ofs[2]);
+	
+	return err;
+}
+/*----------------------------------------------------------------------------*/
+static int KXTJ2_1009_ResetCalibration(struct i2c_client *client)
+{
+	struct kxtj2_1009_i2c_data *obj = i2c_get_clientdata(client);
+	int err = 0;
+
+	memset(obj->cali_sw, 0x00, sizeof(obj->cali_sw));
+	memset(obj->offset, 0x00, sizeof(obj->offset));
+	return err;
+}
+/*----------------------------------------------------------------------------*/
+static int KXTJ2_1009_ReadCalibration(struct i2c_client *client, int dat[KXTJ2_1009_AXES_NUM])
+{
+    struct kxtj2_1009_i2c_data *obj = i2c_get_clientdata(client);
+    int mul;
+
+	#ifdef SW_CALIBRATION
+		mul = 0;//only SW Calibration, disable HW Calibration
+	#else
+	    if ((err = KXTJ2_1009_ReadOffset(client, obj->offset))) {
+        GSE_ERR("read offset fail, %d\n", err);
+        return err;
+    	}    
+    	mul = obj->reso->sensitivity/kxtj2_1009_offset_resolution.sensitivity;
+	#endif
+
+    dat[obj->cvt.map[KXTJ2_1009_AXIS_X]] = obj->cvt.sign[KXTJ2_1009_AXIS_X]*(obj->offset[KXTJ2_1009_AXIS_X]*mul + obj->cali_sw[KXTJ2_1009_AXIS_X]);
+    dat[obj->cvt.map[KXTJ2_1009_AXIS_Y]] = obj->cvt.sign[KXTJ2_1009_AXIS_Y]*(obj->offset[KXTJ2_1009_AXIS_Y]*mul + obj->cali_sw[KXTJ2_1009_AXIS_Y]);
+    dat[obj->cvt.map[KXTJ2_1009_AXIS_Z]] = obj->cvt.sign[KXTJ2_1009_AXIS_Z]*(obj->offset[KXTJ2_1009_AXIS_Z]*mul + obj->cali_sw[KXTJ2_1009_AXIS_Z]);                        
+                                       
+    return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int KXTJ2_1009_ReadCalibrationEx(struct i2c_client *client, int act[KXTJ2_1009_AXES_NUM], int raw[KXTJ2_1009_AXES_NUM])
+{  
+	/*raw: the raw calibration data; act: the actual calibration data*/
+	struct kxtj2_1009_i2c_data *obj = i2c_get_clientdata(client);
+    #ifdef SW_CALIBRATION
+    #else
+	int err;
+    #endif
+	int mul;
+
+ 
+
+	#ifdef SW_CALIBRATION
+		mul = 0;//only SW Calibration, disable HW Calibration
+	#else
+		if(err = KXTJ2_1009_ReadOffset(client, obj->offset))
+		{
+			GSE_ERR("read offset fail, %d\n", err);
+			return err;
+		}   
+		mul = obj->reso->sensitivity/kxtj2_1009_offset_resolution.sensitivity;
+	#endif
+	
+	raw[KXTJ2_1009_AXIS_X] = obj->offset[KXTJ2_1009_AXIS_X]*mul + obj->cali_sw[KXTJ2_1009_AXIS_X];
+	raw[KXTJ2_1009_AXIS_Y] = obj->offset[KXTJ2_1009_AXIS_Y]*mul + obj->cali_sw[KXTJ2_1009_AXIS_Y];
+	raw[KXTJ2_1009_AXIS_Z] = obj->offset[KXTJ2_1009_AXIS_Z]*mul + obj->cali_sw[KXTJ2_1009_AXIS_Z];
+
+	act[obj->cvt.map[KXTJ2_1009_AXIS_X]] = obj->cvt.sign[KXTJ2_1009_AXIS_X]*raw[KXTJ2_1009_AXIS_X];
+	act[obj->cvt.map[KXTJ2_1009_AXIS_Y]] = obj->cvt.sign[KXTJ2_1009_AXIS_Y]*raw[KXTJ2_1009_AXIS_Y];
+	act[obj->cvt.map[KXTJ2_1009_AXIS_Z]] = obj->cvt.sign[KXTJ2_1009_AXIS_Z]*raw[KXTJ2_1009_AXIS_Z];                        
+	                       
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int KXTJ2_1009_WriteCalibration(struct i2c_client *client, int dat[KXTJ2_1009_AXES_NUM])
+{
+	struct kxtj2_1009_i2c_data *obj = i2c_get_clientdata(client);
+	int err;
+	int cali[KXTJ2_1009_AXES_NUM], raw[KXTJ2_1009_AXES_NUM];
+#ifdef SW_CALIBRATION
+#else
+    int lsb = kxtj2_1009_offset_resolution.sensitivity;
+	int divisor = obj->reso->sensitivity/lsb;
+#endif
+
+	if(0 != (err = KXTJ2_1009_ReadCalibrationEx(client, cali, raw)))	/*offset will be updated in obj->offset*/
+	{ 
+		GSE_ERR("read offset fail, %d\n", err);
+		return err;
+	}
+
+	GSE_LOG("OLDOFF: (%+3d %+3d %+3d): (%+3d %+3d %+3d) / (%+3d %+3d %+3d)\n", 
+		raw[KXTJ2_1009_AXIS_X], raw[KXTJ2_1009_AXIS_Y], raw[KXTJ2_1009_AXIS_Z],
+		obj->offset[KXTJ2_1009_AXIS_X], obj->offset[KXTJ2_1009_AXIS_Y], obj->offset[KXTJ2_1009_AXIS_Z],
+		obj->cali_sw[KXTJ2_1009_AXIS_X], obj->cali_sw[KXTJ2_1009_AXIS_Y], obj->cali_sw[KXTJ2_1009_AXIS_Z]);
+
+	/*calculate the real offset expected by caller*/
+	cali[KXTJ2_1009_AXIS_X] += dat[KXTJ2_1009_AXIS_X];
+	cali[KXTJ2_1009_AXIS_Y] += dat[KXTJ2_1009_AXIS_Y];
+	cali[KXTJ2_1009_AXIS_Z] += dat[KXTJ2_1009_AXIS_Z];
+
+	GSE_LOG("UPDATE: (%+3d %+3d %+3d)\n", 
+		dat[KXTJ2_1009_AXIS_X], dat[KXTJ2_1009_AXIS_Y], dat[KXTJ2_1009_AXIS_Z]);
+
+#ifdef SW_CALIBRATION
+	obj->cali_sw[KXTJ2_1009_AXIS_X] = obj->cvt.sign[KXTJ2_1009_AXIS_X]*(cali[obj->cvt.map[KXTJ2_1009_AXIS_X]]);
+	obj->cali_sw[KXTJ2_1009_AXIS_Y] = obj->cvt.sign[KXTJ2_1009_AXIS_Y]*(cali[obj->cvt.map[KXTJ2_1009_AXIS_Y]]);
+	obj->cali_sw[KXTJ2_1009_AXIS_Z] = obj->cvt.sign[KXTJ2_1009_AXIS_Z]*(cali[obj->cvt.map[KXTJ2_1009_AXIS_Z]]);	
+#else
+	obj->offset[KXTJ2_1009_AXIS_X] = (s8)(obj->cvt.sign[KXTJ2_1009_AXIS_X]*(cali[obj->cvt.map[KXTJ2_1009_AXIS_X]])/(divisor));
+	obj->offset[KXTJ2_1009_AXIS_Y] = (s8)(obj->cvt.sign[KXTJ2_1009_AXIS_Y]*(cali[obj->cvt.map[KXTJ2_1009_AXIS_Y]])/(divisor));
+	obj->offset[KXTJ2_1009_AXIS_Z] = (s8)(obj->cvt.sign[KXTJ2_1009_AXIS_Z]*(cali[obj->cvt.map[KXTJ2_1009_AXIS_Z]])/(divisor));
+
+	/*convert software calibration using standard calibration*/
+	obj->cali_sw[KXTJ2_1009_AXIS_X] = obj->cvt.sign[KXTJ2_1009_AXIS_X]*(cali[obj->cvt.map[KXTJ2_1009_AXIS_X]])%(divisor);
+	obj->cali_sw[KXTJ2_1009_AXIS_Y] = obj->cvt.sign[KXTJ2_1009_AXIS_Y]*(cali[obj->cvt.map[KXTJ2_1009_AXIS_Y]])%(divisor);
+	obj->cali_sw[KXTJ2_1009_AXIS_Z] = obj->cvt.sign[KXTJ2_1009_AXIS_Z]*(cali[obj->cvt.map[KXTJ2_1009_AXIS_Z]])%(divisor);
+
+	GSE_LOG("NEWOFF: (%+3d %+3d %+3d): (%+3d %+3d %+3d) / (%+3d %+3d %+3d)\n", 
+		obj->offset[KXTJ2_1009_AXIS_X]*divisor + obj->cali_sw[KXTJ2_1009_AXIS_X], 
+		obj->offset[KXTJ2_1009_AXIS_Y]*divisor + obj->cali_sw[KXTJ2_1009_AXIS_Y], 
+		obj->offset[KXTJ2_1009_AXIS_Z]*divisor + obj->cali_sw[KXTJ2_1009_AXIS_Z], 
+		obj->offset[KXTJ2_1009_AXIS_X], obj->offset[KXTJ2_1009_AXIS_Y], obj->offset[KXTJ2_1009_AXIS_Z],
+		obj->cali_sw[KXTJ2_1009_AXIS_X], obj->cali_sw[KXTJ2_1009_AXIS_Y], obj->cali_sw[KXTJ2_1009_AXIS_Z]);
+
+	if(err = hwmsen_write_block(obj->client, KXTJ2_1009_REG_OFSX, obj->offset, KXTJ2_1009_AXES_NUM))
+	{
+		GSE_ERR("write offset fail: %d\n", err);
+		return err;
+	}
+#endif
+
+	return err;
+}
+/*----------------------------------------------------------------------------*/
+static int KXTJ2_1009_CheckDeviceID(struct i2c_client *client)
+{
+	u8 databuf[10];    
+	int res = 0;
+
+	memset(databuf, 0, sizeof(u8)*10);    
+	databuf[0] = KXTJ2_1009_REG_DEVID;   
+
+	res = i2c_master_send(client, databuf, 0x1);
+	if(res <= 0)
+	{
+		goto exit_KXTJ2_1009_CheckDeviceID;
+	}
+	
+	udelay(500);
+
+	databuf[0] = 0x0;        
+	res = i2c_master_recv(client, databuf, 0x01);
+	if(res <= 0)
+	{
+		goto exit_KXTJ2_1009_CheckDeviceID;
+	}
+	
+
+	if(false)
+	{
+		printk("KXTJ2_1009_CheckDeviceID 0x%x failt!\n ", databuf[0]);
+		return KXTJ2_1009_ERR_IDENTIFICATION;
+	}
+	else
+	{
+		printk("KXTJ2_1009_CheckDeviceID 0x%x pass!\n ", databuf[0]);
+	}
+	
+	exit_KXTJ2_1009_CheckDeviceID:
+	if (res <= 0)
+	{
+		return KXTJ2_1009_ERR_I2C;
+	}
+	
+	return KXTJ2_1009_SUCCESS;
+}
+/*----------------------------------------------------------------------------*/
+static int KXTJ2_1009_SetPowerMode(struct i2c_client *client, bool enable)
+{
+	u8 databuf[2];    
+	int res = 0;
+	u8 addr = KXTJ2_1009_REG_POWER_CTL;
+	
+	if(enable == sensor_power)
+	{
+		GSE_LOG("Sensor power status is newest!\n");
+		return KXTJ2_1009_SUCCESS;
+	}
+
+	if(hwmsen_read_block(client, addr, databuf, 0x01))
+	{
+		GSE_ERR("read power ctl register err!\n");
+		return KXTJ2_1009_ERR_I2C;
+	}
+
+	
+	if(enable == TRUE)
+	{
+		databuf[0] |= KXTJ2_1009_MEASURE_MODE;
+	}
+	else
+	{
+		databuf[0] &= ~KXTJ2_1009_MEASURE_MODE;
+	}
+	databuf[1] = databuf[0];
+	databuf[0] = KXTJ2_1009_REG_POWER_CTL;
+	
+
+	res = i2c_master_send(client, databuf, 0x2);
+
+	if(res <= 0)
+	{
+		return KXTJ2_1009_ERR_I2C;
+	}
+
+
+	GSE_LOG("KXTJ2_1009_SetPowerMode %d!\n ",enable);
+
+
+	sensor_power = enable;
+
+	mdelay(5);
+	
+	return KXTJ2_1009_SUCCESS;    
+}
+/*----------------------------------------------------------------------------*/
+static int KXTJ2_1009_SetDataFormat(struct i2c_client *client, u8 dataformat)
+{
+	struct kxtj2_1009_i2c_data *obj = i2c_get_clientdata(client);
+	u8 databuf[10];    
+	int res = 0;
+	bool cur_sensor_power = sensor_power;
+
+	memset(databuf, 0, sizeof(u8)*10);  
+
+	KXTJ2_1009_SetPowerMode(client, false);
+
+	if(hwmsen_read_block(client, KXTJ2_1009_REG_DATA_FORMAT, databuf, 0x01))
+	{
+		printk("kxtj2_1009 read Dataformat failt \n");
+		return KXTJ2_1009_ERR_I2C;
+	}
+
+	databuf[0] &= ~KXTJ2_1009_RANGE_MASK;
+	databuf[0] |= dataformat;
+	databuf[1] = databuf[0];
+	databuf[0] = KXTJ2_1009_REG_DATA_FORMAT;
+
+
+	res = i2c_master_send(client, databuf, 0x2);
+
+	if(res <= 0)
+	{
+		return KXTJ2_1009_ERR_I2C;
+	}
+
+	KXTJ2_1009_SetPowerMode(client, cur_sensor_power/*true*/);
+	
+	printk("KXTJ2_1009_SetDataFormat OK! \n");
+	
+
+	return KXTJ2_1009_SetDataResolution(obj);    
+}
+/*----------------------------------------------------------------------------*/
+static int KXTJ2_1009_SetBWRate(struct i2c_client *client, u8 bwrate)
+{
+	u8 databuf[10];    
+	int res = 0;
+	bool cur_sensor_power = sensor_power;
+
+	memset(databuf, 0, sizeof(u8)*10);    
+
+	
+	KXTJ2_1009_SetPowerMode(client, false);
+
+	if(hwmsen_read_block(client, KXTJ2_1009_REG_BW_RATE, databuf, 0x01))
+	{
+		printk("kxtj2_1009 read rate failt \n");
+		return KXTJ2_1009_ERR_I2C;
+	}
+
+	databuf[0] &= 0xf0;
+	databuf[0] |= bwrate;
+	databuf[1] = databuf[0];
+	databuf[0] = KXTJ2_1009_REG_BW_RATE;
+
+
+	res = i2c_master_send(client, databuf, 0x2);
+
+	if(res <= 0)
+	{
+		return KXTJ2_1009_ERR_I2C;
+	}
+
+	
+	KXTJ2_1009_SetPowerMode(client, cur_sensor_power/*true*/);
+	printk("KXTJ2_1009_SetBWRate OK! \n");
+	
+	return KXTJ2_1009_SUCCESS;    
+}
+/*----------------------------------------------------------------------------*/
+static int KXTJ2_1009_SetIntEnable(struct i2c_client *client, u8 intenable)
+{
+	u8 databuf[10];    
+	int res = 0;
+
+	memset(databuf, 0, sizeof(u8)*10);    
+	databuf[0] = KXTJ2_1009_REG_INT_ENABLE;    
+	databuf[1] = 0x00;
+
+	res = i2c_master_send(client, databuf, 0x2);
+
+	if(res <= 0)
+	{
+		return KXTJ2_1009_ERR_I2C;
+	}
+	
+	return KXTJ2_1009_SUCCESS;    
+}
+/*----------------------------------------------------------------------------*/
+static int kxtj2_1009_init_client(struct i2c_client *client, int reset_cali)
+{
+	struct kxtj2_1009_i2c_data *obj = i2c_get_clientdata(client);
+	int res = 0;
+
+	res = KXTJ2_1009_CheckDeviceID(client); 
+	if(res != KXTJ2_1009_SUCCESS)
+	{
+		return res;
+	}	
+
+	res = KXTJ2_1009_SetPowerMode(client, enable_status/*false*/);
+	if(res != KXTJ2_1009_SUCCESS)
+	{
+		return res;
+	}
+	
+
+	res = KXTJ2_1009_SetBWRate(client, KXTJ2_1009_BW_100HZ);
+	if(res != KXTJ2_1009_SUCCESS ) //0x2C->BW=100Hz
+	{
+		return res;
+	}
+
+	res = KXTJ2_1009_SetDataFormat(client, KXTJ2_1009_RANGE_2G);
+	if(res != KXTJ2_1009_SUCCESS) //0x2C->BW=100Hz
+	{
+		return res;
+	}
+
+	gsensor_gain.x = gsensor_gain.y = gsensor_gain.z = obj->reso->sensitivity;
+
+
+	res = KXTJ2_1009_SetIntEnable(client, 0x00);        
+	if(res != KXTJ2_1009_SUCCESS)//0x2E->0x80
+	{
+		return res;
+	}
+
+	if(0 != reset_cali)
+	{ 
+		/*reset calibration only in power on*/
+		res = KXTJ2_1009_ResetCalibration(client);
+		if(res != KXTJ2_1009_SUCCESS)
+		{
+			return res;
+		}
+	}
+	printk("kxtj2_1009_init_client OK!\n");
+#ifdef CONFIG_KXTJ2_1009_LOWPASS
+	memset(&obj->fir, 0x00, sizeof(obj->fir));  
+#endif
+
+	return KXTJ2_1009_SUCCESS;
+}
+/*----------------------------------------------------------------------------*/
+static int KXTJ2_1009_ReadChipInfo(struct i2c_client *client, char *buf, int bufsize)
+{
+	u8 databuf[10];    
+
+	memset(databuf, 0, sizeof(u8)*10);
+
+	if((NULL == buf)||(bufsize<=30))
+	{
+		return -1;
+	}
+	
+	if(NULL == client)
+	{
+		*buf = 0;
+		return -2;
+	}
+
+	sprintf(buf, "KXTJ2_1009 Chip");
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int KXTJ2_1009_ReadSensorData(struct i2c_client *client, char *buf, int bufsize)
+{
+	struct kxtj2_1009_i2c_data *obj = (struct kxtj2_1009_i2c_data*)i2c_get_clientdata(client);
+	u8 databuf[20];
+	int acc[KXTJ2_1009_AXES_NUM];
+	int res = 0;
+	memset(databuf, 0, sizeof(u8)*10);
+
+	if(NULL == buf)
+	{
+		return -1;
+	}
+	if(NULL == client)
+	{
+		*buf = 0;
+		return -2;
+	}
+
+	if (atomic_read(&obj->suspend))
+	{
+		return 0;
+	}
+	/*if(sensor_power == FALSE)
+	{
+		res = KXTJ2_1009_SetPowerMode(client, true);
+		if(res)
+		{
+			GSE_ERR("Power on kxtj2_1009 error %d!\n", res);
+		}
+	}*/
+
+	if(0 != (res = KXTJ2_1009_ReadData(client, obj->data)))
+	{        
+		GSE_ERR("I2C error: ret value=%d", res);
+		return -3;
+	}
+	else
+	{
+		//printk("raw data x=%d, y=%d, z=%d \n",obj->data[KXTJ2_1009_AXIS_X],obj->data[KXTJ2_1009_AXIS_Y],obj->data[KXTJ2_1009_AXIS_Z]);
+		obj->data[KXTJ2_1009_AXIS_X] += obj->cali_sw[KXTJ2_1009_AXIS_X];
+		obj->data[KXTJ2_1009_AXIS_Y] += obj->cali_sw[KXTJ2_1009_AXIS_Y];
+		obj->data[KXTJ2_1009_AXIS_Z] += obj->cali_sw[KXTJ2_1009_AXIS_Z];
+		
+		//printk("cali_sw x=%d, y=%d, z=%d \n",obj->cali_sw[KXTJ2_1009_AXIS_X],obj->cali_sw[KXTJ2_1009_AXIS_Y],obj->cali_sw[KXTJ2_1009_AXIS_Z]);
+		
+		/*remap coordinate*/
+		acc[obj->cvt.map[KXTJ2_1009_AXIS_X]] = obj->cvt.sign[KXTJ2_1009_AXIS_X]*obj->data[KXTJ2_1009_AXIS_X];
+		acc[obj->cvt.map[KXTJ2_1009_AXIS_Y]] = obj->cvt.sign[KXTJ2_1009_AXIS_Y]*obj->data[KXTJ2_1009_AXIS_Y];
+		acc[obj->cvt.map[KXTJ2_1009_AXIS_Z]] = obj->cvt.sign[KXTJ2_1009_AXIS_Z]*obj->data[KXTJ2_1009_AXIS_Z];
+		//printk("cvt x=%d, y=%d, z=%d \n",obj->cvt.sign[KXTJ2_1009_AXIS_X],obj->cvt.sign[KXTJ2_1009_AXIS_Y],obj->cvt.sign[KXTJ2_1009_AXIS_Z]);
+
+
+		//GSE_LOG("Mapped gsensor data: %d, %d, %d!\n", acc[KXTJ2_1009_AXIS_X], acc[KXTJ2_1009_AXIS_Y], acc[KXTJ2_1009_AXIS_Z]);
+
+		//Out put the mg
+		//printk("mg acc=%d, GRAVITY=%d, sensityvity=%d \n",acc[KXTJ2_1009_AXIS_X],GRAVITY_EARTH_1000,obj->reso->sensitivity);
+		acc[KXTJ2_1009_AXIS_X] = acc[KXTJ2_1009_AXIS_X] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+		acc[KXTJ2_1009_AXIS_Y] = acc[KXTJ2_1009_AXIS_Y] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+		acc[KXTJ2_1009_AXIS_Z] = acc[KXTJ2_1009_AXIS_Z] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;		
+		
+	
+
+		sprintf(buf, "%04x %04x %04x", acc[KXTJ2_1009_AXIS_X], acc[KXTJ2_1009_AXIS_Y], acc[KXTJ2_1009_AXIS_Z]);
+		if(atomic_read(&obj->trace) & ADX_TRC_IOCTL)
+		{
+			GSE_LOG("gsensor data: %s!\n", buf);
+		}
+	}
+	
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int KXTJ2_1009_ReadRawData(struct i2c_client *client, char *buf)
+{
+	struct kxtj2_1009_i2c_data *obj = (struct kxtj2_1009_i2c_data*)i2c_get_clientdata(client);
+	int res = 0;
+
+	if (!buf || !client)
+	{
+		return EINVAL;
+	}
+	
+	if(0 != (res = KXTJ2_1009_ReadData(client, obj->data)))
+	{        
+		GSE_ERR("I2C error: ret value=%d", res);
+		return EIO;
+	}
+	else
+	{
+		sprintf(buf, "KXTJ2_1009_ReadRawData %04x %04x %04x", obj->data[KXTJ2_1009_AXIS_X], 
+			obj->data[KXTJ2_1009_AXIS_Y], obj->data[KXTJ2_1009_AXIS_Z]);
+	
+	}
+	
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int KXTJ2_1009_InitSelfTest(struct i2c_client *client)
+{
+	int res = 0;
+	u8  data,result;
+	
+	res = hwmsen_read_byte(client, KXTJ2_1009_REG_CTL_REG3, &data);
+	if(res != KXTJ2_1009_SUCCESS)
+	{
+		return res;
+	}
+//enable selftest bit
+	res = hwmsen_write_byte(client, KXTJ2_1009_REG_CTL_REG3,  KXTJ2_1009_SELF_TEST|data);
+	if(res != KXTJ2_1009_SUCCESS) //0x2C->BW=100Hz
+	{
+		return res;
+	}
+//step 1
+	res = hwmsen_read_byte(client, KXTJ2_1009_DCST_RESP, &result);
+	if(res != KXTJ2_1009_SUCCESS)
+	{
+		return res;
+	}
+	printk("step1: result = %x",result);
+	if(result != 0xaa)
+		return -EINVAL;
+
+//step 2
+	res = hwmsen_write_byte(client, KXTJ2_1009_REG_CTL_REG3,  KXTJ2_1009_SELF_TEST|data);
+	if(res != KXTJ2_1009_SUCCESS) //0x2C->BW=100Hz
+	{
+		return res;
+	}
+//step 3
+	res = hwmsen_read_byte(client, KXTJ2_1009_DCST_RESP, &result);
+	if(res != KXTJ2_1009_SUCCESS)
+	{
+		return res;
+	}
+	printk("step3: result = %x",result);
+	if(result != 0xAA)
+		return -EINVAL;
+		
+//step 4
+	res = hwmsen_read_byte(client, KXTJ2_1009_DCST_RESP, &result);
+	if(res != KXTJ2_1009_SUCCESS)
+	{
+		return res;
+	}
+	printk("step4: result = %x",result);
+	if(result != 0x55)
+		return -EINVAL;
+	else
+		return KXTJ2_1009_SUCCESS;
+}
+/*----------------------------------------------------------------------------*/
+#if 0
+static int KXTJ2_1009_JudgeTestResult(struct i2c_client *client, s32 prv[KXTJ2_1009_AXES_NUM], s32 nxt[KXTJ2_1009_AXES_NUM])
+{
+
+    int res=0;
+	u8 test_result=0;
+    if(0 != (res = hwmsen_read_byte(client, 0x0c, &test_result)))
+        return res;
+
+	printk("test_result = %x \n",test_result);
+    if ( test_result != 0xaa ) 
+	{
+        GSE_ERR("KXTJ2_1009_JudgeTestResult failt\n");
+        res = -EINVAL;
+    }
+    return res;
+}
+#endif
+/*----------------------------------------------------------------------------*/
+static ssize_t show_chipinfo_value(struct device_driver *ddri, char *buf)
+{
+	struct i2c_client *client = kxtj2_1009_i2c_client;
+	char strbuf[KXTJ2_1009_BUFSIZE];
+	if(NULL == client)
+	{
+		GSE_ERR("i2c client is null!!\n");
+		return 0;
+	}
+	
+	KXTJ2_1009_ReadChipInfo(client, strbuf, KXTJ2_1009_BUFSIZE);
+	return snprintf(buf, PAGE_SIZE, "%s\n", strbuf);        
+}
+#if 0
+static ssize_t gsensor_init(struct device_driver *ddri, char *buf, size_t count)
+	{
+		struct i2c_client *client = kxtj2_1009_i2c_client;
+		char strbuf[KXTJ2_1009_BUFSIZE];
+		
+		if(NULL == client)
+		{
+			GSE_ERR("i2c client is null!!\n");
+			return 0;
+		}
+		kxtj2_1009_init_client(client, 1);
+		return snprintf(buf, PAGE_SIZE, "%s\n", strbuf);			
+	}
+#endif
+/*----------------------------------------------------------------------------*/
+static ssize_t show_sensordata_value(struct device_driver *ddri, char *buf)
+{
+	struct i2c_client *client = kxtj2_1009_i2c_client;
+	char strbuf[KXTJ2_1009_BUFSIZE];
+	
+	if(NULL == client)
+	{
+		GSE_ERR("i2c client is null!!\n");
+		return 0;
+	}
+	KXTJ2_1009_ReadSensorData(client, strbuf, KXTJ2_1009_BUFSIZE);
+	//KXTJ2_1009_ReadRawData(client, strbuf);
+	return snprintf(buf, PAGE_SIZE, "%s\n", strbuf);            
+}
+#if 0
+static ssize_t show_sensorrawdata_value(struct device_driver *ddri, char *buf, size_t count)
+	{
+		struct i2c_client *client = kxtj2_1009_i2c_client;
+		char strbuf[KXTJ2_1009_BUFSIZE];
+		
+		if(NULL == client)
+		{
+			GSE_ERR("i2c client is null!!\n");
+			return 0;
+		}
+		//KXTJ2_1009_ReadSensorData(client, strbuf, KXTJ2_1009_BUFSIZE);
+		KXTJ2_1009_ReadRawData(client, strbuf);
+		return snprintf(buf, PAGE_SIZE, "%s\n", strbuf);			
+	}
+#endif
+/*----------------------------------------------------------------------------*/
+static ssize_t show_cali_value(struct device_driver *ddri, char *buf)
+{
+	struct i2c_client *client = kxtj2_1009_i2c_client;
+	struct kxtj2_1009_i2c_data *obj;
+	int err, len = 0, mul;
+	int tmp[KXTJ2_1009_AXES_NUM];
+
+	if(NULL == client)
+	{
+		GSE_ERR("i2c client is null!!\n");
+		return 0;
+	}
+
+	obj = i2c_get_clientdata(client);
+
+
+
+	if(0 != (err = KXTJ2_1009_ReadOffset(client, obj->offset)))
+	{
+		return -EINVAL;
+	}
+	else if(0 != (err = KXTJ2_1009_ReadCalibration(client, tmp)))
+	{
+		return -EINVAL;
+	}
+	else
+	{    
+		mul = obj->reso->sensitivity/kxtj2_1009_offset_resolution.sensitivity;
+		len += snprintf(buf+len, PAGE_SIZE-len, "[HW ][%d] (%+3d, %+3d, %+3d) : (0x%02X, 0x%02X, 0x%02X)\n", mul,                        
+			obj->offset[KXTJ2_1009_AXIS_X], obj->offset[KXTJ2_1009_AXIS_Y], obj->offset[KXTJ2_1009_AXIS_Z],
+			obj->offset[KXTJ2_1009_AXIS_X], obj->offset[KXTJ2_1009_AXIS_Y], obj->offset[KXTJ2_1009_AXIS_Z]);
+		len += snprintf(buf+len, PAGE_SIZE-len, "[SW ][%d] (%+3d, %+3d, %+3d)\n", 1, 
+			obj->cali_sw[KXTJ2_1009_AXIS_X], obj->cali_sw[KXTJ2_1009_AXIS_Y], obj->cali_sw[KXTJ2_1009_AXIS_Z]);
+
+		len += snprintf(buf+len, PAGE_SIZE-len, "[ALL]    (%+3d, %+3d, %+3d) : (%+3d, %+3d, %+3d)\n", 
+			obj->offset[KXTJ2_1009_AXIS_X]*mul + obj->cali_sw[KXTJ2_1009_AXIS_X],
+			obj->offset[KXTJ2_1009_AXIS_Y]*mul + obj->cali_sw[KXTJ2_1009_AXIS_Y],
+			obj->offset[KXTJ2_1009_AXIS_Z]*mul + obj->cali_sw[KXTJ2_1009_AXIS_Z],
+			tmp[KXTJ2_1009_AXIS_X], tmp[KXTJ2_1009_AXIS_Y], tmp[KXTJ2_1009_AXIS_Z]);
+		
+		return len;
+    }
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t store_cali_value(struct device_driver *ddri, const char *buf, size_t count)
+{
+	struct i2c_client *client = kxtj2_1009_i2c_client;  
+	int err, x, y, z;
+	int dat[KXTJ2_1009_AXES_NUM];
+
+	if(!strncmp(buf, "rst", 3))
+	{
+		if(0 != (err = KXTJ2_1009_ResetCalibration(client)))
+		{
+			GSE_ERR("reset offset err = %d\n", err);
+		}	
+	}
+	else if(3 == sscanf(buf, "0x%02X 0x%02X 0x%02X", &x, &y, &z))
+	{
+		dat[KXTJ2_1009_AXIS_X] = x;
+		dat[KXTJ2_1009_AXIS_Y] = y;
+		dat[KXTJ2_1009_AXIS_Z] = z;
+		if(0 != (err = KXTJ2_1009_WriteCalibration(client, dat)))
+		{
+			GSE_ERR("write calibration err = %d\n", err);
+		}		
+	}
+	else
+	{
+		GSE_ERR("invalid format\n");
+	}
+	
+	return count;
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t show_self_value(struct device_driver *ddri, char *buf)
+{
+	struct i2c_client *client = kxtj2_1009_i2c_client;
+
+	if(NULL == client)
+	{
+		GSE_ERR("i2c client is null!!\n");
+		return 0;
+	}
+	
+    return snprintf(buf, 8, "%s\n", selftestRes);
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t store_self_value(struct device_driver *ddri, const char *buf, size_t count)
+{   /*write anything to this register will trigger the process*/
+	struct item{
+	s16 raw[KXTJ2_1009_AXES_NUM];
+	};
+	
+	struct i2c_client *client = kxtj2_1009_i2c_client;  
+	int res, num;
+	struct item *prv = NULL, *nxt = NULL;
+	u8 data;
+
+	if(1 != sscanf(buf, "%d", &num))
+	{
+		GSE_ERR("parse number fail\n");
+		return count;
+	}
+	else if(num == 0)
+	{
+		GSE_ERR("invalid data count\n");
+		return count;
+	}
+
+	prv = kzalloc(sizeof(*prv) * num, GFP_KERNEL);
+	nxt = kzalloc(sizeof(*nxt) * num, GFP_KERNEL);
+	if (!prv || !nxt)
+	{
+		goto exit;
+	}
+
+
+	GSE_LOG("NORMAL:\n");
+	KXTJ2_1009_SetPowerMode(client,true); 
+
+	/*initial setting for self test*/
+	if(!KXTJ2_1009_InitSelfTest(client))
+	{
+		GSE_LOG("SELFTEST : PASS\n");
+		strcpy(selftestRes,"y");
+	}	
+	else
+	{
+		GSE_LOG("SELFTEST : FAIL\n");		
+		strcpy(selftestRes,"n");
+	}
+
+	res = hwmsen_read_byte(client, KXTJ2_1009_REG_CTL_REG3, &data);
+	if(res != KXTJ2_1009_SUCCESS)
+	{
+		return res;
+	}
+
+	res = hwmsen_write_byte(client, KXTJ2_1009_REG_CTL_REG3,  ~KXTJ2_1009_SELF_TEST&data);
+	if(res != KXTJ2_1009_SUCCESS) //0x2C->BW=100Hz
+	{
+		return res;
+	}
+	
+	exit:
+	/*restore the setting*/    
+	kxtj2_1009_init_client(client, 0);
+	kfree(prv);
+	kfree(nxt);
+	return count;
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t show_selftest_value(struct device_driver *ddri, char *buf)
+{
+	struct i2c_client *client = kxtj2_1009_i2c_client;
+	struct kxtj2_1009_i2c_data *obj;
+
+	if(NULL == client)
+	{
+		GSE_ERR("i2c client is null!!\n");
+		return 0;
+	}
+
+	obj = i2c_get_clientdata(client);
+	return snprintf(buf, PAGE_SIZE, "%d\n", atomic_read(&obj->selftest));
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t store_selftest_value(struct device_driver *ddri, const char *buf, size_t count)
+{
+	struct kxtj2_1009_i2c_data *obj = obj_i2c_data;
+	int tmp;
+
+	if(NULL == obj)
+	{
+		GSE_ERR("i2c data obj is null!!\n");
+		return 0;
+	}
+	
+	
+	if(1 == sscanf(buf, "%d", &tmp))
+	{        
+		if(atomic_read(&obj->selftest) && !tmp)
+		{
+			/*enable -> disable*/
+			kxtj2_1009_init_client(obj->client, 0);
+		}
+		else if(!atomic_read(&obj->selftest) && tmp)
+		{
+			/*disable -> enable*/
+			KXTJ2_1009_InitSelfTest(obj->client);            
+		}
+		
+		GSE_LOG("selftest: %d => %d\n", atomic_read(&obj->selftest), tmp);
+		atomic_set(&obj->selftest, tmp); 
+	}
+	else
+	{ 
+		GSE_ERR("invalid content: '%s', length = %d\n", buf, count);   
+	}
+	return count;
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t show_firlen_value(struct device_driver *ddri, char *buf)
+{
+#ifdef CONFIG_KXTJ2_1009_LOWPASS
+	struct i2c_client *client = kxtj2_1009_i2c_client;
+	struct kxtj2_1009_i2c_data *obj = i2c_get_clientdata(client);
+	if(atomic_read(&obj->firlen))
+	{
+		int idx, len = atomic_read(&obj->firlen);
+		GSE_LOG("len = %2d, idx = %2d\n", obj->fir.num, obj->fir.idx);
+
+		for(idx = 0; idx < len; idx++)
+		{
+			GSE_LOG("[%5d %5d %5d]\n", obj->fir.raw[idx][KXTJ2_1009_AXIS_X], obj->fir.raw[idx][KXTJ2_1009_AXIS_Y], obj->fir.raw[idx][KXTJ2_1009_AXIS_Z]);
+		}
+		
+		GSE_LOG("sum = [%5d %5d %5d]\n", obj->fir.sum[KXTJ2_1009_AXIS_X], obj->fir.sum[KXTJ2_1009_AXIS_Y], obj->fir.sum[KXTJ2_1009_AXIS_Z]);
+		GSE_LOG("avg = [%5d %5d %5d]\n", obj->fir.sum[KXTJ2_1009_AXIS_X]/len, obj->fir.sum[KXTJ2_1009_AXIS_Y]/len, obj->fir.sum[KXTJ2_1009_AXIS_Z]/len);
+	}
+	return snprintf(buf, PAGE_SIZE, "%d\n", atomic_read(&obj->firlen));
+#else
+	return snprintf(buf, PAGE_SIZE, "not support\n");
+#endif
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t store_firlen_value(struct device_driver *ddri, const char *buf, size_t count)
+{
+#ifdef CONFIG_KXTJ2_1009_LOWPASS
+	struct i2c_client *client = kxtj2_1009_i2c_client;  
+	struct kxtj2_1009_i2c_data *obj = i2c_get_clientdata(client);
+	int firlen;
+
+	if(1 != sscanf(buf, "%d", &firlen))
+	{
+		GSE_ERR("invallid format\n");
+	}
+	else if(firlen > C_MAX_FIR_LENGTH)
+	{
+		GSE_ERR("exceeds maximum filter length\n");
+	}
+	else
+	{ 
+		atomic_set(&obj->firlen, firlen);
+		if(NULL == firlen)
+		{
+			atomic_set(&obj->fir_en, 0);
+		}
+		else
+		{
+			memset(&obj->fir, 0x00, sizeof(obj->fir));
+			atomic_set(&obj->fir_en, 1);
+		}
+	}
+#endif    
+	return count;
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t show_trace_value(struct device_driver *ddri, char *buf)
+{
+	ssize_t res;
+	struct kxtj2_1009_i2c_data *obj = obj_i2c_data;
+	if (obj == NULL)
+	{
+		GSE_ERR("i2c_data obj is null!!\n");
+		return 0;
+	}
+	
+	res = snprintf(buf, PAGE_SIZE, "0x%04X\n", atomic_read(&obj->trace));     
+	return res;    
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t store_trace_value(struct device_driver *ddri, const char *buf, size_t count)
+{
+	struct kxtj2_1009_i2c_data *obj = obj_i2c_data;
+	int trace;
+	if (obj == NULL)
+	{
+		GSE_ERR("i2c_data obj is null!!\n");
+		return 0;
+	}
+	
+	if(1 == sscanf(buf, "0x%x", &trace))
+	{
+		atomic_set(&obj->trace, trace);
+	}	
+	else
+	{
+		GSE_ERR("invalid content: '%s', length = %d\n", buf, count);
+	}
+	
+	return count;    
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t show_status_value(struct device_driver *ddri, char *buf)
+{
+	ssize_t len = 0;    
+	struct kxtj2_1009_i2c_data *obj = obj_i2c_data;
+	if (obj == NULL)
+	{
+		GSE_ERR("i2c_data obj is null!!\n");
+		return 0;
+	}	
+	
+	if(obj->hw)
+	{
+		len += snprintf(buf+len, PAGE_SIZE-len, "CUST: %d %d (%d %d)\n", 
+	            obj->hw->i2c_num, obj->hw->direction, obj->hw->power_id, obj->hw->power_vol);   
+	}
+	else
+	{
+		len += snprintf(buf+len, PAGE_SIZE-len, "CUST: NULL\n");
+	}
+	return len;    
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t show_power_status_value(struct device_driver *ddri, char *buf)
+{
+	u8 databuf[2];    
+	u8 addr = KXTJ2_1009_REG_POWER_CTL;
+	if(hwmsen_read_block(kxtj2_1009_i2c_client, addr, databuf, 0x01))
+	{
+		GSE_ERR("read power ctl register err!\n");
+		return KXTJ2_1009_ERR_I2C;
+	}
+    
+	if(sensor_power)
+		printk("G sensor is in work mode, sensor_power = %d\n", sensor_power);
+	else
+		printk("G sensor is in standby mode, sensor_power = %d\n", sensor_power);
+
+	return snprintf(buf, PAGE_SIZE, "%x\n", databuf[0]);
+}
+
+/*----------------------------------------------------------------------------*/
+static DRIVER_ATTR(chipinfo,   S_IWUSR | S_IRUGO, show_chipinfo_value,      NULL);
+static DRIVER_ATTR(sensordata, S_IWUSR | S_IRUGO, show_sensordata_value,    NULL);
+static DRIVER_ATTR(cali,       S_IWUSR | S_IRUGO, show_cali_value,          store_cali_value);
+static DRIVER_ATTR(selftest, S_IWUSR | S_IRUGO, show_self_value,  store_self_value);
+static DRIVER_ATTR(self,   S_IWUSR | S_IRUGO, show_selftest_value,      store_selftest_value);
+static DRIVER_ATTR(firlen,     S_IWUSR | S_IRUGO, show_firlen_value,        store_firlen_value);
+static DRIVER_ATTR(trace,      S_IWUSR | S_IRUGO, show_trace_value,         store_trace_value);
+static DRIVER_ATTR(status,               S_IRUGO, show_status_value,        NULL);
+static DRIVER_ATTR(powerstatus,               S_IRUGO, show_power_status_value,        NULL);
+
+
+/*----------------------------------------------------------------------------*/
+static u8 i2c_dev_reg =0 ;
+
+static ssize_t show_register(struct device_driver *pdri, char *buf)
+{
+	printk("i2c_dev_reg is 0x%2x \n", i2c_dev_reg);
+
+	return 0;
+}
+
+static ssize_t store_register(struct device_driver *ddri, const char *buf, size_t count)
+{
+	i2c_dev_reg = simple_strtoul(buf, NULL, 16);
+	printk("set i2c_dev_reg = 0x%2x \n", i2c_dev_reg);
+
+	return 0;
+}
+static ssize_t store_register_value(struct device_driver *ddri, const char *buf, size_t count)
+{
+	struct kxtj2_1009_i2c_data *obj = obj_i2c_data;
+	u8 databuf[2];  
+	unsigned long input_value;
+	int res;
+	
+	memset(databuf, 0, sizeof(u8)*2);    
+
+	input_value = simple_strtoul(buf, NULL, 16);
+	printk("input_value = 0x%2x \n", (unsigned int)input_value);
+
+	if(NULL == obj)
+	{
+		GSE_ERR("i2c data obj is null!!\n");
+		return 0;
+	}
+
+	databuf[0] = i2c_dev_reg;
+	databuf[1] = input_value;
+	printk("databuf[0]=0x%2x  databuf[1]=0x%2x \n", databuf[0],databuf[1]);
+
+	res = i2c_master_send(obj->client, databuf, 0x2);
+
+	if(res <= 0)
+	{
+		return KXTJ2_1009_ERR_I2C;
+	}
+	return 0;
+	
+}
+
+static ssize_t show_register_value(struct device_driver *ddri, char *buf)
+{
+		struct kxtj2_1009_i2c_data *obj = obj_i2c_data;
+		u8 databuf[1];	
+		
+		memset(databuf, 0, sizeof(u8)*1);	 
+	
+		if(NULL == obj)
+		{
+			GSE_ERR("i2c data obj is null!!\n");
+			return 0;
+		}
+		
+		if(hwmsen_read_block(obj->client, i2c_dev_reg, databuf, 0x01))
+		{
+			GSE_ERR("read power ctl register err!\n");
+			return KXTJ2_1009_ERR_I2C;
+		}
+
+		printk("i2c_dev_reg=0x%2x  data=0x%2x \n", i2c_dev_reg,databuf[0]);
+	
+		return 0;
+		
+}
+
+
+static DRIVER_ATTR(i2c,      S_IWUSR | S_IRUGO, show_register_value,         store_register_value);
+static DRIVER_ATTR(register,      S_IWUSR | S_IRUGO, show_register,         store_register);
+
+
+/*----------------------------------------------------------------------------*/
+static struct driver_attribute *kxtj2_1009_attr_list[] = {
+	&driver_attr_chipinfo,     /*chip information*/
+	&driver_attr_sensordata,   /*dump sensor data*/
+	&driver_attr_cali,         /*show calibration data*/
+	&driver_attr_self,         /*self test demo*/
+	&driver_attr_selftest,     /*self control: 0: disable, 1: enable*/
+	&driver_attr_firlen,       /*filter length: 0: disable, others: enable*/
+	&driver_attr_trace,        /*trace log*/
+	&driver_attr_status,
+	&driver_attr_powerstatus,
+	&driver_attr_register,
+	&driver_attr_i2c,
+};
+/*----------------------------------------------------------------------------*/
+static int kxtj2_1009_create_attr(struct device_driver *driver) 
+{
+	int idx, err = 0;
+	int num = (int)(sizeof(kxtj2_1009_attr_list)/sizeof(kxtj2_1009_attr_list[0]));
+	if (driver == NULL)
+	{
+		return -EINVAL;
+	}
+
+	for(idx = 0; idx < num; idx++)
+	{
+		if(0 != (err = driver_create_file(driver, kxtj2_1009_attr_list[idx])))
+		{            
+			GSE_ERR("driver_create_file (%s) = %d\n", kxtj2_1009_attr_list[idx]->attr.name, err);
+			break;
+		}
+	}    
+	return err;
+}
+/*----------------------------------------------------------------------------*/
+static int kxtj2_1009_delete_attr(struct device_driver *driver)
+{
+	int idx ,err = 0;
+	int num = (int)(sizeof(kxtj2_1009_attr_list)/sizeof(kxtj2_1009_attr_list[0]));
+
+	if(driver == NULL)
+	{
+		return -EINVAL;
+	}
+	
+
+	for(idx = 0; idx < num; idx++)
+	{
+		driver_remove_file(driver, kxtj2_1009_attr_list[idx]);
+	}
+	
+
+	return err;
+}
+
+/*----------------------------------------------------------------------------*/
+int gsensor_operate(void* self, uint32_t command, void* buff_in, int size_in,
+		void* buff_out, int size_out, int* actualout)
+{
+	int err = 0;
+	int value, sample_delay;	
+	struct kxtj2_1009_i2c_data *priv = (struct kxtj2_1009_i2c_data*)self;
+	hwm_sensor_data* gsensor_data;
+	char buff[KXTJ2_1009_BUFSIZE];
+	
+	//GSE_FUN(f);
+	switch (command)
+	{
+		case SENSOR_DELAY:
+			if((buff_in == NULL) || (size_in < sizeof(int)))
+			{
+				GSE_ERR("Set delay parameter error!\n");
+				err = -EINVAL;
+			}
+			else
+			{
+				value = *(int *)buff_in;
+				if(value <= 5)
+				{
+					sample_delay = KXTJ2_1009_BW_200HZ;
+				}
+				else if(value <= 10)
+				{
+					sample_delay = KXTJ2_1009_BW_100HZ;
+				}
+				else
+				{
+					sample_delay = KXTJ2_1009_BW_50HZ;
+				}
+
+				mutex_lock(&kxtj2_1009_mutex);
+				err = KXTJ2_1009_SetBWRate(priv->client, sample_delay);
+				mutex_unlock(&kxtj2_1009_mutex);
+				if(err != KXTJ2_1009_SUCCESS ) //0x2C->BW=100Hz
+				{
+					GSE_ERR("Set delay parameter error!\n");
+				}
+
+				if(value >= 50)
+				{
+					atomic_set(&priv->filter, 0);
+				}
+				else
+				{	
+				#if defined(CONFIG_KXTJ2_1009_LOWPASS)
+					priv->fir.num = 0;
+					priv->fir.idx = 0;
+					priv->fir.sum[KXTJ2_1009_AXIS_X] = 0;
+					priv->fir.sum[KXTJ2_1009_AXIS_Y] = 0;
+					priv->fir.sum[KXTJ2_1009_AXIS_Z] = 0;
+					atomic_set(&priv->filter, 1);
+				#endif
+				}
+			}
+			break;
+
+		case SENSOR_ENABLE:
+			if((buff_in == NULL) || (size_in < sizeof(int)))
+			{
+				GSE_ERR("Enable sensor parameter error!\n");
+				err = -EINVAL;
+			}
+			else
+			{
+				value = *(int *)buff_in;
+				mutex_lock(&kxtj2_1009_mutex);
+				if(((value == 0) && (sensor_power == false)) ||((value == 1) && (sensor_power == true)))
+				{
+					enable_status = sensor_power;
+					GSE_LOG("Gsensor device have updated!\n");
+				}
+				else
+				{
+					enable_status = !sensor_power;
+					if (atomic_read(&priv->suspend) == 0)
+					{
+						err = KXTJ2_1009_SetPowerMode( priv->client, enable_status);
+						GSE_LOG("Gsensor not in suspend KXTJ2_1009_SetPowerMode!, enable_status = %d\n",enable_status);
+					}
+					else
+					{
+						GSE_LOG("Gsensor in suspend and can not enable or disable!enable_status = %d\n",enable_status);
+					}
+				}
+				mutex_unlock(&kxtj2_1009_mutex);
+			}
+			break;
+
+		case SENSOR_GET_DATA:
+			if((buff_out == NULL) || (size_out< sizeof(hwm_sensor_data)))
+			{
+				GSE_ERR("get sensor data parameter error!\n");
+				err = -EINVAL;
+			}
+			else
+			{
+				gsensor_data = (hwm_sensor_data *)buff_out;
+				mutex_lock(&kxtj2_1009_mutex);
+				KXTJ2_1009_ReadSensorData(priv->client, buff, KXTJ2_1009_BUFSIZE);
+				mutex_unlock(&kxtj2_1009_mutex);
+				sscanf(buff, "%x %x %x", &gsensor_data->values[0], 
+					&gsensor_data->values[1], &gsensor_data->values[2]);				
+				gsensor_data->status = SENSOR_STATUS_ACCURACY_MEDIUM;				
+				gsensor_data->value_divide = 1000;
+			}
+			break;
+		default:
+			GSE_ERR("gsensor operate function no this parameter %d!\n", command);
+			err = -1;
+			break;
+	}
+	
+	return err;
+}
+
+/****************************************************************************** 
+ * Function Configuration
+******************************************************************************/
+static int kxtj2_1009_open(struct inode *inode, struct file *file)
+{
+	file->private_data = kxtj2_1009_i2c_client;
+
+	if(file->private_data == NULL)
+	{
+		GSE_ERR("null pointer!!\n");
+		return -EINVAL;
+	}
+	return nonseekable_open(inode, file);
+}
+/*----------------------------------------------------------------------------*/
+static int kxtj2_1009_release(struct inode *inode, struct file *file)
+{
+	file->private_data = NULL;
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+//static int kxtj2_1009_ioctl(struct inode *inode, struct file *file, unsigned int cmd,
+//       unsigned long arg)
+static long kxtj2_1009_unlocked_ioctl(struct file *file, unsigned int cmd,unsigned long arg)
+{
+	struct i2c_client *client = (struct i2c_client*)file->private_data;
+	struct kxtj2_1009_i2c_data *obj = (struct kxtj2_1009_i2c_data*)i2c_get_clientdata(client);	
+	char strbuf[KXTJ2_1009_BUFSIZE];
+	void __user *data;
+	SENSOR_DATA sensor_data;
+	long err = 0;
+	int cali[3];
+
+	//GSE_FUN(f);
+	if(_IOC_DIR(cmd) & _IOC_READ)
+	{
+		err = !access_ok(VERIFY_WRITE, (void __user *)arg, _IOC_SIZE(cmd));
+	}
+	else if(_IOC_DIR(cmd) & _IOC_WRITE)
+	{
+		err = !access_ok(VERIFY_READ, (void __user *)arg, _IOC_SIZE(cmd));
+	}
+
+	if(err)
+	{
+		GSE_ERR("access error: %08X, (%2d, %2d)\n", cmd, _IOC_DIR(cmd), _IOC_SIZE(cmd));
+		return -EFAULT;
+	}
+
+	switch(cmd)
+	{
+		case GSENSOR_IOCTL_INIT:
+			kxtj2_1009_init_client(client, 0);			
+			break;
+
+		case GSENSOR_IOCTL_READ_CHIPINFO:
+			data = (void __user *) arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}
+			
+			KXTJ2_1009_ReadChipInfo(client, strbuf, KXTJ2_1009_BUFSIZE);
+			if(copy_to_user(data, strbuf, strlen(strbuf)+1))
+			{
+				err = -EFAULT;
+				break;
+			}				 
+			break;	  
+
+		case GSENSOR_IOCTL_READ_SENSORDATA:
+			data = (void __user *) arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}
+			KXTJ2_1009_SetPowerMode(obj->client, true);
+			KXTJ2_1009_ReadSensorData(client, strbuf, KXTJ2_1009_BUFSIZE);
+			if(copy_to_user(data, strbuf, strlen(strbuf)+1))
+			{
+				err = -EFAULT;
+				break;	  
+			}				 
+			break;
+
+		case GSENSOR_IOCTL_READ_GAIN:
+			data = (void __user *) arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}			
+			
+			if(copy_to_user(data, &gsensor_gain, sizeof(GSENSOR_VECTOR3D)))
+			{
+				err = -EFAULT;
+				break;
+			}				 
+			break;
+
+		case GSENSOR_IOCTL_READ_RAW_DATA:
+			data = (void __user *) arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}
+			KXTJ2_1009_ReadRawData(client, strbuf);
+			if(copy_to_user(data, &strbuf, strlen(strbuf)+1))
+			{
+				err = -EFAULT;
+				break;	  
+			}
+			break;	  
+
+		case GSENSOR_IOCTL_SET_CALI:
+			data = (void __user*)arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}
+			if(copy_from_user(&sensor_data, data, sizeof(sensor_data)))
+			{
+				err = -EFAULT;
+				break;	  
+			}
+			if(atomic_read(&obj->suspend))
+			{
+				GSE_ERR("Perform calibration in suspend state!!\n");
+				err = -EINVAL;
+			}
+			else
+			{
+				cali[KXTJ2_1009_AXIS_X] = sensor_data.x * obj->reso->sensitivity / GRAVITY_EARTH_1000;
+				cali[KXTJ2_1009_AXIS_Y] = sensor_data.y * obj->reso->sensitivity / GRAVITY_EARTH_1000;
+				cali[KXTJ2_1009_AXIS_Z] = sensor_data.z * obj->reso->sensitivity / GRAVITY_EARTH_1000;			  
+				err = KXTJ2_1009_WriteCalibration(client, cali);			 
+			}
+			break;
+
+		case GSENSOR_IOCTL_CLR_CALI:
+			err = KXTJ2_1009_ResetCalibration(client);
+			break;
+
+		case GSENSOR_IOCTL_GET_CALI:
+			data = (void __user*)arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}
+			if(0 != (err = KXTJ2_1009_ReadCalibration(client, cali)))
+			{
+				break;
+			}
+			
+			sensor_data.x = cali[KXTJ2_1009_AXIS_X] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+			sensor_data.y = cali[KXTJ2_1009_AXIS_Y] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+			sensor_data.z = cali[KXTJ2_1009_AXIS_Z] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+			if(copy_to_user(data, &sensor_data, sizeof(sensor_data)))
+			{
+				err = -EFAULT;
+				break;
+			}		
+			break;
+		
+
+		default:
+			GSE_ERR("unknown IOCTL: 0x%08x\n", cmd);
+			err = -ENOIOCTLCMD;
+			break;
+			
+	}
+
+	return err;
+}
+ 
+
+/*----------------------------------------------------------------------------*/
+static struct file_operations kxtj2_1009_fops = {
+	.owner = THIS_MODULE,
+	.open = kxtj2_1009_open,
+	.release = kxtj2_1009_release,
+	.unlocked_ioctl = kxtj2_1009_unlocked_ioctl,
+	//.ioctl = kxtj2_1009_ioctl,
+};
+/*----------------------------------------------------------------------------*/
+static struct miscdevice kxtj2_1009_device = {
+	.minor = MISC_DYNAMIC_MINOR,
+	.name = "gsensor",
+	.fops = &kxtj2_1009_fops,
+};
+/*----------------------------------------------------------------------------*/
+#if !defined(CONFIG_HAS_EARLYSUSPEND) || !defined(USE_EARLY_SUSPEND)
+/*----------------------------------------------------------------------------*/
+static int kxtj2_1009_suspend(struct i2c_client *client, pm_message_t msg) 
+{
+	struct kxtj2_1009_i2c_data *obj = i2c_get_clientdata(client);    
+	int err = 0;
+	GSE_FUN();    
+
+	if(msg.event == PM_EVENT_SUSPEND)
+	{   
+		if(obj == NULL)
+		{
+			GSE_ERR("null pointer!!\n");
+			return -EINVAL;
+		}
+        mutex_lock(&kxtj2_1009_mutex);
+		atomic_set(&obj->suspend, 1);
+		if(0 != (err = KXTJ2_1009_SetPowerMode(obj->client, false)))
+		{
+			GSE_ERR("write power control fail!!\n");
+			mutex_unlock(&kxtj2_1009_mutex);
+			return -1;
+		}
+        mutex_unlock(&kxtj2_1009_mutex);
+
+		//sensor_power = false;      
+		KXTJ2_1009_power(obj->hw, 0);
+	}
+	return err;
+}
+/*----------------------------------------------------------------------------*/
+static int kxtj2_1009_resume(struct i2c_client *client)
+{
+	struct kxtj2_1009_i2c_data *obj = i2c_get_clientdata(client);        
+	int err;
+	GSE_FUN();
+
+	if(obj == NULL)
+	{
+		GSE_ERR("null pointer!!\n");
+		return -EINVAL;
+	}
+
+	KXTJ2_1009_power(obj->hw, 1);
+    mutex_lock(&kxtj2_1009_mutex);
+	if(0 != (err = kxtj2_1009_init_client(client, 0)))
+	{
+		GSE_ERR("initialize client fail!!\n");
+		mutex_unlock(&kxtj2_1009_mutex);
+		return err;        
+	}
+	atomic_set(&obj->suspend, 0);
+    mutex_unlock(&kxtj2_1009_mutex);
+
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+#else //!defined(CONFIG_HAS_EARLYSUSPEND) || !defined(USE_EARLY_SUSPEND)
+/*----------------------------------------------------------------------------*/
+static void kxtj2_1009_early_suspend(struct early_suspend *h) 
+{
+	struct kxtj2_1009_i2c_data *obj = container_of(h, struct kxtj2_1009_i2c_data, early_drv);   
+	int err;
+	GSE_FUN();    
+
+	if(obj == NULL)
+	{
+		GSE_ERR("null pointer!!\n");
+		return;
+	}
+	mutex_lock(&kxtj2_1009_mutex);
+	atomic_set(&obj->suspend, 1); 
+	if(err = KXTJ2_1009_SetPowerMode(obj->client, false))
+	{
+		GSE_ERR("write power control fail!!\n");
+		mutex_unlock(&kxtj2_1009_mutex);
+		return;
+	}
+	mutex_unlock(&kxtj2_1009_mutex);
+
+	//sensor_power = false;
+	
+	KXTJ2_1009_power(obj->hw, 0);
+}
+/*----------------------------------------------------------------------------*/
+static void kxtj2_1009_late_resume(struct early_suspend *h)
+{
+	struct kxtj2_1009_i2c_data *obj = container_of(h, struct kxtj2_1009_i2c_data, early_drv);         
+	int err;
+	GSE_FUN();
+
+	if(obj == NULL)
+	{
+		GSE_ERR("null pointer!!\n");
+		return;
+	}
+
+	KXTJ2_1009_power(obj->hw, 1);
+	mutex_lock(&kxtj2_1009_mutex);
+	if(err = kxtj2_1009_init_client(obj->client, 0))
+	{
+		GSE_ERR("initialize client fail!!\n");
+		mutex_unlock(&kxtj2_1009_mutex);
+		return;        
+	}
+	atomic_set(&obj->suspend, 0); 
+	mutex_unlock(&kxtj2_1009_mutex);
+}
+/*----------------------------------------------------------------------------*/
+#endif //!defined(CONFIG_HAS_EARLYSUSPEND) || !defined(USE_EARLY_SUSPEND)
+/*----------------------------------------------------------------------------*/
+static int kxtj2_1009_i2c_detect(struct i2c_client *client/*, int kind*/, struct i2c_board_info *info) 
+{    
+	strcpy(info->type, KXTJ2_1009_DEV_NAME);
+	return 0;
+}
+
+/*----------------------------------------------------------------------------*/
+static int kxtj2_1009_i2c_probe(struct i2c_client *client, const struct i2c_device_id *id)
+{
+	struct i2c_client *new_client;
+	struct kxtj2_1009_i2c_data *obj;
+	struct hwmsen_object sobj;
+	int err = 0;
+	GSE_FUN();
+
+	if(!(obj = kzalloc(sizeof(*obj), GFP_KERNEL)))
+	{
+		err = -ENOMEM;
+		goto exit;
+	}
+	
+	memset(obj, 0, sizeof(struct kxtj2_1009_i2c_data));
+
+	obj->hw = get_cust_acc_hw();
+	
+	if(0 != (err = hwmsen_get_convert(obj->hw->direction, &obj->cvt)))
+	{
+		GSE_ERR("invalid direction: %d\n", obj->hw->direction);
+		goto exit;
+	}
+
+	obj_i2c_data = obj;
+	obj->client = client;
+	new_client = obj->client;
+	i2c_set_clientdata(new_client,obj);
+	
+	atomic_set(&obj->trace, 0);
+	atomic_set(&obj->suspend, 0);
+	
+#ifdef CONFIG_KXTJ2_1009_LOWPASS
+	if(obj->hw->firlen > C_MAX_FIR_LENGTH)
+	{
+		atomic_set(&obj->firlen, C_MAX_FIR_LENGTH);
+	}	
+	else
+	{
+		atomic_set(&obj->firlen, obj->hw->firlen);
+	}
+	
+	if(atomic_read(&obj->firlen) > 0)
+	{
+		atomic_set(&obj->fir_en, 1);
+	}
+	
+#endif
+
+	kxtj2_1009_i2c_client = new_client;	
+
+	if(0 != (err = kxtj2_1009_init_client(new_client, 1)))
+	{
+		goto exit_init_failed;
+	}
+	
+
+	if(0 != (err = misc_register(&kxtj2_1009_device)))
+	{
+		GSE_ERR("kxtj2_1009_device register failed\n");
+		goto exit_misc_device_register_failed;
+	}
+
+	if(0 != (err = kxtj2_1009_create_attr(&kxtj2_1009_gsensor_driver.driver)))
+	{
+		GSE_ERR("create attribute err = %d\n", err);
+		goto exit_create_attr_failed;
+	}
+
+	sobj.self = obj;
+    sobj.polling = 1;
+    sobj.sensor_operate = gsensor_operate;
+	if(0 != (err = hwmsen_attach(ID_ACCELEROMETER, &sobj)))
+	{
+		GSE_ERR("attach fail = %d\n", err);
+		goto exit_kfree;
+	}
+
+#if defined(CONFIG_HAS_EARLYSUSPEND) && defined(USE_EARLY_SUSPEND)
+	obj->early_drv.level    = EARLY_SUSPEND_LEVEL_DISABLE_FB - 1,
+	obj->early_drv.suspend  = kxtj2_1009_early_suspend,
+	obj->early_drv.resume   = kxtj2_1009_late_resume,    
+	register_early_suspend(&obj->early_drv);
+#endif 
+
+	GSE_LOG("%s: OK\n", __func__);    
+	return 0;
+
+	exit_create_attr_failed:
+	misc_deregister(&kxtj2_1009_device);
+	exit_misc_device_register_failed:
+	exit_init_failed:
+	//i2c_detach_client(new_client);
+	exit_kfree:
+	kfree(obj);
+	exit:
+	GSE_ERR("%s: err = %d\n", __func__, err);        
+	return err;
+}
+
+/*----------------------------------------------------------------------------*/
+static int kxtj2_1009_i2c_remove(struct i2c_client *client)
+{
+	int err = 0;	
+	
+	if(0 != (err = kxtj2_1009_delete_attr(&kxtj2_1009_gsensor_driver.driver)))
+	{
+		GSE_ERR("kxtj2_1009_delete_attr fail: %d\n", err);
+	}
+	
+	if(0 != (err = misc_deregister(&kxtj2_1009_device)))
+	{
+		GSE_ERR("misc_deregister fail: %d\n", err);
+	}
+
+	if(0 != (err = hwmsen_detach(ID_ACCELEROMETER)))
+    {
+		GSE_ERR("hwmsen_detach fail: %d\n", err);
+	}    
+
+	kxtj2_1009_i2c_client = NULL;
+	i2c_unregister_device(client);
+	kfree(i2c_get_clientdata(client));
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int kxtj2_1009_probe(struct platform_device *pdev) 
+{
+	struct acc_hw *hw = get_cust_acc_hw();
+	GSE_FUN();
+
+	KXTJ2_1009_power(hw, 1);
+	//kxtj2_1009_force[0] = hw->i2c_num;
+	if(i2c_add_driver(&kxtj2_1009_i2c_driver))
+	{
+		GSE_ERR("add driver error\n");
+		return -1;
+	}
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int kxtj2_1009_remove(struct platform_device *pdev)
+{
+    struct acc_hw *hw = get_cust_acc_hw();
+
+    GSE_FUN();    
+    KXTJ2_1009_power(hw, 0);    
+    i2c_del_driver(&kxtj2_1009_i2c_driver);
+    return 0;
+}
+/*----------------------------------------------------------------------------*/
+static struct platform_driver kxtj2_1009_gsensor_driver = {
+	.probe      = kxtj2_1009_probe,
+	.remove     = kxtj2_1009_remove,    
+	.driver     = {
+		.name  = "gsensor",
+		.owner = THIS_MODULE,
+	}
+};
+
+/*----------------------------------------------------------------------------*/
+static int __init kxtj2_1009_init(void)
+{
+	struct acc_hw *hw = get_cust_acc_hw();
+    GSE_FUN();
+	GSE_LOG("%s: i2c_number=%d\n", __func__,hw->i2c_num);
+	i2c_register_board_info(hw->i2c_num, &i2c_kxtj2_1009, 1);
+	if(platform_driver_register(&kxtj2_1009_gsensor_driver))
+	{
+		GSE_ERR("failed to register driver");
+		return -ENODEV;
+	}
+	return 0;    
+}
+/*----------------------------------------------------------------------------*/
+static void __exit kxtj2_1009_exit(void)
+{
+	GSE_FUN();
+	platform_driver_unregister(&kxtj2_1009_gsensor_driver);
+}
+/*----------------------------------------------------------------------------*/
+module_init(kxtj2_1009_init);
+module_exit(kxtj2_1009_exit);
+/*----------------------------------------------------------------------------*/
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("KXTJ2_1009 I2C driver");
+MODULE_AUTHOR("Dexiang.Liu@mediatek.com");
diff --git a/drivers/misc/mediatek/accelerometer/KXTJ2_1009/kxtj2_1009.h b/drivers/misc/mediatek/accelerometer/KXTJ2_1009/kxtj2_1009.h
new file mode 100644
index 000000000..0966b9076
--- /dev/null
+++ b/drivers/misc/mediatek/accelerometer/KXTJ2_1009/kxtj2_1009.h
@@ -0,0 +1,65 @@
+/* linux/drivers/hwmon/adxl345.c
+ *
+ * (C) Copyright 2008 
+ * MediaTek <www.mediatek.com>
+ *
+ * KXTJ2_1009 driver for MT6575
+ *
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA  KXTIK
+ */
+#ifndef KXTJ2_1009_H
+#define KXTJ2_1009_H
+	 
+#include <linux/ioctl.h>
+	 
+#define KXTJ2_1009_I2C_SLAVE_ADDR		0x1C
+	 
+ /* KXTJ2_1009 Register Map  (Please refer to KXTJ2_1009 Specifications) */
+#define KXTJ2_1009_REG_DEVID			0x0F
+#define	KXTJ2_1009_REG_BW_RATE			0x21
+#define KXTJ2_1009_REG_POWER_CTL		0x1B
+#define KXTJ2_1009_REG_CTL_REG3		0x1D
+#define KXTJ2_1009_DCST_RESP			0x0C
+#define KXTJ2_1009_REG_DATA_FORMAT		0x1B
+#define KXTJ2_1009_REG_DATA_RESOLUTION		0x1B
+#define KXTJ2_1009_RANGE_DATA_RESOLUTION_MASK	0x40
+#define KXTJ2_1009_REG_DATAX0			0x06	 
+#define KXTJ2_1009_FIXED_DEVID			0x09	 
+#define KXTJ2_1009_BW_200HZ				0x05
+#define KXTJ2_1009_BW_100HZ				0x04
+#define KXTJ2_1009_BW_50HZ				0x03	 
+#define KXTJ2_1009_MEASURE_MODE		0x80		 
+#define KXTJ2_1009_RANGE_MASK		0x18
+#define KXTJ2_1009_RANGE_2G			0x00
+#define KXTJ2_1009_RANGE_4G			0x08
+#define KXTJ2_1009_RANGE_8G			0x10
+#define KXTJ2_1009_REG_INT_ENABLE	0x1E
+
+#define KXTJ2_1009_SELF_TEST           0x10
+	 	 
+	 
+#define KXTJ2_1009_SUCCESS						0
+#define KXTJ2_1009_ERR_I2C						-1
+#define KXTJ2_1009_ERR_STATUS					-3
+#define KXTJ2_1009_ERR_SETUP_FAILURE				-4
+#define KXTJ2_1009_ERR_GETGSENSORDATA			-5
+#define KXTJ2_1009_ERR_IDENTIFICATION			-6
+	 
+	 
+	 
+#define KXTJ2_1009_BUFSIZE				256
+	 
+#endif
+
diff --git a/drivers/misc/mediatek/accelerometer/Makefile b/drivers/misc/mediatek/accelerometer/Makefile
new file mode 100755
index 000000000..7bf42b718
--- /dev/null
+++ b/drivers/misc/mediatek/accelerometer/Makefile
@@ -0,0 +1,84 @@
+include $(srctree)/drivers/misc/mediatek/Makefile.custom
+
+# In case the platform does NOT support this type of sensors
+
+obj-y   +=  accel.o accel_factory.o
+
+ifeq ($(CONFIG_MTK_K2DH),y)
+obj-y   +=  k2dh/
+endif
+
+ifeq ($(CONFIG_MTK_BMA050),y)
+obj-y   +=  bma050/
+endif
+ifeq ($(CONFIG_MTK_BMA050_NEW),y)
+obj-y   +=  bma050-new/
+endif
+ifeq ($(CONFIG_MTK_BMA222E),y)
+obj-y   +=  bma222E/
+endif
+ifeq ($(CONFIG_MTK_BMA222E_NEW),y)
+obj-y   +=  bma222E-new/
+endif
+ifeq ($(CONFIG_MTK_MPU6050G),y)
+obj-y   +=  mpu6050g/
+endif
+ifeq ($(CONFIG_MTK_MPU6050G_NEW),y)
+obj-y   +=  mpu6050g-new/
+endif
+ifeq ($(CONFIG_MTK_BMA250),y)
+obj-y   +=  bma250/
+endif
+ifeq ($(CONFIG_MTK_KXTIK1004),y)
+obj-y   +=  kxtik1004/
+endif
+
+ifeq ($(CONFIG_MTK_MPU6515A),y)
+obj-y   +=  mpu6515/
+endif
+
+ifeq ($(CONFIG_MTK_KXTJ2_1009),y)
+obj-y   +=  KXTJ2_1009/
+endif
+
+ifeq ($(CONFIG_MTK_KXTJ2_1009_NEW),y)
+obj-y   +=  KXTJ2_1009-new/
+endif
+
+ifeq ($(CONFIG_MTK_BMA250E),y)
+obj-y   +=  bma250e/
+endif
+ifeq ($(CONFIG_MTK_MC3410_NEW),y)
+obj-y   +=  mc3410-new/
+endif
+
+ifeq ($(CONFIG_MTK_MC3XXX_AUTO),y)
+obj-y   +=  mc3xxx_auto/
+endif
+
+ifeq ($(CONFIG_MTK_MPU60X0),y)
+obj-y   +=  mpu60x0/
+endif
+
+ifeq ($(CONFIG_MTK_BMA255_SDO0),y)
+obj-y   +=  bma255-sdo0/
+endif
+
+ifeq ($(CONFIG_MTK_BMA255_SDO1),y)
+obj-y   +=  bma255-sdo1/
+endif
+ifeq ($(CONFIG_MTK_BMA056),y)
+obj-y   +=  bma056/
+endif
+
+ifeq ($(CONFIG_MTK_LSM6DS3),y)
+obj-y   +=  lsm6ds3/
+endif
+
+ifeq ($(CONFIG_MTK_MXC400X_NEW),y)
+obj-y   +=  mxc400x-new/
+endif
+
+ifeq ($(CONFIG_MTK_DA213),y)
+obj-y   +=  da213/
+endif
diff --git a/drivers/misc/mediatek/accelerometer/accel.c b/drivers/misc/mediatek/accelerometer/accel.c
new file mode 100644
index 000000000..fb7d603ae
--- /dev/null
+++ b/drivers/misc/mediatek/accelerometer/accel.c
@@ -0,0 +1,762 @@
+
+#include "accel.h"
+#include "accel_factory.h"
+
+struct acc_context *acc_context_obj = NULL;
+
+
+static struct acc_init_info *gsensor_init_list[MAX_CHOOSE_G_NUM] = { 0 };	/* modified */
+#if defined(CONFIG_HAS_EARLYSUSPEND)
+static void acc_early_suspend(struct early_suspend *h);
+static void acc_late_resume(struct early_suspend *h);
+#endif
+
+static void acc_work_func(struct work_struct *work)
+{
+
+	struct acc_context *cxt = NULL;
+	/* hwm_sensor_data sensor_data; */
+	int x, y, z, status;
+	int64_t nt;
+	struct timespec time;
+	int err;
+
+	cxt = acc_context_obj;
+
+	if (NULL == cxt->acc_data.get_data) {
+        ACC_ERR("acc driver not register data path\n");
+        return;
+	}
+
+
+	time.tv_sec = time.tv_nsec = 0;
+	time = get_monotonic_coarse();
+	nt = time.tv_sec * 1000000000LL + time.tv_nsec;
+
+	err = cxt->acc_data.get_data(&x,&y,&z,&status);
+
+	if (err) {
+		ACC_ERR("get acc data fails!!\n");
+		goto acc_loop;
+	} else {
+			if (0 == x && 0 == y && 0 == z) {
+				goto acc_loop;
+			}
+
+			cxt->drv_data.acc_data.values[0] = x;
+			cxt->drv_data.acc_data.values[1] = y;
+			cxt->drv_data.acc_data.values[2] = z;
+			cxt->drv_data.acc_data.status = status;
+			cxt->drv_data.acc_data.time = nt;
+
+	}
+
+	if (true == cxt->is_first_data_after_enable) {
+		cxt->is_first_data_after_enable = false;
+		/* filter -1 value */
+		if (ACC_INVALID_VALUE == cxt->drv_data.acc_data.values[0] ||
+		    ACC_INVALID_VALUE == cxt->drv_data.acc_data.values[1] ||
+		    ACC_INVALID_VALUE == cxt->drv_data.acc_data.values[2]) {
+			ACC_LOG(" read invalid data\n");
+			goto acc_loop;
+
+		}
+	}
+	/* report data to input device */
+	/* printk("new acc work run....\n"); */
+	/* ACC_LOG("acc data[%d,%d,%d] \n" ,cxt->drv_data.acc_data.values[0], */
+	/* cxt->drv_data.acc_data.values[1],cxt->drv_data.acc_data.values[2]); */
+
+	acc_data_report(cxt->drv_data.acc_data.values[0],
+			cxt->drv_data.acc_data.values[1], cxt->drv_data.acc_data.values[2],
+			cxt->drv_data.acc_data.status);
+
+ acc_loop:
+	if (true == cxt->is_polling_run) {
+		mod_timer(&cxt->timer, jiffies + atomic_read(&cxt->delay) / (1000 / HZ));
+	}
+}
+
+static void acc_poll(unsigned long data)
+{
+	struct acc_context *obj = (struct acc_context *)data;
+	if (obj != NULL) {
+		schedule_work(&obj->report);
+	}
+}
+
+static struct acc_context *acc_context_alloc_object(void)
+{
+
+	struct acc_context *obj = kzalloc(sizeof(*obj), GFP_KERNEL);
+	ACC_LOG("acc_context_alloc_object++++\n");
+	if (!obj) {
+		ACC_ERR("Alloc accel object error!\n");
+		return NULL;
+	}
+	atomic_set(&obj->delay, 200);	/*5Hz *//*  set work queue delay time 200ms */
+	atomic_set(&obj->wake, 0);
+	INIT_WORK(&obj->report, acc_work_func);
+	init_timer(&obj->timer);
+	obj->timer.expires = jiffies + atomic_read(&obj->delay) / (1000 / HZ);
+	obj->timer.function = acc_poll;
+	obj->timer.data = (unsigned long)obj;
+	obj->is_first_data_after_enable = false;
+	obj->is_polling_run = false;
+	mutex_init(&obj->acc_op_mutex);
+	obj->is_batch_enable = false;//for batch mode init
+	obj->cali_sw[ACC_AXIS_X]=0;
+	obj->cali_sw[ACC_AXIS_Y]=0;
+	obj->cali_sw[ACC_AXIS_Z]=0;
+	ACC_LOG("acc_context_alloc_object----\n");
+	return obj;
+}
+
+static int acc_real_enable(int enable)
+{
+	int err = 0;
+	struct acc_context *cxt = NULL;
+	cxt = acc_context_obj;
+	if (1 == enable) {
+
+		if (true == cxt->is_active_data || true == cxt->is_active_nodata) {
+			err = cxt->acc_ctl.enable_nodata(1);
+			if (err) {
+				err = cxt->acc_ctl.enable_nodata(1);
+				if (err) {
+					err = cxt->acc_ctl.enable_nodata(1);
+					if (err)
+						ACC_ERR("acc enable(%d) err 3 timers = %d\n",
+							enable, err);
+				}
+			}
+			ACC_LOG("acc real enable \n");
+		}
+
+	}
+	if (0 == enable) {
+		if (false == cxt->is_active_data && false == cxt->is_active_nodata) {
+			err = cxt->acc_ctl.enable_nodata(0);
+			if (err) {
+				ACC_ERR("acc enable(%d) err = %d\n", enable, err);
+			}
+			ACC_LOG("acc real disable \n");
+		}
+
+	}
+
+	return err;
+}
+
+static int acc_enable_data(int enable)
+{
+	struct acc_context *cxt = NULL;
+	cxt = acc_context_obj;
+	if (NULL == cxt->acc_ctl.open_report_data) {
+		ACC_ERR("no acc control path\n");
+		return -1;
+	}
+
+	if (1 == enable) {
+		ACC_LOG("ACC enable data\n");
+		cxt->is_active_data = true;
+		cxt->is_first_data_after_enable = true;
+		cxt->acc_ctl.open_report_data(1);
+        acc_real_enable(enable);
+		if (false == cxt->is_polling_run && cxt->is_batch_enable == false) {
+			if (false == cxt->acc_ctl.is_report_input_direct) {
+				mod_timer(&cxt->timer,
+					  jiffies + atomic_read(&cxt->delay) / (1000 / HZ));
+				cxt->is_polling_run = true;
+			}
+		}
+	}
+	if (0 == enable) {
+		ACC_LOG("ACC disable\n");
+
+		cxt->is_active_data = false;
+		cxt->acc_ctl.open_report_data(0);
+		if (true == cxt->is_polling_run) {
+			if (false == cxt->acc_ctl.is_report_input_direct) {
+				cxt->is_polling_run = false;
+                smp_mb();
+				del_timer_sync(&cxt->timer);
+                smp_mb();
+				cancel_work_sync(&cxt->report);
+				cxt->drv_data.acc_data.values[0] = ACC_INVALID_VALUE;
+				cxt->drv_data.acc_data.values[1] = ACC_INVALID_VALUE;
+				cxt->drv_data.acc_data.values[2] = ACC_INVALID_VALUE;
+			}
+		}
+        acc_real_enable(enable);
+	}
+	return 0;
+}
+
+
+
+int acc_enable_nodata(int enable)
+{
+	struct acc_context *cxt = NULL;
+	cxt = acc_context_obj;
+	if (NULL == cxt->acc_ctl.enable_nodata) {
+		ACC_ERR("acc_enable_nodata:acc ctl path is NULL\n");
+		return -1;
+	}
+
+	if (1 == enable) {
+		cxt->is_active_nodata = true;
+	}
+
+	if (0 == enable) {
+		cxt->is_active_nodata = false;
+	}
+	acc_real_enable(enable);
+	return 0;
+}
+
+
+static ssize_t acc_show_enable_nodata(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	int len = 0;
+	ACC_LOG(" not support now\n");
+	return len;
+}
+
+static ssize_t acc_store_enable_nodata(struct device *dev, struct device_attribute *attr,
+				       const char *buf, size_t count)
+{
+	struct acc_context *cxt = NULL;
+	ACC_LOG("acc_store_enable nodata buf=%s\n", buf);
+	mutex_lock(&acc_context_obj->acc_op_mutex);
+	cxt = acc_context_obj;
+	if (NULL == cxt->acc_ctl.enable_nodata) {
+		ACC_LOG("acc_ctl enable nodata NULL\n");
+		mutex_unlock(&acc_context_obj->acc_op_mutex);
+		return count;
+	}
+	if (!strncmp(buf, "1", 1)) {
+		/* cxt->acc_ctl.enable_nodata(1); */
+		acc_enable_nodata(1);
+	} else if (!strncmp(buf, "0", 1)) {
+		/* cxt->acc_ctl.enable_nodata(0); */
+		acc_enable_nodata(0);
+	} else {
+		ACC_ERR(" acc_store enable nodata cmd error !!\n");
+	}
+	mutex_unlock(&acc_context_obj->acc_op_mutex);
+	return count;
+}
+
+static ssize_t acc_store_active(struct device *dev, struct device_attribute *attr,
+				const char *buf, size_t count)
+{
+	struct acc_context *cxt = NULL;
+	ACC_LOG("acc_store_active buf=%s\n", buf);
+	mutex_lock(&acc_context_obj->acc_op_mutex);
+	cxt = acc_context_obj;
+	if (NULL == cxt->acc_ctl.open_report_data) {
+		ACC_LOG("acc_ctl enable NULL\n");
+		mutex_unlock(&acc_context_obj->acc_op_mutex);
+		return count;
+	}
+	if (!strncmp(buf, "1", 1)) {
+		/* cxt->acc_ctl.enable(1); */
+		acc_enable_data(1);
+
+	} else if (!strncmp(buf, "0", 1)) {
+
+		/* cxt->acc_ctl.enable(0); */
+		acc_enable_data(0);
+	} else {
+		ACC_ERR(" acc_store_active error !!\n");
+	}
+	mutex_unlock(&acc_context_obj->acc_op_mutex);
+	ACC_LOG(" acc_store_active done\n");
+	return count;
+}
+
+/*----------------------------------------------------------------------------*/
+static ssize_t acc_show_active(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct acc_context *cxt = NULL;
+	int div = 0;
+	cxt = acc_context_obj;
+	div=cxt->acc_data.vender_div;
+	ACC_LOG("acc vender_div value: %d\n", div);
+	return snprintf(buf, PAGE_SIZE, "%d\n", div);
+}
+
+static ssize_t acc_store_delay(struct device *dev, struct device_attribute *attr,
+			       const char *buf, size_t count)
+{
+    int delay = 0;
+	int mdelay = 0;
+	struct acc_context *cxt = NULL;
+	mutex_lock(&acc_context_obj->acc_op_mutex);
+	cxt = acc_context_obj;
+	if (NULL == cxt->acc_ctl.set_delay) {
+		ACC_LOG("acc_ctl set_delay NULL\n");
+		mutex_unlock(&acc_context_obj->acc_op_mutex);
+		return count;
+	}
+
+	if (1 != sscanf(buf, "%d", &delay)) {
+		ACC_ERR("invalid format!!\n");
+		mutex_unlock(&acc_context_obj->acc_op_mutex);
+		return count;
+	}
+
+	if (false == cxt->acc_ctl.is_report_input_direct) {
+		mdelay = (int)delay / 1000 / 1000;
+		atomic_set(&acc_context_obj->delay, mdelay);
+	}
+	cxt->acc_ctl.set_delay(delay);
+	ACC_LOG(" acc_delay %d ns\n", delay);
+	mutex_unlock(&acc_context_obj->acc_op_mutex);
+	return count;
+}
+
+static ssize_t acc_show_delay(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	int len = 0;
+	ACC_LOG(" not support now\n");
+	return len;
+}
+
+static ssize_t acc_show_sensordevnum(struct device* dev, 
+                                 struct device_attribute *attr, char *buf) 
+{
+	struct acc_context *cxt = NULL;
+	const char *devname = NULL;
+	cxt = acc_context_obj;
+	devname = dev_name(&cxt->idev->dev);
+	return snprintf(buf, PAGE_SIZE, "%s\n", devname+5); 
+}
+
+static ssize_t acc_store_batch(struct device* dev, struct device_attribute *attr,
+                                  const char *buf, size_t count)
+{
+	struct acc_context *cxt = NULL;
+	ACC_LOG("acc_store_batch buf=%s\n",buf);
+	mutex_lock(&acc_context_obj->acc_op_mutex);
+	cxt = acc_context_obj;
+	if(cxt->acc_ctl.is_support_batch){
+	    	if (!strncmp(buf, "1", 1)) 
+		{
+	    		cxt->is_batch_enable = true;
+			if(true == cxt->is_polling_run)
+			{
+				cxt->is_polling_run = false;
+				del_timer_sync(&cxt->timer);
+				cancel_work_sync(&cxt->report);
+				cxt->drv_data.acc_data.values[0] = ACC_INVALID_VALUE;
+				cxt->drv_data.acc_data.values[1] = ACC_INVALID_VALUE;
+				cxt->drv_data.acc_data.values[2] = ACC_INVALID_VALUE;
+			}
+	    	} 
+		else if (!strncmp(buf, "0", 1))
+		{
+			cxt->is_batch_enable = false;
+			if(false == cxt->is_polling_run)
+			{
+				if(false == cxt->acc_ctl.is_report_input_direct)
+				{
+					mod_timer(&cxt->timer, jiffies + atomic_read(&cxt->delay)/(1000/HZ));
+					cxt->is_polling_run = true;
+				}
+			}
+	    	}
+		else
+		{
+			ACC_ERR(" acc_store_batch error !!\n");
+		}
+	}else{
+		ACC_LOG(" acc_store_batch mot supported\n");
+	}
+	mutex_unlock(&acc_context_obj->acc_op_mutex);
+	ACC_LOG(" acc_store_batch done: %d\n", cxt->is_batch_enable);
+    	return count;
+
+}
+static ssize_t acc_show_batch(struct device* dev, 
+                                 struct device_attribute *attr, char *buf) 
+{
+	return snprintf(buf, PAGE_SIZE, "%d\n", 0); 
+}
+static ssize_t acc_store_flush(struct device* dev, struct device_attribute *attr,
+                                  const char *buf, size_t count)
+{
+	//mutex_lock(&acc_context_obj->acc_op_mutex);
+   // struct acc_context *devobj = (struct acc_context*)dev_get_drvdata(dev);
+	//do read FIFO data function and report data immediately
+	//mutex_unlock(&acc_context_obj->acc_op_mutex);
+    return count;
+}
+
+static ssize_t acc_show_flush(struct device* dev, 
+                                 struct device_attribute *attr, char *buf) 
+{
+	return snprintf(buf, PAGE_SIZE, "%d\n", 0); 
+}
+static int gsensor_remove(struct platform_device *pdev)
+{
+	ACC_LOG("gsensor_remove\n");
+	return 0;
+}
+
+static int gsensor_probe(struct platform_device *pdev)
+{
+	ACC_LOG("gsensor_probe\n");
+	return 0;
+}
+
+#ifdef CONFIG_OF
+static const struct of_device_id gsensor_of_match[] = {
+	{ .compatible = "mediatek,gsensor", },
+	{},
+};
+#endif
+static struct platform_driver gsensor_driver = {
+	.probe = gsensor_probe,
+	.remove = gsensor_remove,
+	.driver = {
+		   .name = "gsensor",
+        #ifdef CONFIG_OF
+		.of_match_table = gsensor_of_match,
+		#endif
+		   }
+};
+
+static int acc_real_driver_init(void)
+{
+	int i = 0;
+	int err = 0;
+	ACC_LOG(" acc_real_driver_init +\n");
+	for (i = 0; i < MAX_CHOOSE_G_NUM; i++) {
+		ACC_LOG(" i=%d\n", i);
+		if (0 != gsensor_init_list[i]) {
+			ACC_LOG(" acc try to init driver %s\n", gsensor_init_list[i]->name);
+			err = gsensor_init_list[i]->init();
+			if (0 == err) {
+				ACC_LOG(" acc real driver %s probe ok\n",
+					gsensor_init_list[i]->name);
+				break;
+			}
+		}
+	}
+
+	if (i == MAX_CHOOSE_G_NUM) {
+		ACC_LOG(" acc_real_driver_init fail\n");
+		err = -1;
+	}
+	return err;
+}
+
+int acc_driver_add(struct acc_init_info *obj)
+{
+	int err = 0;
+	int i = 0;
+
+	ACC_FUN();
+	if (!obj) {
+		ACC_ERR("ACC driver add fail, acc_init_info is NULL \n");
+		return -1;
+	}
+	for (i = 0; i < MAX_CHOOSE_G_NUM; i++) {
+        if ((i == 0) && (NULL == gsensor_init_list[0])) {
+			ACC_LOG("register gensor driver for the first time\n");
+			if (platform_driver_register(&gsensor_driver)) {
+				ACC_ERR("failed to register gensor driver already exist\n");
+			}
+		}
+
+		if (NULL == gsensor_init_list[i]) {
+			obj->platform_diver_addr = &gsensor_driver;
+			gsensor_init_list[i] = obj;
+			break;
+		}
+	}
+	if (i >= MAX_CHOOSE_G_NUM) {
+		ACC_ERR("ACC driver add err\n");
+		err = -1;
+	}
+
+	return err;
+}
+EXPORT_SYMBOL_GPL(acc_driver_add);
+
+static int acc_misc_init(struct acc_context *cxt)
+{
+
+	int err = 0;
+	cxt->mdev.minor = MISC_DYNAMIC_MINOR;
+	cxt->mdev.name = ACC_MISC_DEV_NAME;
+	if ((err = misc_register(&cxt->mdev))) {
+		ACC_ERR("unable to register acc misc device!!\n");
+	}
+	/* dev_set_drvdata(cxt->mdev.this_device, cxt); */
+	return err;
+}
+
+
+static int acc_input_init(struct acc_context *cxt)
+{
+	struct input_dev *dev;
+	int err = 0;
+
+	dev = input_allocate_device();
+	if (NULL == dev)
+		return -ENOMEM;
+
+	dev->name = ACC_INPUTDEV_NAME;
+
+	input_set_capability(dev, EV_ABS, EVENT_TYPE_ACCEL_X);
+	input_set_capability(dev, EV_ABS, EVENT_TYPE_ACCEL_Y);
+	input_set_capability(dev, EV_ABS, EVENT_TYPE_ACCEL_Z);
+	input_set_capability(dev, EV_ABS, EVENT_TYPE_ACCEL_STATUS);
+	input_set_capability(dev, EV_REL, EVENT_TYPE_ACCEL_UPDATE);
+
+	input_set_abs_params(dev, EVENT_TYPE_ACCEL_X, ACC_VALUE_MIN, ACC_VALUE_MAX, 0, 0);
+	input_set_abs_params(dev, EVENT_TYPE_ACCEL_Y, ACC_VALUE_MIN, ACC_VALUE_MAX, 0, 0);
+	input_set_abs_params(dev, EVENT_TYPE_ACCEL_Z, ACC_VALUE_MIN, ACC_VALUE_MAX, 0, 0);
+	input_set_abs_params(dev, EVENT_TYPE_ACCEL_STATUS, ACC_STATUS_MIN, ACC_STATUS_MAX, 0, 0);
+	input_set_drvdata(dev, cxt);
+
+	err = input_register_device(dev);
+	if (err < 0) {
+		input_free_device(dev);
+		return err;
+	}
+	cxt->idev = dev;
+
+	return 0;
+}
+
+DEVICE_ATTR(accenablenodata, S_IWUSR | S_IRUGO, acc_show_enable_nodata, acc_store_enable_nodata);
+DEVICE_ATTR(accactive, S_IWUSR | S_IRUGO, acc_show_active, acc_store_active);
+DEVICE_ATTR(accdelay, S_IWUSR | S_IRUGO, acc_show_delay, acc_store_delay);
+DEVICE_ATTR(accbatch,      		S_IWUSR | S_IRUGO, acc_show_batch,  acc_store_batch);
+DEVICE_ATTR(accflush,      		S_IWUSR | S_IRUGO, acc_show_flush,  acc_store_flush);
+DEVICE_ATTR(accdevnum,      		S_IWUSR | S_IRUGO, acc_show_sensordevnum,  NULL);
+
+static struct attribute *acc_attributes[] = {
+	&dev_attr_accenablenodata.attr,
+	&dev_attr_accactive.attr,
+	&dev_attr_accdelay.attr,
+	&dev_attr_accbatch.attr,
+	&dev_attr_accflush.attr,
+	&dev_attr_accdevnum.attr,
+	NULL
+};
+
+static struct attribute_group acc_attribute_group = {
+	.attrs = acc_attributes
+};
+
+int acc_register_data_path(struct acc_data_path *data)
+{
+	struct acc_context *cxt = NULL;
+	cxt = acc_context_obj;
+	cxt->acc_data.get_data = data->get_data;
+	cxt->acc_data.get_raw_data = data->get_raw_data;
+	cxt->acc_data.vender_div = data->vender_div;
+	ACC_LOG("acc register data path vender_div: %d\n", cxt->acc_data.vender_div);
+	if (NULL == cxt->acc_data.get_data) {
+		ACC_LOG("acc register data path fail\n");
+		return -1;
+	}
+	return 0;
+}
+
+int acc_register_control_path(struct acc_control_path *ctl)
+{
+	struct acc_context *cxt = NULL;
+	int err = 0;
+	cxt = acc_context_obj;
+	cxt->acc_ctl.set_delay = ctl->set_delay;
+	cxt->acc_ctl.open_report_data = ctl->open_report_data;
+	cxt->acc_ctl.enable_nodata = ctl->enable_nodata;
+	cxt->acc_ctl.is_support_batch = ctl->is_support_batch;
+	cxt->acc_ctl.is_report_input_direct= ctl->is_report_input_direct;
+	cxt->acc_ctl.acc_calibration = ctl->acc_calibration;
+
+	if (NULL == cxt->acc_ctl.set_delay || NULL == cxt->acc_ctl.open_report_data
+	    || NULL == cxt->acc_ctl.enable_nodata) {
+		ACC_LOG("acc register control path fail\n");
+		return -1;
+	}
+	/* add misc dev for sensor hal control cmd */
+	err = acc_misc_init(acc_context_obj);
+	if (err) {
+		ACC_ERR("unable to register acc misc device!!\n");
+		return -2;
+	}
+	err = sysfs_create_group(&acc_context_obj->mdev.this_device->kobj, &acc_attribute_group);
+	if (err < 0) {
+		ACC_ERR("unable to create acc attribute file\n");
+		return -3;
+	}
+
+	kobject_uevent(&acc_context_obj->mdev.this_device->kobj, KOBJ_ADD);
+	return 0;
+}
+
+int acc_data_report(int x, int y, int z, int status)
+{
+	/* ACC_LOG("+acc_data_report! %d, %d, %d, %d\n",x,y,z,status); */
+	struct acc_context *cxt = NULL;
+	int err = 0;
+	cxt = acc_context_obj;
+	input_report_abs(cxt->idev, EVENT_TYPE_ACCEL_X, x);
+	input_report_abs(cxt->idev, EVENT_TYPE_ACCEL_Y, y);
+	input_report_abs(cxt->idev, EVENT_TYPE_ACCEL_Z, z);
+	input_report_abs(cxt->idev, EVENT_TYPE_ACCEL_STATUS, status);
+	input_report_rel(cxt->idev, EVENT_TYPE_ACCEL_UPDATE, 1);
+	input_sync(cxt->idev);
+	return err;
+}
+
+static int acc_probe(struct platform_device *pdev)
+{
+
+	int err;
+	ACC_LOG("+++++++++++++accel_probe!!\n");
+
+	acc_context_obj = acc_context_alloc_object();
+	if (!acc_context_obj) {
+		err = -ENOMEM;
+		ACC_ERR("unable to allocate devobj!\n");
+		goto exit_alloc_data_failed;
+	}
+	/* init real acceleration driver */
+	err = acc_real_driver_init();
+	if (err) {
+		ACC_ERR("acc real driver init fail\n");
+		goto real_driver_init_fail;
+	}
+	//init acc common factory mode misc device
+	err = acc_factory_device_init();
+	if(err)
+	{
+		ACC_ERR("acc factory device already registed\n");
+	}
+	/* init input dev */
+	err = acc_input_init(acc_context_obj);
+	if (err) {
+		ACC_ERR("unable to register acc input device!\n");
+		goto exit_alloc_input_dev_failed;
+	}
+
+#if defined(CONFIG_HAS_EARLYSUSPEND) && defined(CONFIG_EARLYSUSPEND)
+	atomic_set(&(acc_context_obj->early_suspend), 0);
+	acc_context_obj->early_drv.level = EARLY_SUSPEND_LEVEL_STOP_DRAWING - 1,
+	    acc_context_obj->early_drv.suspend = acc_early_suspend,
+	    acc_context_obj->early_drv.resume = acc_late_resume,
+	    register_early_suspend(&acc_context_obj->early_drv);
+#endif
+
+  
+	ACC_LOG("----accel_probe OK !!\n");
+	return 0;
+
+
+ real_driver_init_fail:
+ exit_alloc_input_dev_failed:
+	kfree(acc_context_obj);
+
+ exit_alloc_data_failed:
+
+
+	ACC_LOG("----accel_probe fail !!!\n");
+	return err;
+}
+
+
+
+static int acc_remove(struct platform_device *pdev)
+{
+	int err=0;
+	ACC_FUN(f);
+	input_unregister_device(acc_context_obj->idev);
+	sysfs_remove_group(&acc_context_obj->idev->dev.kobj, &acc_attribute_group);
+
+	if ((err = misc_deregister(&acc_context_obj->mdev))) {
+		ACC_ERR("misc_deregister fail: %d\n", err);
+	}
+	kfree(acc_context_obj);
+
+	return 0;
+}
+#if defined(CONFIG_HAS_EARLYSUSPEND)
+static void acc_early_suspend(struct early_suspend *h)
+{
+	atomic_set(&(acc_context_obj->early_suspend), 1);
+	ACC_LOG(" acc_early_suspend ok------->hwm_obj->early_suspend=%d\n",
+		atomic_read(&(acc_context_obj->early_suspend)));
+	return;
+}
+
+/*----------------------------------------------------------------------------*/
+static void acc_late_resume(struct early_suspend *h)
+{
+	atomic_set(&(acc_context_obj->early_suspend), 0);
+	ACC_LOG(" acc_late_resume ok------->hwm_obj->early_suspend=%d\n",
+		atomic_read(&(acc_context_obj->early_suspend)));
+	return;
+}
+#endif
+static int acc_suspend(struct platform_device *dev, pm_message_t state)
+{
+	return 0;
+}
+
+/*----------------------------------------------------------------------------*/
+static int acc_resume(struct platform_device *dev)
+{
+	return 0;
+}
+
+#ifdef CONFIG_OF
+static const struct of_device_id m_acc_pl_of_match[] = {
+	{ .compatible = "mediatek,m_acc_pl", },
+	{},
+};
+#endif
+
+static struct platform_driver acc_driver = {
+	.probe = acc_probe,
+	.remove = acc_remove,
+	.suspend = acc_suspend,
+	.resume = acc_resume,
+	.driver = {
+		   .name = ACC_PL_DEV_NAME,
+        #ifdef CONFIG_OF
+		.of_match_table = m_acc_pl_of_match,
+		#endif
+		   }
+};
+
+static int __init acc_init(void)
+{
+	ACC_FUN();
+
+	if (platform_driver_register(&acc_driver)) {
+		ACC_ERR("failed to register acc driver\n");
+		return -ENODEV;
+	}
+
+	return 0;
+}
+
+static void __exit acc_exit(void)
+{
+	platform_driver_unregister(&acc_driver);
+	platform_driver_unregister(&gsensor_driver);
+}
+late_initcall(acc_init);
+//module_init(acc_init);
+//module_exit(acc_exit);
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("ACCELEROMETER device driver");
+MODULE_AUTHOR("Mediatek");
diff --git a/drivers/misc/mediatek/accelerometer/accel_factory.c b/drivers/misc/mediatek/accelerometer/accel_factory.c
new file mode 100644
index 000000000..13f04afe0
--- /dev/null
+++ b/drivers/misc/mediatek/accelerometer/accel_factory.c
@@ -0,0 +1,261 @@
+#include "accel_factory.h"
+static int acc_factory_open(struct inode *inode, struct file *file)
+{
+    file->private_data = acc_context_obj;
+
+    if (file->private_data == NULL)
+    {
+        ACC_ERR("null pointer!!\n");
+        return -EINVAL;
+    }
+    return nonseekable_open(inode, file);
+}
+
+static int acc_factory_release(struct inode *inode, struct file *file)
+{
+    file->private_data = NULL;
+    return 0;
+}
+static int acc_set_cali(int data[ACC_AXES_NUM])
+{
+	struct acc_context *cxt = acc_context_obj;
+	ACC_LOG(" factory cali %d,%d,%d \n",data[ACC_AXIS_X],data[ACC_AXIS_Y],data[ACC_AXIS_Z] );
+	ACC_LOG(" original cali %d,%d,%d \n",cxt->cali_sw[ACC_AXIS_X],cxt->cali_sw[ACC_AXIS_Y],cxt->cali_sw[ACC_AXIS_Z]);
+	cxt->cali_sw[ACC_AXIS_X] += data[ACC_AXIS_X];
+    cxt->cali_sw[ACC_AXIS_Y] += data[ACC_AXIS_Y];
+    cxt->cali_sw[ACC_AXIS_Z] += data[ACC_AXIS_Z];
+	ACC_LOG(" new cali %d,%d,%d \n",cxt->cali_sw[ACC_AXIS_X],cxt->cali_sw[ACC_AXIS_Y],cxt->cali_sw[ACC_AXIS_Z]);
+
+	return 0;
+}
+
+static int acc_clear_cali(void)
+{
+	struct acc_context *cxt = acc_context_obj;
+	cxt->cali_sw[ACC_AXIS_X] = 0;
+    cxt->cali_sw[ACC_AXIS_Y] = 0;
+    cxt->cali_sw[ACC_AXIS_Z] = 0;
+	ACC_LOG(" after clear cali %d,%d,%d \n",cxt->cali_sw[ACC_AXIS_X],cxt->cali_sw[ACC_AXIS_Y],cxt->cali_sw[ACC_AXIS_Z] );
+	return 0;
+}
+
+static long acc_factory_unlocked_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
+{
+    void __user *data;
+    int err = 0;
+	struct acc_context *cxt = acc_context_obj;
+	int x,y,z;
+	char strbuf[256];
+    int cali[3] = {0};
+    SENSOR_DATA sensor_data = {0};
+	
+    if (_IOC_DIR(cmd) & _IOC_READ)
+    {
+        err = !access_ok(VERIFY_WRITE, (void __user *)arg, _IOC_SIZE(cmd));
+    }
+    else if (_IOC_DIR(cmd) & _IOC_WRITE)
+    {
+        err = !access_ok(VERIFY_READ, (void __user *)arg, _IOC_SIZE(cmd));
+    }
+
+    if (err)
+    {
+        ACC_ERR("access error: %08X, (%2d, %2d)\n", cmd, _IOC_DIR(cmd), _IOC_SIZE(cmd));
+        return -EFAULT;
+    }
+
+    switch (cmd)
+    {
+    case GSENSOR_IOCTL_INIT:
+	    break;
+    case GSENSOR_IOCTL_READ_CHIPINFO:
+        break;    
+    case GSENSOR_IOCTL_READ_SENSORDATA:
+		data = (void __user *) arg;
+        if (data == NULL)
+        {
+            err = -EINVAL;
+            break;    
+        }
+		if(cxt->acc_ctl.enable_nodata != NULL){
+			err = cxt->acc_ctl.enable_nodata(1);
+        	if(err)
+        	{ 
+           		err = cxt->acc_ctl.enable_nodata(1);
+		   		if(err)
+		  		 {
+		   			err = cxt->acc_ctl.enable_nodata(1);
+					if(err)
+					ACC_ERR("acc ioctl enable err 3 timers = %d\n", err);
+		   		}
+         	}
+			ACC_LOG("acc ioctl real enable  \n" );
+		}
+		
+		if(cxt->acc_data.get_data != NULL){
+			//err = cxt->acc_data.get_data(&x, &y, &z, &status);
+			err = cxt->acc_data.get_raw_data(&x, &y, &z);
+			if(err < 0)
+			{
+				ACC_ERR("GSENSOR_IOCTL_READ_SENSORDATA read data fail!\n");
+				break;
+			}
+			x+=cxt->cali_sw[0];
+			y+=cxt->cali_sw[1];
+			z+=cxt->cali_sw[2];
+			sprintf(strbuf, "%x %x %x", x, y, z);
+			ACC_LOG("GSENSOR_IOCTL_READ_SENSORDATA read data : (%d, %d, %d)!\n", x, y, z);
+			ACC_LOG("GSENSOR_IOCTL_READ_SENSORDATA read strbuf : (%s)!\n", strbuf);
+			
+	        if (copy_to_user(data, strbuf, strlen(strbuf)+1))
+	        {
+	            err = -EFAULT;
+	            break;    
+	        }
+		}else{
+			ACC_ERR("GSENSOR_IOCTL_READ_SENSORDATA ");
+		}
+        break;
+		
+    case GSENSOR_IOCTL_READ_RAW_DATA:
+		data = (void __user *) arg;
+        if (data == NULL)
+        {
+            err = -EINVAL;
+            break;    
+        }
+		if(cxt->acc_data.get_raw_data != NULL){
+			err = cxt->acc_data.get_raw_data(&x, &y, &z);
+			if(err < 0)
+			{
+				ACC_ERR("GSENSOR_IOCTL_READ_RAW_DATA read data fail!\n");
+				break;
+			}
+			x+=cxt->cali_sw[0];
+			y+=cxt->cali_sw[1];
+			z+=cxt->cali_sw[2];
+			sprintf(strbuf, "%x %x %x", x, y, z);
+			ACC_LOG("GSENSOR_IOCTL_READ_RAW_DATA read data : (%d, %d, %d)!\n", x, y, z);
+	        if (copy_to_user(data, strbuf, strlen(strbuf)+1))
+	        {
+	            err = -EFAULT;
+	            break;    
+	        }
+		}else{
+			ACC_ERR("GSENSOR_IOCTL_READ_SENSORDATA ");
+		}
+        break;   
+    case GSENSOR_IOCTL_SET_CALI:
+        data = (void __user*)arg;
+        if (data == NULL)
+        {
+            err = -EINVAL;
+            break;    
+        }
+        if (copy_from_user(&sensor_data, data, sizeof(sensor_data)))
+        {
+            err = -EFAULT;
+            break;    
+        }
+        cali[0] = sensor_data.x ;
+        cali[1] = sensor_data.y ;
+        cali[2] = sensor_data.z ;
+		ACC_LOG("GSENSOR_IOCTL_SET_CALI data : (%d, %d, %d)!\n", cali[0], cali[1], cali[2]);
+
+		acc_set_cali(cali);
+		/*if(cxt->acc_ctl.acc_calibration != NULL)
+		{
+			err = cxt->acc_ctl.acc_calibration(SETCALI, cali);
+			if(err < 0)
+			{
+				ACC_ERR("GSENSOR_IOCTL_SET_CALI fail!\n");
+				break;
+			}
+		}
+		*/
+        break;
+
+    case GSENSOR_IOCTL_CLR_CALI:
+		/*
+        if(cxt->acc_ctl.acc_calibration != NULL)
+		{
+			err = cxt->acc_ctl.acc_calibration(CLRCALI, cali);
+			if(err < 0)
+			{
+				ACC_ERR("GSENSOR_IOCTL_CLR_CALI fail!\n");
+				break;
+			}
+		} 
+		*/
+		acc_clear_cali();
+        break;
+
+    case GSENSOR_IOCTL_GET_CALI:
+        data = (void __user*)arg;
+        if (data == NULL)
+        {
+            err = -EINVAL;
+            break;    
+        }
+		/*
+        if(cxt->acc_ctl.acc_calibration != NULL)
+		{
+			err = cxt->acc_ctl.acc_calibration(GETCALI, cali);
+			if(err < 0)
+			{
+				ACC_ERR("GSENSOR_IOCTL_GET_CALI fail!\n");
+				break;
+			}
+		}
+		*/
+		ACC_LOG("GSENSOR_IOCTL_GET_CALI data : (%d, %d, %d)!\n", cxt->cali_sw[0], cxt->cali_sw[1], cxt->cali_sw[2]);
+        sensor_data.x = cxt->cali_sw[0]; 
+        sensor_data.y = cxt->cali_sw[1];
+        sensor_data.z = cxt->cali_sw[2];
+        if (copy_to_user(data, &sensor_data, sizeof(sensor_data)))
+        {
+            err = -EFAULT;
+            break;
+        }
+        break;
+		
+    default:
+        ACC_ERR("unknown IOCTL: 0x%08x\n", cmd);
+        err = -ENOIOCTLCMD;
+        break;
+
+    }
+    return err;
+}
+
+
+static struct file_operations acc_factory_fops = {
+    .open = acc_factory_open,
+    .release = acc_factory_release,
+    .unlocked_ioctl = acc_factory_unlocked_ioctl,
+};
+
+static struct miscdevice acc_factory_device = {
+    .minor = MISC_DYNAMIC_MINOR,
+    .name = "gsensor",
+    .fops = &acc_factory_fops,
+};
+
+int acc_factory_device_init()
+{
+	int error = 0;
+	struct acc_context *cxt = acc_context_obj;
+	if (!cxt->acc_ctl.is_use_common_factory) {
+		ACC_LOG("Node of '/dev/gsensor' has already existed!\n");
+		return -1;
+	}
+    if ((error = misc_register(&acc_factory_device)))
+    {
+        ACC_LOG("acc_factory_device register failed\n");
+		error = -1;
+    }	
+	return error;
+}
+
+
+
diff --git a/drivers/misc/mediatek/accelerometer/bma050-new/Makefile b/drivers/misc/mediatek/accelerometer/bma050-new/Makefile
new file mode 100644
index 000000000..d2af27b9f
--- /dev/null
+++ b/drivers/misc/mediatek/accelerometer/bma050-new/Makefile
@@ -0,0 +1,4 @@
+include $(srctree)/drivers/misc/mediatek/Makefile.custom
+
+obj-y	:=  bma050.o
+
diff --git a/drivers/misc/mediatek/accelerometer/bma050-new/bma050.c b/drivers/misc/mediatek/accelerometer/bma050-new/bma050.c
new file mode 100644
index 000000000..99f45048c
--- /dev/null
+++ b/drivers/misc/mediatek/accelerometer/bma050-new/bma050.c
@@ -0,0 +1,2342 @@
+/* BMA250 motion sensor driver
+ *
+ *
+ * This software program is licensed subject to the GNU General Public License
+ * (GPL).Version 2,June 1991, available at http://www.fsf.org/copyleft/gpl.html
+
+ * (C) Copyright 2011 Bosch Sensortec GmbH
+ * All Rights Reserved
+ */
+
+#include <linux/interrupt.h>
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include <linux/irq.h>
+#include <linux/miscdevice.h>
+#include <asm/uaccess.h>
+#include <linux/delay.h>
+#include <linux/input.h>
+#include <linux/workqueue.h>
+#include <linux/kobject.h>
+#include <linux/earlysuspend.h>
+#include <linux/platform_device.h>
+#include <asm/atomic.h>
+
+#include <mach/mt_typedefs.h>
+#include <mach/mt_gpio.h>
+#include <mach/mt_pm_ldo.h>
+
+#include <accel.h>
+#include <cust_acc.h>
+#include <linux/hwmsensor.h>
+#include <linux/hwmsen_dev.h>
+#include <linux/sensors_io.h>
+#include "bma050.h"
+#include <linux/hwmsen_helper.h>
+
+
+#define POWER_NONE_MACRO MT65XX_POWER_NONE
+
+/*----------------------------------------------------------------------------*/
+#define I2C_DRIVERID_BMA250 250
+/*----------------------------------------------------------------------------*/
+#define DEBUG 1
+/*----------------------------------------------------------------------------*/
+//#define CONFIG_BMA250_LOWPASS   /*apply low pass filter on output*/       
+#define SW_CALIBRATION
+
+/*----------------------------------------------------------------------------*/
+#define BMA250_AXIS_X          0
+#define BMA250_AXIS_Y          1
+#define BMA250_AXIS_Z          2
+#define BMA250_AXES_NUM        3
+#define BMA250_DATA_LEN        6
+#define BMA250_DEV_NAME        "BMA250"
+
+#define BMA250_MODE_NORMAL      0
+#define BMA250_MODE_LOWPOWER    1
+#define BMA250_MODE_SUSPEND     2
+
+#define BMA250_ACC_X_LSB__POS           6
+#define BMA250_ACC_X_LSB__LEN           2
+#define BMA250_ACC_X_LSB__MSK           0xC0
+//#define BMA250_ACC_X_LSB__REG           BMA250_X_AXIS_LSB_REG
+
+#define BMA250_ACC_X_MSB__POS           0
+#define BMA250_ACC_X_MSB__LEN           8
+#define BMA250_ACC_X_MSB__MSK           0xFF
+//#define BMA250_ACC_X_MSB__REG           BMA250_X_AXIS_MSB_REG
+
+#define BMA250_ACC_Y_LSB__POS           6
+#define BMA250_ACC_Y_LSB__LEN           2
+#define BMA250_ACC_Y_LSB__MSK           0xC0
+//#define BMA250_ACC_Y_LSB__REG           BMA250_Y_AXIS_LSB_REG
+
+#define BMA250_ACC_Y_MSB__POS           0
+#define BMA250_ACC_Y_MSB__LEN           8
+#define BMA250_ACC_Y_MSB__MSK           0xFF
+//#define BMA250_ACC_Y_MSB__REG           BMA250_Y_AXIS_MSB_REG
+
+#define BMA250_ACC_Z_LSB__POS           6
+#define BMA250_ACC_Z_LSB__LEN           2
+#define BMA250_ACC_Z_LSB__MSK           0xC0
+//#define BMA250_ACC_Z_LSB__REG           BMA250_Z_AXIS_LSB_REG
+
+#define BMA250_ACC_Z_MSB__POS           0
+#define BMA250_ACC_Z_MSB__LEN           8
+#define BMA250_ACC_Z_MSB__MSK           0xFF
+//#define BMA250_ACC_Z_MSB__REG           BMA250_Z_AXIS_MSB_REG
+
+#define BMA250_EN_LOW_POWER__POS          6
+#define BMA250_EN_LOW_POWER__LEN          1
+#define BMA250_EN_LOW_POWER__MSK          0x40
+#define BMA250_EN_LOW_POWER__REG          BMA250_REG_POWER_CTL
+
+#define BMA250_EN_SUSPEND__POS            7
+#define BMA250_EN_SUSPEND__LEN            1
+#define BMA250_EN_SUSPEND__MSK            0x80
+#define BMA250_EN_SUSPEND__REG            BMA250_REG_POWER_CTL
+
+#define BMA250_RANGE_SEL__POS             0
+#define BMA250_RANGE_SEL__LEN             4
+#define BMA250_RANGE_SEL__MSK             0x0F
+#define BMA250_RANGE_SEL__REG             BMA250_REG_DATA_FORMAT
+
+#define BMA250_BANDWIDTH__POS             0
+#define BMA250_BANDWIDTH__LEN             5
+#define BMA250_BANDWIDTH__MSK             0x1F
+#define BMA250_BANDWIDTH__REG             BMA250_REG_BW_RATE
+
+#define BMA250_GET_BITSLICE(regvar, bitname)\
+	((regvar & bitname##__MSK) >> bitname##__POS)
+
+#define BMA250_SET_BITSLICE(regvar, bitname, val)\
+	((regvar & ~bitname##__MSK) | ((val<<bitname##__POS)&bitname##__MSK))
+
+/*----------------------------------------------------------------------------*/
+
+/*----------------------------------------------------------------------------*/
+static const struct i2c_device_id bma250_i2c_id[] = {{BMA250_DEV_NAME,0},{}};
+static struct i2c_board_info __initdata i2c_bma250={ I2C_BOARD_INFO("BMA250", (0x30>>1))};
+
+
+/*the adapter id will be available in customization*/
+//static unsigned short bma250_force[] = {0x00, BMA250_I2C_SLAVE_WRITE_ADDR, I2C_CLIENT_END, I2C_CLIENT_END};
+//static const unsigned short *const bma250_forces[] = { bma250_force, NULL };
+//static struct i2c_client_address_data bma250_addr_data = { .forces = bma250_forces,};
+
+/*----------------------------------------------------------------------------*/
+static int bma250_i2c_probe(struct i2c_client *client, const struct i2c_device_id *id); 
+static int bma250_i2c_remove(struct i2c_client *client);
+static int bma250_suspend(struct i2c_client *client, pm_message_t msg);
+static int bma250_resume(struct i2c_client *client);
+
+static int bma050_local_init(void);
+static int  bma050_remove(void);
+
+//static int bma250_i2c_detect(struct i2c_client *client, int kind, struct i2c_board_info *info);
+
+/*----------------------------------------------------------------------------*/
+typedef enum {
+    ADX_TRC_FILTER  = 0x01,
+    ADX_TRC_RAWDATA = 0x02,
+    ADX_TRC_IOCTL   = 0x04,
+    ADX_TRC_CALI	= 0X08,
+    ADX_TRC_INFO	= 0X10,
+} ADX_TRC;
+/*----------------------------------------------------------------------------*/
+struct scale_factor{
+    u8  whole;
+    u8  fraction;
+};
+/*----------------------------------------------------------------------------*/
+struct data_resolution {
+    struct scale_factor scalefactor;
+    int                 sensitivity;
+};
+/*----------------------------------------------------------------------------*/
+#define C_MAX_FIR_LENGTH (32)
+//#define USE_EARLY_SUSPEND
+static bool enable_status = false;
+static DEFINE_MUTEX(bma050_i2c_mutex);
+static DEFINE_MUTEX(bma050_op_mutex);
+
+static int bma050_init_flag =-1; // 0<==>OK -1 <==> fail
+
+static struct acc_init_info bma050_init_info = {
+		.name = "bma050",
+		.init = bma050_local_init,
+		.uninit = bma050_remove,
+	
+};
+
+/*----------------------------------------------------------------------------*/
+struct data_filter {
+    s16 raw[C_MAX_FIR_LENGTH][BMA250_AXES_NUM];
+    int sum[BMA250_AXES_NUM];
+    int num;
+    int idx;
+};
+/*----------------------------------------------------------------------------*/
+struct bma250_i2c_data {
+    struct i2c_client *client;
+    struct acc_hw *hw;
+    struct hwmsen_convert   cvt;
+    
+    /*misc*/
+    struct data_resolution *reso;
+    atomic_t                trace;
+    atomic_t                suspend;
+    atomic_t                selftest;
+	atomic_t				filter;
+    s16                     cali_sw[BMA250_AXES_NUM+1];
+
+    /*data*/
+    s8                      offset[BMA250_AXES_NUM+1];  /*+1: for 4-byte alignment*/
+    s16                     data[BMA250_AXES_NUM+1];
+
+#if defined(CONFIG_BMA250_LOWPASS)
+    atomic_t                firlen;
+    atomic_t                fir_en;
+    struct data_filter      fir;
+#endif 
+    /*early suspend*/
+#if defined(USE_EARLY_SUSPEND)
+    struct early_suspend    early_drv;
+#endif     
+};
+/*----------------------------------------------------------------------------*/
+static struct i2c_driver bma250_i2c_driver = {
+    .driver = {
+ //       .owner          = THIS_MODULE,
+        .name           = BMA250_DEV_NAME,
+    },
+	.probe      		= bma250_i2c_probe,
+	.remove    			= bma250_i2c_remove,
+//	.detect				= bma250_i2c_detect,
+#if !defined(USE_EARLY_SUSPEND)    
+    .suspend            = bma250_suspend,
+    .resume             = bma250_resume,
+#endif
+	.id_table = bma250_i2c_id,
+//	.address_data = &bma250_addr_data,
+};
+
+/*----------------------------------------------------------------------------*/
+static struct i2c_client *bma250_i2c_client = NULL;
+static struct bma250_i2c_data *obj_i2c_data = NULL;
+static bool sensor_power = true;
+static GSENSOR_VECTOR3D gsensor_gain;
+//static char selftestRes[8]= {0}; 
+
+/*----------------------------------------------------------------------------*/
+#define GSE_TAG                  "[Gsensor] "
+#define GSE_FUN(f)               printk(KERN_ERR GSE_TAG"%s\n", __FUNCTION__)
+#define GSE_ERR(fmt, args...)    printk(KERN_ERR GSE_TAG"%s %d : "fmt, __FUNCTION__, __LINE__, ##args)
+#define GSE_LOG(fmt, args...)    printk(KERN_ERR GSE_TAG fmt, ##args)
+/*----------------------------------------------------------------------------*/
+static struct data_resolution bma250_data_resolution[1] = {
+ /* combination by {FULL_RES,RANGE}*/
+    {{ 3, 9}, 256},   // dataformat +/-2g  in 10-bit resolution;  { 3, 9} = 3.9= (2*2*1000)/(2^10);  256 = (2^10)/(2*2)          
+};
+/*----------------------------------------------------------------------------*/
+static struct data_resolution bma250_offset_resolution = {{3, 9}, 256};
+
+#define C_I2C_FIFO_SIZE         8        
+static int bma050_i2c_read_block(struct i2c_client *client, u8 addr, u8 *data, u8 len)
+{
+    u8 beg = addr;
+	int err;
+	struct i2c_msg msgs[2]={{0},{0}};
+	
+	mutex_lock(&bma050_i2c_mutex);
+	
+	msgs[0].addr = client->addr;
+	msgs[0].flags = 0;
+	msgs[0].len =1;
+	msgs[0].buf = &beg;
+
+	msgs[1].addr = client->addr;
+	msgs[1].flags = I2C_M_RD;
+	msgs[1].len =len;
+	msgs[1].buf = data;
+	
+	if (!client)
+	{
+	    mutex_unlock(&bma050_i2c_mutex);
+		return -EINVAL;
+	}
+	else if (len > C_I2C_FIFO_SIZE) 
+	{
+		GSE_ERR(" length %d exceeds %d\n", len, C_I2C_FIFO_SIZE);
+		mutex_unlock(&bma050_i2c_mutex);
+		return -EINVAL;
+	}
+	err = i2c_transfer(client->adapter, msgs, sizeof(msgs)/sizeof(msgs[0]));
+	if (err != 2) 
+	{
+		GSE_ERR("i2c_transfer error: (%d %p %d) %d\n",addr, data, len, err);
+		err = -EIO;
+	} 
+	else 
+	{
+		err = 0;
+	}
+	mutex_unlock(&bma050_i2c_mutex);
+	return err;
+
+}
+
+static int bma050_i2c_write_block(struct i2c_client *client, u8 addr, u8 *data, u8 len)
+{   /*because address also occupies one byte, the maximum length for write is 7 bytes*/
+    int err, idx, num;
+    char buf[C_I2C_FIFO_SIZE];
+    err =0;
+	mutex_lock(&bma050_i2c_mutex);
+    if (!client)
+    {
+        mutex_unlock(&bma050_i2c_mutex);
+        return -EINVAL;
+    }
+    else if (len >= C_I2C_FIFO_SIZE) 
+	{        
+        GSE_ERR(" length %d exceeds %d\n", len, C_I2C_FIFO_SIZE);
+		mutex_unlock(&bma050_i2c_mutex);
+        return -EINVAL;
+    }    
+
+    num = 0;
+    buf[num++] = addr;
+    for (idx = 0; idx < len; idx++)
+    {
+        buf[num++] = data[idx];
+    }
+
+    err = i2c_master_send(client, buf, num);
+    if (err < 0)
+	{
+        GSE_ERR("send command error!!\n");
+		mutex_unlock(&bma050_i2c_mutex);
+        return -EFAULT;
+    } 
+	mutex_unlock(&bma050_i2c_mutex);
+    return err;
+}
+
+
+/*--------------------BMA250 power control function----------------------------------*/
+static void BMA250_power(struct acc_hw *hw, unsigned int on) 
+{
+	static unsigned int power_on = 0;
+
+	if(hw->power_id != POWER_NONE_MACRO)		// have externel LDO
+	{        
+		GSE_LOG("power %s\n", on ? "on" : "off");
+		if(power_on == on)	// power status not change
+		{
+			GSE_LOG("ignore power control: %d\n", on);
+		}
+		else if(on)	// power on
+		{
+			if(!hwPowerOn(hw->power_id, hw->power_vol, "BMA250"))
+			{
+				GSE_ERR("power on fails!!\n");
+			}
+		}
+		else	// power off
+		{
+			if (!hwPowerDown(hw->power_id, "BMA250"))
+			{
+				GSE_ERR("power off fail!!\n");
+			}			  
+		}
+	}
+	power_on = on;    
+}
+/*----------------------------------------------------------------------------*/
+
+/*----------------------------------------------------------------------------*/
+static int BMA250_SetDataResolution(struct bma250_i2c_data *obj)
+{
+
+/*set g sensor dataresolution here*/
+
+/*BMA250 only can set to 10-bit dataresolution, so do nothing in bma250 driver here*/
+
+/*end of set dataresolution*/
+
+
+ 
+ /*we set measure range from -2g to +2g in BMA250_SetDataFormat(client, BMA250_RANGE_2G), 
+                                                    and set 10-bit dataresolution BMA250_SetDataResolution()*/
+                                                    
+ /*so bma250_data_resolution[0] set value as {{ 3, 9}, 256} when declaration, and assign the value to obj->reso here*/  
+
+ 	obj->reso = &bma250_data_resolution[0];
+	return 0;
+	
+/*if you changed the measure range, for example call: BMA250_SetDataFormat(client, BMA250_RANGE_4G), 
+you must set the right value to bma250_data_resolution*/
+
+}
+/*----------------------------------------------------------------------------*/
+static int BMA250_ReadData(struct i2c_client *client, s16 data[BMA250_AXES_NUM])
+{
+	//struct bma250_i2c_data *priv = i2c_get_clientdata(client);        
+	u8 addr = BMA250_REG_DATAXLOW;
+	u8 buf[BMA250_DATA_LEN] = {0};
+	int err = 0;
+
+	if(NULL == client)
+	{
+		err = -EINVAL;
+	}
+	else if((err = bma050_i2c_read_block(client, addr, buf, BMA250_DATA_LEN)))
+	{
+		GSE_ERR("error: %d\n", err);
+	}
+	else
+	{
+		/* Convert sensor raw data to 16-bit integer */
+		data[BMA250_AXIS_X] = BMA250_GET_BITSLICE(buf[0], BMA250_ACC_X_LSB)
+			|(BMA250_GET_BITSLICE(buf[1],
+						BMA250_ACC_X_MSB)<<BMA250_ACC_X_LSB__LEN);
+		data[BMA250_AXIS_X] = data[BMA250_AXIS_X] << (sizeof(short)*8-(BMA250_ACC_X_LSB__LEN
+					+ BMA250_ACC_X_MSB__LEN));
+		data[BMA250_AXIS_X] = data[BMA250_AXIS_X] >> (sizeof(short)*8-(BMA250_ACC_X_LSB__LEN
+					+ BMA250_ACC_X_MSB__LEN));
+		data[BMA250_AXIS_Y] = BMA250_GET_BITSLICE(buf[2], BMA250_ACC_Y_LSB)
+			| (BMA250_GET_BITSLICE(buf[3],
+						BMA250_ACC_Y_MSB)<<BMA250_ACC_Y_LSB__LEN);
+		data[BMA250_AXIS_Y] = data[BMA250_AXIS_Y] << (sizeof(short)*8-(BMA250_ACC_Y_LSB__LEN
+					+ BMA250_ACC_Y_MSB__LEN));
+		data[BMA250_AXIS_Y] = data[BMA250_AXIS_Y] >> (sizeof(short)*8-(BMA250_ACC_Y_LSB__LEN
+					+ BMA250_ACC_Y_MSB__LEN));
+		data[BMA250_AXIS_Z] = BMA250_GET_BITSLICE(buf[4], BMA250_ACC_Z_LSB)
+			| (BMA250_GET_BITSLICE(buf[5],
+						BMA250_ACC_Z_MSB)<<BMA250_ACC_Z_LSB__LEN);
+		data[BMA250_AXIS_Z] = data[BMA250_AXIS_Z] << (sizeof(short)*8-(BMA250_ACC_Z_LSB__LEN
+					+ BMA250_ACC_Z_MSB__LEN));
+		data[BMA250_AXIS_Z] = data[BMA250_AXIS_Z] >> (sizeof(short)*8-(BMA250_ACC_Z_LSB__LEN
+					+ BMA250_ACC_Z_MSB__LEN));
+
+#ifdef CONFIG_BMA250_LOWPASS
+		if(atomic_read(&priv->filter))
+		{
+			if(atomic_read(&priv->fir_en) && !atomic_read(&priv->suspend))
+			{
+				int idx, firlen = atomic_read(&priv->firlen);   
+				if(priv->fir.num < firlen)
+				{                
+					priv->fir.raw[priv->fir.num][BMA250_AXIS_X] = data[BMA250_AXIS_X];
+					priv->fir.raw[priv->fir.num][BMA250_AXIS_Y] = data[BMA250_AXIS_Y];
+					priv->fir.raw[priv->fir.num][BMA250_AXIS_Z] = data[BMA250_AXIS_Z];
+					priv->fir.sum[BMA250_AXIS_X] += data[BMA250_AXIS_X];
+					priv->fir.sum[BMA250_AXIS_Y] += data[BMA250_AXIS_Y];
+					priv->fir.sum[BMA250_AXIS_Z] += data[BMA250_AXIS_Z];
+					if(atomic_read(&priv->trace) & ADX_TRC_FILTER)
+					{
+						GSE_LOG("add [%2d] [%5d %5d %5d] => [%5d %5d %5d]\n", priv->fir.num,
+							priv->fir.raw[priv->fir.num][BMA250_AXIS_X], priv->fir.raw[priv->fir.num][BMA250_AXIS_Y], priv->fir.raw[priv->fir.num][BMA250_AXIS_Z],
+							priv->fir.sum[BMA250_AXIS_X], priv->fir.sum[BMA250_AXIS_Y], priv->fir.sum[BMA250_AXIS_Z]);
+					}
+					priv->fir.num++;
+					priv->fir.idx++;
+				}
+				else
+				{
+					idx = priv->fir.idx % firlen;
+					priv->fir.sum[BMA250_AXIS_X] -= priv->fir.raw[idx][BMA250_AXIS_X];
+					priv->fir.sum[BMA250_AXIS_Y] -= priv->fir.raw[idx][BMA250_AXIS_Y];
+					priv->fir.sum[BMA250_AXIS_Z] -= priv->fir.raw[idx][BMA250_AXIS_Z];
+					priv->fir.raw[idx][BMA250_AXIS_X] = data[BMA250_AXIS_X];
+					priv->fir.raw[idx][BMA250_AXIS_Y] = data[BMA250_AXIS_Y];
+					priv->fir.raw[idx][BMA250_AXIS_Z] = data[BMA250_AXIS_Z];
+					priv->fir.sum[BMA250_AXIS_X] += data[BMA250_AXIS_X];
+					priv->fir.sum[BMA250_AXIS_Y] += data[BMA250_AXIS_Y];
+					priv->fir.sum[BMA250_AXIS_Z] += data[BMA250_AXIS_Z];
+					priv->fir.idx++;
+					data[BMA250_AXIS_X] = priv->fir.sum[BMA250_AXIS_X]/firlen;
+					data[BMA250_AXIS_Y] = priv->fir.sum[BMA250_AXIS_Y]/firlen;
+					data[BMA250_AXIS_Z] = priv->fir.sum[BMA250_AXIS_Z]/firlen;
+					if(atomic_read(&priv->trace) & ADX_TRC_FILTER)
+					{
+						GSE_LOG("add [%2d] [%5d %5d %5d] => [%5d %5d %5d] : [%5d %5d %5d]\n", idx,
+						priv->fir.raw[idx][BMA250_AXIS_X], priv->fir.raw[idx][BMA250_AXIS_Y], priv->fir.raw[idx][BMA250_AXIS_Z],
+						priv->fir.sum[BMA250_AXIS_X], priv->fir.sum[BMA250_AXIS_Y], priv->fir.sum[BMA250_AXIS_Z],
+						data[BMA250_AXIS_X], data[BMA250_AXIS_Y], data[BMA250_AXIS_Z]);
+					}
+				}
+			}
+		}	
+#endif         
+	}
+	return err;
+}
+/*----------------------------------------------------------------------------*/
+static int BMA250_ReadOffset(struct i2c_client *client, s8 ofs[BMA250_AXES_NUM])
+{    
+	int err = 0;
+#ifdef SW_CALIBRATION
+	ofs[0]=ofs[1]=ofs[2]=0x0;
+#else
+	if(err = bma050_i2c_read_block(client, BMA250_REG_OFSX, ofs, BMA250_AXES_NUM))
+	{
+		GSE_ERR("error: %d\n", err);
+	}
+#endif
+	//printk("offesx=%x, y=%x, z=%x",ofs[0],ofs[1],ofs[2]);
+	
+	return err;    
+}
+/*----------------------------------------------------------------------------*/
+static int BMA250_ResetCalibration(struct i2c_client *client)
+{
+	struct bma250_i2c_data *obj = i2c_get_clientdata(client);
+	//u8 ofs[4]={0,0,0,0};
+	int err  = 0;
+	
+	#ifdef SW_CALIBRATION
+		
+	#else
+		if(err = bma050_i2c_write_block(client, BMA250_REG_OFSX, ofs, 4))
+		{
+			GSE_ERR("error: %d\n", err);
+		}
+	#endif
+
+	memset(obj->cali_sw, 0x00, sizeof(obj->cali_sw));
+	memset(obj->offset, 0x00, sizeof(obj->offset));
+	return err;    
+}
+/*----------------------------------------------------------------------------*/
+static int BMA250_ReadCalibration(struct i2c_client *client, int dat[BMA250_AXES_NUM])
+{
+    struct bma250_i2c_data *obj = i2c_get_clientdata(client);
+    //int err;
+    int mul;
+
+	#ifdef SW_CALIBRATION
+		mul = 0;//only SW Calibration, disable HW Calibration
+	#else
+	    if ((err = BMA250_ReadOffset(client, obj->offset))) {
+        GSE_ERR("read offset fail, %d\n", err);
+        return err;
+    	}    
+    	mul = obj->reso->sensitivity/bma250_offset_resolution.sensitivity;
+	#endif
+
+    dat[obj->cvt.map[BMA250_AXIS_X]] = obj->cvt.sign[BMA250_AXIS_X]*(obj->offset[BMA250_AXIS_X]*mul + obj->cali_sw[BMA250_AXIS_X]);
+    dat[obj->cvt.map[BMA250_AXIS_Y]] = obj->cvt.sign[BMA250_AXIS_Y]*(obj->offset[BMA250_AXIS_Y]*mul + obj->cali_sw[BMA250_AXIS_Y]);
+    dat[obj->cvt.map[BMA250_AXIS_Z]] = obj->cvt.sign[BMA250_AXIS_Z]*(obj->offset[BMA250_AXIS_Z]*mul + obj->cali_sw[BMA250_AXIS_Z]);                        
+                                       
+    return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int BMA250_ReadCalibrationEx(struct i2c_client *client, int act[BMA250_AXES_NUM], int raw[BMA250_AXES_NUM])
+{  
+	/*raw: the raw calibration data; act: the actual calibration data*/
+	struct bma250_i2c_data *obj = i2c_get_clientdata(client);
+	//int err;
+	int mul;
+
+ 
+
+	#ifdef SW_CALIBRATION
+		mul = 0;//only SW Calibration, disable HW Calibration
+	#else
+		if(err = BMA250_ReadOffset(client, obj->offset))
+		{
+			GSE_ERR("read offset fail, %d\n", err);
+			return err;
+		}   
+		mul = obj->reso->sensitivity/bma250_offset_resolution.sensitivity;
+	#endif
+	
+	raw[BMA250_AXIS_X] = obj->offset[BMA250_AXIS_X]*mul + obj->cali_sw[BMA250_AXIS_X];
+	raw[BMA250_AXIS_Y] = obj->offset[BMA250_AXIS_Y]*mul + obj->cali_sw[BMA250_AXIS_Y];
+	raw[BMA250_AXIS_Z] = obj->offset[BMA250_AXIS_Z]*mul + obj->cali_sw[BMA250_AXIS_Z];
+
+	act[obj->cvt.map[BMA250_AXIS_X]] = obj->cvt.sign[BMA250_AXIS_X]*raw[BMA250_AXIS_X];
+	act[obj->cvt.map[BMA250_AXIS_Y]] = obj->cvt.sign[BMA250_AXIS_Y]*raw[BMA250_AXIS_Y];
+	act[obj->cvt.map[BMA250_AXIS_Z]] = obj->cvt.sign[BMA250_AXIS_Z]*raw[BMA250_AXIS_Z];                        
+	                       
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int BMA250_WriteCalibration(struct i2c_client *client, int dat[BMA250_AXES_NUM])
+{
+	struct bma250_i2c_data *obj = i2c_get_clientdata(client);
+	int err;
+	int cali[BMA250_AXES_NUM], raw[BMA250_AXES_NUM];
+	//int lsb = bma250_offset_resolution.sensitivity;
+	//int divisor = obj->reso->sensitivity/lsb;
+
+	if((err = BMA250_ReadCalibrationEx(client, cali, raw)))	/*offset will be updated in obj->offset*/
+	{ 
+		GSE_ERR("read offset fail, %d\n", err);
+		return err;
+	}
+
+	GSE_LOG("OLDOFF: (%+3d %+3d %+3d): (%+3d %+3d %+3d) / (%+3d %+3d %+3d)\n", 
+		raw[BMA250_AXIS_X], raw[BMA250_AXIS_Y], raw[BMA250_AXIS_Z],
+		obj->offset[BMA250_AXIS_X], obj->offset[BMA250_AXIS_Y], obj->offset[BMA250_AXIS_Z],
+		obj->cali_sw[BMA250_AXIS_X], obj->cali_sw[BMA250_AXIS_Y], obj->cali_sw[BMA250_AXIS_Z]);
+
+	/*calculate the real offset expected by caller*/
+	cali[BMA250_AXIS_X] += dat[BMA250_AXIS_X];
+	cali[BMA250_AXIS_Y] += dat[BMA250_AXIS_Y];
+	cali[BMA250_AXIS_Z] += dat[BMA250_AXIS_Z];
+
+	GSE_LOG("UPDATE: (%+3d %+3d %+3d)\n", 
+		dat[BMA250_AXIS_X], dat[BMA250_AXIS_Y], dat[BMA250_AXIS_Z]);
+
+#ifdef SW_CALIBRATION
+	obj->cali_sw[BMA250_AXIS_X] = obj->cvt.sign[BMA250_AXIS_X]*(cali[obj->cvt.map[BMA250_AXIS_X]]);
+	obj->cali_sw[BMA250_AXIS_Y] = obj->cvt.sign[BMA250_AXIS_Y]*(cali[obj->cvt.map[BMA250_AXIS_Y]]);
+	obj->cali_sw[BMA250_AXIS_Z] = obj->cvt.sign[BMA250_AXIS_Z]*(cali[obj->cvt.map[BMA250_AXIS_Z]]);	
+#else
+	obj->offset[BMA250_AXIS_X] = (s8)(obj->cvt.sign[BMA250_AXIS_X]*(cali[obj->cvt.map[BMA250_AXIS_X]])/(divisor));
+	obj->offset[BMA250_AXIS_Y] = (s8)(obj->cvt.sign[BMA250_AXIS_Y]*(cali[obj->cvt.map[BMA250_AXIS_Y]])/(divisor));
+	obj->offset[BMA250_AXIS_Z] = (s8)(obj->cvt.sign[BMA250_AXIS_Z]*(cali[obj->cvt.map[BMA250_AXIS_Z]])/(divisor));
+
+	/*convert software calibration using standard calibration*/
+	obj->cali_sw[BMA250_AXIS_X] = obj->cvt.sign[BMA250_AXIS_X]*(cali[obj->cvt.map[BMA250_AXIS_X]])%(divisor);
+	obj->cali_sw[BMA250_AXIS_Y] = obj->cvt.sign[BMA250_AXIS_Y]*(cali[obj->cvt.map[BMA250_AXIS_Y]])%(divisor);
+	obj->cali_sw[BMA250_AXIS_Z] = obj->cvt.sign[BMA250_AXIS_Z]*(cali[obj->cvt.map[BMA250_AXIS_Z]])%(divisor);
+
+	GSE_LOG("NEWOFF: (%+3d %+3d %+3d): (%+3d %+3d %+3d) / (%+3d %+3d %+3d)\n", 
+		obj->offset[BMA250_AXIS_X]*divisor + obj->cali_sw[BMA250_AXIS_X], 
+		obj->offset[BMA250_AXIS_Y]*divisor + obj->cali_sw[BMA250_AXIS_Y], 
+		obj->offset[BMA250_AXIS_Z]*divisor + obj->cali_sw[BMA250_AXIS_Z], 
+		obj->offset[BMA250_AXIS_X], obj->offset[BMA250_AXIS_Y], obj->offset[BMA250_AXIS_Z],
+		obj->cali_sw[BMA250_AXIS_X], obj->cali_sw[BMA250_AXIS_Y], obj->cali_sw[BMA250_AXIS_Z]);
+
+	if(err = bma050_i2c_write_block(obj->client, BMA250_REG_OFSX, obj->offset, BMA250_AXES_NUM))
+	{
+		GSE_ERR("write offset fail: %d\n", err);
+		return err;
+	}
+#endif
+
+	return err;
+}
+/*----------------------------------------------------------------------------*/
+static int BMA250_CheckDeviceID(struct i2c_client *client)
+{
+	u8 databuf[2];    
+	int res = 0;
+
+	memset(databuf, 0, sizeof(u8)*2);    
+	databuf[0] = BMA250_REG_DEVID;    
+
+    res = bma050_i2c_read_block(client,BMA250_REG_DEVID,databuf,0x1);
+	if(res < 0)
+	{
+		goto exit_BMA250_CheckDeviceID;
+	}
+	if(databuf[0]!=BMA250_FIXED_DEVID)
+	{
+		printk("BMA250_CheckDeviceID %d failt!\n ", databuf[0]);
+		return BMA250_ERR_IDENTIFICATION;
+	}
+	else
+	{
+		printk("BMA250_CheckDeviceID %d pass!\n ", databuf[0]);
+	}
+
+	exit_BMA250_CheckDeviceID:
+	if (res < 0)
+	{
+		return BMA250_ERR_I2C;
+	}
+	
+	return BMA250_SUCCESS;
+}
+/*----------------------------------------------------------------------------*/
+static int BMA250_SetPowerMode(struct i2c_client *client, bool enable)
+{
+	u8 databuf[2];    
+	int res = 0;
+	u8 addr = BMA250_REG_POWER_CTL;
+	struct bma250_i2c_data *obj = i2c_get_clientdata(client);
+	
+	
+	if(enable == sensor_power )
+	{
+		GSE_LOG("Sensor power status is newest!\n");
+		return BMA250_SUCCESS;
+	}
+
+	if(bma050_i2c_read_block(client, addr, databuf, 0x01))
+	{
+		GSE_ERR("read power ctl register err!\n");
+		return BMA250_ERR_I2C;
+	}
+
+	
+	if(enable == TRUE)
+	{
+		databuf[0] &= ~BMA250_MEASURE_MODE;
+	}
+	else
+	{
+		databuf[0] |= BMA250_MEASURE_MODE;
+	}
+	//databuf[1] = databuf[0];
+	//databuf[0] = BMA250_REG_POWER_CTL;
+	
+    res = bma050_i2c_write_block(client,BMA250_REG_POWER_CTL,databuf,0x1);
+	if(res < 0)
+	{
+		GSE_LOG("set power mode failed!\n");
+		return BMA250_ERR_I2C;
+	}
+	else if(atomic_read(&obj->trace) & ADX_TRC_INFO)
+	{
+		GSE_LOG("set power mode ok %d!\n", databuf[1]);
+	}
+
+	//GSE_LOG("BMA250_SetPowerMode ok!\n");
+
+
+	sensor_power = enable;
+
+	mdelay(20);
+	
+	return BMA250_SUCCESS;    
+}
+/*----------------------------------------------------------------------------*/
+static int BMA250_SetDataFormat(struct i2c_client *client, u8 dataformat)
+{
+	struct bma250_i2c_data *obj = i2c_get_clientdata(client);
+	u8 databuf[10];    
+	int res = 0;
+
+	memset(databuf, 0, sizeof(u8)*10);    
+
+	if(bma050_i2c_read_block(client, BMA250_REG_DATA_FORMAT, databuf, 0x01))
+	{
+		printk("bma250 read Dataformat failt \n");
+		return BMA250_ERR_I2C;
+	}
+
+	databuf[0] &= ~BMA250_RANGE_MASK;
+	databuf[0] |= dataformat;
+	
+    res = bma050_i2c_write_block(client,BMA250_REG_DATA_FORMAT,databuf,0x1);
+	if(res < 0)
+	{
+		return BMA250_ERR_I2C;
+	}
+	
+	//printk("BMA250_SetDataFormat OK! \n");
+	
+
+	return BMA250_SetDataResolution(obj);    
+}
+/*----------------------------------------------------------------------------*/
+static int BMA250_SetBWRate(struct i2c_client *client, u8 bwrate)
+{
+	u8 databuf[10];    
+	int res = 0;
+
+	memset(databuf, 0, sizeof(u8)*10);    
+
+	if(bma050_i2c_read_block(client, BMA250_REG_BW_RATE, databuf, 0x01))
+	{
+		printk("bma250 read rate failt \n");
+		return BMA250_ERR_I2C;
+	}
+
+	databuf[0] &= ~BMA250_BW_MASK;
+	databuf[0] |= bwrate;
+
+    res = bma050_i2c_write_block(client,BMA250_REG_BW_RATE,databuf,0x1);
+	if(res < 0)
+	{
+		return BMA250_ERR_I2C;
+	}
+	
+	//printk("BMA250_SetBWRate OK! \n");
+	
+	return BMA250_SUCCESS;    
+}
+/*----------------------------------------------------------------------------*/
+static int BMA250_SetIntEnable(struct i2c_client *client, u8 intenable)
+{
+			//u8 databuf[10];    
+			int res = 0;
+		
+			res = hwmsen_write_byte(client, BMA250_INT_REG_1, 0x00);
+			if(res != BMA250_SUCCESS) 
+			{
+				return res;
+			}
+			res = hwmsen_write_byte(client, BMA250_INT_REG_2, 0x00);
+			if(res != BMA250_SUCCESS) 
+			{
+				return res;
+			}
+			printk("BMA250 disable interrupt ...\n");
+		
+			/*for disable interrupt function*/
+			
+			return BMA250_SUCCESS;	  
+}
+
+/*----------------------------------------------------------------------------*/
+static int bma250_init_client(struct i2c_client *client, int reset_cali)
+{
+	struct bma250_i2c_data *obj = i2c_get_clientdata(client);
+	int res = 0;
+	int a = 0;
+	printk("bma250_init_client \n");
+
+	//for fix check device id error
+	do{
+	udelay(100);
+	res = BMA250_CheckDeviceID(client); 
+	if(res == BMA250_SUCCESS)
+	{
+	printk("BMA250_CheckDeviceID ok \n");
+		break;
+	}	
+	a++;	
+	printk("bma250_init_client count: %d\n", a);
+	}while(a < 1000);
+	
+	res = BMA250_SetBWRate(client, BMA250_BW_50HZ);
+	if(res != BMA250_SUCCESS ) 
+	{
+		return res;
+	}
+	printk("BMA250_SetBWRate OK!\n");
+	
+	res = BMA250_SetDataFormat(client, BMA250_RANGE_2G);
+	if(res != BMA250_SUCCESS) 
+	{
+		return res;
+	}
+	printk("BMA250_SetDataFormat OK!\n");
+
+	gsensor_gain.x = gsensor_gain.y = gsensor_gain.z = obj->reso->sensitivity;
+
+
+	res = BMA250_SetIntEnable(client, 0x00);        
+	if(res != BMA250_SUCCESS)
+	{
+		return res;
+	}
+	printk("BMA250 disable interrupt function!\n");
+
+	res = BMA250_SetPowerMode(client, enable_status);
+		if(res != BMA250_SUCCESS)
+		{
+			return res;
+		}
+		printk("BMA250_SetPowerMode OK!\n");
+
+
+	if(0 != reset_cali)
+	{ 
+		/*reset calibration only in power on*/
+		res = BMA250_ResetCalibration(client);
+		if(res != BMA250_SUCCESS)
+		{
+			return res;
+		}
+	}
+	printk("bma250_init_client OK!\n");
+#ifdef CONFIG_BMA250_LOWPASS
+	memset(&obj->fir, 0x00, sizeof(obj->fir));  
+#endif
+
+	mdelay(20);
+
+	return BMA250_SUCCESS;
+}
+/*----------------------------------------------------------------------------*/
+static int BMA250_ReadChipInfo(struct i2c_client *client, char *buf, int bufsize)
+{
+	u8 databuf[10];    
+
+	memset(databuf, 0, sizeof(u8)*10);
+
+	if((NULL == buf)||(bufsize<=30))
+	{
+		return -1;
+	}
+	
+	if(NULL == client)
+	{
+		*buf = 0;
+		return -2;
+	}
+
+	sprintf(buf, "BMA250 Chip");
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int BMA250_CompassReadData(struct i2c_client *client, char *buf, int bufsize)
+{
+	struct bma250_i2c_data *obj = (struct bma250_i2c_data*)i2c_get_clientdata(client);
+	u8 databuf[20];
+	int acc[BMA250_AXES_NUM];
+	int res = 0;
+	memset(databuf, 0, sizeof(u8)*10);
+
+	if(NULL == buf)
+	{
+		return -1;
+	}
+	if(NULL == client)
+	{
+		*buf = 0;
+		return -2;
+	}
+/*
+	if(sensor_power == FALSE)
+	{
+		res = BMA250_SetPowerMode(client, true);
+		if(res)
+		{
+			GSE_ERR("Power on bma250 error %d!\n", res);
+		}
+	}
+*/
+	if((res = BMA250_ReadData(client, obj->data)))
+	{        
+		GSE_ERR("I2C error: ret value=%d", res);
+		return -3;
+	}
+	else
+	{
+		/*remap coordinate*/
+		acc[obj->cvt.map[BMA250_AXIS_X]] = obj->cvt.sign[BMA250_AXIS_X]*obj->data[BMA250_AXIS_X];
+		acc[obj->cvt.map[BMA250_AXIS_Y]] = obj->cvt.sign[BMA250_AXIS_Y]*obj->data[BMA250_AXIS_Y];
+		acc[obj->cvt.map[BMA250_AXIS_Z]] = obj->cvt.sign[BMA250_AXIS_Z]*obj->data[BMA250_AXIS_Z];
+		//printk("cvt x=%d, y=%d, z=%d \n",obj->cvt.sign[BMA250_AXIS_X],obj->cvt.sign[BMA250_AXIS_Y],obj->cvt.sign[BMA250_AXIS_Z]);
+
+		//GSE_LOG("Mapped gsensor data: %d, %d, %d!\n", acc[BMA250_AXIS_X], acc[BMA250_AXIS_Y], acc[BMA250_AXIS_Z]);
+
+		sprintf(buf, "%d %d %d", (s16)acc[BMA250_AXIS_X], (s16)acc[BMA250_AXIS_Y], (s16)acc[BMA250_AXIS_Z]);
+		if(atomic_read(&obj->trace) & ADX_TRC_IOCTL)
+		{
+			GSE_LOG("gsensor data for compass: %s!\n", buf);
+		}
+	}
+	
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int BMA250_ReadSensorData(struct i2c_client *client, char *buf, int bufsize)
+{
+	struct bma250_i2c_data *obj = (struct bma250_i2c_data*)i2c_get_clientdata(client);
+	u8 databuf[20];
+	int acc[BMA250_AXES_NUM];
+	int res = 0;
+	memset(databuf, 0, sizeof(u8)*10);
+
+	if(NULL == buf)
+	{
+		return -1;
+	}
+	if(NULL == client)
+	{
+		*buf = 0;
+		return -2;
+	}
+	/*
+	if(false == enable_status )
+	{
+		
+		acc[BMA250_AXIS_X]=-1;
+		acc[BMA250_AXIS_Y]=-1;
+		acc[BMA250_AXIS_Z]=-1;
+		sprintf(buf, "%04x %04x %04x", acc[BMA250_AXIS_X], acc[BMA250_AXIS_Y], acc[BMA250_AXIS_Z]);
+        GSE_ERR("sensor disable read invalid data!\n");
+		return 0;
+	}
+	*/
+	if((res = BMA250_ReadData(client, obj->data)))
+	{        
+		GSE_ERR("I2C error: ret value=%d", res);
+		return -3;
+	}
+	else
+	{
+		//printk("raw data x=%d, y=%d, z=%d \n",obj->data[BMA250_AXIS_X],obj->data[BMA250_AXIS_Y],obj->data[BMA250_AXIS_Z]);
+		obj->data[BMA250_AXIS_X] += obj->cali_sw[BMA250_AXIS_X];
+		obj->data[BMA250_AXIS_Y] += obj->cali_sw[BMA250_AXIS_Y];
+		obj->data[BMA250_AXIS_Z] += obj->cali_sw[BMA250_AXIS_Z];
+		
+		//printk("cali_sw x=%d, y=%d, z=%d \n",obj->cali_sw[BMA250_AXIS_X],obj->cali_sw[BMA250_AXIS_Y],obj->cali_sw[BMA250_AXIS_Z]);
+		
+		/*remap coordinate*/
+		acc[obj->cvt.map[BMA250_AXIS_X]] = obj->cvt.sign[BMA250_AXIS_X]*obj->data[BMA250_AXIS_X];
+		acc[obj->cvt.map[BMA250_AXIS_Y]] = obj->cvt.sign[BMA250_AXIS_Y]*obj->data[BMA250_AXIS_Y];
+		acc[obj->cvt.map[BMA250_AXIS_Z]] = obj->cvt.sign[BMA250_AXIS_Z]*obj->data[BMA250_AXIS_Z];
+		//printk("cvt x=%d, y=%d, z=%d \n",obj->cvt.sign[BMA250_AXIS_X],obj->cvt.sign[BMA250_AXIS_Y],obj->cvt.sign[BMA250_AXIS_Z]);
+
+
+		//GSE_LOG("Mapped gsensor data: %d, %d, %d!\n", acc[BMA250_AXIS_X], acc[BMA250_AXIS_Y], acc[BMA250_AXIS_Z]);
+
+		//Out put the mg
+		//printk("mg acc=%d, GRAVITY=%d, sensityvity=%d \n",acc[BMA250_AXIS_X],GRAVITY_EARTH_1000,obj->reso->sensitivity);
+		acc[BMA250_AXIS_X] = acc[BMA250_AXIS_X] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+		acc[BMA250_AXIS_Y] = acc[BMA250_AXIS_Y] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+		acc[BMA250_AXIS_Z] = acc[BMA250_AXIS_Z] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;		
+		
+	
+
+		sprintf(buf, "%04x %04x %04x", acc[BMA250_AXIS_X], acc[BMA250_AXIS_Y], acc[BMA250_AXIS_Z]);
+		if(atomic_read(&obj->trace) & ADX_TRC_IOCTL)
+		{
+			GSE_LOG("gsensor data: %s!\n", buf);
+		}
+	}
+	
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int BMA250_ReadRawData(struct i2c_client *client, char *buf)
+{
+	struct bma250_i2c_data *obj = (struct bma250_i2c_data*)i2c_get_clientdata(client);
+	int res = 0;
+
+	if (!buf || !client)
+	{
+		return EINVAL;
+	}
+	
+	if((res = BMA250_ReadData(client, obj->data)))
+	{        
+		GSE_ERR("I2C error: ret value=%d", res);
+		return EIO;
+	}
+	else
+	{
+		sprintf(buf, "BMA250_ReadRawData %04x %04x %04x", obj->data[BMA250_AXIS_X], 
+			obj->data[BMA250_AXIS_Y], obj->data[BMA250_AXIS_Z]);
+	
+	}
+	
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int bma250_set_mode(struct i2c_client *client, unsigned char mode)
+{
+	int comres = 0;
+	unsigned char data[2] = {BMA250_EN_LOW_POWER__REG};
+
+	if ((client == NULL) || (mode >= 3))
+	{
+		return -1;
+	}
+
+	comres = bma050_i2c_read_block(client,
+			BMA250_EN_LOW_POWER__REG, data, 1);
+	switch (mode) {
+	case BMA250_MODE_NORMAL:
+		data[0]  = BMA250_SET_BITSLICE(data[0],
+				BMA250_EN_LOW_POWER, 0);
+		data[0]  = BMA250_SET_BITSLICE(data[0],
+				BMA250_EN_SUSPEND, 0);
+		break;
+	case BMA250_MODE_LOWPOWER:
+		data[0]  = BMA250_SET_BITSLICE(data[0],
+				BMA250_EN_LOW_POWER, 1);
+		data[0]  = BMA250_SET_BITSLICE(data[0],
+				BMA250_EN_SUSPEND, 0);
+		break;
+	case BMA250_MODE_SUSPEND:
+		data[0]  = BMA250_SET_BITSLICE(data[0],
+				BMA250_EN_LOW_POWER, 0);
+		data[0]  = BMA250_SET_BITSLICE(data[0],
+				BMA250_EN_SUSPEND, 1);
+		break;
+	default:
+		break;
+	}
+
+    comres = bma050_i2c_write_block(client,BMA250_EN_LOW_POWER__REG,data,0x1);
+	if(comres < 0)
+	{
+		return BMA250_ERR_I2C;
+	}
+	else
+	{
+		return comres;
+	}
+}
+/*----------------------------------------------------------------------------*/
+static int bma250_get_mode(struct i2c_client *client, unsigned char *mode)
+{
+	int comres = 0;
+
+	if (client == NULL) 
+	{
+		return -1;
+	}
+	comres = bma050_i2c_read_block(client,
+			BMA250_EN_LOW_POWER__REG, mode, 1);
+	*mode  = (*mode) >> 6;
+		
+	return comres;
+}
+
+/*----------------------------------------------------------------------------*/
+static int bma250_set_range(struct i2c_client *client, unsigned char range)
+{
+	int comres = 0;
+	unsigned char data[2] = {BMA250_RANGE_SEL__REG};
+
+	if (client == NULL)
+	{
+		return -1;
+	}
+	
+	comres = bma050_i2c_read_block(client,
+			BMA250_RANGE_SEL__REG, data, 1);
+
+	data[0]  = BMA250_SET_BITSLICE(data[0],
+			BMA250_RANGE_SEL, range);
+    comres= bma050_i2c_write_block(client,BMA250_RANGE_SEL__REG,data,0x1);
+	if(comres < 0)
+	{
+		return BMA250_ERR_I2C;
+	}
+	else
+	{
+		return comres;
+	}
+}
+/*----------------------------------------------------------------------------*/
+static int bma250_get_range(struct i2c_client *client, unsigned char *range)
+{
+	int comres = 0;
+	unsigned char data;
+
+	if (client == NULL) 
+	{
+		return -1;
+	}
+
+	comres = bma050_i2c_read_block(client, BMA250_RANGE_SEL__REG,	&data, 1);
+	*range = BMA250_GET_BITSLICE(data, BMA250_RANGE_SEL);
+
+	return comres;
+}
+/*----------------------------------------------------------------------------*/
+static int bma250_set_bandwidth(struct i2c_client *client, unsigned char bandwidth)
+{
+	int comres = 0;
+	unsigned char data[2] = {BMA250_BANDWIDTH__REG};
+
+	if (client == NULL)
+	{
+		return -1;
+	}
+	
+	comres = bma050_i2c_read_block(client,
+			BMA250_BANDWIDTH__REG, data, 1);
+
+	data[0]  = BMA250_SET_BITSLICE(data[0],
+			BMA250_BANDWIDTH, bandwidth);
+
+    comres = bma050_i2c_write_block(client,BMA250_BANDWIDTH__REG,data,0x1);
+	if(comres < 0)
+	{
+		return BMA250_ERR_I2C;
+	}
+	else
+	{
+		return comres;
+	}
+}
+/*----------------------------------------------------------------------------*/
+static int bma250_get_bandwidth(struct i2c_client *client, unsigned char *bandwidth)
+{
+	int comres = 0;
+	unsigned char data;
+
+	if (client == NULL) 
+	{
+		return -1;
+	}
+
+	comres = bma050_i2c_read_block(client, BMA250_BANDWIDTH__REG, &data, 1);
+	data = BMA250_GET_BITSLICE(data, BMA250_BANDWIDTH);
+
+	if (data < 0x08) //7.81Hz
+	{
+		*bandwidth = 0x08;
+	}
+	else if (data > 0x0f)	// 1000Hz
+	{
+		*bandwidth = 0x0f;
+	}
+	else
+	{
+		*bandwidth = data;
+	}
+	return comres;
+}
+/*----------------------------------------------------------------------------*/
+
+static ssize_t show_chipinfo_value(struct device_driver *ddri, char *buf)
+{
+	struct i2c_client *client = bma250_i2c_client;
+	char strbuf[BMA250_BUFSIZE];
+	if(NULL == client)
+	{
+		GSE_ERR("i2c client is null!!\n");
+		return 0;
+	}
+	
+	BMA250_ReadChipInfo(client, strbuf, BMA250_BUFSIZE);
+	return snprintf(buf, PAGE_SIZE, "%s\n", strbuf);        
+}
+
+
+/*----------------------------------------------------------------------------*/
+/*
+g sensor opmode for compass tilt compensation
+*/
+static ssize_t show_cpsopmode_value(struct device_driver *ddri, char *buf)
+{
+	unsigned char data;
+
+	if (bma250_get_mode(bma250_i2c_client, &data) < 0)
+	{
+		return sprintf(buf, "Read error\n");
+	}
+	else
+	{
+		return sprintf(buf, "%d\n", data);
+	}
+}
+
+/*----------------------------------------------------------------------------*/
+/*
+g sensor opmode for compass tilt compensation
+*/
+static ssize_t store_cpsopmode_value(struct device_driver *ddri, const char *buf, size_t count)
+{
+	unsigned long data;
+	int error;
+
+	if ((error = strict_strtoul(buf, 10, &data)))
+	{
+		return error;
+	}
+	if (data == BMA250_MODE_NORMAL)
+	{
+		//BMA250_SetPowerMode(bma250_i2c_client, true);
+	}
+	else if (data == BMA250_MODE_SUSPEND)
+	{
+		//BMA250_SetPowerMode(bma250_i2c_client, false);
+	}
+	else if (bma250_set_mode(bma250_i2c_client, (unsigned char) data) < 0)
+	{
+		GSE_ERR("invalid content: '%s', length = %d\n", buf, count);
+	}
+	return count;
+}
+
+/*----------------------------------------------------------------------------*/
+/*
+g sensor range for compass tilt compensation
+*/
+static ssize_t show_cpsrange_value(struct device_driver *ddri, char *buf)
+{
+	unsigned char data;
+
+	if (bma250_get_range(bma250_i2c_client, &data) < 0)
+	{
+		return sprintf(buf, "Read error\n");
+	}
+	else
+	{
+		return sprintf(buf, "%d\n", data);
+	}
+}
+
+/*----------------------------------------------------------------------------*/
+/*
+g sensor range for compass tilt compensation
+*/
+static ssize_t store_cpsrange_value(struct device_driver *ddri, const char *buf, size_t count)
+{
+	unsigned long data;
+	int error;
+
+	if ((error = strict_strtoul(buf, 10, &data)))
+	{
+		return error;
+	}
+	if (bma250_set_range(bma250_i2c_client, (unsigned char) data) < 0)
+	{
+		GSE_ERR("invalid content: '%s', length = %d\n", buf, count);
+	}
+
+	return count;
+}
+/*----------------------------------------------------------------------------*/
+/*
+g sensor bandwidth for compass tilt compensation
+*/
+static ssize_t show_cpsbandwidth_value(struct device_driver *ddri, char *buf)
+{
+	unsigned char data;
+
+	if (bma250_get_bandwidth(bma250_i2c_client, &data) < 0)
+	{
+		return sprintf(buf, "Read error\n");
+	}
+	else
+	{
+		return sprintf(buf, "%d\n", data);
+	}
+}
+
+/*----------------------------------------------------------------------------*/
+/*
+g sensor bandwidth for compass tilt compensation
+*/
+static ssize_t store_cpsbandwidth_value(struct device_driver *ddri, const char *buf, size_t count)
+{
+	unsigned long data;
+	int error;
+
+	if ((error = strict_strtoul(buf, 10, &data)))
+	{
+		return error;
+	}
+	if (bma250_set_bandwidth(bma250_i2c_client, (unsigned char) data) < 0)
+	{
+		GSE_ERR("invalid content: '%s', length = %d\n", buf, count);
+	}
+
+	return count;
+}
+
+/*----------------------------------------------------------------------------*/
+/*
+g sensor data for compass tilt compensation
+*/
+static ssize_t show_cpsdata_value(struct device_driver *ddri, char *buf)
+{
+	struct i2c_client *client = bma250_i2c_client;
+	char strbuf[BMA250_BUFSIZE];
+	
+	if(NULL == client)
+	{
+		GSE_ERR("i2c client is null!!\n");
+		return 0;
+	}
+	BMA250_CompassReadData(client, strbuf, BMA250_BUFSIZE);
+	return snprintf(buf, PAGE_SIZE, "%s\n", strbuf);            
+}
+
+/*----------------------------------------------------------------------------*/
+static ssize_t show_sensordata_value(struct device_driver *ddri, char *buf)
+{
+	struct i2c_client *client = bma250_i2c_client;
+	char strbuf[BMA250_BUFSIZE];
+	
+	if(NULL == client)
+	{
+		GSE_ERR("i2c client is null!!\n");
+		return 0;
+	}
+	BMA250_ReadSensorData(client, strbuf, BMA250_BUFSIZE);
+	//BMA250_ReadRawData(client, strbuf);
+	return snprintf(buf, PAGE_SIZE, "%s\n", strbuf);            
+}
+
+/*
+static ssize_t show_sensorrawdata_value(struct device_driver *ddri, char *buf, size_t count)
+	{
+		struct i2c_client *client = bma250_i2c_client;
+		char strbuf[BMA250_BUFSIZE];
+		
+		if(NULL == client)
+		{
+			GSE_ERR("i2c client is null!!\n");
+			return 0;
+		}
+		//BMA250_ReadSensorData(client, strbuf, BMA250_BUFSIZE);
+		BMA250_ReadRawData(client, strbuf);
+		return snprintf(buf, PAGE_SIZE, "%s\n", strbuf);			
+	}
+*/
+/*----------------------------------------------------------------------------*/
+static ssize_t show_cali_value(struct device_driver *ddri, char *buf)
+{
+	struct i2c_client *client = bma250_i2c_client;
+	struct bma250_i2c_data *obj;
+	int err, len = 0, mul;
+	int tmp[BMA250_AXES_NUM];
+
+	if(NULL == client)
+	{
+		GSE_ERR("i2c client is null!!\n");
+		return 0;
+	}
+
+	obj = i2c_get_clientdata(client);
+
+
+
+	if((err = BMA250_ReadOffset(client, obj->offset)))
+	{
+		return -EINVAL;
+	}
+	else if((err = BMA250_ReadCalibration(client, tmp)))
+	{
+		return -EINVAL;
+	}
+	else
+	{    
+		mul = obj->reso->sensitivity/bma250_offset_resolution.sensitivity;
+		len += snprintf(buf+len, PAGE_SIZE-len, "[HW ][%d] (%+3d, %+3d, %+3d) : (0x%02X, 0x%02X, 0x%02X)\n", mul,                        
+			obj->offset[BMA250_AXIS_X], obj->offset[BMA250_AXIS_Y], obj->offset[BMA250_AXIS_Z],
+			obj->offset[BMA250_AXIS_X], obj->offset[BMA250_AXIS_Y], obj->offset[BMA250_AXIS_Z]);
+		len += snprintf(buf+len, PAGE_SIZE-len, "[SW ][%d] (%+3d, %+3d, %+3d)\n", 1, 
+			obj->cali_sw[BMA250_AXIS_X], obj->cali_sw[BMA250_AXIS_Y], obj->cali_sw[BMA250_AXIS_Z]);
+
+		len += snprintf(buf+len, PAGE_SIZE-len, "[ALL]    (%+3d, %+3d, %+3d) : (%+3d, %+3d, %+3d)\n", 
+			obj->offset[BMA250_AXIS_X]*mul + obj->cali_sw[BMA250_AXIS_X],
+			obj->offset[BMA250_AXIS_Y]*mul + obj->cali_sw[BMA250_AXIS_Y],
+			obj->offset[BMA250_AXIS_Z]*mul + obj->cali_sw[BMA250_AXIS_Z],
+			tmp[BMA250_AXIS_X], tmp[BMA250_AXIS_Y], tmp[BMA250_AXIS_Z]);
+		
+		return len;
+    }
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t store_cali_value(struct device_driver *ddri, const char *buf, size_t count)
+{
+	struct i2c_client *client = bma250_i2c_client;  
+	int err, x, y, z;
+	int dat[BMA250_AXES_NUM];
+
+	if(!strncmp(buf, "rst", 3))
+	{
+		if((err = BMA250_ResetCalibration(client)))
+		{
+			GSE_ERR("reset offset err = %d\n", err);
+		}	
+	}
+	else if(3 == sscanf(buf, "0x%02X 0x%02X 0x%02X", &x, &y, &z))
+	{
+		dat[BMA250_AXIS_X] = x;
+		dat[BMA250_AXIS_Y] = y;
+		dat[BMA250_AXIS_Z] = z;
+		if((err = BMA250_WriteCalibration(client, dat)))
+		{
+			GSE_ERR("write calibration err = %d\n", err);
+		}		
+	}
+	else
+	{
+		GSE_ERR("invalid format\n");
+	}
+	
+	return count;
+}
+
+
+/*----------------------------------------------------------------------------*/
+static ssize_t show_firlen_value(struct device_driver *ddri, char *buf)
+{
+#ifdef CONFIG_BMA250_LOWPASS
+	struct i2c_client *client = bma250_i2c_client;
+	struct bma250_i2c_data *obj = i2c_get_clientdata(client);
+	if(atomic_read(&obj->firlen))
+	{
+		int idx, len = atomic_read(&obj->firlen);
+		GSE_LOG("len = %2d, idx = %2d\n", obj->fir.num, obj->fir.idx);
+
+		for(idx = 0; idx < len; idx++)
+		{
+			GSE_LOG("[%5d %5d %5d]\n", obj->fir.raw[idx][BMA250_AXIS_X], obj->fir.raw[idx][BMA250_AXIS_Y], obj->fir.raw[idx][BMA250_AXIS_Z]);
+		}
+		
+		GSE_LOG("sum = [%5d %5d %5d]\n", obj->fir.sum[BMA250_AXIS_X], obj->fir.sum[BMA250_AXIS_Y], obj->fir.sum[BMA250_AXIS_Z]);
+		GSE_LOG("avg = [%5d %5d %5d]\n", obj->fir.sum[BMA250_AXIS_X]/len, obj->fir.sum[BMA250_AXIS_Y]/len, obj->fir.sum[BMA250_AXIS_Z]/len);
+	}
+	return snprintf(buf, PAGE_SIZE, "%d\n", atomic_read(&obj->firlen));
+#else
+	return snprintf(buf, PAGE_SIZE, "not support\n");
+#endif
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t store_firlen_value(struct device_driver *ddri, const char *buf, size_t count)
+{
+#ifdef CONFIG_BMA250_LOWPASS
+	struct i2c_client *client = bma250_i2c_client;  
+	struct bma250_i2c_data *obj = i2c_get_clientdata(client);
+	int firlen;
+
+	if(1 != sscanf(buf, "%d", &firlen))
+	{
+		GSE_ERR("invallid format\n");
+	}
+	else if(firlen > C_MAX_FIR_LENGTH)
+	{
+		GSE_ERR("exceeds maximum filter length\n");
+	}
+	else
+	{ 
+		atomic_set(&obj->firlen, firlen);
+		if(NULL == firlen)
+		{
+			atomic_set(&obj->fir_en, 0);
+		}
+		else
+		{
+			memset(&obj->fir, 0x00, sizeof(obj->fir));
+			atomic_set(&obj->fir_en, 1);
+		}
+	}
+#endif    
+	return count;
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t show_trace_value(struct device_driver *ddri, char *buf)
+{
+	ssize_t res;
+	struct bma250_i2c_data *obj = obj_i2c_data;
+	if (obj == NULL)
+	{
+		GSE_ERR("i2c_data obj is null!!\n");
+		return 0;
+	}
+	
+	res = snprintf(buf, PAGE_SIZE, "0x%04X\n", atomic_read(&obj->trace));     
+	return res;    
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t store_trace_value(struct device_driver *ddri, const char *buf, size_t count)
+{
+	struct bma250_i2c_data *obj = obj_i2c_data;
+	int trace;
+	if (obj == NULL)
+	{
+		GSE_ERR("i2c_data obj is null!!\n");
+		return 0;
+	}
+	
+	if(1 == sscanf(buf, "0x%x", &trace))
+	{
+		atomic_set(&obj->trace, trace);
+	}	
+	else
+	{
+		GSE_ERR("invalid content: '%s', length = %d\n", buf, count);
+	}
+	
+	return count;    
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t show_status_value(struct device_driver *ddri, char *buf)
+{
+	ssize_t len = 0;    
+	struct bma250_i2c_data *obj = obj_i2c_data;
+	if (obj == NULL)
+	{
+		GSE_ERR("i2c_data obj is null!!\n");
+		return 0;
+	}	
+	
+	if(obj->hw)
+	{
+		len += snprintf(buf+len, PAGE_SIZE-len, "CUST: %d %d (%d %d)\n", 
+	            obj->hw->i2c_num, obj->hw->direction, obj->hw->power_id, obj->hw->power_vol);   
+	}
+	else
+	{
+		len += snprintf(buf+len, PAGE_SIZE-len, "CUST: NULL\n");
+	}
+	return len;    
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t show_power_status_value(struct device_driver *ddri, char *buf)
+{
+	if(sensor_power)
+		printk("G sensor is in work mode, sensor_power = %d\n", sensor_power);
+	else
+		printk("G sensor is in standby mode, sensor_power = %d\n", sensor_power);
+
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static DRIVER_ATTR(chipinfo,   S_IWUSR | S_IRUGO, show_chipinfo_value,      NULL);
+static DRIVER_ATTR(cpsdata, S_IWUSR | S_IRUGO, show_cpsdata_value,    NULL);
+static DRIVER_ATTR(cpsopmode, S_IRUGO|S_IWUSR, show_cpsopmode_value,    store_cpsopmode_value);
+static DRIVER_ATTR(cpsrange, S_IRUGO|S_IWUSR, show_cpsrange_value,    store_cpsrange_value);
+static DRIVER_ATTR(cpsbandwidth, S_IRUGO|S_IWUSR, show_cpsbandwidth_value,    store_cpsbandwidth_value);
+static DRIVER_ATTR(sensordata, S_IWUSR | S_IRUGO, show_sensordata_value,    NULL);
+static DRIVER_ATTR(cali,       S_IWUSR | S_IRUGO, show_cali_value,          store_cali_value);
+static DRIVER_ATTR(firlen,     S_IWUSR | S_IRUGO, show_firlen_value,        store_firlen_value);
+static DRIVER_ATTR(trace,      S_IWUSR | S_IRUGO, show_trace_value,         store_trace_value);
+static DRIVER_ATTR(status,               S_IRUGO, show_status_value,        NULL);
+static DRIVER_ATTR(powerstatus,               S_IRUGO, show_power_status_value,        NULL);
+
+/*----------------------------------------------------------------------------*/
+static struct driver_attribute *bma250_attr_list[] = {
+	&driver_attr_chipinfo,     /*chip information*/
+	&driver_attr_sensordata,   /*dump sensor data*/
+	&driver_attr_cali,         /*show calibration data*/
+	&driver_attr_firlen,       /*filter length: 0: disable, others: enable*/
+	&driver_attr_trace,        /*trace log*/
+	&driver_attr_status,
+	&driver_attr_powerstatus,
+	&driver_attr_cpsdata,	/*g sensor data for compass tilt compensation*/
+	&driver_attr_cpsopmode,	/*g sensor opmode for compass tilt compensation*/
+	&driver_attr_cpsrange,	/*g sensor range for compass tilt compensation*/
+	&driver_attr_cpsbandwidth,	/*g sensor bandwidth for compass tilt compensation*/
+};
+/*----------------------------------------------------------------------------*/
+static int bma250_create_attr(struct device_driver *driver) 
+{
+	int idx, err = 0;
+	int num = (int)(sizeof(bma250_attr_list)/sizeof(bma250_attr_list[0]));
+	if (driver == NULL)
+	{
+		return -EINVAL;
+	}
+
+	for(idx = 0; idx < num; idx++)
+	{
+		if((err = driver_create_file(driver, bma250_attr_list[idx])))
+		{            
+			GSE_ERR("driver_create_file (%s) = %d\n", bma250_attr_list[idx]->attr.name, err);
+			break;
+		}
+	}    
+	return err;
+}
+/*----------------------------------------------------------------------------*/
+static int bma250_delete_attr(struct device_driver *driver)
+{
+	int idx ,err = 0;
+	int num = (int)(sizeof(bma250_attr_list)/sizeof(bma250_attr_list[0]));
+
+	if(driver == NULL)
+	{
+		return -EINVAL;
+	}
+	
+
+	for(idx = 0; idx < num; idx++)
+	{
+		driver_remove_file(driver, bma250_attr_list[idx]);
+	}
+	
+
+	return err;
+} 
+
+/*----------------------------------------------------------------------------*/
+int gsensor_operate(void* self, uint32_t command, void* buff_in, int size_in,
+		void* buff_out, int size_out, int* actualout)
+{
+	int err = 0;
+	int value, sample_delay;	
+	struct bma250_i2c_data *priv = (struct bma250_i2c_data*)self;
+	hwm_sensor_data* gsensor_data;
+	char buff[BMA250_BUFSIZE];
+	
+	//GSE_FUN(f);
+	switch (command)
+	{
+		case SENSOR_DELAY:
+			if((buff_in == NULL) || (size_in < sizeof(int)))
+			{
+				GSE_ERR("Set delay parameter error!\n");
+				err = -EINVAL;
+			}
+			else
+			{
+				value = *(int *)buff_in;
+				if(value <= 5)
+				{
+					sample_delay = BMA250_BW_200HZ;
+				}
+				else if(value <= 10)
+				{
+					sample_delay = BMA250_BW_100HZ;
+				}
+				else
+				{
+					sample_delay = BMA250_BW_50HZ;
+				}
+				
+				err = BMA250_SetBWRate(priv->client, sample_delay);
+				if(err != BMA250_SUCCESS ) //0x2C->BW=100Hz
+				{
+					GSE_ERR("Set delay parameter error!\n");
+				}
+
+				if(value >= 50)
+				{
+					atomic_set(&priv->filter, 0);
+				}
+				else
+				{	
+				#if defined(CONFIG_BMA250_LOWPASS)
+					priv->fir.num = 0;
+					priv->fir.idx = 0;
+					priv->fir.sum[BMA250_AXIS_X] = 0;
+					priv->fir.sum[BMA250_AXIS_Y] = 0;
+					priv->fir.sum[BMA250_AXIS_Z] = 0;
+					atomic_set(&priv->filter, 1);
+				#endif
+				}
+			}
+			break;
+
+		case SENSOR_ENABLE:
+			if((buff_in == NULL) || (size_in < sizeof(int)))
+			{
+				GSE_ERR("Enable sensor parameter error!\n");
+				err = -EINVAL;
+			}
+			else
+			{
+				value = *(int *)buff_in;
+				mutex_lock(&bma050_op_mutex);
+				GSE_LOG("Gsensor enable_status value = %d,sensor_power=%d\n",value,sensor_power);
+				if(((value == 0) && (sensor_power == false)) ||((value == 1) && (sensor_power == true)))
+				{
+					GSE_LOG("Gsensor device have updated!\n");
+					enable_status = sensor_power;
+				}
+				else
+				{
+				    enable_status = !sensor_power;
+					err = BMA250_SetPowerMode( priv->client, !sensor_power);
+					
+				}
+				GSE_LOG("Gsensor enable_status = %d\n",enable_status);
+				mutex_unlock(&bma050_op_mutex);
+			}
+			break;
+
+		case SENSOR_GET_DATA:
+			if((buff_out == NULL) || (size_out< sizeof(hwm_sensor_data)))
+			{
+				GSE_ERR("get sensor data parameter error!\n");
+				err = -EINVAL;
+			}
+			else
+			{
+				gsensor_data = (hwm_sensor_data *)buff_out;
+				BMA250_ReadSensorData(priv->client, buff, BMA250_BUFSIZE);
+				sscanf(buff, "%x %x %x", &gsensor_data->values[0], 
+					&gsensor_data->values[1], &gsensor_data->values[2]);				
+				gsensor_data->status = SENSOR_STATUS_ACCURACY_MEDIUM;				
+				gsensor_data->value_divide = 1000;
+			}
+			break;
+		default:
+			GSE_ERR("gsensor operate function no this parameter %d!\n", command);
+			err = -1;
+			break;
+	}
+	
+	return err;
+}
+
+/****************************************************************************** 
+ * Function Configuration
+******************************************************************************/
+static int bma250_open(struct inode *inode, struct file *file)
+{
+	file->private_data = bma250_i2c_client;
+
+	if(file->private_data == NULL)
+	{
+		GSE_ERR("null pointer!!\n");
+		return -EINVAL;
+	}
+	return nonseekable_open(inode, file);
+}
+/*----------------------------------------------------------------------------*/
+static int bma250_release(struct inode *inode, struct file *file)
+{
+	file->private_data = NULL;
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+//static int bma250_ioctl(struct inode *inode, struct file *file, unsigned int cmd,
+//       unsigned long arg)
+static long bma250_unlocked_ioctl(struct file *file, unsigned int cmd,unsigned long arg)       
+{
+	struct i2c_client *client = (struct i2c_client*)file->private_data;
+	struct bma250_i2c_data *obj = (struct bma250_i2c_data*)i2c_get_clientdata(client);	
+	char strbuf[BMA250_BUFSIZE];
+	void __user *data;
+	SENSOR_DATA sensor_data;
+	long err = 0;
+	int cali[3];
+
+	//GSE_FUN(f);
+	if(_IOC_DIR(cmd) & _IOC_READ)
+	{
+		err = !access_ok(VERIFY_WRITE, (void __user *)arg, _IOC_SIZE(cmd));
+	}
+	else if(_IOC_DIR(cmd) & _IOC_WRITE)
+	{
+		err = !access_ok(VERIFY_READ, (void __user *)arg, _IOC_SIZE(cmd));
+	}
+
+	if(err)
+	{
+		GSE_ERR("access error: %08X, (%2d, %2d)\n", cmd, _IOC_DIR(cmd), _IOC_SIZE(cmd));
+		return -EFAULT;
+	}
+
+	switch(cmd)
+	{
+		case GSENSOR_IOCTL_INIT:
+			bma250_init_client(client, 0);			
+			break;
+
+		case GSENSOR_IOCTL_READ_CHIPINFO:
+			data = (void __user *) arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}
+			
+			BMA250_ReadChipInfo(client, strbuf, BMA250_BUFSIZE);
+			if(copy_to_user(data, strbuf, strlen(strbuf)+1))
+			{
+				err = -EFAULT;
+				break;
+			}				 
+			break;	  
+
+		case GSENSOR_IOCTL_READ_SENSORDATA:
+			data = (void __user *) arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}
+			BMA250_SetPowerMode(client,true);
+			BMA250_ReadSensorData(client, strbuf, BMA250_BUFSIZE);
+			if(copy_to_user(data, strbuf, strlen(strbuf)+1))
+			{
+				err = -EFAULT;
+				break;	  
+			}				 
+			break;
+
+		case GSENSOR_IOCTL_READ_GAIN:
+			data = (void __user *) arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}			
+			
+			if(copy_to_user(data, &gsensor_gain, sizeof(GSENSOR_VECTOR3D)))
+			{
+				err = -EFAULT;
+				break;
+			}				 
+			break;
+
+		case GSENSOR_IOCTL_READ_RAW_DATA:
+			data = (void __user *) arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}
+			BMA250_ReadRawData(client, strbuf);
+			if(copy_to_user(data, &strbuf, strlen(strbuf)+1))
+			{
+				err = -EFAULT;
+				break;	  
+			}
+			break;	  
+
+		case GSENSOR_IOCTL_SET_CALI:
+			data = (void __user*)arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}
+			if(copy_from_user(&sensor_data, data, sizeof(sensor_data)))
+			{
+				err = -EFAULT;
+				break;	  
+			}
+			if(atomic_read(&obj->suspend))
+			{
+				GSE_ERR("Perform calibration in suspend state!!\n");
+				err = -EINVAL;
+			}
+			else
+			{
+				cali[BMA250_AXIS_X] = sensor_data.x * obj->reso->sensitivity / GRAVITY_EARTH_1000;
+				cali[BMA250_AXIS_Y] = sensor_data.y * obj->reso->sensitivity / GRAVITY_EARTH_1000;
+				cali[BMA250_AXIS_Z] = sensor_data.z * obj->reso->sensitivity / GRAVITY_EARTH_1000;			  
+				err = BMA250_WriteCalibration(client, cali);			 
+			}
+			break;
+
+		case GSENSOR_IOCTL_CLR_CALI:
+			err = BMA250_ResetCalibration(client);
+			break;
+
+		case GSENSOR_IOCTL_GET_CALI:
+			data = (void __user*)arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}
+			if((err = BMA250_ReadCalibration(client, cali)))
+			{
+				break;
+			}
+			
+			sensor_data.x = cali[BMA250_AXIS_X] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+			sensor_data.y = cali[BMA250_AXIS_Y] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+			sensor_data.z = cali[BMA250_AXIS_Z] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+			if(copy_to_user(data, &sensor_data, sizeof(sensor_data)))
+			{
+				err = -EFAULT;
+				break;
+			}		
+			break;
+		
+
+		default:
+			GSE_ERR("unknown IOCTL: 0x%08x\n", cmd);
+			err = -ENOIOCTLCMD;
+			break;
+			
+	}
+
+	return err;
+}
+
+
+/*----------------------------------------------------------------------------*/
+static struct file_operations bma250_fops = {
+	//.owner = THIS_MODULE,
+	.open = bma250_open,
+	.release = bma250_release,
+	.unlocked_ioctl = bma250_unlocked_ioctl,
+};
+/*----------------------------------------------------------------------------*/
+static struct miscdevice bma250_device = {
+	.minor = MISC_DYNAMIC_MINOR,
+	.name = "gsensor",
+	.fops = &bma250_fops,
+};
+/*----------------------------------------------------------------------------*/
+#ifndef USE_EARLY_SUSPEND
+/*----------------------------------------------------------------------------*/
+static int bma250_suspend(struct i2c_client *client, pm_message_t msg) 
+{
+	struct bma250_i2c_data *obj = i2c_get_clientdata(client);    
+	int err = 0;
+	GSE_FUN();    
+    mutex_lock(&bma050_op_mutex);
+	if(msg.event == PM_EVENT_SUSPEND)
+	{   
+		if(obj == NULL)
+		{
+			GSE_ERR("null pointer!!\n");
+			mutex_unlock(&bma050_op_mutex);
+			return -EINVAL;
+		}
+		atomic_set(&obj->suspend, 1);
+		if((err = BMA250_SetPowerMode(obj->client, false)))
+		{
+			GSE_ERR("write power control fail!!\n");
+			mutex_unlock(&bma050_op_mutex);
+			return -EINVAL;
+		}
+		sensor_power = false;
+		BMA250_power(obj->hw, 0);
+	}
+	mutex_unlock(&bma050_op_mutex);
+	return err;
+}
+/*----------------------------------------------------------------------------*/
+static int bma250_resume(struct i2c_client *client)
+{
+	struct bma250_i2c_data *obj = i2c_get_clientdata(client);        
+	int err;
+	GSE_FUN();
+	udelay(500);//for fix check device id error
+	if(obj == NULL)
+	{
+		GSE_ERR("null pointer!!\n");
+		return -EINVAL;
+	}
+    mutex_lock(&bma050_op_mutex);
+	BMA250_power(obj->hw, 1);
+	if((err = bma250_init_client(client, 0)))
+	{
+		GSE_ERR("initialize client fail!!\n");
+		mutex_unlock(&bma050_op_mutex);
+		return err;        
+	}
+	atomic_set(&obj->suspend, 0);
+    mutex_unlock(&bma050_op_mutex);
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+#else /*CONFIG_HAS_EARLY_SUSPEND is defined*/
+/*----------------------------------------------------------------------------*/
+static void bma250_early_suspend(struct early_suspend *h) 
+{
+	struct bma250_i2c_data *obj = container_of(h, struct bma250_i2c_data, early_drv);   
+	int err;
+	GSE_FUN();    
+
+	if(obj == NULL)
+	{
+		GSE_ERR("null pointer!!\n");
+		return;
+	}
+	atomic_set(&obj->suspend, 1); 
+	if((err = BMA250_SetPowerMode(obj->client, false)))
+	{
+		GSE_ERR("write power control fail!!\n");
+		return;
+	}
+
+	sensor_power = false;
+	
+	BMA250_power(obj->hw, 0);
+}
+/*----------------------------------------------------------------------------*/
+static void bma250_late_resume(struct early_suspend *h)
+{
+	struct bma250_i2c_data *obj = container_of(h, struct bma250_i2c_data, early_drv);         
+	int err;
+	GSE_FUN();
+
+	if(obj == NULL)
+	{
+		GSE_ERR("null pointer!!\n");
+		return;
+	}
+
+	BMA250_power(obj->hw, 1);
+	if((err = bma250_init_client(obj->client, 0)))
+	{
+		GSE_ERR("initialize client fail!!\n");
+		return;        
+	}
+	atomic_set(&obj->suspend, 0);    
+}
+/*----------------------------------------------------------------------------*/
+#endif /*USE_EARLY_SUSPEND*/
+/*----------------------------------------------------------------------------*/
+
+// if use  this typ of enable , Gsensor should report inputEvent(x, y, z ,stats, div) to HAL
+static int bma050_open_report_data(int open)
+{
+	//should queuq work to report event if  is_report_input_direct=true
+	return 0;
+}
+
+// if use  this typ of enable , Gsensor only enabled but not report inputEvent to HAL
+
+static int bma050_enable_nodata(int en)
+{
+	int res =0;
+	int retry = 0;
+	bool power=false;
+	
+	if(1==en)
+	{
+		power=true;
+	}
+	if(0==en)
+	{
+		power =false;
+	}
+
+	for(retry = 0; retry < 3; retry++){
+		res = BMA250_SetPowerMode(obj_i2c_data->client, power);
+		if(res == 0)
+		{
+			GSE_LOG("BMA250_SetPowerMode done\n");
+			break;
+		}
+		GSE_LOG("BMA250_SetPowerMode fail\n");
+	}
+
+	
+	if(res != BMA250_SUCCESS)
+	{
+		printk("BMA050_SetPowerMode fail!\n");
+		return -1;
+	}
+	printk("bma050_enable_nodata OK!\n");
+	return 0;
+}
+
+static int bma050_set_delay(u64 ns)
+{
+    int value =0;
+	int sample_delay=0;
+	int err;
+	value = (int)ns/1000/1000;
+	if(value <= 5)
+	{
+		sample_delay = BMA250_BW_200HZ;
+	}
+	else if(value <= 10)
+	{
+		sample_delay = BMA250_BW_100HZ;
+	}
+	else
+	{
+		sample_delay = BMA250_BW_50HZ;
+	}
+				
+	err = BMA250_SetBWRate(obj_i2c_data->client, sample_delay);
+	if(err != BMA250_SUCCESS ) //0x2C->BW=100Hz
+	{
+		GSE_ERR("bma050_set_delay Set delay parameter error!\n");
+		return -1;
+	}
+	GSE_LOG("bma050_set_delay (%d)\n",value);
+	return 0;
+}
+
+static int bma050_get_data(int* x ,int* y,int* z, int* status)
+{
+	char buff[BMA250_BUFSIZE];
+	BMA250_ReadSensorData(obj_i2c_data->client, buff, BMA250_BUFSIZE);
+	
+	sscanf(buff, "%x %x %x", x, y, z);		
+	*status = SENSOR_STATUS_ACCURACY_MEDIUM;
+
+	return 0;
+}
+
+/*----------------------------------------------------------------------------*/
+static int bma250_i2c_probe(struct i2c_client *client, const struct i2c_device_id *id)
+{
+	struct i2c_client *new_client;
+	struct bma250_i2c_data *obj;
+	//struct acc_drv_obj sobj;
+	int err = 0;
+	int retry = 0;
+	struct acc_control_path ctl={0};
+	struct acc_data_path data={0};
+	GSE_FUN();
+
+	if(!(obj = kzalloc(sizeof(*obj), GFP_KERNEL)))
+	{
+		err = -ENOMEM;
+		goto exit;
+	}
+	
+	memset(obj, 0, sizeof(struct bma250_i2c_data));
+
+	obj->hw = get_cust_acc_hw();
+	
+	if((err = hwmsen_get_convert(obj->hw->direction, &obj->cvt)))
+	{
+		GSE_ERR("invalid direction: %d\n", obj->hw->direction);
+		goto exit;
+	}
+
+	obj_i2c_data = obj;
+	obj->client = client;
+	new_client = obj->client;
+	i2c_set_clientdata(new_client,obj);
+	
+	atomic_set(&obj->trace, 0);
+	atomic_set(&obj->suspend, 0);
+	
+#ifdef CONFIG_BMA250_LOWPASS
+	if(obj->hw->firlen > C_MAX_FIR_LENGTH)
+	{
+		atomic_set(&obj->firlen, C_MAX_FIR_LENGTH);
+	}	
+	else
+	{
+		atomic_set(&obj->firlen, obj->hw->firlen);
+	}
+	
+	if(atomic_read(&obj->firlen) > 0)
+	{
+		atomic_set(&obj->fir_en, 1);
+	}
+	
+#endif
+
+	bma250_i2c_client = new_client;	
+
+	for(retry = 0; retry < 3; retry++){
+		err = bma250_init_client(new_client, 1);
+		if(err == 0)
+		{
+			GSE_LOG("init client done\n");
+			break;
+		}
+		GSE_LOG("init client fail\n");
+	}
+
+	if(err != 0)
+		goto exit_init_failed;
+		
+	if((err = misc_register(&bma250_device)))
+	{
+		GSE_ERR("bma250_device register failed\n");
+		goto exit_misc_device_register_failed;
+	}
+
+	if((err = bma250_create_attr(&(bma050_init_info.platform_diver_addr->driver))))
+	{
+		GSE_ERR("create attribute err = %d\n", err);
+		goto exit_create_attr_failed;
+	}
+
+	ctl.open_report_data= bma050_open_report_data;
+	ctl.enable_nodata = bma050_enable_nodata;
+	ctl.set_delay  = bma050_set_delay;
+	ctl.is_report_input_direct = false;
+	
+	err = acc_register_control_path(&ctl);
+	if(err)
+	{
+	 	GSE_ERR("register acc control path err\n");
+		goto exit_kfree;
+	}
+
+	data.get_data = bma050_get_data;
+	data.vender_div = 1000;
+	err = acc_register_data_path(&data);
+	if(err)
+	{
+	 	GSE_ERR("register acc data path err\n");
+		goto exit_kfree;
+	}
+
+#ifdef USE_EARLY_SUSPEND
+	obj->early_drv.level    = EARLY_SUSPEND_LEVEL_DISABLE_FB - 1,
+	obj->early_drv.suspend  = bma250_early_suspend,
+	obj->early_drv.resume   = bma250_late_resume,    
+	register_early_suspend(&obj->early_drv);
+#endif 
+
+	bma050_init_flag =0;
+	printk("%s: OK new\n", __func__);    
+	
+
+	return 0;
+
+	exit_create_attr_failed:
+	misc_deregister(&bma250_device);
+	exit_misc_device_register_failed:
+	exit_init_failed:
+	//i2c_detach_client(new_client);
+	exit_kfree:
+	kfree(obj);
+	exit:
+	GSE_ERR("%s: err = %d\n", __func__, err);     
+	bma050_init_flag =-1;
+	return err;
+}
+
+/*----------------------------------------------------------------------------*/
+static int bma250_i2c_remove(struct i2c_client *client)
+{
+	int err = 0;	
+
+	if((err = bma250_delete_attr(&(bma050_init_info.platform_diver_addr->driver))))
+	{
+		GSE_ERR("bma150_delete_attr fail: %d\n", err);
+	}
+	
+	if((err = misc_deregister(&bma250_device)))
+	{
+		GSE_ERR("misc_deregister fail: %d\n", err);
+	}
+
+	if((err = hwmsen_detach(ID_ACCELEROMETER)))
+	    
+
+	bma250_i2c_client = NULL;
+	i2c_unregister_device(client);
+	kfree(i2c_get_clientdata(client));
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+
+static int bma050_remove(void)
+{
+    struct acc_hw *hw = get_cust_acc_hw();
+
+    GSE_FUN();    
+    BMA250_power(hw, 0);    
+    i2c_del_driver(&bma250_i2c_driver);
+    return 0;
+}
+
+static int  bma050_local_init(void)
+{
+    struct acc_hw *hw = get_cust_acc_hw();
+	//printk("fwq loccal init+++\n");
+
+	BMA250_power(hw, 1);
+	if(i2c_add_driver(&bma250_i2c_driver))
+	{
+		GSE_ERR("add driver error\n");
+		return -1;
+	}
+	if(-1 == bma050_init_flag)
+	{
+	   return -1;
+	}
+	//printk("fwq loccal init---\n");
+	return 0;
+}
+
+
+/*----------------------------------------------------------------------------*/
+static int __init bma250_init(void)
+{
+	//GSE_FUN();
+	struct acc_hw *hw = get_cust_acc_hw();
+	GSE_LOG("%s: i2c_number=%d\n", __func__,hw->i2c_num); 
+	i2c_register_board_info(hw->i2c_num, &i2c_bma250, 1);
+	acc_driver_add(&bma050_init_info);
+
+	return 0;    
+}
+/*----------------------------------------------------------------------------*/
+static void __exit bma250_exit(void)
+{
+	GSE_FUN();
+}
+/*----------------------------------------------------------------------------*/
+module_init(bma250_init);
+module_exit(bma250_exit);
+/*----------------------------------------------------------------------------*/
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("BMA250 I2C driver");
+MODULE_AUTHOR("hongji.zhou@bosch-sensortec.com");
diff --git a/drivers/misc/mediatek/accelerometer/bma050-new/bma050.h b/drivers/misc/mediatek/accelerometer/bma050-new/bma050.h
new file mode 100644
index 000000000..d163c924d
--- /dev/null
+++ b/drivers/misc/mediatek/accelerometer/bma050-new/bma050.h
@@ -0,0 +1,60 @@
+/* BMA250 motion sensor driver
+ *
+ *
+ * This software program is licensed subject to the GNU General Public License
+ * (GPL).Version 2,June 1991, available at http://www.fsf.org/copyleft/gpl.html
+
+ * (C) Copyright 2011 Bosch Sensortec GmbH
+ * All Rights Reserved
+ */
+
+#ifndef BMA250_H
+#define BMA250_H
+	 
+#include <linux/ioctl.h>
+	 
+	#define BMA250_I2C_SLAVE_WRITE_ADDR		0x30
+	#define BMA250_FIXED_DEVID			0x03
+	
+	 /* BMA250 Register Map  (Please refer to BMA250 Specifications) */
+	#define BMA250_REG_DEVID				0x00
+	#define BMA250_REG_OFSX				0x16
+	#define BMA250_REG_OFSX_HIGH			0x1A
+	#define BMA250_REG_BW_RATE			0x10
+	#define BMA250_BW_MASK				0x1f
+	#define BMA250_BW_200HZ				0x0d
+	#define BMA250_BW_100HZ				0x0c
+	#define BMA250_BW_50HZ				0x0b
+	#define BMA250_BW_25HZ				0x0a
+	#define BMA250_REG_POWER_CTL		0x11		
+	#define BMA250_REG_DATA_FORMAT		0x0f
+	#define BMA250_RANGE_MASK			0x0f
+	#define BMA250_RANGE_2G				0x03
+	#define BMA250_RANGE_4G				0x05
+	#define BMA250_RANGE_8G				0x08
+	#define BMA250_REG_DATAXLOW			0x02
+	#define BMA250_REG_DATA_RESOLUTION	0x14
+	#define BMA250_MEASURE_MODE			0x80	
+	#define BMA250_SELF_TEST           			0x32
+	#define BMA250_SELF_TEST_AXIS_X		0x01
+	#define BMA250_SELF_TEST_AXIS_Y		0x02
+	#define BMA250_SELF_TEST_AXIS_Z		0x03
+	#define BMA250_SELF_TEST_POSITIVE	0x00
+	#define BMA250_SELF_TEST_NEGATIVE	0x04
+	#define BMA250_INT_REG_1           			0x16
+	#define BMA250_INT_REG_2          		 	0x17
+
+	
+#define BMA250_SUCCESS						0
+#define BMA250_ERR_I2C						-1
+#define BMA250_ERR_STATUS					-3
+#define BMA250_ERR_SETUP_FAILURE			-4
+#define BMA250_ERR_GETGSENSORDATA			-5
+#define BMA250_ERR_IDENTIFICATION			-6
+	 
+	 
+	 
+#define BMA250_BUFSIZE				256
+	 
+#endif
+
diff --git a/drivers/misc/mediatek/accelerometer/bma050/Makefile b/drivers/misc/mediatek/accelerometer/bma050/Makefile
new file mode 100644
index 000000000..d2af27b9f
--- /dev/null
+++ b/drivers/misc/mediatek/accelerometer/bma050/Makefile
@@ -0,0 +1,4 @@
+include $(srctree)/drivers/misc/mediatek/Makefile.custom
+
+obj-y	:=  bma050.o
+
diff --git a/drivers/misc/mediatek/accelerometer/bma050/bma050.c b/drivers/misc/mediatek/accelerometer/bma050/bma050.c
new file mode 100644
index 000000000..6c9c2f1c1
--- /dev/null
+++ b/drivers/misc/mediatek/accelerometer/bma050/bma050.c
@@ -0,0 +1,2285 @@
+/* BMA250 motion sensor driver
+ *
+ *
+ * This software program is licensed subject to the GNU General Public License
+ * (GPL).Version 2,June 1991, available at http://www.fsf.org/copyleft/gpl.html
+
+ * (C) Copyright 2011 Bosch Sensortec GmbH
+ * All Rights Reserved
+ */
+
+#include <linux/interrupt.h>
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include <linux/irq.h>
+#include <linux/miscdevice.h>
+#include <asm/uaccess.h>
+#include <linux/delay.h>
+#include <linux/input.h>
+#include <linux/workqueue.h>
+#include <linux/kobject.h>
+#include <linux/earlysuspend.h>
+#include <linux/platform_device.h>
+#include <asm/atomic.h>
+
+#include <mach/mt_typedefs.h>
+#include <mach/mt_gpio.h>
+#include <mach/mt_pm_ldo.h>
+
+#include <cust_acc.h>
+#include <linux/hwmsensor.h>
+#include <linux/hwmsen_dev.h>
+#include <linux/sensors_io.h>
+#include "bma050.h"
+#include <linux/hwmsen_helper.h>
+
+#define POWER_NONE_MACRO MT65XX_POWER_NONE
+
+/*----------------------------------------------------------------------------*/
+#define I2C_DRIVERID_BMA250 250
+/*----------------------------------------------------------------------------*/
+#define DEBUG 1
+/*----------------------------------------------------------------------------*/
+//#define CONFIG_BMA250_LOWPASS   /*apply low pass filter on output*/       
+#define SW_CALIBRATION
+
+/*----------------------------------------------------------------------------*/
+#define BMA250_AXIS_X          0
+#define BMA250_AXIS_Y          1
+#define BMA250_AXIS_Z          2
+#define BMA250_AXES_NUM        3
+#define BMA250_DATA_LEN        6
+#define BMA250_DEV_NAME        "BMA250"
+
+#define BMA250_MODE_NORMAL      0
+#define BMA250_MODE_LOWPOWER    1
+#define BMA250_MODE_SUSPEND     2
+
+#define BMA250_ACC_X_LSB__POS           6
+#define BMA250_ACC_X_LSB__LEN           2
+#define BMA250_ACC_X_LSB__MSK           0xC0
+//#define BMA250_ACC_X_LSB__REG           BMA250_X_AXIS_LSB_REG
+
+#define BMA250_ACC_X_MSB__POS           0
+#define BMA250_ACC_X_MSB__LEN           8
+#define BMA250_ACC_X_MSB__MSK           0xFF
+//#define BMA250_ACC_X_MSB__REG           BMA250_X_AXIS_MSB_REG
+
+#define BMA250_ACC_Y_LSB__POS           6
+#define BMA250_ACC_Y_LSB__LEN           2
+#define BMA250_ACC_Y_LSB__MSK           0xC0
+//#define BMA250_ACC_Y_LSB__REG           BMA250_Y_AXIS_LSB_REG
+
+#define BMA250_ACC_Y_MSB__POS           0
+#define BMA250_ACC_Y_MSB__LEN           8
+#define BMA250_ACC_Y_MSB__MSK           0xFF
+//#define BMA250_ACC_Y_MSB__REG           BMA250_Y_AXIS_MSB_REG
+
+#define BMA250_ACC_Z_LSB__POS           6
+#define BMA250_ACC_Z_LSB__LEN           2
+#define BMA250_ACC_Z_LSB__MSK           0xC0
+//#define BMA250_ACC_Z_LSB__REG           BMA250_Z_AXIS_LSB_REG
+
+#define BMA250_ACC_Z_MSB__POS           0
+#define BMA250_ACC_Z_MSB__LEN           8
+#define BMA250_ACC_Z_MSB__MSK           0xFF
+//#define BMA250_ACC_Z_MSB__REG           BMA250_Z_AXIS_MSB_REG
+
+#define BMA250_EN_LOW_POWER__POS          6
+#define BMA250_EN_LOW_POWER__LEN          1
+#define BMA250_EN_LOW_POWER__MSK          0x40
+#define BMA250_EN_LOW_POWER__REG          BMA250_REG_POWER_CTL
+
+#define BMA250_EN_SUSPEND__POS            7
+#define BMA250_EN_SUSPEND__LEN            1
+#define BMA250_EN_SUSPEND__MSK            0x80
+#define BMA250_EN_SUSPEND__REG            BMA250_REG_POWER_CTL
+
+#define BMA250_RANGE_SEL__POS             0
+#define BMA250_RANGE_SEL__LEN             4
+#define BMA250_RANGE_SEL__MSK             0x0F
+#define BMA250_RANGE_SEL__REG             BMA250_REG_DATA_FORMAT
+
+#define BMA250_BANDWIDTH__POS             0
+#define BMA250_BANDWIDTH__LEN             5
+#define BMA250_BANDWIDTH__MSK             0x1F
+#define BMA250_BANDWIDTH__REG             BMA250_REG_BW_RATE
+
+#define BMA250_GET_BITSLICE(regvar, bitname)\
+	((regvar & bitname##__MSK) >> bitname##__POS)
+
+#define BMA250_SET_BITSLICE(regvar, bitname, val)\
+	((regvar & ~bitname##__MSK) | ((val<<bitname##__POS)&bitname##__MSK))
+
+/*----------------------------------------------------------------------------*/
+
+/*----------------------------------------------------------------------------*/
+static const struct i2c_device_id bma250_i2c_id[] = {{BMA250_DEV_NAME,0},{}};
+static struct i2c_board_info __initdata i2c_bma250={ I2C_BOARD_INFO("BMA250", (0x30>>1))};
+
+
+/*the adapter id will be available in customization*/
+//static unsigned short bma250_force[] = {0x00, BMA250_I2C_SLAVE_WRITE_ADDR, I2C_CLIENT_END, I2C_CLIENT_END};
+//static const unsigned short *const bma250_forces[] = { bma250_force, NULL };
+//static struct i2c_client_address_data bma250_addr_data = { .forces = bma250_forces,};
+
+/*----------------------------------------------------------------------------*/
+static int bma250_i2c_probe(struct i2c_client *client, const struct i2c_device_id *id); 
+static int bma250_i2c_remove(struct i2c_client *client);
+static int bma250_suspend(struct i2c_client *client, pm_message_t msg);
+static int bma250_resume(struct i2c_client *client);
+
+
+//static int bma250_i2c_detect(struct i2c_client *client, int kind, struct i2c_board_info *info);
+
+/*----------------------------------------------------------------------------*/
+typedef enum {
+    ADX_TRC_FILTER  = 0x01,
+    ADX_TRC_RAWDATA = 0x02,
+    ADX_TRC_IOCTL   = 0x04,
+    ADX_TRC_CALI	= 0X08,
+    ADX_TRC_INFO	= 0X10,
+} ADX_TRC;
+/*----------------------------------------------------------------------------*/
+struct scale_factor{
+    u8  whole;
+    u8  fraction;
+};
+/*----------------------------------------------------------------------------*/
+struct data_resolution {
+    struct scale_factor scalefactor;
+    int                 sensitivity;
+};
+/*----------------------------------------------------------------------------*/
+#define C_MAX_FIR_LENGTH (32)
+//#define USE_EARLY_SUSPEND
+static bool enable_status = false;
+static DEFINE_MUTEX(bma050_i2c_mutex);
+static DEFINE_MUTEX(bma050_op_mutex);
+
+
+
+/*----------------------------------------------------------------------------*/
+struct data_filter {
+    s16 raw[C_MAX_FIR_LENGTH][BMA250_AXES_NUM];
+    int sum[BMA250_AXES_NUM];
+    int num;
+    int idx;
+};
+/*----------------------------------------------------------------------------*/
+struct bma250_i2c_data {
+    struct i2c_client *client;
+    struct acc_hw *hw;
+    struct hwmsen_convert   cvt;
+    
+    /*misc*/
+    struct data_resolution *reso;
+    atomic_t                trace;
+    atomic_t                suspend;
+    atomic_t                selftest;
+	atomic_t				filter;
+    s16                     cali_sw[BMA250_AXES_NUM+1];
+
+    /*data*/
+    s8                      offset[BMA250_AXES_NUM+1];  /*+1: for 4-byte alignment*/
+    s16                     data[BMA250_AXES_NUM+1];
+
+#if defined(CONFIG_BMA250_LOWPASS)
+    atomic_t                firlen;
+    atomic_t                fir_en;
+    struct data_filter      fir;
+#endif 
+    /*early suspend*/
+#if defined(USE_EARLY_SUSPEND)
+    struct early_suspend    early_drv;
+#endif     
+};
+/*----------------------------------------------------------------------------*/
+static struct i2c_driver bma250_i2c_driver = {
+    .driver = {
+        .name           = BMA250_DEV_NAME,
+    },
+	.probe      		= bma250_i2c_probe,
+	.remove    			= bma250_i2c_remove,
+#if !defined(USE_EARLY_SUSPEND)    
+    .suspend            = bma250_suspend,
+    .resume             = bma250_resume,
+#endif
+	.id_table = bma250_i2c_id,
+};
+
+/*----------------------------------------------------------------------------*/
+static struct i2c_client *bma250_i2c_client = NULL;
+static struct platform_driver bma250_gsensor_driver;
+static struct bma250_i2c_data *obj_i2c_data = NULL;
+static bool sensor_power = true;
+static GSENSOR_VECTOR3D gsensor_gain;
+//static char selftestRes[8]= {0}; 
+
+/*----------------------------------------------------------------------------*/
+#define GSE_TAG                  "[Gsensor] "
+#define GSE_FUN(f)               printk(KERN_INFO GSE_TAG"%s\n", __FUNCTION__)
+#define GSE_ERR(fmt, args...)    printk(KERN_ERR GSE_TAG"%s %d : "fmt, __FUNCTION__, __LINE__, ##args)
+#define GSE_LOG(fmt, args...)    printk(KERN_INFO GSE_TAG fmt, ##args)
+/*----------------------------------------------------------------------------*/
+static struct data_resolution bma250_data_resolution[1] = {
+ /* combination by {FULL_RES,RANGE}*/
+    {{ 3, 9}, 256},   // dataformat +/-2g  in 10-bit resolution;  { 3, 9} = 3.9= (2*2*1000)/(2^10);  256 = (2^10)/(2*2)          
+};
+/*----------------------------------------------------------------------------*/
+static struct data_resolution bma250_offset_resolution = {{3, 9}, 256};
+
+#define C_I2C_FIFO_SIZE         8        
+static int bma050_i2c_read_block(struct i2c_client *client, u8 addr, u8 *data, u8 len)
+{
+    u8 beg = addr;
+	int err;
+	struct i2c_msg msgs[2]={{0},{0}};
+	
+	mutex_lock(&bma050_i2c_mutex);
+	
+	msgs[0].addr = client->addr;
+	msgs[0].flags = 0;
+	msgs[0].len =1;
+	msgs[0].buf = &beg;
+
+	msgs[1].addr = client->addr;
+	msgs[1].flags = I2C_M_RD;
+	msgs[1].len =len;
+	msgs[1].buf = data;
+	
+	if (!client)
+	{
+	    mutex_unlock(&bma050_i2c_mutex);
+		return -EINVAL;
+	}
+	else if (len > C_I2C_FIFO_SIZE) 
+	{
+		GSE_ERR(" length %d exceeds %d\n", len, C_I2C_FIFO_SIZE);
+		mutex_unlock(&bma050_i2c_mutex);
+		return -EINVAL;
+	}
+	err = i2c_transfer(client->adapter, msgs, sizeof(msgs)/sizeof(msgs[0]));
+	if (err != 2) 
+	{
+		GSE_ERR("i2c_transfer error: (%d %p %d) %d\n",addr, data, len, err);
+		err = -EIO;
+	} 
+	else 
+	{
+		err = 0;
+	}
+	mutex_unlock(&bma050_i2c_mutex);
+	return err;
+
+}
+
+static int bma050_i2c_write_block(struct i2c_client *client, u8 addr, u8 *data, u8 len)
+{   /*because address also occupies one byte, the maximum length for write is 7 bytes*/
+    int err, idx, num;
+    char buf[C_I2C_FIFO_SIZE];
+    err =0;
+	mutex_lock(&bma050_i2c_mutex);
+    if (!client)
+    {
+        mutex_unlock(&bma050_i2c_mutex);
+        return -EINVAL;
+    }
+    else if (len >= C_I2C_FIFO_SIZE) 
+	{        
+        GSE_ERR(" length %d exceeds %d\n", len, C_I2C_FIFO_SIZE);
+		mutex_unlock(&bma050_i2c_mutex);
+        return -EINVAL;
+    }    
+
+    num = 0;
+    buf[num++] = addr;
+    for (idx = 0; idx < len; idx++)
+    {
+        buf[num++] = data[idx];
+    }
+
+    err = i2c_master_send(client, buf, num);
+    if (err < 0)
+	{
+        GSE_ERR("send command error!!\n");
+		mutex_unlock(&bma050_i2c_mutex);
+        return -EFAULT;
+    } 
+	mutex_unlock(&bma050_i2c_mutex);
+    return err;
+}
+
+
+/*--------------------BMA250 power control function----------------------------------*/
+static void BMA250_power(struct acc_hw *hw, unsigned int on) 
+{
+	static unsigned int power_on = 0;
+
+	if(hw->power_id != POWER_NONE_MACRO)		// have externel LDO
+	{        
+		GSE_LOG("power %s\n", on ? "on" : "off");
+		if(power_on == on)	// power status not change
+		{
+			GSE_LOG("ignore power control: %d\n", on);
+		}
+		else if(on)	// power on
+		{
+			if(!hwPowerOn(hw->power_id, hw->power_vol, "BMA250"))
+			{
+				GSE_ERR("power on fails!!\n");
+			}
+		}
+		else	// power off
+		{
+			if (!hwPowerDown(hw->power_id, "BMA250"))
+			{
+				GSE_ERR("power off fail!!\n");
+			}			  
+		}
+	}
+	power_on = on;    
+}
+/*----------------------------------------------------------------------------*/
+
+/*----------------------------------------------------------------------------*/
+static int BMA250_SetDataResolution(struct bma250_i2c_data *obj)
+{
+
+/*set g sensor dataresolution here*/
+
+/*BMA250 only can set to 10-bit dataresolution, so do nothing in bma250 driver here*/
+
+/*end of set dataresolution*/
+
+
+ 
+ /*we set measure range from -2g to +2g in BMA250_SetDataFormat(client, BMA250_RANGE_2G), 
+                                                    and set 10-bit dataresolution BMA250_SetDataResolution()*/
+                                                    
+ /*so bma250_data_resolution[0] set value as {{ 3, 9}, 256} when declaration, and assign the value to obj->reso here*/  
+
+ 	obj->reso = &bma250_data_resolution[0];
+	return 0;
+	
+/*if you changed the measure range, for example call: BMA250_SetDataFormat(client, BMA250_RANGE_4G), 
+you must set the right value to bma250_data_resolution*/
+
+}
+/*----------------------------------------------------------------------------*/
+static int BMA250_ReadData(struct i2c_client *client, s16 data[BMA250_AXES_NUM])
+{
+	//struct bma250_i2c_data *priv = i2c_get_clientdata(client);        
+	u8 addr = BMA250_REG_DATAXLOW;
+	u8 buf[BMA250_DATA_LEN] = {0};
+	int err = 0;
+
+	if(NULL == client)
+	{
+		err = -EINVAL;
+	}
+	else if((err = bma050_i2c_read_block(client, addr, buf, BMA250_DATA_LEN)))
+	{
+		GSE_ERR("error: %d\n", err);
+	}
+	else
+	{
+		/* Convert sensor raw data to 16-bit integer */
+		data[BMA250_AXIS_X] = BMA250_GET_BITSLICE(buf[0], BMA250_ACC_X_LSB)
+			|(BMA250_GET_BITSLICE(buf[1],
+						BMA250_ACC_X_MSB)<<BMA250_ACC_X_LSB__LEN);
+		data[BMA250_AXIS_X] = data[BMA250_AXIS_X] << (sizeof(short)*8-(BMA250_ACC_X_LSB__LEN
+					+ BMA250_ACC_X_MSB__LEN));
+		data[BMA250_AXIS_X] = data[BMA250_AXIS_X] >> (sizeof(short)*8-(BMA250_ACC_X_LSB__LEN
+					+ BMA250_ACC_X_MSB__LEN));
+		data[BMA250_AXIS_Y] = BMA250_GET_BITSLICE(buf[2], BMA250_ACC_Y_LSB)
+			| (BMA250_GET_BITSLICE(buf[3],
+						BMA250_ACC_Y_MSB)<<BMA250_ACC_Y_LSB__LEN);
+		data[BMA250_AXIS_Y] = data[BMA250_AXIS_Y] << (sizeof(short)*8-(BMA250_ACC_Y_LSB__LEN
+					+ BMA250_ACC_Y_MSB__LEN));
+		data[BMA250_AXIS_Y] = data[BMA250_AXIS_Y] >> (sizeof(short)*8-(BMA250_ACC_Y_LSB__LEN
+					+ BMA250_ACC_Y_MSB__LEN));
+		data[BMA250_AXIS_Z] = BMA250_GET_BITSLICE(buf[4], BMA250_ACC_Z_LSB)
+			| (BMA250_GET_BITSLICE(buf[5],
+						BMA250_ACC_Z_MSB)<<BMA250_ACC_Z_LSB__LEN);
+		data[BMA250_AXIS_Z] = data[BMA250_AXIS_Z] << (sizeof(short)*8-(BMA250_ACC_Z_LSB__LEN
+					+ BMA250_ACC_Z_MSB__LEN));
+		data[BMA250_AXIS_Z] = data[BMA250_AXIS_Z] >> (sizeof(short)*8-(BMA250_ACC_Z_LSB__LEN
+					+ BMA250_ACC_Z_MSB__LEN));
+
+#ifdef CONFIG_BMA250_LOWPASS
+		if(atomic_read(&priv->filter))
+		{
+			if(atomic_read(&priv->fir_en) && !atomic_read(&priv->suspend))
+			{
+				int idx, firlen = atomic_read(&priv->firlen);   
+				if(priv->fir.num < firlen)
+				{                
+					priv->fir.raw[priv->fir.num][BMA250_AXIS_X] = data[BMA250_AXIS_X];
+					priv->fir.raw[priv->fir.num][BMA250_AXIS_Y] = data[BMA250_AXIS_Y];
+					priv->fir.raw[priv->fir.num][BMA250_AXIS_Z] = data[BMA250_AXIS_Z];
+					priv->fir.sum[BMA250_AXIS_X] += data[BMA250_AXIS_X];
+					priv->fir.sum[BMA250_AXIS_Y] += data[BMA250_AXIS_Y];
+					priv->fir.sum[BMA250_AXIS_Z] += data[BMA250_AXIS_Z];
+					if(atomic_read(&priv->trace) & ADX_TRC_FILTER)
+					{
+						GSE_LOG("add [%2d] [%5d %5d %5d] => [%5d %5d %5d]\n", priv->fir.num,
+							priv->fir.raw[priv->fir.num][BMA250_AXIS_X], priv->fir.raw[priv->fir.num][BMA250_AXIS_Y], priv->fir.raw[priv->fir.num][BMA250_AXIS_Z],
+							priv->fir.sum[BMA250_AXIS_X], priv->fir.sum[BMA250_AXIS_Y], priv->fir.sum[BMA250_AXIS_Z]);
+					}
+					priv->fir.num++;
+					priv->fir.idx++;
+				}
+				else
+				{
+					idx = priv->fir.idx % firlen;
+					priv->fir.sum[BMA250_AXIS_X] -= priv->fir.raw[idx][BMA250_AXIS_X];
+					priv->fir.sum[BMA250_AXIS_Y] -= priv->fir.raw[idx][BMA250_AXIS_Y];
+					priv->fir.sum[BMA250_AXIS_Z] -= priv->fir.raw[idx][BMA250_AXIS_Z];
+					priv->fir.raw[idx][BMA250_AXIS_X] = data[BMA250_AXIS_X];
+					priv->fir.raw[idx][BMA250_AXIS_Y] = data[BMA250_AXIS_Y];
+					priv->fir.raw[idx][BMA250_AXIS_Z] = data[BMA250_AXIS_Z];
+					priv->fir.sum[BMA250_AXIS_X] += data[BMA250_AXIS_X];
+					priv->fir.sum[BMA250_AXIS_Y] += data[BMA250_AXIS_Y];
+					priv->fir.sum[BMA250_AXIS_Z] += data[BMA250_AXIS_Z];
+					priv->fir.idx++;
+					data[BMA250_AXIS_X] = priv->fir.sum[BMA250_AXIS_X]/firlen;
+					data[BMA250_AXIS_Y] = priv->fir.sum[BMA250_AXIS_Y]/firlen;
+					data[BMA250_AXIS_Z] = priv->fir.sum[BMA250_AXIS_Z]/firlen;
+					if(atomic_read(&priv->trace) & ADX_TRC_FILTER)
+					{
+						GSE_LOG("add [%2d] [%5d %5d %5d] => [%5d %5d %5d] : [%5d %5d %5d]\n", idx,
+						priv->fir.raw[idx][BMA250_AXIS_X], priv->fir.raw[idx][BMA250_AXIS_Y], priv->fir.raw[idx][BMA250_AXIS_Z],
+						priv->fir.sum[BMA250_AXIS_X], priv->fir.sum[BMA250_AXIS_Y], priv->fir.sum[BMA250_AXIS_Z],
+						data[BMA250_AXIS_X], data[BMA250_AXIS_Y], data[BMA250_AXIS_Z]);
+					}
+				}
+			}
+		}	
+#endif         
+	}
+	return err;
+}
+/*----------------------------------------------------------------------------*/
+static int BMA250_ReadOffset(struct i2c_client *client, s8 ofs[BMA250_AXES_NUM])
+{    
+	int err = 0;
+#ifdef SW_CALIBRATION
+	ofs[0]=ofs[1]=ofs[2]=0x0;
+#else
+	if(err = bma050_i2c_read_block(client, BMA250_REG_OFSX, ofs, BMA250_AXES_NUM))
+	{
+		GSE_ERR("error: %d\n", err);
+	}
+#endif
+	//printk("offesx=%x, y=%x, z=%x",ofs[0],ofs[1],ofs[2]);
+	
+	return err;    
+}
+/*----------------------------------------------------------------------------*/
+static int BMA250_ResetCalibration(struct i2c_client *client)
+{
+	struct bma250_i2c_data *obj = i2c_get_clientdata(client);
+	//u8 ofs[4]={0,0,0,0};
+	int err  = 0;
+	
+	#ifdef SW_CALIBRATION
+		
+	#else
+		if(err = bma050_i2c_write_block(client, BMA250_REG_OFSX, ofs, 4))
+		{
+			GSE_ERR("error: %d\n", err);
+		}
+	#endif
+
+	memset(obj->cali_sw, 0x00, sizeof(obj->cali_sw));
+	memset(obj->offset, 0x00, sizeof(obj->offset));
+	return err;    
+}
+/*----------------------------------------------------------------------------*/
+static int BMA250_ReadCalibration(struct i2c_client *client, int dat[BMA250_AXES_NUM])
+{
+    struct bma250_i2c_data *obj = i2c_get_clientdata(client);
+    //int err;
+    int mul;
+
+	#ifdef SW_CALIBRATION
+		mul = 0;//only SW Calibration, disable HW Calibration
+	#else
+	    if ((err = BMA250_ReadOffset(client, obj->offset))) {
+        GSE_ERR("read offset fail, %d\n", err);
+        return err;
+    	}    
+    	mul = obj->reso->sensitivity/bma250_offset_resolution.sensitivity;
+	#endif
+
+    dat[obj->cvt.map[BMA250_AXIS_X]] = obj->cvt.sign[BMA250_AXIS_X]*(obj->offset[BMA250_AXIS_X]*mul*GRAVITY_EARTH_1000/(obj->reso->sensitivity) + obj->cali_sw[BMA250_AXIS_X]);
+    dat[obj->cvt.map[BMA250_AXIS_Y]] = obj->cvt.sign[BMA250_AXIS_Y]*(obj->offset[BMA250_AXIS_Y]*mul*GRAVITY_EARTH_1000/(obj->reso->sensitivity) + obj->cali_sw[BMA250_AXIS_Y]);
+    dat[obj->cvt.map[BMA250_AXIS_Z]] = obj->cvt.sign[BMA250_AXIS_Z]*(obj->offset[BMA250_AXIS_Z]*mul*GRAVITY_EARTH_1000/(obj->reso->sensitivity) + obj->cali_sw[BMA250_AXIS_Z]);                        
+                                       
+    return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int BMA250_ReadCalibrationEx(struct i2c_client *client, int act[BMA250_AXES_NUM], int raw[BMA250_AXES_NUM])
+{  
+	/*raw: the raw calibration data; act: the actual calibration data*/
+	struct bma250_i2c_data *obj = i2c_get_clientdata(client);
+	//int err;
+	int mul;
+
+ 
+
+	#ifdef SW_CALIBRATION
+		mul = 0;//only SW Calibration, disable HW Calibration
+	#else
+		if(err = BMA250_ReadOffset(client, obj->offset))
+		{
+			GSE_ERR("read offset fail, %d\n", err);
+			return err;
+		}   
+		mul = obj->reso->sensitivity/bma250_offset_resolution.sensitivity;
+	#endif
+	
+	raw[BMA250_AXIS_X] = obj->offset[BMA250_AXIS_X]*mul*GRAVITY_EARTH_1000/(obj->reso->sensitivity) + obj->cali_sw[BMA250_AXIS_X];
+	raw[BMA250_AXIS_Y] = obj->offset[BMA250_AXIS_Y]*mul*GRAVITY_EARTH_1000/(obj->reso->sensitivity) + obj->cali_sw[BMA250_AXIS_Y];
+	raw[BMA250_AXIS_Z] = obj->offset[BMA250_AXIS_Z]*mul*GRAVITY_EARTH_1000/(obj->reso->sensitivity) + obj->cali_sw[BMA250_AXIS_Z];
+
+	act[obj->cvt.map[BMA250_AXIS_X]] = obj->cvt.sign[BMA250_AXIS_X]*raw[BMA250_AXIS_X];
+	act[obj->cvt.map[BMA250_AXIS_Y]] = obj->cvt.sign[BMA250_AXIS_Y]*raw[BMA250_AXIS_Y];
+	act[obj->cvt.map[BMA250_AXIS_Z]] = obj->cvt.sign[BMA250_AXIS_Z]*raw[BMA250_AXIS_Z];                        
+	                       
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int BMA250_WriteCalibration(struct i2c_client *client, int dat[BMA250_AXES_NUM])
+{
+	struct bma250_i2c_data *obj = i2c_get_clientdata(client);
+	int err;
+	int cali[BMA250_AXES_NUM], raw[BMA250_AXES_NUM];
+	//int lsb = bma250_offset_resolution.sensitivity;
+	//int divisor = obj->reso->sensitivity/lsb;
+
+	if((err = BMA250_ReadCalibrationEx(client, cali, raw)))	/*offset will be updated in obj->offset*/
+	{ 
+		GSE_ERR("read offset fail, %d\n", err);
+		return err;
+	}
+
+	GSE_LOG("OLDOFF: (%+3d %+3d %+3d): (%+3d %+3d %+3d) / (%+3d %+3d %+3d)\n", 
+		raw[BMA250_AXIS_X], raw[BMA250_AXIS_Y], raw[BMA250_AXIS_Z],
+		obj->offset[BMA250_AXIS_X], obj->offset[BMA250_AXIS_Y], obj->offset[BMA250_AXIS_Z],
+		obj->cali_sw[BMA250_AXIS_X], obj->cali_sw[BMA250_AXIS_Y], obj->cali_sw[BMA250_AXIS_Z]);
+
+	/*calculate the real offset expected by caller*/
+	cali[BMA250_AXIS_X] += dat[BMA250_AXIS_X];
+	cali[BMA250_AXIS_Y] += dat[BMA250_AXIS_Y];
+	cali[BMA250_AXIS_Z] += dat[BMA250_AXIS_Z];
+
+	GSE_LOG("UPDATE: (%+3d %+3d %+3d)\n", 
+		dat[BMA250_AXIS_X], dat[BMA250_AXIS_Y], dat[BMA250_AXIS_Z]);
+
+#ifdef SW_CALIBRATION
+	obj->cali_sw[BMA250_AXIS_X] = obj->cvt.sign[BMA250_AXIS_X]*(cali[obj->cvt.map[BMA250_AXIS_X]]);
+	obj->cali_sw[BMA250_AXIS_Y] = obj->cvt.sign[BMA250_AXIS_Y]*(cali[obj->cvt.map[BMA250_AXIS_Y]]);
+	obj->cali_sw[BMA250_AXIS_Z] = obj->cvt.sign[BMA250_AXIS_Z]*(cali[obj->cvt.map[BMA250_AXIS_Z]]);	
+#else
+	obj->offset[BMA250_AXIS_X] = (s8)(obj->cvt.sign[BMA250_AXIS_X]*(cali[obj->cvt.map[BMA250_AXIS_X]])*(obj->reso->sensitivity)/GRAVITY_EARTH_1000/(divisor));
+	obj->offset[BMA250_AXIS_Y] = (s8)(obj->cvt.sign[BMA250_AXIS_Y]*(cali[obj->cvt.map[BMA250_AXIS_Y]])*(obj->reso->sensitivity)/GRAVITY_EARTH_1000/(divisor));
+	obj->offset[BMA250_AXIS_Z] = (s8)(obj->cvt.sign[BMA250_AXIS_Z]*(cali[obj->cvt.map[BMA250_AXIS_Z]])*(obj->reso->sensitivity)/GRAVITY_EARTH_1000/(divisor));
+
+	/*convert software calibration using standard calibration*/
+	obj->cali_sw[BMA250_AXIS_X] = obj->cvt.sign[BMA250_AXIS_X]*(cali[obj->cvt.map[BMA250_AXIS_X]])%(divisor);
+	obj->cali_sw[BMA250_AXIS_Y] = obj->cvt.sign[BMA250_AXIS_Y]*(cali[obj->cvt.map[BMA250_AXIS_Y]])%(divisor);
+	obj->cali_sw[BMA250_AXIS_Z] = obj->cvt.sign[BMA250_AXIS_Z]*(cali[obj->cvt.map[BMA250_AXIS_Z]])%(divisor);
+
+	GSE_LOG("NEWOFF: (%+3d %+3d %+3d): (%+3d %+3d %+3d) / (%+3d %+3d %+3d)\n", 
+		obj->offset[BMA250_AXIS_X]*divisor + obj->cali_sw[BMA250_AXIS_X], 
+		obj->offset[BMA250_AXIS_Y]*divisor + obj->cali_sw[BMA250_AXIS_Y], 
+		obj->offset[BMA250_AXIS_Z]*divisor + obj->cali_sw[BMA250_AXIS_Z], 
+		obj->offset[BMA250_AXIS_X], obj->offset[BMA250_AXIS_Y], obj->offset[BMA250_AXIS_Z],
+		obj->cali_sw[BMA250_AXIS_X], obj->cali_sw[BMA250_AXIS_Y], obj->cali_sw[BMA250_AXIS_Z]);
+
+	if(err = bma050_i2c_write_block(obj->client, BMA250_REG_OFSX, obj->offset, BMA250_AXES_NUM))
+	{
+		GSE_ERR("write offset fail: %d\n", err);
+		return err;
+	}
+#endif
+
+	return err;
+}
+/*----------------------------------------------------------------------------*/
+static int BMA250_CheckDeviceID(struct i2c_client *client)
+{
+	u8 databuf[2];    
+	int res = 0;
+
+	memset(databuf, 0, sizeof(u8)*2);    
+	databuf[0] = BMA250_REG_DEVID;    
+
+    res = bma050_i2c_read_block(client,BMA250_REG_DEVID,databuf,0x1);
+	if(res < 0)
+	{
+		goto exit_BMA250_CheckDeviceID;
+	}
+	if(databuf[0]!=BMA250_FIXED_DEVID)
+	{
+		printk("BMA250_CheckDeviceID %d failt!\n ", databuf[0]);
+		return BMA250_ERR_IDENTIFICATION;
+	}
+	else
+	{
+		printk("BMA250_CheckDeviceID %d pass!\n ", databuf[0]);
+	}
+
+	exit_BMA250_CheckDeviceID:
+	if (res < 0)
+	{
+		return BMA250_ERR_I2C;
+	}
+	
+	return BMA250_SUCCESS;
+}
+/*----------------------------------------------------------------------------*/
+static int BMA250_SetPowerMode(struct i2c_client *client, bool enable)
+{
+	u8 databuf[2];    
+	int res = 0;
+	u8 addr = BMA250_REG_POWER_CTL;
+	struct bma250_i2c_data *obj = i2c_get_clientdata(client);
+	
+	
+	if(enable == sensor_power )
+	{
+		GSE_LOG("Sensor power status is newest!\n");
+		return BMA250_SUCCESS;
+	}
+
+	if(bma050_i2c_read_block(client, addr, databuf, 0x01))
+	{
+		GSE_ERR("read power ctl register err!\n");
+		return BMA250_ERR_I2C;
+	}
+
+	
+	if(enable == TRUE)
+	{
+		databuf[0] &= ~BMA250_MEASURE_MODE;
+	}
+	else
+	{
+		databuf[0] |= BMA250_MEASURE_MODE;
+	}
+	//databuf[1] = databuf[0];
+	//databuf[0] = BMA250_REG_POWER_CTL;
+	
+    res = bma050_i2c_write_block(client,BMA250_REG_POWER_CTL,databuf,0x1);
+	if(res < 0)
+	{
+		GSE_LOG("set power mode failed!\n");
+		return BMA250_ERR_I2C;
+	}
+	else if(atomic_read(&obj->trace) & ADX_TRC_INFO)
+	{
+		GSE_LOG("set power mode ok %d!\n", databuf[1]);
+	}
+
+	//GSE_LOG("BMA250_SetPowerMode ok!\n");
+
+
+	sensor_power = enable;
+
+	mdelay(20);
+	
+	return BMA250_SUCCESS;    
+}
+/*----------------------------------------------------------------------------*/
+static int BMA250_SetDataFormat(struct i2c_client *client, u8 dataformat)
+{
+	struct bma250_i2c_data *obj = i2c_get_clientdata(client);
+	u8 databuf[10];    
+	int res = 0;
+
+	memset(databuf, 0, sizeof(u8)*10);    
+
+	if(bma050_i2c_read_block(client, BMA250_REG_DATA_FORMAT, databuf, 0x01))
+	{
+		printk("bma250 read Dataformat failt \n");
+		return BMA250_ERR_I2C;
+	}
+
+	databuf[0] &= ~BMA250_RANGE_MASK;
+	databuf[0] |= dataformat;
+	
+    res = bma050_i2c_write_block(client,BMA250_REG_DATA_FORMAT,databuf,0x1);
+	if(res < 0)
+	{
+		return BMA250_ERR_I2C;
+	}
+	
+	//printk("BMA250_SetDataFormat OK! \n");
+	
+
+	return BMA250_SetDataResolution(obj);    
+}
+/*----------------------------------------------------------------------------*/
+static int BMA250_SetBWRate(struct i2c_client *client, u8 bwrate)
+{
+	u8 databuf[10];    
+	int res = 0;
+
+	memset(databuf, 0, sizeof(u8)*10);    
+
+	if(bma050_i2c_read_block(client, BMA250_REG_BW_RATE, databuf, 0x01))
+	{
+		printk("bma250 read rate failt \n");
+		return BMA250_ERR_I2C;
+	}
+
+	databuf[0] &= ~BMA250_BW_MASK;
+	databuf[0] |= bwrate;
+
+    res = bma050_i2c_write_block(client,BMA250_REG_BW_RATE,databuf,0x1);
+	if(res < 0)
+	{
+		return BMA250_ERR_I2C;
+	}
+	
+	//printk("BMA250_SetBWRate OK! \n");
+	
+	return BMA250_SUCCESS;    
+}
+/*----------------------------------------------------------------------------*/
+static int BMA250_SetIntEnable(struct i2c_client *client, u8 intenable)
+{
+			//u8 databuf[10];    
+			int res = 0;
+		
+			res = hwmsen_write_byte(client, BMA250_INT_REG_1, 0x00);
+			if(res != BMA250_SUCCESS) 
+			{
+				return res;
+			}
+			res = hwmsen_write_byte(client, BMA250_INT_REG_2, 0x00);
+			if(res != BMA250_SUCCESS) 
+			{
+				return res;
+			}
+			printk("BMA250 disable interrupt ...\n");
+		
+			/*for disable interrupt function*/
+			
+			return BMA250_SUCCESS;	  
+}
+
+/*----------------------------------------------------------------------------*/
+static int bma250_init_client(struct i2c_client *client, int reset_cali)
+{
+	struct bma250_i2c_data *obj = i2c_get_clientdata(client);
+	int res = 0;
+	int a = 0;
+	printk("bma250_init_client \n");
+
+	//for fix check device id error
+	do{
+	udelay(100);
+	res = BMA250_CheckDeviceID(client); 
+	if(res == BMA250_SUCCESS)
+	{
+	printk("BMA250_CheckDeviceID ok \n");
+		break;
+	}	
+	a++;	
+	printk("bma250_init_client count: %d\n", a);
+	}while(a < 1000);
+	
+	res = BMA250_SetBWRate(client, BMA250_BW_50HZ);
+	if(res != BMA250_SUCCESS ) 
+	{
+		return res;
+	}
+	printk("BMA250_SetBWRate OK!\n");
+	
+	res = BMA250_SetDataFormat(client, BMA250_RANGE_2G);
+	if(res != BMA250_SUCCESS) 
+	{
+		return res;
+	}
+	printk("BMA250_SetDataFormat OK!\n");
+
+	gsensor_gain.x = gsensor_gain.y = gsensor_gain.z = obj->reso->sensitivity;
+
+
+	res = BMA250_SetIntEnable(client, 0x00);        
+	if(res != BMA250_SUCCESS)
+	{
+		return res;
+	}
+	printk("BMA250 disable interrupt function!\n");
+
+	res = BMA250_SetPowerMode(client, enable_status);
+		if(res != BMA250_SUCCESS)
+		{
+			return res;
+		}
+		printk("BMA250_SetPowerMode OK!\n");
+
+
+	if(0 != reset_cali)
+	{ 
+		/*reset calibration only in power on*/
+		res = BMA250_ResetCalibration(client);
+		if(res != BMA250_SUCCESS)
+		{
+			return res;
+		}
+	}
+	printk("bma250_init_client OK!\n");
+#ifdef CONFIG_BMA250_LOWPASS
+	memset(&obj->fir, 0x00, sizeof(obj->fir));  
+#endif
+
+	mdelay(20);
+
+	return BMA250_SUCCESS;
+}
+/*----------------------------------------------------------------------------*/
+static int BMA250_ReadChipInfo(struct i2c_client *client, char *buf, int bufsize)
+{
+	u8 databuf[10];    
+
+	memset(databuf, 0, sizeof(u8)*10);
+
+	if((NULL == buf)||(bufsize<=30))
+	{
+		return -1;
+	}
+	
+	if(NULL == client)
+	{
+		*buf = 0;
+		return -2;
+	}
+
+	sprintf(buf, "BMA250 Chip");
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int BMA250_CompassReadData(struct i2c_client *client, char *buf, int bufsize)
+{
+	struct bma250_i2c_data *obj = (struct bma250_i2c_data*)i2c_get_clientdata(client);
+	u8 databuf[20];
+	int acc[BMA250_AXES_NUM];
+	int res = 0;
+	memset(databuf, 0, sizeof(u8)*10);
+
+	if(NULL == buf)
+	{
+		return -1;
+	}
+	if(NULL == client)
+	{
+		*buf = 0;
+		return -2;
+	}
+
+	if(sensor_power == FALSE)
+	{
+		res = BMA250_SetPowerMode(client, true);
+		if(res)
+		{
+			GSE_ERR("Power on bma250 error %d!\n", res);
+		}
+	}
+
+	if((res = BMA250_ReadData(client, obj->data)))
+	{        
+		GSE_ERR("I2C error: ret value=%d", res);
+		return -3;
+	}
+	else
+	{
+		/*remap coordinate*/
+		acc[obj->cvt.map[BMA250_AXIS_X]] = obj->cvt.sign[BMA250_AXIS_X]*obj->data[BMA250_AXIS_X];
+		acc[obj->cvt.map[BMA250_AXIS_Y]] = obj->cvt.sign[BMA250_AXIS_Y]*obj->data[BMA250_AXIS_Y];
+		acc[obj->cvt.map[BMA250_AXIS_Z]] = obj->cvt.sign[BMA250_AXIS_Z]*obj->data[BMA250_AXIS_Z];
+		//printk("cvt x=%d, y=%d, z=%d \n",obj->cvt.sign[BMA250_AXIS_X],obj->cvt.sign[BMA250_AXIS_Y],obj->cvt.sign[BMA250_AXIS_Z]);
+
+		//GSE_LOG("Mapped gsensor data: %d, %d, %d!\n", acc[BMA250_AXIS_X], acc[BMA250_AXIS_Y], acc[BMA250_AXIS_Z]);
+
+		sprintf(buf, "%d %d %d", (s16)acc[BMA250_AXIS_X], (s16)acc[BMA250_AXIS_Y], (s16)acc[BMA250_AXIS_Z]);
+		if(atomic_read(&obj->trace) & ADX_TRC_IOCTL)
+		{
+			GSE_LOG("gsensor data for compass: %s!\n", buf);
+		}
+	}
+	
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int BMA250_ReadSensorData(struct i2c_client *client, char *buf, int bufsize)
+{
+	struct bma250_i2c_data *obj = (struct bma250_i2c_data*)i2c_get_clientdata(client);
+	u8 databuf[20];
+	int acc[BMA250_AXES_NUM];
+	int res = 0;
+	memset(databuf, 0, sizeof(u8)*10);
+
+	if(NULL == buf)
+	{
+		return -1;
+	}
+	if(NULL == client)
+	{
+		*buf = 0;
+		return -2;
+	}
+	/*
+	if(false == enable_status )
+	{
+		
+		acc[BMA250_AXIS_X]=-1;
+		acc[BMA250_AXIS_Y]=-1;
+		acc[BMA250_AXIS_Z]=-1;
+		sprintf(buf, "%04x %04x %04x", acc[BMA250_AXIS_X], acc[BMA250_AXIS_Y], acc[BMA250_AXIS_Z]);
+        GSE_ERR("sensor disable read invalid data!\n");
+		return 0;
+	}
+	*/
+	if((res = BMA250_ReadData(client, obj->data)))
+	{        
+		GSE_ERR("I2C error: ret value=%d", res);
+		return -3;
+	}
+	else
+	{
+	#if 0
+		obj->data[BMA250_AXIS_X] = obj->data[BMA250_AXIS_X] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+		obj->data[BMA250_AXIS_Y] = obj->data[BMA250_AXIS_Y] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+		obj->data[BMA250_AXIS_Z] = obj->data[BMA250_AXIS_Z] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;		
+	#endif	
+		//printk("raw data x=%d, y=%d, z=%d \n",obj->data[BMA250_AXIS_X],obj->data[BMA250_AXIS_Y],obj->data[BMA250_AXIS_Z]);
+		obj->data[BMA250_AXIS_X] += obj->cali_sw[BMA250_AXIS_X];
+		obj->data[BMA250_AXIS_Y] += obj->cali_sw[BMA250_AXIS_Y];
+		obj->data[BMA250_AXIS_Z] += obj->cali_sw[BMA250_AXIS_Z];
+		
+		//printk("cali_sw x=%d, y=%d, z=%d \n",obj->cali_sw[BMA250_AXIS_X],obj->cali_sw[BMA250_AXIS_Y],obj->cali_sw[BMA250_AXIS_Z]);
+		
+		/*remap coordinate*/
+		acc[obj->cvt.map[BMA250_AXIS_X]] = obj->cvt.sign[BMA250_AXIS_X]*obj->data[BMA250_AXIS_X];
+		acc[obj->cvt.map[BMA250_AXIS_Y]] = obj->cvt.sign[BMA250_AXIS_Y]*obj->data[BMA250_AXIS_Y];
+		acc[obj->cvt.map[BMA250_AXIS_Z]] = obj->cvt.sign[BMA250_AXIS_Z]*obj->data[BMA250_AXIS_Z];
+		//printk("cvt x=%d, y=%d, z=%d \n",obj->cvt.sign[BMA250_AXIS_X],obj->cvt.sign[BMA250_AXIS_Y],obj->cvt.sign[BMA250_AXIS_Z]);
+
+
+		//GSE_LOG("Mapped gsensor data: %d, %d, %d!\n", acc[BMA250_AXIS_X], acc[BMA250_AXIS_Y], acc[BMA250_AXIS_Z]);
+
+		//Out put the mg
+		//printk("mg acc=%d, GRAVITY=%d, sensityvity=%d \n",acc[BMA250_AXIS_X],GRAVITY_EARTH_1000,obj->reso->sensitivity);
+#if 0
+		acc[BMA250_AXIS_X] = acc[BMA250_AXIS_X] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+		acc[BMA250_AXIS_Y] = acc[BMA250_AXIS_Y] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+		acc[BMA250_AXIS_Z] = acc[BMA250_AXIS_Z] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;		
+	#endif	
+	
+
+		sprintf(buf, "%04x %04x %04x", acc[BMA250_AXIS_X], acc[BMA250_AXIS_Y], acc[BMA250_AXIS_Z]);
+		if(atomic_read(&obj->trace) & ADX_TRC_IOCTL)
+		{
+			GSE_LOG("gsensor data: %s!\n", buf);
+		}
+	}
+	
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int BMA250_ReadRawData(struct i2c_client *client, char *buf)
+{
+	struct bma250_i2c_data *obj = (struct bma250_i2c_data*)i2c_get_clientdata(client);
+	int res = 0;
+
+	if (!buf || !client)
+	{
+		return EINVAL;
+	}
+	
+	if((res = BMA250_ReadData(client, obj->data)))
+	{        
+		GSE_ERR("I2C error: ret value=%d", res);
+		return EIO;
+	}
+	else
+	{
+		sprintf(buf, "BMA250_ReadRawData %04x %04x %04x", obj->data[BMA250_AXIS_X], 
+			obj->data[BMA250_AXIS_Y], obj->data[BMA250_AXIS_Z]);
+	
+	}
+	
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int bma250_set_mode(struct i2c_client *client, unsigned char mode)
+{
+	int comres = 0;
+	unsigned char data[2] = {BMA250_EN_LOW_POWER__REG};
+
+	if ((client == NULL) || (mode >= 3))
+	{
+		return -1;
+	}
+
+	comres = bma050_i2c_read_block(client,
+			BMA250_EN_LOW_POWER__REG, data, 1);
+	switch (mode) {
+	case BMA250_MODE_NORMAL:
+		data[0]  = BMA250_SET_BITSLICE(data[0],
+				BMA250_EN_LOW_POWER, 0);
+		data[0]  = BMA250_SET_BITSLICE(data[0],
+				BMA250_EN_SUSPEND, 0);
+		break;
+	case BMA250_MODE_LOWPOWER:
+		data[0]  = BMA250_SET_BITSLICE(data[0],
+				BMA250_EN_LOW_POWER, 1);
+		data[0]  = BMA250_SET_BITSLICE(data[0],
+				BMA250_EN_SUSPEND, 0);
+		break;
+	case BMA250_MODE_SUSPEND:
+		data[0]  = BMA250_SET_BITSLICE(data[0],
+				BMA250_EN_LOW_POWER, 0);
+		data[0]  = BMA250_SET_BITSLICE(data[0],
+				BMA250_EN_SUSPEND, 1);
+		break;
+	default:
+		break;
+	}
+
+    comres = bma050_i2c_write_block(client,BMA250_EN_LOW_POWER__REG,data,0x1);
+	if(comres < 0)
+	{
+		return BMA250_ERR_I2C;
+	}
+	else
+	{
+		return comres;
+	}
+}
+/*----------------------------------------------------------------------------*/
+static int bma250_get_mode(struct i2c_client *client, unsigned char *mode)
+{
+	int comres = 0;
+
+	if (client == NULL) 
+	{
+		return -1;
+	}
+	comres = bma050_i2c_read_block(client,
+			BMA250_EN_LOW_POWER__REG, mode, 1);
+	*mode  = (*mode) >> 6;
+		
+	return comres;
+}
+
+/*----------------------------------------------------------------------------*/
+static int bma250_set_range(struct i2c_client *client, unsigned char range)
+{
+	int comres = 0;
+	unsigned char data[2] = {BMA250_RANGE_SEL__REG};
+
+	if (client == NULL)
+	{
+		return -1;
+	}
+	
+	comres = bma050_i2c_read_block(client,
+			BMA250_RANGE_SEL__REG, data, 1);
+
+	data[0]  = BMA250_SET_BITSLICE(data[0],
+			BMA250_RANGE_SEL, range);
+    comres= bma050_i2c_write_block(client,BMA250_RANGE_SEL__REG,data,0x1);
+	if(comres < 0)
+	{
+		return BMA250_ERR_I2C;
+	}
+	else
+	{
+		return comres;
+	}
+}
+/*----------------------------------------------------------------------------*/
+static int bma250_get_range(struct i2c_client *client, unsigned char *range)
+{
+	int comres = 0;
+	unsigned char data;
+
+	if (client == NULL) 
+	{
+		return -1;
+	}
+
+	comres = bma050_i2c_read_block(client, BMA250_RANGE_SEL__REG,	&data, 1);
+	*range = BMA250_GET_BITSLICE(data, BMA250_RANGE_SEL);
+
+	return comres;
+}
+/*----------------------------------------------------------------------------*/
+static int bma250_set_bandwidth(struct i2c_client *client, unsigned char bandwidth)
+{
+	int comres = 0;
+	unsigned char data[2] = {BMA250_BANDWIDTH__REG};
+
+	if (client == NULL)
+	{
+		return -1;
+	}
+	
+	comres = bma050_i2c_read_block(client,
+			BMA250_BANDWIDTH__REG, data, 1);
+
+	data[0]  = BMA250_SET_BITSLICE(data[0],
+			BMA250_BANDWIDTH, bandwidth);
+
+    comres = bma050_i2c_write_block(client,BMA250_BANDWIDTH__REG,data,0x1);
+	if(comres < 0)
+	{
+		return BMA250_ERR_I2C;
+	}
+	else
+	{
+		return comres;
+	}
+}
+/*----------------------------------------------------------------------------*/
+static int bma250_get_bandwidth(struct i2c_client *client, unsigned char *bandwidth)
+{
+	int comres = 0;
+	unsigned char data;
+
+	if (client == NULL) 
+	{
+		return -1;
+	}
+
+	comres = bma050_i2c_read_block(client, BMA250_BANDWIDTH__REG, &data, 1);
+	data = BMA250_GET_BITSLICE(data, BMA250_BANDWIDTH);
+
+	if (data < 0x08) //7.81Hz
+	{
+		*bandwidth = 0x08;
+	}
+	else if (data > 0x0f)	// 1000Hz
+	{
+		*bandwidth = 0x0f;
+	}
+	else
+	{
+		*bandwidth = data;
+	}
+	return comres;
+}
+/*----------------------------------------------------------------------------*/
+
+static ssize_t show_chipinfo_value(struct device_driver *ddri, char *buf)
+{
+	struct i2c_client *client = bma250_i2c_client;
+	char strbuf[BMA250_BUFSIZE];
+	if(NULL == client)
+	{
+		GSE_ERR("i2c client is null!!\n");
+		return 0;
+	}
+	
+	BMA250_ReadChipInfo(client, strbuf, BMA250_BUFSIZE);
+	return snprintf(buf, PAGE_SIZE, "%s\n", strbuf);        
+}
+
+/*
+static ssize_t gsensor_init(struct device_driver *ddri, char *buf, size_t count)
+	{
+		struct i2c_client *client = bma250_i2c_client;
+		char strbuf[BMA250_BUFSIZE];
+		
+		if(NULL == client)
+		{
+			GSE_ERR("i2c client is null!!\n");
+			return 0;
+		}
+		bma250_init_client(client, 1);
+		return snprintf(buf, PAGE_SIZE, "%s\n", strbuf);			
+	}
+*/
+/*----------------------------------------------------------------------------*/
+/*
+g sensor opmode for compass tilt compensation
+*/
+static ssize_t show_cpsopmode_value(struct device_driver *ddri, char *buf)
+{
+	unsigned char data;
+
+	if (bma250_get_mode(bma250_i2c_client, &data) < 0)
+	{
+		return sprintf(buf, "Read error\n");
+	}
+	else
+	{
+		return sprintf(buf, "%d\n", data);
+	}
+}
+
+/*----------------------------------------------------------------------------*/
+/*
+g sensor opmode for compass tilt compensation
+*/
+static ssize_t store_cpsopmode_value(struct device_driver *ddri, const char *buf, size_t count)
+{
+	unsigned long data;
+	int error;
+
+	if ((error = strict_strtoul(buf, 10, &data)))
+	{
+		return error;
+	}
+	if (data == BMA250_MODE_NORMAL)
+	{
+		BMA250_SetPowerMode(bma250_i2c_client, true);
+	}
+	else if (data == BMA250_MODE_SUSPEND)
+	{
+		BMA250_SetPowerMode(bma250_i2c_client, false);
+	}
+	else if (bma250_set_mode(bma250_i2c_client, (unsigned char) data) < 0)
+	{
+		GSE_ERR("invalid content: '%s', length = %d\n", buf, count);
+	}
+
+	return count;
+}
+
+/*----------------------------------------------------------------------------*/
+/*
+g sensor range for compass tilt compensation
+*/
+static ssize_t show_cpsrange_value(struct device_driver *ddri, char *buf)
+{
+	unsigned char data;
+
+	if (bma250_get_range(bma250_i2c_client, &data) < 0)
+	{
+		return sprintf(buf, "Read error\n");
+	}
+	else
+	{
+		return sprintf(buf, "%d\n", data);
+	}
+}
+
+/*----------------------------------------------------------------------------*/
+/*
+g sensor range for compass tilt compensation
+*/
+static ssize_t store_cpsrange_value(struct device_driver *ddri, const char *buf, size_t count)
+{
+	unsigned long data;
+	int error;
+
+	if ((error = strict_strtoul(buf, 10, &data)))
+	{
+		return error;
+	}
+	if (bma250_set_range(bma250_i2c_client, (unsigned char) data) < 0)
+	{
+		GSE_ERR("invalid content: '%s', length = %d\n", buf, count);
+	}
+
+	return count;
+}
+/*----------------------------------------------------------------------------*/
+/*
+g sensor bandwidth for compass tilt compensation
+*/
+static ssize_t show_cpsbandwidth_value(struct device_driver *ddri, char *buf)
+{
+	unsigned char data;
+
+	if (bma250_get_bandwidth(bma250_i2c_client, &data) < 0)
+	{
+		return sprintf(buf, "Read error\n");
+	}
+	else
+	{
+		return sprintf(buf, "%d\n", data);
+	}
+}
+
+/*----------------------------------------------------------------------------*/
+/*
+g sensor bandwidth for compass tilt compensation
+*/
+static ssize_t store_cpsbandwidth_value(struct device_driver *ddri, const char *buf, size_t count)
+{
+	unsigned long data;
+	int error;
+
+	if ((error = strict_strtoul(buf, 10, &data)))
+	{
+		return error;
+	}
+	if (bma250_set_bandwidth(bma250_i2c_client, (unsigned char) data) < 0)
+	{
+		GSE_ERR("invalid content: '%s', length = %d\n", buf, count);
+	}
+
+	return count;
+}
+
+/*----------------------------------------------------------------------------*/
+/*
+g sensor data for compass tilt compensation
+*/
+static ssize_t show_cpsdata_value(struct device_driver *ddri, char *buf)
+{
+	struct i2c_client *client = bma250_i2c_client;
+	char strbuf[BMA250_BUFSIZE];
+	
+	if(NULL == client)
+	{
+		GSE_ERR("i2c client is null!!\n");
+		return 0;
+	}
+	BMA250_CompassReadData(client, strbuf, BMA250_BUFSIZE);
+	return snprintf(buf, PAGE_SIZE, "%s\n", strbuf);            
+}
+
+/*----------------------------------------------------------------------------*/
+static ssize_t show_sensordata_value(struct device_driver *ddri, char *buf)
+{
+	struct i2c_client *client = bma250_i2c_client;
+	char strbuf[BMA250_BUFSIZE];
+	
+	if(NULL == client)
+	{
+		GSE_ERR("i2c client is null!!\n");
+		return 0;
+	}
+	BMA250_ReadSensorData(client, strbuf, BMA250_BUFSIZE);
+	//BMA250_ReadRawData(client, strbuf);
+	return snprintf(buf, PAGE_SIZE, "%s\n", strbuf);            
+}
+
+/*
+static ssize_t show_sensorrawdata_value(struct device_driver *ddri, char *buf, size_t count)
+	{
+		struct i2c_client *client = bma250_i2c_client;
+		char strbuf[BMA250_BUFSIZE];
+		
+		if(NULL == client)
+		{
+			GSE_ERR("i2c client is null!!\n");
+			return 0;
+		}
+		//BMA250_ReadSensorData(client, strbuf, BMA250_BUFSIZE);
+		BMA250_ReadRawData(client, strbuf);
+		return snprintf(buf, PAGE_SIZE, "%s\n", strbuf);			
+	}
+*/
+/*----------------------------------------------------------------------------*/
+static ssize_t show_cali_value(struct device_driver *ddri, char *buf)
+{
+	struct i2c_client *client = bma250_i2c_client;
+	struct bma250_i2c_data *obj;
+	int err, len = 0, mul;
+	int tmp[BMA250_AXES_NUM];
+
+	if(NULL == client)
+	{
+		GSE_ERR("i2c client is null!!\n");
+		return 0;
+	}
+
+	obj = i2c_get_clientdata(client);
+
+
+
+	if((err = BMA250_ReadOffset(client, obj->offset)))
+	{
+		return -EINVAL;
+	}
+	else if((err = BMA250_ReadCalibration(client, tmp)))
+	{
+		return -EINVAL;
+	}
+	else
+	{    
+		mul = obj->reso->sensitivity/bma250_offset_resolution.sensitivity;
+		len += snprintf(buf+len, PAGE_SIZE-len, "[HW ][%d] (%+3d, %+3d, %+3d) : (0x%02X, 0x%02X, 0x%02X)\n", mul,                        
+			obj->offset[BMA250_AXIS_X], obj->offset[BMA250_AXIS_Y], obj->offset[BMA250_AXIS_Z],
+			obj->offset[BMA250_AXIS_X], obj->offset[BMA250_AXIS_Y], obj->offset[BMA250_AXIS_Z]);
+		len += snprintf(buf+len, PAGE_SIZE-len, "[SW ][%d] (%+3d, %+3d, %+3d)\n", 1, 
+			obj->cali_sw[BMA250_AXIS_X], obj->cali_sw[BMA250_AXIS_Y], obj->cali_sw[BMA250_AXIS_Z]);
+
+		len += snprintf(buf+len, PAGE_SIZE-len, "[ALL]    (%+3d, %+3d, %+3d) : (%+3d, %+3d, %+3d)\n", 
+			obj->offset[BMA250_AXIS_X]*mul + obj->cali_sw[BMA250_AXIS_X],
+			obj->offset[BMA250_AXIS_Y]*mul + obj->cali_sw[BMA250_AXIS_Y],
+			obj->offset[BMA250_AXIS_Z]*mul + obj->cali_sw[BMA250_AXIS_Z],
+			tmp[BMA250_AXIS_X], tmp[BMA250_AXIS_Y], tmp[BMA250_AXIS_Z]);
+		
+		return len;
+    }
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t store_cali_value(struct device_driver *ddri, const char *buf, size_t count)
+{
+	struct i2c_client *client = bma250_i2c_client;  
+	int err, x, y, z;
+	int dat[BMA250_AXES_NUM];
+
+	if(!strncmp(buf, "rst", 3))
+	{
+		if((err = BMA250_ResetCalibration(client)))
+		{
+			GSE_ERR("reset offset err = %d\n", err);
+		}	
+	}
+	else if(3 == sscanf(buf, "0x%02X 0x%02X 0x%02X", &x, &y, &z))
+	{
+		dat[BMA250_AXIS_X] = x;
+		dat[BMA250_AXIS_Y] = y;
+		dat[BMA250_AXIS_Z] = z;
+		if((err = BMA250_WriteCalibration(client, dat)))
+		{
+			GSE_ERR("write calibration err = %d\n", err);
+		}		
+	}
+	else
+	{
+		GSE_ERR("invalid format\n");
+	}
+	
+	return count;
+}
+
+
+/*----------------------------------------------------------------------------*/
+static ssize_t show_firlen_value(struct device_driver *ddri, char *buf)
+{
+#ifdef CONFIG_BMA250_LOWPASS
+	struct i2c_client *client = bma250_i2c_client;
+	struct bma250_i2c_data *obj = i2c_get_clientdata(client);
+	if(atomic_read(&obj->firlen))
+	{
+		int idx, len = atomic_read(&obj->firlen);
+		GSE_LOG("len = %2d, idx = %2d\n", obj->fir.num, obj->fir.idx);
+
+		for(idx = 0; idx < len; idx++)
+		{
+			GSE_LOG("[%5d %5d %5d]\n", obj->fir.raw[idx][BMA250_AXIS_X], obj->fir.raw[idx][BMA250_AXIS_Y], obj->fir.raw[idx][BMA250_AXIS_Z]);
+		}
+		
+		GSE_LOG("sum = [%5d %5d %5d]\n", obj->fir.sum[BMA250_AXIS_X], obj->fir.sum[BMA250_AXIS_Y], obj->fir.sum[BMA250_AXIS_Z]);
+		GSE_LOG("avg = [%5d %5d %5d]\n", obj->fir.sum[BMA250_AXIS_X]/len, obj->fir.sum[BMA250_AXIS_Y]/len, obj->fir.sum[BMA250_AXIS_Z]/len);
+	}
+	return snprintf(buf, PAGE_SIZE, "%d\n", atomic_read(&obj->firlen));
+#else
+	return snprintf(buf, PAGE_SIZE, "not support\n");
+#endif
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t store_firlen_value(struct device_driver *ddri, const char *buf, size_t count)
+{
+#ifdef CONFIG_BMA250_LOWPASS
+	struct i2c_client *client = bma250_i2c_client;  
+	struct bma250_i2c_data *obj = i2c_get_clientdata(client);
+	int firlen;
+
+	if(1 != sscanf(buf, "%d", &firlen))
+	{
+		GSE_ERR("invallid format\n");
+	}
+	else if(firlen > C_MAX_FIR_LENGTH)
+	{
+		GSE_ERR("exceeds maximum filter length\n");
+	}
+	else
+	{ 
+		atomic_set(&obj->firlen, firlen);
+		if(NULL == firlen)
+		{
+			atomic_set(&obj->fir_en, 0);
+		}
+		else
+		{
+			memset(&obj->fir, 0x00, sizeof(obj->fir));
+			atomic_set(&obj->fir_en, 1);
+		}
+	}
+#endif    
+	return count;
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t show_trace_value(struct device_driver *ddri, char *buf)
+{
+	ssize_t res;
+	struct bma250_i2c_data *obj = obj_i2c_data;
+	if (obj == NULL)
+	{
+		GSE_ERR("i2c_data obj is null!!\n");
+		return 0;
+	}
+	
+	res = snprintf(buf, PAGE_SIZE, "0x%04X\n", atomic_read(&obj->trace));     
+	return res;    
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t store_trace_value(struct device_driver *ddri, const char *buf, size_t count)
+{
+	struct bma250_i2c_data *obj = obj_i2c_data;
+	int trace;
+	if (obj == NULL)
+	{
+		GSE_ERR("i2c_data obj is null!!\n");
+		return 0;
+	}
+	
+	if(1 == sscanf(buf, "0x%x", &trace))
+	{
+		atomic_set(&obj->trace, trace);
+	}	
+	else
+	{
+		GSE_ERR("invalid content: '%s', length = %d\n", buf, count);
+	}
+	
+	return count;    
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t show_status_value(struct device_driver *ddri, char *buf)
+{
+	ssize_t len = 0;    
+	struct bma250_i2c_data *obj = obj_i2c_data;
+	if (obj == NULL)
+	{
+		GSE_ERR("i2c_data obj is null!!\n");
+		return 0;
+	}	
+	
+	if(obj->hw)
+	{
+		len += snprintf(buf+len, PAGE_SIZE-len, "CUST: %d %d (%d %d)\n", 
+	            obj->hw->i2c_num, obj->hw->direction, obj->hw->power_id, obj->hw->power_vol);   
+	}
+	else
+	{
+		len += snprintf(buf+len, PAGE_SIZE-len, "CUST: NULL\n");
+	}
+	return len;    
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t show_power_status_value(struct device_driver *ddri, char *buf)
+{
+	if(sensor_power)
+		printk("G sensor is in work mode, sensor_power = %d\n", sensor_power);
+	else
+		printk("G sensor is in standby mode, sensor_power = %d\n", sensor_power);
+
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static DRIVER_ATTR(chipinfo,   S_IWUSR | S_IRUGO, show_chipinfo_value,      NULL);
+static DRIVER_ATTR(cpsdata, S_IWUSR | S_IRUGO, show_cpsdata_value,    NULL);
+static DRIVER_ATTR(cpsopmode, S_IRUGO|S_IWUSR, show_cpsopmode_value,    store_cpsopmode_value);
+static DRIVER_ATTR(cpsrange, S_IRUGO|S_IWUSR, show_cpsrange_value,    store_cpsrange_value);
+static DRIVER_ATTR(cpsbandwidth, S_IRUGO|S_IWUSR, show_cpsbandwidth_value,    store_cpsbandwidth_value);
+static DRIVER_ATTR(sensordata, S_IWUSR | S_IRUGO, show_sensordata_value,    NULL);
+static DRIVER_ATTR(cali,       S_IWUSR | S_IRUGO, show_cali_value,          store_cali_value);
+static DRIVER_ATTR(firlen,     S_IWUSR | S_IRUGO, show_firlen_value,        store_firlen_value);
+static DRIVER_ATTR(trace,      S_IWUSR | S_IRUGO, show_trace_value,         store_trace_value);
+static DRIVER_ATTR(status,               S_IRUGO, show_status_value,        NULL);
+static DRIVER_ATTR(powerstatus,               S_IRUGO, show_power_status_value,        NULL);
+
+/*----------------------------------------------------------------------------*/
+static struct driver_attribute *bma250_attr_list[] = {
+	&driver_attr_chipinfo,     /*chip information*/
+	&driver_attr_sensordata,   /*dump sensor data*/
+	&driver_attr_cali,         /*show calibration data*/
+	&driver_attr_firlen,       /*filter length: 0: disable, others: enable*/
+	&driver_attr_trace,        /*trace log*/
+	&driver_attr_status,
+	&driver_attr_powerstatus,
+	&driver_attr_cpsdata,	/*g sensor data for compass tilt compensation*/
+	&driver_attr_cpsopmode,	/*g sensor opmode for compass tilt compensation*/
+	&driver_attr_cpsrange,	/*g sensor range for compass tilt compensation*/
+	&driver_attr_cpsbandwidth,	/*g sensor bandwidth for compass tilt compensation*/
+};
+/*----------------------------------------------------------------------------*/
+static int bma250_create_attr(struct device_driver *driver) 
+{
+	int idx, err = 0;
+	int num = (int)(sizeof(bma250_attr_list)/sizeof(bma250_attr_list[0]));
+	if (driver == NULL)
+	{
+		return -EINVAL;
+	}
+
+	for(idx = 0; idx < num; idx++)
+	{
+		if((err = driver_create_file(driver, bma250_attr_list[idx])))
+		{            
+			GSE_ERR("driver_create_file (%s) = %d\n", bma250_attr_list[idx]->attr.name, err);
+			break;
+		}
+	}    
+	return err;
+}
+/*----------------------------------------------------------------------------*/
+static int bma250_delete_attr(struct device_driver *driver)
+{
+	int idx ,err = 0;
+	int num = (int)(sizeof(bma250_attr_list)/sizeof(bma250_attr_list[0]));
+
+	if(driver == NULL)
+	{
+		return -EINVAL;
+	}
+	
+
+	for(idx = 0; idx < num; idx++)
+	{
+		driver_remove_file(driver, bma250_attr_list[idx]);
+	}
+	
+
+	return err;
+}
+
+/*----------------------------------------------------------------------------*/
+int gsensor_operate(void* self, uint32_t command, void* buff_in, int size_in,
+		void* buff_out, int size_out, int* actualout)
+{
+	int err = 0;
+	int value, sample_delay;	
+	struct bma250_i2c_data *priv = (struct bma250_i2c_data*)self;
+	hwm_sensor_data* gsensor_data;
+	char buff[BMA250_BUFSIZE];
+	
+	//GSE_FUN(f);
+	switch (command)
+	{
+		case SENSOR_DELAY:
+			if((buff_in == NULL) || (size_in < sizeof(int)))
+			{
+				GSE_ERR("Set delay parameter error!\n");
+				err = -EINVAL;
+			}
+			else
+			{
+				value = *(int *)buff_in;
+				if(value <= 5)
+				{
+					sample_delay = BMA250_BW_200HZ;
+				}
+				else if(value <= 10)
+				{
+					sample_delay = BMA250_BW_100HZ;
+				}
+				else
+				{
+					sample_delay = BMA250_BW_50HZ;
+				}
+				
+				err = BMA250_SetBWRate(priv->client, sample_delay);
+				if(err != BMA250_SUCCESS ) //0x2C->BW=100Hz
+				{
+					GSE_ERR("Set delay parameter error!\n");
+				}
+
+				if(value >= 50)
+				{
+					atomic_set(&priv->filter, 0);
+				}
+				else
+				{	
+				#if defined(CONFIG_BMA250_LOWPASS)
+					priv->fir.num = 0;
+					priv->fir.idx = 0;
+					priv->fir.sum[BMA250_AXIS_X] = 0;
+					priv->fir.sum[BMA250_AXIS_Y] = 0;
+					priv->fir.sum[BMA250_AXIS_Z] = 0;
+					atomic_set(&priv->filter, 1);
+				#endif
+				}
+			}
+			break;
+
+		case SENSOR_ENABLE:
+			if((buff_in == NULL) || (size_in < sizeof(int)))
+			{
+				GSE_ERR("Enable sensor parameter error!\n");
+				err = -EINVAL;
+			}
+			else
+			{
+				value = *(int *)buff_in;
+				mutex_lock(&bma050_op_mutex);
+				GSE_LOG("Gsensor enable_status value = %d,sensor_power=%d\n",value,sensor_power);
+				if(((value == 0) && (sensor_power == false)) ||((value == 1) && (sensor_power == true)))
+				{
+					GSE_LOG("Gsensor device have updated!\n");
+					enable_status = sensor_power;
+				}
+				else
+				{
+				    enable_status = !sensor_power;
+					err = BMA250_SetPowerMode( priv->client, !sensor_power);
+					
+				}
+				GSE_LOG("Gsensor enable_status = %d\n",enable_status);
+				mutex_unlock(&bma050_op_mutex);
+			}
+			break;
+
+		case SENSOR_GET_DATA:
+			if((buff_out == NULL) || (size_out< sizeof(hwm_sensor_data)))
+			{
+				GSE_ERR("get sensor data parameter error!\n");
+				err = -EINVAL;
+			}
+			else
+			{
+				gsensor_data = (hwm_sensor_data *)buff_out;
+				BMA250_ReadSensorData(priv->client, buff, BMA250_BUFSIZE);
+				sscanf(buff, "%x %x %x", &gsensor_data->values[0], 
+					&gsensor_data->values[1], &gsensor_data->values[2]);				
+				gsensor_data->status = SENSOR_STATUS_ACCURACY_MEDIUM;				
+				gsensor_data->value_divide = 1000;
+			}
+			break;
+		default:
+			GSE_ERR("gsensor operate function no this parameter %d!\n", command);
+			err = -1;
+			break;
+	}
+	
+	return err;
+}
+
+/****************************************************************************** 
+ * Function Configuration
+******************************************************************************/
+static int bma250_open(struct inode *inode, struct file *file)
+{
+	file->private_data = bma250_i2c_client;
+
+	if(file->private_data == NULL)
+	{
+		GSE_ERR("null pointer!!\n");
+		return -EINVAL;
+	}
+	return nonseekable_open(inode, file);
+}
+/*----------------------------------------------------------------------------*/
+static int bma250_release(struct inode *inode, struct file *file)
+{
+	file->private_data = NULL;
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+//static int bma250_ioctl(struct inode *inode, struct file *file, unsigned int cmd,
+//       unsigned long arg)
+static long bma250_unlocked_ioctl(struct file *file, unsigned int cmd,unsigned long arg)       
+{
+	struct i2c_client *client = (struct i2c_client*)file->private_data;
+	struct bma250_i2c_data *obj = (struct bma250_i2c_data*)i2c_get_clientdata(client);	
+	char strbuf[BMA250_BUFSIZE];
+	void __user *data;
+	SENSOR_DATA sensor_data;
+	long err = 0;
+	int cali[3];
+
+	//GSE_FUN(f);
+	if(_IOC_DIR(cmd) & _IOC_READ)
+	{
+		err = !access_ok(VERIFY_WRITE, (void __user *)arg, _IOC_SIZE(cmd));
+	}
+	else if(_IOC_DIR(cmd) & _IOC_WRITE)
+	{
+		err = !access_ok(VERIFY_READ, (void __user *)arg, _IOC_SIZE(cmd));
+	}
+
+	if(err)
+	{
+		GSE_ERR("access error: %08X, (%2d, %2d)\n", cmd, _IOC_DIR(cmd), _IOC_SIZE(cmd));
+		return -EFAULT;
+	}
+
+	switch(cmd)
+	{
+		case GSENSOR_IOCTL_INIT:
+			bma250_init_client(client, 0);			
+			break;
+
+		case GSENSOR_IOCTL_READ_CHIPINFO:
+			data = (void __user *) arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}
+			
+			BMA250_ReadChipInfo(client, strbuf, BMA250_BUFSIZE);
+			if(copy_to_user(data, strbuf, strlen(strbuf)+1))
+			{
+				err = -EFAULT;
+				break;
+			}				 
+			break;	  
+
+		case GSENSOR_IOCTL_READ_SENSORDATA:
+			data = (void __user *) arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}
+			BMA250_SetPowerMode(client,true);
+			BMA250_ReadSensorData(client, strbuf, BMA250_BUFSIZE);
+			if(copy_to_user(data, strbuf, strlen(strbuf)+1))
+			{
+				err = -EFAULT;
+				break;	  
+			}				 
+			break;
+
+		case GSENSOR_IOCTL_READ_GAIN:
+			data = (void __user *) arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}			
+			
+			if(copy_to_user(data, &gsensor_gain, sizeof(GSENSOR_VECTOR3D)))
+			{
+				err = -EFAULT;
+				break;
+			}				 
+			break;
+
+		case GSENSOR_IOCTL_READ_RAW_DATA:
+			data = (void __user *) arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}
+			BMA250_ReadRawData(client, strbuf);
+			if(copy_to_user(data, &strbuf, strlen(strbuf)+1))
+			{
+				err = -EFAULT;
+				break;	  
+			}
+			break;	  
+
+		case GSENSOR_IOCTL_SET_CALI:
+			data = (void __user*)arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}
+			if(copy_from_user(&sensor_data, data, sizeof(sensor_data)))
+			{
+				err = -EFAULT;
+				break;	  
+			}
+			if(atomic_read(&obj->suspend))
+			{
+				GSE_ERR("Perform calibration in suspend state!!\n");
+				err = -EINVAL;
+			}
+			else
+			{
+			#if 0
+				cali[BMA250_AXIS_X] = sensor_data.x * obj->reso->sensitivity / GRAVITY_EARTH_1000;
+				cali[BMA250_AXIS_Y] = sensor_data.y * obj->reso->sensitivity / GRAVITY_EARTH_1000;
+				cali[BMA250_AXIS_Z] = sensor_data.z * obj->reso->sensitivity / GRAVITY_EARTH_1000;			  
+			#else
+			cali[BMA250_AXIS_X] = sensor_data.x;
+			cali[BMA250_AXIS_Y] = sensor_data.y;
+			cali[BMA250_AXIS_Z] = sensor_data.z;			  
+						
+
+			#endif
+				err = BMA250_WriteCalibration(client, cali);			 
+			}
+			break;
+
+		case GSENSOR_IOCTL_CLR_CALI:
+			err = BMA250_ResetCalibration(client);
+			break;
+
+		case GSENSOR_IOCTL_GET_CALI:
+			data = (void __user*)arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}
+			if((err = BMA250_ReadCalibration(client, cali)))
+			{
+				break;
+			}
+			#if 0
+			sensor_data.x = cali[BMA250_AXIS_X] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+			sensor_data.y = cali[BMA250_AXIS_Y] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+			sensor_data.z = cali[BMA250_AXIS_Z] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+			#else
+			sensor_data.x = cali[BMA250_AXIS_X];
+			sensor_data.y = cali[BMA250_AXIS_Y];
+			sensor_data.z = cali[BMA250_AXIS_Z];
+						
+			#endif
+			if(copy_to_user(data, &sensor_data, sizeof(sensor_data)))
+			{
+				err = -EFAULT;
+				break;
+			}		
+			break;
+		
+
+		default:
+			GSE_ERR("unknown IOCTL: 0x%08x\n", cmd);
+			err = -ENOIOCTLCMD;
+			break;
+			
+	}
+
+	return err;
+}
+
+
+/*----------------------------------------------------------------------------*/
+static struct file_operations bma250_fops = {
+	//.owner = THIS_MODULE,
+	.open = bma250_open,
+	.release = bma250_release,
+	.unlocked_ioctl = bma250_unlocked_ioctl,
+};
+/*----------------------------------------------------------------------------*/
+static struct miscdevice bma250_device = {
+	.minor = MISC_DYNAMIC_MINOR,
+	.name = "gsensor",
+	.fops = &bma250_fops,
+};
+/*----------------------------------------------------------------------------*/
+#ifndef USE_EARLY_SUSPEND
+/*----------------------------------------------------------------------------*/
+static int bma250_suspend(struct i2c_client *client, pm_message_t msg) 
+{
+	struct bma250_i2c_data *obj = i2c_get_clientdata(client);    
+	int err = 0;
+	GSE_FUN();    
+    mutex_lock(&bma050_op_mutex);
+	if(msg.event == PM_EVENT_SUSPEND)
+	{   
+		if(obj == NULL)
+		{
+			GSE_ERR("null pointer!!\n");
+			mutex_unlock(&bma050_op_mutex);
+			return -EINVAL;
+		}
+		atomic_set(&obj->suspend, 1);
+		if((err = BMA250_SetPowerMode(obj->client, false)))
+		{
+			GSE_ERR("write power control fail!!\n");
+			mutex_unlock(&bma050_op_mutex);
+			return -EINVAL;
+		}
+		sensor_power = false;
+		BMA250_power(obj->hw, 0);
+	}
+	mutex_unlock(&bma050_op_mutex);
+	return err;
+}
+/*----------------------------------------------------------------------------*/
+static int bma250_resume(struct i2c_client *client)
+{
+	struct bma250_i2c_data *obj = i2c_get_clientdata(client);        
+	int err;
+	GSE_FUN();
+	udelay(500);//for fix resume check device id error
+	if(obj == NULL)
+	{
+		GSE_ERR("null pointer!!\n");
+		return -EINVAL;
+	}
+    mutex_lock(&bma050_op_mutex);
+	BMA250_power(obj->hw, 1);
+	if((err = bma250_init_client(client, 0)))
+	{
+		GSE_ERR("initialize client fail!!\n");
+		mutex_unlock(&bma050_op_mutex);
+		return err;        
+	}
+	atomic_set(&obj->suspend, 0);
+    mutex_unlock(&bma050_op_mutex);
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+#else /*CONFIG_HAS_EARLY_SUSPEND is defined*/
+/*----------------------------------------------------------------------------*/
+static void bma250_early_suspend(struct early_suspend *h) 
+{
+	struct bma250_i2c_data *obj = container_of(h, struct bma250_i2c_data, early_drv);   
+	int err;
+	GSE_FUN();    
+
+	if(obj == NULL)
+	{
+		GSE_ERR("null pointer!!\n");
+		return;
+	}
+	atomic_set(&obj->suspend, 1); 
+	if((err = BMA250_SetPowerMode(obj->client, false)))
+	{
+		GSE_ERR("write power control fail!!\n");
+		return;
+	}
+
+	sensor_power = false;
+	
+	BMA250_power(obj->hw, 0);
+}
+/*----------------------------------------------------------------------------*/
+static void bma250_late_resume(struct early_suspend *h)
+{
+	struct bma250_i2c_data *obj = container_of(h, struct bma250_i2c_data, early_drv);         
+	int err;
+	GSE_FUN();
+
+	if(obj == NULL)
+	{
+		GSE_ERR("null pointer!!\n");
+		return;
+	}
+
+	BMA250_power(obj->hw, 1);
+	if((err = bma250_init_client(obj->client, 0)))
+	{
+		GSE_ERR("initialize client fail!!\n");
+		return;        
+	}
+	atomic_set(&obj->suspend, 0);    
+}
+/*----------------------------------------------------------------------------*/
+#endif /*USE_EARLY_SUSPEND*/
+/*----------------------------------------------------------------------------*/
+//static int bma250_i2c_detect(struct i2c_client *client, int kind, struct i2c_board_info *info) 
+//{    
+//	strcpy(info->type, BMA250_DEV_NAME);
+//	return 0;
+//}
+
+/*----------------------------------------------------------------------------*/
+static int bma250_i2c_probe(struct i2c_client *client, const struct i2c_device_id *id)
+{
+	struct i2c_client *new_client;
+	struct bma250_i2c_data *obj;
+	struct hwmsen_object sobj;
+	int err = 0;
+	int retry = 0;
+	GSE_FUN();
+
+	if(!(obj = kzalloc(sizeof(*obj), GFP_KERNEL)))
+	{
+		err = -ENOMEM;
+		goto exit;
+	}
+	
+	memset(obj, 0, sizeof(struct bma250_i2c_data));
+
+	obj->hw = get_cust_acc_hw();
+	
+	if((err = hwmsen_get_convert(obj->hw->direction, &obj->cvt)))
+	{
+		GSE_ERR("invalid direction: %d\n", obj->hw->direction);
+		goto exit;
+	}
+
+	obj_i2c_data = obj;
+	obj->client = client;
+	new_client = obj->client;
+	i2c_set_clientdata(new_client,obj);
+	
+	atomic_set(&obj->trace, 0);
+	atomic_set(&obj->suspend, 0);
+	
+#ifdef CONFIG_BMA250_LOWPASS
+	if(obj->hw->firlen > C_MAX_FIR_LENGTH)
+	{
+		atomic_set(&obj->firlen, C_MAX_FIR_LENGTH);
+	}	
+	else
+	{
+		atomic_set(&obj->firlen, obj->hw->firlen);
+	}
+	
+	if(atomic_read(&obj->firlen) > 0)
+	{
+		atomic_set(&obj->fir_en, 1);
+	}
+	
+#endif
+
+	bma250_i2c_client = new_client;	
+
+	for(retry = 0; retry < 3; retry++){
+		err = bma250_init_client(new_client, 1);
+		if(err == 0)
+		{
+			GSE_LOG("init client done\n");
+			break;
+		}
+		GSE_LOG("init client fail\n");
+	}
+
+	if(err != 0)
+		goto exit_init_failed;
+		
+	if((err = misc_register(&bma250_device)))
+	{
+		GSE_ERR("bma250_device register failed\n");
+		goto exit_misc_device_register_failed;
+	}
+
+	if((err = bma250_create_attr(&bma250_gsensor_driver.driver)))
+	{
+		GSE_ERR("create attribute err = %d\n", err);
+		goto exit_create_attr_failed;
+	}
+
+	sobj.self = obj;
+    sobj.polling = 1;
+    sobj.sensor_operate = gsensor_operate;
+	if((err = hwmsen_attach(ID_ACCELEROMETER, &sobj)))
+	{
+		GSE_ERR("attach fail = %d\n", err);
+		goto exit_kfree;
+	}
+
+#ifdef USE_EARLY_SUSPEND
+	obj->early_drv.level    = EARLY_SUSPEND_LEVEL_DISABLE_FB - 1,
+	obj->early_drv.suspend  = bma250_early_suspend,
+	obj->early_drv.resume   = bma250_late_resume,    
+	register_early_suspend(&obj->early_drv);
+#endif 
+
+	GSE_LOG("%s: OK\n", __func__);    
+	return 0;
+
+	exit_create_attr_failed:
+	misc_deregister(&bma250_device);
+	exit_misc_device_register_failed:
+	exit_init_failed:
+	//i2c_detach_client(new_client);
+	exit_kfree:
+	kfree(obj);
+	exit:
+	GSE_ERR("%s: err = %d\n", __func__, err);        
+	return err;
+}
+
+/*----------------------------------------------------------------------------*/
+static int bma250_i2c_remove(struct i2c_client *client)
+{
+	int err = 0;	
+	
+	if((err = bma250_delete_attr(&bma250_gsensor_driver.driver)))
+	{
+		GSE_ERR("bma150_delete_attr fail: %d\n", err);
+	}
+	
+	if((err = misc_deregister(&bma250_device)))
+	{
+		GSE_ERR("misc_deregister fail: %d\n", err);
+	}
+
+	if((err = hwmsen_detach(ID_ACCELEROMETER)))
+	    
+
+	bma250_i2c_client = NULL;
+	i2c_unregister_device(client);
+	kfree(i2c_get_clientdata(client));
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int bma250_probe(struct platform_device *pdev) 
+{
+	struct acc_hw *hw = get_cust_acc_hw();
+	GSE_FUN();
+
+	BMA250_power(hw, 1);
+	//bma250_force[0] = hw->i2c_num;
+	if(i2c_add_driver(&bma250_i2c_driver))
+	{
+		GSE_ERR("add driver error\n");
+		return -1;
+	}
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int bma250_remove(struct platform_device *pdev)
+{
+    struct acc_hw *hw = get_cust_acc_hw();
+
+    GSE_FUN();    
+    BMA250_power(hw, 0);    
+    i2c_del_driver(&bma250_i2c_driver);
+    return 0;
+}
+/*----------------------------------------------------------------------------*/
+
+#ifdef CONFIG_OF
+static const struct of_device_id gsensor_of_match[] = {
+	{ .compatible = "mediatek,gsensor", },
+	{},
+};
+#endif
+
+static struct platform_driver bma250_gsensor_driver = {
+	.probe      = bma250_probe,
+	.remove     = bma250_remove,    
+	.driver     = 
+	{
+		.name  = "gsensor",
+        #ifdef CONFIG_OF
+		.of_match_table = gsensor_of_match,
+		#endif
+	}
+};
+
+/*----------------------------------------------------------------------------*/
+static int __init bma250_init(void)
+{
+	//GSE_FUN();
+	struct acc_hw *hw = get_cust_acc_hw();
+	GSE_LOG("%s: i2c_number=%d\n", __func__,hw->i2c_num); 
+	i2c_register_board_info(hw->i2c_num, &i2c_bma250, 1);
+	if(platform_driver_register(&bma250_gsensor_driver))
+	{
+		GSE_ERR("failed to register driver");
+		return -ENODEV;
+	}
+	return 0;    
+}
+/*----------------------------------------------------------------------------*/
+static void __exit bma250_exit(void)
+{
+	GSE_FUN();
+	platform_driver_unregister(&bma250_gsensor_driver);
+}
+/*----------------------------------------------------------------------------*/
+module_init(bma250_init);
+module_exit(bma250_exit);
+/*----------------------------------------------------------------------------*/
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("BMA250 I2C driver");
+MODULE_AUTHOR("hongji.zhou@bosch-sensortec.com");
diff --git a/drivers/misc/mediatek/accelerometer/bma050/bma050.h b/drivers/misc/mediatek/accelerometer/bma050/bma050.h
new file mode 100644
index 000000000..d163c924d
--- /dev/null
+++ b/drivers/misc/mediatek/accelerometer/bma050/bma050.h
@@ -0,0 +1,60 @@
+/* BMA250 motion sensor driver
+ *
+ *
+ * This software program is licensed subject to the GNU General Public License
+ * (GPL).Version 2,June 1991, available at http://www.fsf.org/copyleft/gpl.html
+
+ * (C) Copyright 2011 Bosch Sensortec GmbH
+ * All Rights Reserved
+ */
+
+#ifndef BMA250_H
+#define BMA250_H
+	 
+#include <linux/ioctl.h>
+	 
+	#define BMA250_I2C_SLAVE_WRITE_ADDR		0x30
+	#define BMA250_FIXED_DEVID			0x03
+	
+	 /* BMA250 Register Map  (Please refer to BMA250 Specifications) */
+	#define BMA250_REG_DEVID				0x00
+	#define BMA250_REG_OFSX				0x16
+	#define BMA250_REG_OFSX_HIGH			0x1A
+	#define BMA250_REG_BW_RATE			0x10
+	#define BMA250_BW_MASK				0x1f
+	#define BMA250_BW_200HZ				0x0d
+	#define BMA250_BW_100HZ				0x0c
+	#define BMA250_BW_50HZ				0x0b
+	#define BMA250_BW_25HZ				0x0a
+	#define BMA250_REG_POWER_CTL		0x11		
+	#define BMA250_REG_DATA_FORMAT		0x0f
+	#define BMA250_RANGE_MASK			0x0f
+	#define BMA250_RANGE_2G				0x03
+	#define BMA250_RANGE_4G				0x05
+	#define BMA250_RANGE_8G				0x08
+	#define BMA250_REG_DATAXLOW			0x02
+	#define BMA250_REG_DATA_RESOLUTION	0x14
+	#define BMA250_MEASURE_MODE			0x80	
+	#define BMA250_SELF_TEST           			0x32
+	#define BMA250_SELF_TEST_AXIS_X		0x01
+	#define BMA250_SELF_TEST_AXIS_Y		0x02
+	#define BMA250_SELF_TEST_AXIS_Z		0x03
+	#define BMA250_SELF_TEST_POSITIVE	0x00
+	#define BMA250_SELF_TEST_NEGATIVE	0x04
+	#define BMA250_INT_REG_1           			0x16
+	#define BMA250_INT_REG_2          		 	0x17
+
+	
+#define BMA250_SUCCESS						0
+#define BMA250_ERR_I2C						-1
+#define BMA250_ERR_STATUS					-3
+#define BMA250_ERR_SETUP_FAILURE			-4
+#define BMA250_ERR_GETGSENSORDATA			-5
+#define BMA250_ERR_IDENTIFICATION			-6
+	 
+	 
+	 
+#define BMA250_BUFSIZE				256
+	 
+#endif
+
diff --git a/drivers/misc/mediatek/accelerometer/bma056/Makefile b/drivers/misc/mediatek/accelerometer/bma056/Makefile
new file mode 100644
index 000000000..226a18211
--- /dev/null
+++ b/drivers/misc/mediatek/accelerometer/bma056/Makefile
@@ -0,0 +1,4 @@
+include $(srctree)/drivers/misc/mediatek/Makefile.custom
+
+obj-y	:=  bma056.o
+
diff --git a/drivers/misc/mediatek/accelerometer/bma056/bma056.c b/drivers/misc/mediatek/accelerometer/bma056/bma056.c
new file mode 100644
index 000000000..d8cbaa4c7
--- /dev/null
+++ b/drivers/misc/mediatek/accelerometer/bma056/bma056.c
@@ -0,0 +1,2216 @@
+/* BMA255 motion sensor driver
+ *
+ *
+ * This software program is licensed subject to the GNU General Public License
+ * (GPL).Version 2,June 1991, available at http://www.fsf.org/copyleft/gpl.html
+
+ * (C) Copyright 2011 Bosch Sensortec GmbH
+ * All Rights Reserved
+ */
+
+#include <linux/interrupt.h>
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include <linux/irq.h>
+#include <linux/miscdevice.h>
+#include <asm/uaccess.h>
+#include <linux/delay.h>
+#include <linux/input.h>
+#include <linux/workqueue.h>
+#include <linux/kobject.h>
+#include <linux/earlysuspend.h>
+#include <linux/platform_device.h>
+#include <asm/atomic.h>
+
+
+#include <mach/mt_typedefs.h>
+#include <mach/mt_gpio.h>
+#include <mach/mt_pm_ldo.h>
+
+#include <cust_acc.h>
+#include <linux/hwmsensor.h>
+#include <linux/hwmsen_dev.h>
+#include <linux/sensors_io.h>
+#include "bma056.h"
+#include <linux/hwmsen_helper.h>
+
+
+#define POWER_NONE_MACRO MT65XX_POWER_NONE
+
+
+
+
+/*----------------------------------------------------------------------------*/
+#define I2C_DRIVERID_BMA255 255
+/*----------------------------------------------------------------------------*/
+#define DEBUG 1
+/*----------------------------------------------------------------------------*/
+//#define CONFIG_BMA255_LOWPASS   /*apply low pass filter on output*/       
+#define SW_CALIBRATION
+
+/*----------------------------------------------------------------------------*/
+#define BMA255_AXIS_X          0
+#define BMA255_AXIS_Y          1
+#define BMA255_AXIS_Z          2
+#define BMA255_AXES_NUM        3
+#define BMA255_DATA_LEN        6
+#define BMA255_DEV_NAME        "BMA255"
+
+#define BMA255_MODE_NORMAL      0
+#define BMA255_MODE_LOWPOWER    1
+#define BMA255_MODE_SUSPEND     2
+
+#define BMA255_ACC_X_LSB__POS           4
+#define BMA255_ACC_X_LSB__LEN           4
+#define BMA255_ACC_X_LSB__MSK           0xF0
+//#define BMA255_ACC_X_LSB__REG           BMA255_X_AXIS_LSB_REG
+
+#define BMA255_ACC_X_MSB__POS           0
+#define BMA255_ACC_X_MSB__LEN           8
+#define BMA255_ACC_X_MSB__MSK           0xFF
+//#define BMA255_ACC_X_MSB__REG           BMA255_X_AXIS_MSB_REG
+
+#define BMA255_ACC_Y_LSB__POS           4
+#define BMA255_ACC_Y_LSB__LEN           4
+#define BMA255_ACC_Y_LSB__MSK           0xF0
+//#define BMA255_ACC_Y_LSB__REG           BMA255_Y_AXIS_LSB_REG
+
+#define BMA255_ACC_Y_MSB__POS           0
+#define BMA255_ACC_Y_MSB__LEN           8
+#define BMA255_ACC_Y_MSB__MSK           0xFF
+//#define BMA255_ACC_Y_MSB__REG           BMA255_Y_AXIS_MSB_REG
+
+#define BMA255_ACC_Z_LSB__POS           4
+#define BMA255_ACC_Z_LSB__LEN           4
+#define BMA255_ACC_Z_LSB__MSK           0xF0
+//#define BMA255_ACC_Z_LSB__REG           BMA255_Z_AXIS_LSB_REG
+
+#define BMA255_ACC_Z_MSB__POS           0
+#define BMA255_ACC_Z_MSB__LEN           8
+#define BMA255_ACC_Z_MSB__MSK           0xFF
+//#define BMA255_ACC_Z_MSB__REG           BMA255_Z_AXIS_MSB_REG
+
+#define BMA255_EN_LOW_POWER__POS          6
+#define BMA255_EN_LOW_POWER__LEN          1
+#define BMA255_EN_LOW_POWER__MSK          0x40
+#define BMA255_EN_LOW_POWER__REG          BMA255_REG_POWER_CTL
+
+#define BMA255_EN_SUSPEND__POS            7
+#define BMA255_EN_SUSPEND__LEN            1
+#define BMA255_EN_SUSPEND__MSK            0x80
+#define BMA255_EN_SUSPEND__REG            BMA255_REG_POWER_CTL
+
+#define BMA255_RANGE_SEL__POS             0
+#define BMA255_RANGE_SEL__LEN             4
+#define BMA255_RANGE_SEL__MSK             0x0F
+#define BMA255_RANGE_SEL__REG             BMA255_REG_DATA_FORMAT
+
+#define BMA255_BANDWIDTH__POS             0
+#define BMA255_BANDWIDTH__LEN             5
+#define BMA255_BANDWIDTH__MSK             0x1F
+#define BMA255_BANDWIDTH__REG             BMA255_REG_BW_RATE
+
+#define BMA255_GET_BITSLICE(regvar, bitname)\
+	((regvar & bitname##__MSK) >> bitname##__POS)
+
+#define BMA255_SET_BITSLICE(regvar, bitname, val)\
+	((regvar & ~bitname##__MSK) | ((val<<bitname##__POS)&bitname##__MSK))
+
+/*----------------------------------------------------------------------------*/
+
+/*----------------------------------------------------------------------------*/
+static const struct i2c_device_id bma255_i2c_id[] = {{BMA255_DEV_NAME,0},{}};
+static struct i2c_board_info __initdata bma255_i2c_info ={ I2C_BOARD_INFO(BMA255_DEV_NAME, BMA255_I2C_ADDR)};
+
+/*----------------------------------------------------------------------------*/
+static int bma255_i2c_probe(struct i2c_client *client, const struct i2c_device_id *id); 
+static int bma255_i2c_remove(struct i2c_client *client);
+static int bma255_i2c_detect(struct i2c_client *client, struct i2c_board_info *info);
+#ifndef USE_EARLY_SUSPEND
+static int bma255_suspend(struct i2c_client *client, pm_message_t msg);
+static int bma255_resume(struct i2c_client *client);
+#endif
+
+/*----------------------------------------------------------------------------*/
+typedef enum {
+    BMA_TRC_FILTER  = 0x01,
+    BMA_TRC_RAWDATA = 0x02,
+    BMA_TRC_IOCTL   = 0x04,
+    BMA_TRC_CALI	= 0X08,
+    BMA_TRC_INFO	= 0X10,
+} BMA_TRC;
+/*----------------------------------------------------------------------------*/
+struct scale_factor{
+    u8  whole;
+    u8  fraction;
+};
+/*----------------------------------------------------------------------------*/
+struct data_resolution {
+    struct scale_factor scalefactor;
+    int                 sensitivity;
+};
+/*----------------------------------------------------------------------------*/
+#define C_MAX_FIR_LENGTH (32)
+static bool enable_status = false;
+static DEFINE_MUTEX(bma_i2c_mutex);
+static DEFINE_MUTEX(bma_op_mutex);
+/*----------------------------------------------------------------------------*/
+struct data_filter {
+    s16 raw[C_MAX_FIR_LENGTH][BMA255_AXES_NUM];
+    int sum[BMA255_AXES_NUM];
+    int num;
+    int idx;
+};
+/*----------------------------------------------------------------------------*/
+struct bma255_i2c_data {
+    struct i2c_client *client;
+    struct acc_hw *hw;
+    struct hwmsen_convert   cvt;
+    
+    /*misc*/
+    struct data_resolution *reso;
+    atomic_t                trace;
+    atomic_t                suspend;
+    atomic_t                selftest;
+	atomic_t				filter;
+    s16                     cali_sw[BMA255_AXES_NUM+1];
+
+    /*data*/
+    s8                      offset[BMA255_AXES_NUM+1];  /*+1: for 4-byte alignment*/
+    s16                     data[BMA255_AXES_NUM+1];
+
+#if defined(CONFIG_BMA255_LOWPASS)
+    atomic_t                firlen;
+    atomic_t                fir_en;
+    struct data_filter      fir;
+#endif 
+    /*early suspend*/
+#if defined(USE_EARLY_SUSPEND)
+    struct early_suspend    early_drv;
+#endif     
+};
+/*----------------------------------------------------------------------------*/
+static struct i2c_driver bma255_i2c_driver = {
+    .driver = {
+        .name           = BMA255_DEV_NAME,
+    },
+	.probe      		= bma255_i2c_probe,
+	.remove    			= bma255_i2c_remove,
+	.detect 			= bma255_i2c_detect,
+#if !defined(USE_EARLY_SUSPEND)    
+    .suspend            = bma255_suspend,
+    .resume             = bma255_resume,
+#endif
+	.id_table = bma255_i2c_id,
+};
+
+/*----------------------------------------------------------------------------*/
+static struct i2c_client *bma255_i2c_client = NULL;
+static struct platform_driver bma255_gsensor_driver;
+static struct bma255_i2c_data *obj_i2c_data = NULL;
+static bool sensor_power = true;
+static GSENSOR_VECTOR3D gsensor_gain;
+
+/*----------------------------------------------------------------------------*/
+#define GSE_TAG                  "[Gsensor] "
+#define GSE_FUN(f)               printk(KERN_ERR"%s\n", __FUNCTION__)
+#define GSE_ERR(fmt, args...)    printk(KERN_ERR GSE_TAG"%s %d : "fmt, __FUNCTION__, __LINE__, ##args)
+#define GSE_LOG(fmt, args...)    printk(KERN_ERR GSE_TAG fmt, ##args)
+/*----------------------------------------------------------------------------*/
+static struct data_resolution bma255_data_resolution[1] = {
+ /* combination by {FULL_RES,RANGE}*/
+    {{ 1, 0}, 1024},   // dataformat +/-2g  in 12-bit resolution;  { 1, 0} = 1.0= (2*2*1000)/(2^12);  1024 = (2^12)/(2*2)          
+};
+/*----------------------------------------------------------------------------*/
+static struct data_resolution bma255_offset_resolution = {{1, 0}, 1024};
+
+#define C_I2C_FIFO_SIZE         8        
+static int bma_i2c_read_block(struct i2c_client *client, u8 addr, u8 *data, u8 len)
+{
+    u8 beg = addr;
+	int err;
+	struct i2c_msg msgs[2]={{0},{0}};
+	
+	mutex_lock(&bma_i2c_mutex);
+	
+	msgs[0].addr = client->addr;
+	msgs[0].flags = 0;
+	msgs[0].len =1;
+	msgs[0].buf = &beg;
+
+	msgs[1].addr = client->addr;
+	msgs[1].flags = I2C_M_RD;
+	msgs[1].len =len;
+	msgs[1].buf = data;
+	
+	if (!client)
+	{
+	    mutex_unlock(&bma_i2c_mutex);
+		return -EINVAL;
+	}
+	else if (len > C_I2C_FIFO_SIZE) 
+	{
+		GSE_ERR(" length %d exceeds %d\n", len, C_I2C_FIFO_SIZE);
+		mutex_unlock(&bma_i2c_mutex);
+		return -EINVAL;
+	}
+	err = i2c_transfer(client->adapter, msgs, sizeof(msgs)/sizeof(msgs[0]));
+	if (err != 2) 
+	{
+		GSE_ERR("i2c_transfer error: (%d %p %d) %d\n",addr, data, len, err);
+		err = -EIO;
+	} 
+	else 
+	{
+		err = 0;
+	}
+	mutex_unlock(&bma_i2c_mutex);
+	return err;
+
+}
+
+static int bma_i2c_write_block(struct i2c_client *client, u8 addr, u8 *data, u8 len)
+{   /*because address also occupies one byte, the maximum length for write is 7 bytes*/
+    int err, idx, num;
+    char buf[C_I2C_FIFO_SIZE];
+    err =0;
+	mutex_lock(&bma_i2c_mutex);
+    if (!client)
+    {
+        mutex_unlock(&bma_i2c_mutex);
+        return -EINVAL;
+    }
+    else if (len >= C_I2C_FIFO_SIZE) 
+	{        
+        GSE_ERR(" length %d exceeds %d\n", len, C_I2C_FIFO_SIZE);
+		mutex_unlock(&bma_i2c_mutex);
+        return -EINVAL;
+    }    
+
+    num = 0;
+    buf[num++] = addr;
+    for (idx = 0; idx < len; idx++)
+    {
+        buf[num++] = data[idx];
+    }
+
+    err = i2c_master_send(client, buf, num);
+    if (err < 0)
+	{
+        GSE_ERR("send command error!!\n");
+		mutex_unlock(&bma_i2c_mutex);
+        return -EFAULT;
+    } 
+	mutex_unlock(&bma_i2c_mutex);
+    return err;
+}
+
+/*--------------------BMA255 power control function----------------------------------*/
+static void BMA255_power(struct acc_hw *hw, unsigned int on) 
+{
+	static unsigned int power_on = 0;
+
+	if(hw->power_id != POWER_NONE_MACRO)		// have externel LDO
+	{        
+		GSE_LOG("power %s\n", on ? "on" : "off");
+		if(power_on == on)	// power status not change
+		{
+			GSE_LOG("ignore power control: %d\n", on);
+		}
+		else if(on)	// power on
+		{
+			if(!hwPowerOn(hw->power_id, hw->power_vol, "BMA255"))
+			{
+				GSE_ERR("power on fails!!\n");
+			}
+		}
+		else	// power off
+		{
+			if (!hwPowerDown(hw->power_id, "BMA255"))
+			{
+				GSE_ERR("power off fail!!\n");
+			}			  
+		}
+	}
+	power_on = on;    
+}
+/*----------------------------------------------------------------------------*/
+
+/*----------------------------------------------------------------------------*/
+static int BMA255_SetDataResolution(struct bma255_i2c_data *obj)
+{
+
+/*set g sensor dataresolution here*/
+
+/*BMA255 only can set to 10-bit dataresolution, so do nothing in bma255 driver here*/
+
+/*end of set dataresolution*/
+ 
+ /*we set measure range from -2g to +2g in BMA255_SetDataFormat(client, BMA255_RANGE_2G), 
+                                                    and set 10-bit dataresolution BMA255_SetDataResolution()*/
+                                                    
+ /*so bma255_data_resolution[0] set value as {{ 3, 9}, 256} when declaration, and assign the value to obj->reso here*/  
+
+ 	obj->reso = &bma255_data_resolution[0];
+	return 0;
+	
+/*if you changed the measure range, for example call: BMA255_SetDataFormat(client, BMA255_RANGE_4G), 
+you must set the right value to bma255_data_resolution*/
+
+}
+/*----------------------------------------------------------------------------*/
+static int BMA255_ReadData(struct i2c_client *client, s16 data[BMA255_AXES_NUM])
+{
+	u8 addr = BMA255_REG_DATAXLOW;
+	u8 buf[BMA255_DATA_LEN] = {0};
+	int err = 0;
+
+	if(NULL == client)
+	{
+		err = -EINVAL;
+	}
+	else if((err = bma_i2c_read_block(client, addr, buf, BMA255_DATA_LEN))<0)
+	{
+		GSE_ERR("error: %d\n", err);
+	}
+	else
+	{
+		/* Convert sensor raw data to 16-bit integer */
+		data[BMA255_AXIS_X] = BMA255_GET_BITSLICE(buf[0], BMA255_ACC_X_LSB)
+			|(BMA255_GET_BITSLICE(buf[1],
+						BMA255_ACC_X_MSB)<<BMA255_ACC_X_LSB__LEN);
+		data[BMA255_AXIS_X] = data[BMA255_AXIS_X] << (sizeof(short)*8-(BMA255_ACC_X_LSB__LEN
+					+ BMA255_ACC_X_MSB__LEN));
+		data[BMA255_AXIS_X] = data[BMA255_AXIS_X] >> (sizeof(short)*8-(BMA255_ACC_X_LSB__LEN
+					+ BMA255_ACC_X_MSB__LEN));
+		data[BMA255_AXIS_Y] = BMA255_GET_BITSLICE(buf[2], BMA255_ACC_Y_LSB)
+			| (BMA255_GET_BITSLICE(buf[3],
+						BMA255_ACC_Y_MSB)<<BMA255_ACC_Y_LSB__LEN);
+		data[BMA255_AXIS_Y] = data[BMA255_AXIS_Y] << (sizeof(short)*8-(BMA255_ACC_Y_LSB__LEN
+					+ BMA255_ACC_Y_MSB__LEN));
+		data[BMA255_AXIS_Y] = data[BMA255_AXIS_Y] >> (sizeof(short)*8-(BMA255_ACC_Y_LSB__LEN
+					+ BMA255_ACC_Y_MSB__LEN));
+		data[BMA255_AXIS_Z] = BMA255_GET_BITSLICE(buf[4], BMA255_ACC_Z_LSB)
+			| (BMA255_GET_BITSLICE(buf[5],
+						BMA255_ACC_Z_MSB)<<BMA255_ACC_Z_LSB__LEN);
+		data[BMA255_AXIS_Z] = data[BMA255_AXIS_Z] << (sizeof(short)*8-(BMA255_ACC_Z_LSB__LEN
+					+ BMA255_ACC_Z_MSB__LEN));
+		data[BMA255_AXIS_Z] = data[BMA255_AXIS_Z] >> (sizeof(short)*8-(BMA255_ACC_Z_LSB__LEN
+					+ BMA255_ACC_Z_MSB__LEN));
+
+#ifdef CONFIG_BMA255_LOWPASS
+		if(atomic_read(&priv->filter))
+		{
+			if(atomic_read(&priv->fir_en) && !atomic_read(&priv->suspend))
+			{
+				int idx, firlen = atomic_read(&priv->firlen);   
+				if(priv->fir.num < firlen)
+				{                
+					priv->fir.raw[priv->fir.num][BMA255_AXIS_X] = data[BMA255_AXIS_X];
+					priv->fir.raw[priv->fir.num][BMA255_AXIS_Y] = data[BMA255_AXIS_Y];
+					priv->fir.raw[priv->fir.num][BMA255_AXIS_Z] = data[BMA255_AXIS_Z];
+					priv->fir.sum[BMA255_AXIS_X] += data[BMA255_AXIS_X];
+					priv->fir.sum[BMA255_AXIS_Y] += data[BMA255_AXIS_Y];
+					priv->fir.sum[BMA255_AXIS_Z] += data[BMA255_AXIS_Z];
+					if(atomic_read(&priv->trace) & BMA_TRC_FILTER)
+					{
+						GSE_LOG("add [%2d] [%5d %5d %5d] => [%5d %5d %5d]\n", priv->fir.num,
+							priv->fir.raw[priv->fir.num][BMA255_AXIS_X], priv->fir.raw[priv->fir.num][BMA255_AXIS_Y], priv->fir.raw[priv->fir.num][BMA255_AXIS_Z],
+							priv->fir.sum[BMA255_AXIS_X], priv->fir.sum[BMA255_AXIS_Y], priv->fir.sum[BMA255_AXIS_Z]);
+					}
+					priv->fir.num++;
+					priv->fir.idx++;
+				}
+				else
+				{
+					idx = priv->fir.idx % firlen;
+					priv->fir.sum[BMA255_AXIS_X] -= priv->fir.raw[idx][BMA255_AXIS_X];
+					priv->fir.sum[BMA255_AXIS_Y] -= priv->fir.raw[idx][BMA255_AXIS_Y];
+					priv->fir.sum[BMA255_AXIS_Z] -= priv->fir.raw[idx][BMA255_AXIS_Z];
+					priv->fir.raw[idx][BMA255_AXIS_X] = data[BMA255_AXIS_X];
+					priv->fir.raw[idx][BMA255_AXIS_Y] = data[BMA255_AXIS_Y];
+					priv->fir.raw[idx][BMA255_AXIS_Z] = data[BMA255_AXIS_Z];
+					priv->fir.sum[BMA255_AXIS_X] += data[BMA255_AXIS_X];
+					priv->fir.sum[BMA255_AXIS_Y] += data[BMA255_AXIS_Y];
+					priv->fir.sum[BMA255_AXIS_Z] += data[BMA255_AXIS_Z];
+					priv->fir.idx++;
+					data[BMA255_AXIS_X] = priv->fir.sum[BMA255_AXIS_X]/firlen;
+					data[BMA255_AXIS_Y] = priv->fir.sum[BMA255_AXIS_Y]/firlen;
+					data[BMA255_AXIS_Z] = priv->fir.sum[BMA255_AXIS_Z]/firlen;
+					if(atomic_read(&priv->trace) & BMA_TRC_FILTER)
+					{
+						GSE_LOG("add [%2d] [%5d %5d %5d] => [%5d %5d %5d] : [%5d %5d %5d]\n", idx,
+						priv->fir.raw[idx][BMA255_AXIS_X], priv->fir.raw[idx][BMA255_AXIS_Y], priv->fir.raw[idx][BMA255_AXIS_Z],
+						priv->fir.sum[BMA255_AXIS_X], priv->fir.sum[BMA255_AXIS_Y], priv->fir.sum[BMA255_AXIS_Z],
+						data[BMA255_AXIS_X], data[BMA255_AXIS_Y], data[BMA255_AXIS_Z]);
+					}
+				}
+			}
+		}	
+#endif         
+	}
+	return err;
+}
+/*----------------------------------------------------------------------------*/
+static int BMA255_ReadOffset(struct i2c_client *client, s8 ofs[BMA255_AXES_NUM])
+{    
+	int err = 0;
+#ifdef SW_CALIBRATION
+	ofs[0]=ofs[1]=ofs[2]=0x0;
+#else
+	if((err = bma_i2c_read_block(client, BMA255_REG_OFSX, ofs, BMA255_AXES_NUM))<0)
+	{
+		GSE_ERR("error: %d\n", err);
+	}
+#endif
+	//printk("offesx=%x, y=%x, z=%x",ofs[0],ofs[1],ofs[2]);
+	
+	return err;    
+}
+/*----------------------------------------------------------------------------*/
+static int BMA255_ResetCalibration(struct i2c_client *client)
+{
+	struct bma255_i2c_data *obj = i2c_get_clientdata(client);
+
+	int err = 0;
+	
+	#ifdef SW_CALIBRATION
+		
+	#else
+	u8 ofs[4]={0,0,0,0};
+		if((err = bma_i2c_write_block(client, BMA255_REG_OFSX, ofs, 4))<0)
+		{
+			GSE_ERR("error: %d\n", err);
+		}
+	#endif
+
+	memset(obj->cali_sw, 0x00, sizeof(obj->cali_sw));
+	memset(obj->offset, 0x00, sizeof(obj->offset));
+	return err;    
+}
+/*----------------------------------------------------------------------------*/
+static int BMA255_ReadCalibration(struct i2c_client *client, int dat[BMA255_AXES_NUM])
+{
+    struct bma255_i2c_data *obj = i2c_get_clientdata(client);
+    int err = 0;
+    int mul;
+
+	#ifdef SW_CALIBRATION
+		mul = 0;//only SW Calibration, disable HW Calibration
+	#else
+	    if ((err = BMA255_ReadOffset(client, obj->offset))) {
+        GSE_ERR("read offset fail, %d\n", err);
+        return err;
+    	}    
+    	mul = obj->reso->sensitivity/bma255_offset_resolution.sensitivity;
+	#endif
+
+    dat[obj->cvt.map[BMA255_AXIS_X]] = obj->cvt.sign[BMA255_AXIS_X]*(obj->offset[BMA255_AXIS_X]*mul*GRAVITY_EARTH_1000/(obj->reso->sensitivity) + obj->cali_sw[BMA255_AXIS_X]);
+    dat[obj->cvt.map[BMA255_AXIS_Y]] = obj->cvt.sign[BMA255_AXIS_Y]*(obj->offset[BMA255_AXIS_Y]*mul*GRAVITY_EARTH_1000/(obj->reso->sensitivity) + obj->cali_sw[BMA255_AXIS_Y]);
+    dat[obj->cvt.map[BMA255_AXIS_Z]] = obj->cvt.sign[BMA255_AXIS_Z]*(obj->offset[BMA255_AXIS_Z]*mul*GRAVITY_EARTH_1000/(obj->reso->sensitivity) + obj->cali_sw[BMA255_AXIS_Z]);                        
+                                       
+    return err;
+}
+/*----------------------------------------------------------------------------*/
+static int BMA255_ReadCalibrationEx(struct i2c_client *client, int act[BMA255_AXES_NUM], int raw[BMA255_AXES_NUM])
+{  
+	/*raw: the raw calibration data; act: the actual calibration data*/
+	struct bma255_i2c_data *obj = i2c_get_clientdata(client);
+	//int err;
+	int mul;
+
+	#ifdef SW_CALIBRATION
+		mul = 0;//only SW Calibration, disable HW Calibration
+	#else
+		if(err = BMA255_ReadOffset(client, obj->offset))
+		{
+			GSE_ERR("read offset fail, %d\n", err);
+			return err;
+		}   
+		mul = obj->reso->sensitivity/bma255_offset_resolution.sensitivity;
+	#endif
+	
+	raw[BMA255_AXIS_X] = obj->offset[BMA255_AXIS_X]*mul*GRAVITY_EARTH_1000/(obj->reso->sensitivity) + obj->cali_sw[BMA255_AXIS_X];
+	raw[BMA255_AXIS_Y] = obj->offset[BMA255_AXIS_Y]*mul*GRAVITY_EARTH_1000/(obj->reso->sensitivity) + obj->cali_sw[BMA255_AXIS_Y];
+	raw[BMA255_AXIS_Z] = obj->offset[BMA255_AXIS_Z]*mul*GRAVITY_EARTH_1000/(obj->reso->sensitivity) + obj->cali_sw[BMA255_AXIS_Z];
+
+	act[obj->cvt.map[BMA255_AXIS_X]] = obj->cvt.sign[BMA255_AXIS_X]*raw[BMA255_AXIS_X];
+	act[obj->cvt.map[BMA255_AXIS_Y]] = obj->cvt.sign[BMA255_AXIS_Y]*raw[BMA255_AXIS_Y];
+	act[obj->cvt.map[BMA255_AXIS_Z]] = obj->cvt.sign[BMA255_AXIS_Z]*raw[BMA255_AXIS_Z];                        
+	                       
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int BMA255_WriteCalibration(struct i2c_client *client, int dat[BMA255_AXES_NUM])
+{
+	struct bma255_i2c_data *obj = i2c_get_clientdata(client);
+	int err = 0;
+	int cali[BMA255_AXES_NUM], raw[BMA255_AXES_NUM];
+	//int lsb = bma255_offset_resolution.sensitivity;
+	//int divisor = obj->reso->sensitivity/lsb;
+
+	if((err = BMA255_ReadCalibrationEx(client, cali, raw)))	/*offset will be updated in obj->offset*/
+	{ 
+		GSE_ERR("read offset fail, %d\n", err);
+		return err;
+	}
+
+	GSE_LOG("OLDOFF: (%+3d %+3d %+3d): (%+3d %+3d %+3d) / (%+3d %+3d %+3d)\n", 
+		raw[BMA255_AXIS_X], raw[BMA255_AXIS_Y], raw[BMA255_AXIS_Z],
+		obj->offset[BMA255_AXIS_X], obj->offset[BMA255_AXIS_Y], obj->offset[BMA255_AXIS_Z],
+		obj->cali_sw[BMA255_AXIS_X], obj->cali_sw[BMA255_AXIS_Y], obj->cali_sw[BMA255_AXIS_Z]);
+
+	/*calculate the real offset expected by caller*/
+	cali[BMA255_AXIS_X] += dat[BMA255_AXIS_X];
+	cali[BMA255_AXIS_Y] += dat[BMA255_AXIS_Y];
+	cali[BMA255_AXIS_Z] += dat[BMA255_AXIS_Z];
+
+	GSE_LOG("UPDATE: (%+3d %+3d %+3d)\n", 
+		dat[BMA255_AXIS_X], dat[BMA255_AXIS_Y], dat[BMA255_AXIS_Z]);
+
+#ifdef SW_CALIBRATION
+	obj->cali_sw[BMA255_AXIS_X] = obj->cvt.sign[BMA255_AXIS_X]*(cali[obj->cvt.map[BMA255_AXIS_X]]);
+	obj->cali_sw[BMA255_AXIS_Y] = obj->cvt.sign[BMA255_AXIS_Y]*(cali[obj->cvt.map[BMA255_AXIS_Y]]);
+	obj->cali_sw[BMA255_AXIS_Z] = obj->cvt.sign[BMA255_AXIS_Z]*(cali[obj->cvt.map[BMA255_AXIS_Z]]);	
+#else
+	obj->offset[BMA255_AXIS_X] = (s8)(obj->cvt.sign[BMA255_AXIS_X]*(cali[obj->cvt.map[BMA255_AXIS_X]])*(obj->reso->sensitivity)/GRAVITY_EARTH_1000/(divisor));
+	obj->offset[BMA255_AXIS_Y] = (s8)(obj->cvt.sign[BMA255_AXIS_Y]*(cali[obj->cvt.map[BMA255_AXIS_Y]])*(obj->reso->sensitivity)/GRAVITY_EARTH_1000/(divisor));
+	obj->offset[BMA255_AXIS_Z] = (s8)(obj->cvt.sign[BMA255_AXIS_Z]*(cali[obj->cvt.map[BMA255_AXIS_Z]])*(obj->reso->sensitivity)/GRAVITY_EARTH_1000/(divisor));
+
+	/*convert software calibration using standard calibration*/
+	obj->cali_sw[BMA255_AXIS_X] = obj->cvt.sign[BMA255_AXIS_X]*(cali[obj->cvt.map[BMA255_AXIS_X]])%(divisor);
+	obj->cali_sw[BMA255_AXIS_Y] = obj->cvt.sign[BMA255_AXIS_Y]*(cali[obj->cvt.map[BMA255_AXIS_Y]])%(divisor);
+	obj->cali_sw[BMA255_AXIS_Z] = obj->cvt.sign[BMA255_AXIS_Z]*(cali[obj->cvt.map[BMA255_AXIS_Z]])%(divisor);
+
+	GSE_LOG("NEWOFF: (%+3d %+3d %+3d): (%+3d %+3d %+3d) / (%+3d %+3d %+3d)\n", 
+		obj->offset[BMA255_AXIS_X]*divisor + obj->cali_sw[BMA255_AXIS_X], 
+		obj->offset[BMA255_AXIS_Y]*divisor + obj->cali_sw[BMA255_AXIS_Y], 
+		obj->offset[BMA255_AXIS_Z]*divisor + obj->cali_sw[BMA255_AXIS_Z], 
+		obj->offset[BMA255_AXIS_X], obj->offset[BMA255_AXIS_Y], obj->offset[BMA255_AXIS_Z],
+		obj->cali_sw[BMA255_AXIS_X], obj->cali_sw[BMA255_AXIS_Y], obj->cali_sw[BMA255_AXIS_Z]);
+
+	if((err = bma_i2c_write_block(obj->client, BMA255_REG_OFSX, obj->offset, BMA255_AXES_NUM))<0)
+	{
+		GSE_ERR("write offset fail: %d\n", err);
+		return err;
+	}
+#endif
+
+	return err;
+}
+/*----------------------------------------------------------------------------*/
+static int BMA255_CheckDeviceID(struct i2c_client *client)
+{
+	u8 databuf[2];    
+	int res = 0;
+
+	GSE_FUN();
+	memset(databuf, 0, sizeof(u8)*2);    
+
+    res = bma_i2c_read_block(client,BMA255_REG_DEVID,databuf,0x1);
+	if(res < 0)
+	{
+		goto exit_BMA255_CheckDeviceID;
+	}
+	
+
+	if(databuf[0]!=BMA255_FIXED_DEVID)
+	{
+		GSE_ERR("BMA255_CheckDeviceID fail! ID=%d\n ", databuf[0]);
+		return BMA255_ERR_IDENTIFICATION;
+	}
+	else
+	{
+		GSE_LOG("BMA255_CheckDeviceID %d pass!\n ", databuf[0]);
+	}
+
+	exit_BMA255_CheckDeviceID:
+	if (res < 0)
+	{
+		return BMA255_ERR_I2C;
+	}
+	
+	return BMA255_SUCCESS;
+}
+/*----------------------------------------------------------------------------*/
+static int BMA255_SetPowerMode(struct i2c_client *client, bool enable)
+{
+	u8 databuf[2] = {0};    
+	int res = 0;
+	u8 addr = BMA255_REG_POWER_CTL;
+	struct bma255_i2c_data *obj = i2c_get_clientdata(client);
+	
+	if(enable == sensor_power )
+	{
+		GSE_LOG("Sensor power status is newest!\n");
+		return BMA255_SUCCESS;
+	}
+
+	if((bma_i2c_read_block(client, addr, databuf, 0x01))<0)
+	{
+		GSE_ERR("read power ctl register err!\n");
+		return BMA255_ERR_I2C;
+	}
+
+	if(enable == TRUE)
+	{
+		databuf[0] &= ~BMA255_MEASURE_MODE;
+	}
+	else
+	{
+		databuf[0] |= BMA255_MEASURE_MODE;
+	}
+	
+	res = bma_i2c_write_block(client, BMA255_REG_POWER_CTL, databuf, 0x1);
+	if(res < 0)
+	{
+		GSE_LOG("set power mode failed!\n");
+		return BMA255_ERR_I2C;
+	}
+	else if(atomic_read(&obj->trace) & BMA_TRC_INFO)
+	{
+		GSE_LOG("set power mode ok %d!\n", databuf[1]);
+	}
+
+	sensor_power = enable;
+
+	mdelay(20);
+	
+	return BMA255_SUCCESS;    
+}
+/*----------------------------------------------------------------------------*/
+static int BMA255_SetDataFormat(struct i2c_client *client, u8 dataformat)
+{
+	struct bma255_i2c_data *obj = i2c_get_clientdata(client);
+	u8 databuf[2] = {0};    
+	int res = 0;
+
+	if((bma_i2c_read_block(client, BMA255_REG_DATA_FORMAT, databuf, 0x01))<0)
+	{
+		GSE_ERR("bma255 read Dataformat failt \n");
+		return BMA255_ERR_I2C;
+	}
+
+	databuf[0] &= ~BMA255_RANGE_MASK;
+	databuf[0] |= dataformat;
+
+	res = bma_i2c_write_block(client, BMA255_REG_DATA_FORMAT, databuf, 0x01);
+	if(res < 0)
+	{
+		GSE_ERR("bma255 write Dataformat failt \n");
+		return BMA255_ERR_I2C;
+	}
+	
+	return BMA255_SetDataResolution(obj);    
+}
+/*----------------------------------------------------------------------------*/
+static int BMA255_SetBWRate(struct i2c_client *client, u8 bwrate)
+{
+	u8 databuf[2] = {0};    
+	int res = 0;
+
+	if((bma_i2c_read_block(client, BMA255_REG_BW_RATE, databuf, 0x01))<0)
+	{
+		GSE_ERR("bma255 read rate failt \n");
+		return BMA255_ERR_I2C;
+	}
+
+	databuf[0] &= ~BMA255_BW_MASK;
+	databuf[0] |= bwrate;
+
+	res = bma_i2c_write_block(client, BMA255_REG_BW_RATE, databuf, 0x1);
+	if(res < 0)
+	{
+		GSE_ERR("bma255 write rate failt \n");
+		return BMA255_ERR_I2C;
+	}
+	
+	return BMA255_SUCCESS;    
+}
+/*----------------------------------------------------------------------------*/
+static int BMA255_SetIntEnable(struct i2c_client *client, u8 intenable)
+{
+	int res = 0;
+
+	u8 databuf = 0;
+	res = bma_i2c_write_block(client, BMA255_INT_REG_1, &databuf, 0x01);
+	if(res < 0) 
+	{
+		return res;
+	}
+	res = bma_i2c_write_block(client, BMA255_INT_REG_2, &databuf, 0x01);
+	if(res < 0) 
+	{
+		return res;
+	}
+	GSE_LOG("BMA255 disable interrupt ...\n");
+
+	/*for disable interrupt function*/
+
+	return BMA255_SUCCESS;	  
+}
+
+/*----------------------------------------------------------------------------*/
+static int bma255_init_client(struct i2c_client *client, int reset_cali)
+{
+	struct bma255_i2c_data *obj = i2c_get_clientdata(client);
+	int res = 0;
+	GSE_LOG("bma255_init_client \n");
+
+	res = BMA255_CheckDeviceID(client); 
+	if(res != BMA255_SUCCESS)
+	{
+		return res;
+	}	
+	GSE_LOG("BMA255_CheckDeviceID ok \n");
+	
+	res = BMA255_SetBWRate(client, BMA255_BW_100HZ);
+	if(res != BMA255_SUCCESS ) 
+	{
+		return res;
+	}
+	GSE_LOG("BMA255_SetBWRate OK!\n");
+	
+	res = BMA255_SetDataFormat(client, BMA255_RANGE_2G);
+	if(res != BMA255_SUCCESS) 
+	{
+		return res;
+	}
+	GSE_LOG("BMA255_SetDataFormat OK!\n");
+
+	gsensor_gain.x = gsensor_gain.y = gsensor_gain.z = obj->reso->sensitivity;
+
+	res = BMA255_SetIntEnable(client, 0x00);        
+	if(res != BMA255_SUCCESS)
+	{
+		return res;
+	}
+	GSE_LOG("BMA255 disable interrupt function!\n");
+
+	res = BMA255_SetPowerMode(client, enable_status);
+	if(res != BMA255_SUCCESS)
+	{
+		return res;
+	}
+	GSE_LOG("BMA255_SetPowerMode OK!\n");
+
+	if(0 != reset_cali)
+	{ 
+		/*reset calibration only in power on*/
+		res = BMA255_ResetCalibration(client);
+		if(res != BMA255_SUCCESS)
+		{
+			return res;
+		}
+	}
+	GSE_LOG("bma255_init_client OK!\n");
+#ifdef CONFIG_BMA255_LOWPASS
+	memset(&obj->fir, 0x00, sizeof(obj->fir));  
+#endif
+
+	mdelay(20);
+
+	return BMA255_SUCCESS;
+}
+/*----------------------------------------------------------------------------*/
+static int BMA255_ReadChipInfo(struct i2c_client *client, char *buf, int bufsize)
+{
+	u8 databuf[10];    
+
+	memset(databuf, 0, sizeof(u8)*10);
+
+	if((NULL == buf)||(bufsize<=30))
+	{
+		return -1;
+	}
+	
+	if(NULL == client)
+	{
+		*buf = 0;
+		return -2;
+	}
+
+	sprintf(buf, "BMA056 Chip");
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int BMA255_CompassReadData(struct i2c_client *client, char *buf, int bufsize)
+{
+	struct bma255_i2c_data *obj = (struct bma255_i2c_data*)i2c_get_clientdata(client);
+	u8 databuf[20];
+	int acc[BMA255_AXES_NUM];
+	int res = 0;
+	memset(databuf, 0, sizeof(u8)*10);
+
+	if(NULL == buf)
+	{
+		return -1;
+	}
+	if(NULL == client)
+	{
+		*buf = 0;
+		return -2;
+	}
+
+	if(sensor_power == FALSE)
+	{
+		res = BMA255_SetPowerMode(client, true);
+		if(res)
+		{
+			GSE_ERR("Power on bma255 error %d!\n", res);
+		}
+	}
+
+	if((res = BMA255_ReadData(client, obj->data)))
+	{        
+		GSE_ERR("I2C error: ret value=%d", res);
+		return -3;
+	}
+	else
+	{
+		/*remap coordinate*/
+		acc[obj->cvt.map[BMA255_AXIS_X]] = obj->cvt.sign[BMA255_AXIS_X]*obj->data[BMA255_AXIS_X];
+		acc[obj->cvt.map[BMA255_AXIS_Y]] = obj->cvt.sign[BMA255_AXIS_Y]*obj->data[BMA255_AXIS_Y];
+		acc[obj->cvt.map[BMA255_AXIS_Z]] = obj->cvt.sign[BMA255_AXIS_Z]*obj->data[BMA255_AXIS_Z];
+		//printk("cvt x=%d, y=%d, z=%d \n",obj->cvt.sign[BMA255_AXIS_X],obj->cvt.sign[BMA255_AXIS_Y],obj->cvt.sign[BMA255_AXIS_Z]);
+
+		//GSE_LOG("Mapped gsensor data: %d, %d, %d!\n", acc[BMA255_AXIS_X], acc[BMA255_AXIS_Y], acc[BMA255_AXIS_Z]);
+
+		sprintf(buf, "%d %d %d", (s16)acc[BMA255_AXIS_X], (s16)acc[BMA255_AXIS_Y], (s16)acc[BMA255_AXIS_Z]);
+		if(atomic_read(&obj->trace) & BMA_TRC_IOCTL)
+		{
+			GSE_LOG("gsensor data for compass: %s!\n", buf);
+		}
+	}
+	
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int BMA255_ReadSensorData(struct i2c_client *client, char *buf, int bufsize)
+{
+	struct bma255_i2c_data *obj = (struct bma255_i2c_data*)i2c_get_clientdata(client);
+	u8 databuf[20];
+	int acc[BMA255_AXES_NUM];
+	int res = 0;
+	memset(databuf, 0, sizeof(u8)*10);
+
+	if(NULL == buf)
+	{
+		return -1;
+	}
+	if(NULL == client)
+	{
+		*buf = 0;
+		return -2;
+	}
+	if((res = BMA255_ReadData(client, obj->data)))
+	{        
+		GSE_ERR("I2C error: ret value=%d", res);
+		return -3;
+	}
+	else
+	{
+	#if 1
+		obj->data[BMA255_AXIS_X] = obj->data[BMA255_AXIS_X] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+		obj->data[BMA255_AXIS_Y] = obj->data[BMA255_AXIS_Y] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+		obj->data[BMA255_AXIS_Z] = obj->data[BMA255_AXIS_Z] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;		
+#endif
+		//printk("raw data x=%d, y=%d, z=%d \n",obj->data[BMA255_AXIS_X],obj->data[BMA255_AXIS_Y],obj->data[BMA255_AXIS_Z]);
+		obj->data[BMA255_AXIS_X] += obj->cali_sw[BMA255_AXIS_X];
+		obj->data[BMA255_AXIS_Y] += obj->cali_sw[BMA255_AXIS_Y];
+		obj->data[BMA255_AXIS_Z] += obj->cali_sw[BMA255_AXIS_Z];
+		
+		//printk("cali_sw x=%d, y=%d, z=%d \n",obj->cali_sw[BMA255_AXIS_X],obj->cali_sw[BMA255_AXIS_Y],obj->cali_sw[BMA255_AXIS_Z]);
+		
+		/*remap coordinate*/
+		acc[obj->cvt.map[BMA255_AXIS_X]] = obj->cvt.sign[BMA255_AXIS_X]*obj->data[BMA255_AXIS_X];
+		acc[obj->cvt.map[BMA255_AXIS_Y]] = obj->cvt.sign[BMA255_AXIS_Y]*obj->data[BMA255_AXIS_Y];
+		acc[obj->cvt.map[BMA255_AXIS_Z]] = obj->cvt.sign[BMA255_AXIS_Z]*obj->data[BMA255_AXIS_Z];
+		//printk("cvt x=%d, y=%d, z=%d \n",obj->cvt.sign[BMA255_AXIS_X],obj->cvt.sign[BMA255_AXIS_Y],obj->cvt.sign[BMA255_AXIS_Z]);
+
+		//GSE_LOG("Mapped gsensor data: %d, %d, %d!\n", acc[BMA255_AXIS_X], acc[BMA255_AXIS_Y], acc[BMA255_AXIS_Z]);
+
+		//Out put the mg
+		//printk("mg acc=%d, GRAVITY=%d, sensityvity=%d \n",acc[BMA255_AXIS_X],GRAVITY_EARTH_1000,obj->reso->sensitivity);
+#if 0
+		acc[BMA255_AXIS_X] = acc[BMA255_AXIS_X] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+		acc[BMA255_AXIS_Y] = acc[BMA255_AXIS_Y] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+		acc[BMA255_AXIS_Z] = acc[BMA255_AXIS_Z] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;		
+#endif
+		sprintf(buf, "%04x %04x %04x", acc[BMA255_AXIS_X], acc[BMA255_AXIS_Y], acc[BMA255_AXIS_Z]);
+		if(atomic_read(&obj->trace) & BMA_TRC_IOCTL)
+		{
+			GSE_LOG("gsensor data: %s!\n", buf);
+		}
+	}
+	
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int BMA255_ReadRawData(struct i2c_client *client, char *buf)
+{
+	struct bma255_i2c_data *obj = (struct bma255_i2c_data*)i2c_get_clientdata(client);
+	int res = 0;
+
+	if (!buf || !client)
+	{
+		return EINVAL;
+	}
+	
+	if((res = BMA255_ReadData(client, obj->data)))
+	{        
+		GSE_ERR("I2C error: ret value=%d", res);
+		return EIO;
+	}
+	else
+	{
+		sprintf(buf, "BMA255_ReadRawData %04x %04x %04x", obj->data[BMA255_AXIS_X], 
+			obj->data[BMA255_AXIS_Y], obj->data[BMA255_AXIS_Z]);
+	}
+	
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int bma255_set_mode(struct i2c_client *client, unsigned char mode)
+{
+	int comres = 0;
+	unsigned char data[2] = {BMA255_EN_LOW_POWER__REG};
+
+	if ((client == NULL) || (mode >= 3))
+	{
+		return -1;
+	}
+
+	comres = bma_i2c_read_block(client,
+			BMA255_EN_LOW_POWER__REG, data, 1);
+	switch (mode) {
+	case BMA255_MODE_NORMAL:
+		data[0]  = BMA255_SET_BITSLICE(data[0],
+				BMA255_EN_LOW_POWER, 0);
+		data[0]  = BMA255_SET_BITSLICE(data[0],
+				BMA255_EN_SUSPEND, 0);
+		break;
+	case BMA255_MODE_LOWPOWER:
+		data[0]  = BMA255_SET_BITSLICE(data[0],
+				BMA255_EN_LOW_POWER, 1);
+		data[0]  = BMA255_SET_BITSLICE(data[0],
+				BMA255_EN_SUSPEND, 0);
+		break;
+	case BMA255_MODE_SUSPEND:
+		data[0]  = BMA255_SET_BITSLICE(data[0],
+				BMA255_EN_LOW_POWER, 0);
+		data[0]  = BMA255_SET_BITSLICE(data[0],
+				BMA255_EN_SUSPEND, 1);
+		break;
+	default:
+		break;
+	}
+
+	comres = bma_i2c_write_block(client, BMA255_EN_LOW_POWER__REG, data, 0x1);
+	if(comres < 0)
+	{
+		return BMA255_ERR_I2C;
+	}
+	else
+	{
+		return comres;
+	}
+}
+/*----------------------------------------------------------------------------*/
+static int bma255_get_mode(struct i2c_client *client, unsigned char *mode)
+{
+	int comres = 0;
+
+	if (client == NULL) 
+	{
+		return -1;
+	}
+	comres = bma_i2c_read_block(client,
+			BMA255_EN_LOW_POWER__REG, mode, 0x01);
+	*mode  = (*mode) >> 6;
+		
+	return comres;
+}
+
+/*----------------------------------------------------------------------------*/
+static int bma255_set_range(struct i2c_client *client, unsigned char range)
+{
+	int comres = 0;
+	unsigned char data[2] = {BMA255_RANGE_SEL__REG};
+
+	if (client == NULL)
+	{
+		return -1;
+	}
+	
+	comres = bma_i2c_read_block(client, BMA255_RANGE_SEL__REG, data, 1);
+
+	data[0]  = BMA255_SET_BITSLICE(data[0], BMA255_RANGE_SEL, range);
+
+	comres = bma_i2c_write_block(client, BMA255_RANGE_SEL__REG, data, 0x01);
+	if(comres < 0)
+	{
+		return BMA255_ERR_I2C;
+	}
+	else
+	{
+		return comres;
+	}
+}
+/*----------------------------------------------------------------------------*/
+static int bma255_get_range(struct i2c_client *client, unsigned char *range)
+{
+	int comres = 0;
+	unsigned char data;
+
+	if (client == NULL) 
+	{
+		return -1;
+	}
+
+	comres = bma_i2c_read_block(client, BMA255_RANGE_SEL__REG,	&data, 1);
+	*range = BMA255_GET_BITSLICE(data, BMA255_RANGE_SEL);
+
+	return comres;
+}
+/*----------------------------------------------------------------------------*/
+static int bma255_set_bandwidth(struct i2c_client *client, unsigned char bandwidth)
+{
+	int comres = 0;
+	unsigned char data[2] = {BMA255_BANDWIDTH__REG};
+
+	if (client == NULL)
+	{
+		return -1;
+	}
+	
+	comres = bma_i2c_read_block(client, BMA255_BANDWIDTH__REG, data, 1);
+
+	data[0]  = BMA255_SET_BITSLICE(data[0], BMA255_BANDWIDTH, bandwidth);
+
+	comres = bma_i2c_write_block(client, BMA255_BANDWIDTH__REG, data, 0x01);
+
+	if(comres < 0)
+	{
+		return BMA255_ERR_I2C;
+	}
+	else
+	{
+		return comres;
+	}
+}
+/*----------------------------------------------------------------------------*/
+static int bma255_get_bandwidth(struct i2c_client *client, unsigned char *bandwidth)
+{
+	int comres = 0;
+	unsigned char data;
+
+	if (client == NULL) 
+	{
+		return -1;
+	}
+
+	comres = bma_i2c_read_block(client, BMA255_BANDWIDTH__REG, &data, 1);
+	data = BMA255_GET_BITSLICE(data, BMA255_BANDWIDTH);
+
+	if (data < 0x08) //7.81Hz
+	{
+		*bandwidth = 0x08;
+	}
+	else if (data > 0x0f)	// 1000Hz
+	{
+		*bandwidth = 0x0f;
+	}
+	else
+	{
+		*bandwidth = data;
+	}
+	return comres;
+}
+/*----------------------------------------------------------------------------*/
+
+static ssize_t show_chipinfo_value(struct device_driver *ddri, char *buf)
+{
+	struct i2c_client *client = bma255_i2c_client;
+	char strbuf[BMA255_BUFSIZE];
+	if(NULL == client)
+	{
+		GSE_ERR("i2c client is null!!\n");
+		return 0;
+	}
+	
+	BMA255_ReadChipInfo(client, strbuf, BMA255_BUFSIZE);
+	return snprintf(buf, PAGE_SIZE, "%s\n", strbuf);        
+}
+
+
+/*----------------------------------------------------------------------------*/
+/*
+g sensor opmode for compass tilt compensation
+*/
+static ssize_t show_cpsopmode_value(struct device_driver *ddri, char *buf)
+{
+	unsigned char data;
+
+	if (bma255_get_mode(bma255_i2c_client, &data) < 0)
+	{
+		return sprintf(buf, "Read error\n");
+	}
+	else
+	{
+		return sprintf(buf, "%d\n", data);
+	}
+}
+
+/*----------------------------------------------------------------------------*/
+/*
+g sensor opmode for compass tilt compensation
+*/
+static ssize_t store_cpsopmode_value(struct device_driver *ddri, const char *buf, size_t count)
+{
+	unsigned long data;
+	int error;
+
+	if ((error = strict_strtoul(buf, 10, &data)))
+	{
+		return error;
+	}
+	if (data == BMA255_MODE_NORMAL)
+	{
+		//GSE_ERR("yucong normal mode\n");
+		//BMA255_SetPowerMode(bma255_i2c_client, true);
+	}
+	else if (data == BMA255_MODE_SUSPEND)
+	{
+		//GSE_ERR("yucong suspend mode\n");
+		//BMA255_SetPowerMode(bma255_i2c_client, false);
+	}
+	else if (bma255_set_mode(bma255_i2c_client, (unsigned char) data) < 0)
+	{
+		GSE_ERR("invalid content: '%s', length = %d\n", buf, count);
+	}
+
+	return count;
+}
+
+/*----------------------------------------------------------------------------*/
+/*
+g sensor range for compass tilt compensation
+*/
+static ssize_t show_cpsrange_value(struct device_driver *ddri, char *buf)
+{
+	unsigned char data;
+
+	if (bma255_get_range(bma255_i2c_client, &data) < 0)
+	{
+		return sprintf(buf, "Read error\n");
+	}
+	else
+	{
+		return sprintf(buf, "%d\n", data);
+	}
+}
+
+/*----------------------------------------------------------------------------*/
+/*
+g sensor range for compass tilt compensation
+*/
+static ssize_t store_cpsrange_value(struct device_driver *ddri, const char *buf, size_t count)
+{
+	unsigned long data;
+	int error;
+
+	if ((error = strict_strtoul(buf, 10, &data)))
+	{
+		return error;
+	}
+	if (bma255_set_range(bma255_i2c_client, (unsigned char) data) < 0)
+	{
+		GSE_ERR("invalid content: '%s', length = %d\n", buf, count);
+	}
+
+	return count;
+}
+/*----------------------------------------------------------------------------*/
+/*
+g sensor bandwidth for compass tilt compensation
+*/
+static ssize_t show_cpsbandwidth_value(struct device_driver *ddri, char *buf)
+{
+	unsigned char data;
+
+	if (bma255_get_bandwidth(bma255_i2c_client, &data) < 0)
+	{
+		return sprintf(buf, "Read error\n");
+	}
+	else
+	{
+		return sprintf(buf, "%d\n", data);
+	}
+}
+
+/*----------------------------------------------------------------------------*/
+/*
+g sensor bandwidth for compass tilt compensation
+*/
+static ssize_t store_cpsbandwidth_value(struct device_driver *ddri, const char *buf, size_t count)
+{
+	unsigned long data;
+	int error;
+
+	if ((error = strict_strtoul(buf, 10, &data)))
+	{
+		return error;
+	}
+	if (bma255_set_bandwidth(bma255_i2c_client, (unsigned char) data) < 0)
+	{
+		GSE_ERR("invalid content: '%s', length = %d\n", buf, count);
+	}
+
+	return count;
+}
+
+/*----------------------------------------------------------------------------*/
+/*
+g sensor data for compass tilt compensation
+*/
+static ssize_t show_cpsdata_value(struct device_driver *ddri, char *buf)
+{
+	struct i2c_client *client = bma255_i2c_client;
+	char strbuf[BMA255_BUFSIZE];
+	
+	if(NULL == client)
+	{
+		GSE_ERR("i2c client is null!!\n");
+		return 0;
+	}
+	BMA255_CompassReadData(client, strbuf, BMA255_BUFSIZE);
+	return snprintf(buf, PAGE_SIZE, "%s\n", strbuf);            
+}
+
+/*----------------------------------------------------------------------------*/
+static ssize_t show_sensordata_value(struct device_driver *ddri, char *buf)
+{
+	struct i2c_client *client = bma255_i2c_client;
+	char strbuf[BMA255_BUFSIZE];
+	
+	if(NULL == client)
+	{
+		GSE_ERR("i2c client is null!!\n");
+		return 0;
+	}
+	BMA255_ReadSensorData(client, strbuf, BMA255_BUFSIZE);
+	//BMA255_ReadRawData(client, strbuf);
+	return snprintf(buf, PAGE_SIZE, "%s\n", strbuf);            
+}
+
+/*----------------------------------------------------------------------------*/
+static ssize_t show_cali_value(struct device_driver *ddri, char *buf)
+{
+	struct i2c_client *client = bma255_i2c_client;
+	struct bma255_i2c_data *obj;
+	int err, len = 0, mul;
+	int tmp[BMA255_AXES_NUM];
+
+	if(NULL == client)
+	{
+		GSE_ERR("i2c client is null!!\n");
+		return 0;
+	}
+
+	obj = i2c_get_clientdata(client);
+
+	if((err = BMA255_ReadOffset(client, obj->offset)))
+	{
+		return -EINVAL;
+	}
+	else if((err = BMA255_ReadCalibration(client, tmp)))
+	{
+		return -EINVAL;
+	}
+	else
+	{    
+		mul = obj->reso->sensitivity/bma255_offset_resolution.sensitivity;
+		len += snprintf(buf+len, PAGE_SIZE-len, "[HW ][%d] (%+3d, %+3d, %+3d) : (0x%02X, 0x%02X, 0x%02X)\n", mul,                        
+			obj->offset[BMA255_AXIS_X], obj->offset[BMA255_AXIS_Y], obj->offset[BMA255_AXIS_Z],
+			obj->offset[BMA255_AXIS_X], obj->offset[BMA255_AXIS_Y], obj->offset[BMA255_AXIS_Z]);
+		len += snprintf(buf+len, PAGE_SIZE-len, "[SW ][%d] (%+3d, %+3d, %+3d)\n", 1, 
+			obj->cali_sw[BMA255_AXIS_X], obj->cali_sw[BMA255_AXIS_Y], obj->cali_sw[BMA255_AXIS_Z]);
+
+		len += snprintf(buf+len, PAGE_SIZE-len, "[ALL]    (%+3d, %+3d, %+3d) : (%+3d, %+3d, %+3d)\n", 
+			obj->offset[BMA255_AXIS_X]*mul + obj->cali_sw[BMA255_AXIS_X],
+			obj->offset[BMA255_AXIS_Y]*mul + obj->cali_sw[BMA255_AXIS_Y],
+			obj->offset[BMA255_AXIS_Z]*mul + obj->cali_sw[BMA255_AXIS_Z],
+			tmp[BMA255_AXIS_X], tmp[BMA255_AXIS_Y], tmp[BMA255_AXIS_Z]);
+		
+		return len;
+    }
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t store_cali_value(struct device_driver *ddri, const char *buf, size_t count)
+{
+	struct i2c_client *client = bma255_i2c_client;  
+	int err, x, y, z;
+	int dat[BMA255_AXES_NUM];
+
+	if(!strncmp(buf, "rst", 3))
+	{
+		if((err = BMA255_ResetCalibration(client)))
+		{
+			GSE_ERR("reset offset err = %d\n", err);
+		}	
+	}
+	else if(3 == sscanf(buf, "0x%02X 0x%02X 0x%02X", &x, &y, &z))
+	{
+		dat[BMA255_AXIS_X] = x;
+		dat[BMA255_AXIS_Y] = y;
+		dat[BMA255_AXIS_Z] = z;
+		if((err = BMA255_WriteCalibration(client, dat)))
+		{
+			GSE_ERR("write calibration err = %d\n", err);
+		}		
+	}
+	else
+	{
+		GSE_ERR("invalid format\n");
+	}
+	
+	return count;
+}
+
+
+/*----------------------------------------------------------------------------*/
+static ssize_t show_firlen_value(struct device_driver *ddri, char *buf)
+{
+#ifdef CONFIG_BMA255_LOWPASS
+	struct i2c_client *client = bma255_i2c_client;
+	struct bma255_i2c_data *obj = i2c_get_clientdata(client);
+	if(atomic_read(&obj->firlen))
+	{
+		int idx, len = atomic_read(&obj->firlen);
+		GSE_LOG("len = %2d, idx = %2d\n", obj->fir.num, obj->fir.idx);
+
+		for(idx = 0; idx < len; idx++)
+		{
+			GSE_LOG("[%5d %5d %5d]\n", obj->fir.raw[idx][BMA255_AXIS_X], obj->fir.raw[idx][BMA255_AXIS_Y], obj->fir.raw[idx][BMA255_AXIS_Z]);
+		}
+		
+		GSE_LOG("sum = [%5d %5d %5d]\n", obj->fir.sum[BMA255_AXIS_X], obj->fir.sum[BMA255_AXIS_Y], obj->fir.sum[BMA255_AXIS_Z]);
+		GSE_LOG("avg = [%5d %5d %5d]\n", obj->fir.sum[BMA255_AXIS_X]/len, obj->fir.sum[BMA255_AXIS_Y]/len, obj->fir.sum[BMA255_AXIS_Z]/len);
+	}
+	return snprintf(buf, PAGE_SIZE, "%d\n", atomic_read(&obj->firlen));
+#else
+	return snprintf(buf, PAGE_SIZE, "not support\n");
+#endif
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t store_firlen_value(struct device_driver *ddri, const char *buf, size_t count)
+{
+#ifdef CONFIG_BMA255_LOWPASS
+	struct i2c_client *client = bma255_i2c_client;  
+	struct bma255_i2c_data *obj = i2c_get_clientdata(client);
+	int firlen;
+
+	if(1 != sscanf(buf, "%d", &firlen))
+	{
+		GSE_ERR("invallid format\n");
+	}
+	else if(firlen > C_MAX_FIR_LENGTH)
+	{
+		GSE_ERR("exceeds maximum filter length\n");
+	}
+	else
+	{ 
+		atomic_set(&obj->firlen, firlen);
+		if(NULL == firlen)
+		{
+			atomic_set(&obj->fir_en, 0);
+		}
+		else
+		{
+			memset(&obj->fir, 0x00, sizeof(obj->fir));
+			atomic_set(&obj->fir_en, 1);
+		}
+	}
+#endif    
+	return count;
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t show_trace_value(struct device_driver *ddri, char *buf)
+{
+	ssize_t res;
+	struct bma255_i2c_data *obj = obj_i2c_data;
+	if (obj == NULL)
+	{
+		GSE_ERR("i2c_data obj is null!!\n");
+		return 0;
+	}
+	
+	res = snprintf(buf, PAGE_SIZE, "0x%04X\n", atomic_read(&obj->trace));     
+	return res;    
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t store_trace_value(struct device_driver *ddri, const char *buf, size_t count)
+{
+	struct bma255_i2c_data *obj = obj_i2c_data;
+	int trace;
+	if (obj == NULL)
+	{
+		GSE_ERR("i2c_data obj is null!!\n");
+		return 0;
+	}
+	
+	if(1 == sscanf(buf, "0x%x", &trace))
+	{
+		atomic_set(&obj->trace, trace);
+	}	
+	else
+	{
+		GSE_ERR("invalid content: '%s', length = %d\n", buf, count);
+	}
+	
+	return count;    
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t show_status_value(struct device_driver *ddri, char *buf)
+{
+	ssize_t len = 0;    
+	struct bma255_i2c_data *obj = obj_i2c_data;
+	if (obj == NULL)
+	{
+		GSE_ERR("i2c_data obj is null!!\n");
+		return 0;
+	}	
+	
+	if(obj->hw)
+	{
+		len += snprintf(buf+len, PAGE_SIZE-len, "CUST: %d %d (%d %d)\n", 
+	            obj->hw->i2c_num, obj->hw->direction, obj->hw->power_id, obj->hw->power_vol);   
+	}
+	else
+	{
+		len += snprintf(buf+len, PAGE_SIZE-len, "CUST: NULL\n");
+	}
+	return len;    
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t show_power_status_value(struct device_driver *ddri, char *buf)
+{
+	if(sensor_power)
+		printk("G sensor is in work mode, sensor_power = %d\n", sensor_power);
+	else
+		printk("G sensor is in standby mode, sensor_power = %d\n", sensor_power);
+
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static DRIVER_ATTR(chipinfo,   S_IWUSR | S_IRUGO, show_chipinfo_value,      NULL);
+static DRIVER_ATTR(cpsdata, 	 S_IWUSR | S_IRUGO, show_cpsdata_value,    NULL);
+static DRIVER_ATTR(cpsopmode,  S_IWUSR | S_IRUGO, show_cpsopmode_value,    store_cpsopmode_value);
+static DRIVER_ATTR(cpsrange, 	 S_IWUSR | S_IRUGO, show_cpsrange_value,     store_cpsrange_value);
+static DRIVER_ATTR(cpsbandwidth, S_IWUSR | S_IRUGO, show_cpsbandwidth_value,    store_cpsbandwidth_value);
+static DRIVER_ATTR(sensordata, S_IWUSR | S_IRUGO, show_sensordata_value,    NULL);
+static DRIVER_ATTR(cali,       S_IWUSR | S_IRUGO, show_cali_value,          store_cali_value);
+static DRIVER_ATTR(firlen,     S_IWUSR | S_IRUGO, show_firlen_value,        store_firlen_value);
+static DRIVER_ATTR(trace,      S_IWUSR | S_IRUGO, show_trace_value,         store_trace_value);
+static DRIVER_ATTR(status,               S_IRUGO, show_status_value,        NULL);
+static DRIVER_ATTR(powerstatus,               S_IRUGO, show_power_status_value,        NULL);
+
+/*----------------------------------------------------------------------------*/
+static struct driver_attribute *bma255_attr_list[] = {
+	&driver_attr_chipinfo,     /*chip information*/
+	&driver_attr_sensordata,   /*dump sensor data*/
+	&driver_attr_cali,         /*show calibration data*/
+	&driver_attr_firlen,       /*filter length: 0: disable, others: enable*/
+	&driver_attr_trace,        /*trace log*/
+	&driver_attr_status,
+	&driver_attr_powerstatus,
+	&driver_attr_cpsdata,	/*g sensor data for compass tilt compensation*/
+	&driver_attr_cpsopmode,	/*g sensor opmode for compass tilt compensation*/
+	&driver_attr_cpsrange,	/*g sensor range for compass tilt compensation*/
+	&driver_attr_cpsbandwidth,	/*g sensor bandwidth for compass tilt compensation*/
+};
+/*----------------------------------------------------------------------------*/
+static int bma255_create_attr(struct device_driver *driver) 
+{
+	int idx, err = 0;
+	int num = (int)(sizeof(bma255_attr_list)/sizeof(bma255_attr_list[0]));
+	if (driver == NULL)
+	{
+		return -EINVAL;
+	}
+
+	for(idx = 0; idx < num; idx++)
+	{
+		if((err = driver_create_file(driver, bma255_attr_list[idx])))
+		{            
+			GSE_ERR("driver_create_file (%s) = %d\n", bma255_attr_list[idx]->attr.name, err);
+			break;
+		}
+	}    
+	return err;
+}
+/*----------------------------------------------------------------------------*/
+static int bma255_delete_attr(struct device_driver *driver)
+{
+	int idx ,err = 0;
+	int num = (int)(sizeof(bma255_attr_list)/sizeof(bma255_attr_list[0]));
+
+	if(driver == NULL)
+	{
+		return -EINVAL;
+	}
+
+	for(idx = 0; idx < num; idx++)
+	{
+		driver_remove_file(driver, bma255_attr_list[idx]);
+	}
+	
+	return err;
+}
+
+/*----------------------------------------------------------------------------*/
+int gsensor_operate(void* self, uint32_t command, void* buff_in, int size_in,
+		void* buff_out, int size_out, int* actualout)
+{
+	int err = 0;
+	int value, sample_delay;	
+	struct bma255_i2c_data *priv = (struct bma255_i2c_data*)self;
+	hwm_sensor_data* gsensor_data;
+	char buff[BMA255_BUFSIZE];
+	
+	//GSE_FUN(f);
+	switch (command)
+	{
+		case SENSOR_DELAY:
+			if((buff_in == NULL) || (size_in < sizeof(int)))
+			{
+				GSE_ERR("Set delay parameter error!\n");
+				err = -EINVAL;
+			}
+			else
+			{
+				value = *(int *)buff_in;
+				if(value <= 5)
+				{
+					sample_delay = BMA255_BW_200HZ;
+				}
+				else if(value <= 10)
+				{
+					sample_delay = BMA255_BW_100HZ;
+				}
+				else
+				{
+					sample_delay = BMA255_BW_50HZ;
+				}
+				mutex_lock(&bma_op_mutex);				
+				err = BMA255_SetBWRate(priv->client, sample_delay);
+				if(err != BMA255_SUCCESS ) //0x2C->BW=100Hz
+				{
+					GSE_ERR("Set delay parameter error!\n");
+				}
+				mutex_unlock(&bma_op_mutex);
+
+				if(value >= 50)
+				{
+					atomic_set(&priv->filter, 0);
+				}
+				else
+				{	
+				#if defined(CONFIG_BMA255_LOWPASS)
+					priv->fir.num = 0;
+					priv->fir.idx = 0;
+					priv->fir.sum[BMA255_AXIS_X] = 0;
+					priv->fir.sum[BMA255_AXIS_Y] = 0;
+					priv->fir.sum[BMA255_AXIS_Z] = 0;
+					atomic_set(&priv->filter, 1);
+				#endif
+				}
+			}
+			break;
+
+		case SENSOR_ENABLE:
+			if((buff_in == NULL) || (size_in < sizeof(int)))
+			{
+				GSE_ERR("Enable sensor parameter error!\n");
+				err = -EINVAL;
+			}
+			else
+			{
+				value = *(int *)buff_in;
+				mutex_lock(&bma_op_mutex);
+				GSE_LOG("Gsensor enable_status value = %d,sensor_power=%d\n",value,sensor_power);
+				if(((value == 0) && (sensor_power == false)) ||((value == 1) && (sensor_power == true)))
+				{
+					GSE_LOG("Gsensor device have updated!\n");
+					enable_status = sensor_power;
+				}
+				else
+				{
+				    enable_status = !sensor_power;
+					err = BMA255_SetPowerMode( priv->client, !sensor_power);
+				}
+				GSE_LOG("Gsensor enable_status = %d\n",enable_status);
+				mutex_unlock(&bma_op_mutex);
+			}
+			break;
+
+		case SENSOR_GET_DATA:
+			if((buff_out == NULL) || (size_out< sizeof(hwm_sensor_data)))
+			{
+				GSE_ERR("get sensor data parameter error!\n");
+				err = -EINVAL;
+			}
+			else
+			{
+				mutex_lock(&bma_op_mutex);
+				gsensor_data = (hwm_sensor_data *)buff_out;
+				BMA255_ReadSensorData(priv->client, buff, BMA255_BUFSIZE);
+				sscanf(buff, "%x %x %x", &gsensor_data->values[0], 
+					&gsensor_data->values[1], &gsensor_data->values[2]);				
+				gsensor_data->status = SENSOR_STATUS_ACCURACY_MEDIUM;				
+				gsensor_data->value_divide = 1000;
+				mutex_unlock(&bma_op_mutex);
+			}
+			break;
+		default:
+			GSE_ERR("gsensor operate function no this parameter %d!\n", command);
+			err = -1;
+			break;
+	}
+	
+	return err;
+}
+
+/****************************************************************************** 
+ * Function Configuration
+******************************************************************************/
+static int bma255_open(struct inode *inode, struct file *file)
+{
+	file->private_data = bma255_i2c_client;
+
+	if(file->private_data == NULL)
+	{
+		GSE_ERR("null pointer!!\n");
+		return -EINVAL;
+	}
+	return nonseekable_open(inode, file);
+}
+/*----------------------------------------------------------------------------*/
+static int bma255_release(struct inode *inode, struct file *file)
+{
+	file->private_data = NULL;
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static long bma255_unlocked_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
+{
+	struct i2c_client *client = (struct i2c_client*)file->private_data;
+	struct bma255_i2c_data *obj = (struct bma255_i2c_data*)i2c_get_clientdata(client);	
+	char strbuf[BMA255_BUFSIZE];
+	void __user *data;
+	SENSOR_DATA sensor_data;
+	long err = 0;
+	int cali[3];
+
+	//GSE_FUN(f);
+	if(_IOC_DIR(cmd) & _IOC_READ)
+	{
+		err = !access_ok(VERIFY_WRITE, (void __user *)arg, _IOC_SIZE(cmd));
+	}
+	else if(_IOC_DIR(cmd) & _IOC_WRITE)
+	{
+		err = !access_ok(VERIFY_READ, (void __user *)arg, _IOC_SIZE(cmd));
+	}
+
+	if(err)
+	{
+		GSE_ERR("access error: %08X, (%2d, %2d)\n", cmd, _IOC_DIR(cmd), _IOC_SIZE(cmd));
+		return -EFAULT;
+	}
+
+	switch(cmd)
+	{
+		case GSENSOR_IOCTL_INIT:
+			bma255_init_client(client, 0);			
+			break;
+
+		case GSENSOR_IOCTL_READ_CHIPINFO:
+			data = (void __user *) arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}
+			
+			BMA255_ReadChipInfo(client, strbuf, BMA255_BUFSIZE);
+			if(copy_to_user(data, strbuf, strlen(strbuf)+1))
+			{
+				err = -EFAULT;
+				break;
+			}				 
+			break;	  
+
+		case GSENSOR_IOCTL_READ_SENSORDATA:
+			data = (void __user *) arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}
+			BMA255_SetPowerMode(client,true);
+			BMA255_ReadSensorData(client, strbuf, BMA255_BUFSIZE);
+			if(copy_to_user(data, strbuf, strlen(strbuf)+1))
+			{
+				err = -EFAULT;
+				break;	  
+			}				 
+			break;
+
+		case GSENSOR_IOCTL_READ_GAIN:
+			data = (void __user *) arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}			
+			
+			if(copy_to_user(data, &gsensor_gain, sizeof(GSENSOR_VECTOR3D)))
+			{
+				err = -EFAULT;
+				break;
+			}				 
+			break;
+
+		case GSENSOR_IOCTL_READ_RAW_DATA:
+			data = (void __user *) arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}
+			BMA255_ReadRawData(client, strbuf);
+			if(copy_to_user(data, &strbuf, strlen(strbuf)+1))
+			{
+				err = -EFAULT;
+				break;	  
+			}
+			break;	  
+
+		case GSENSOR_IOCTL_SET_CALI:
+			data = (void __user*)arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}
+			if(copy_from_user(&sensor_data, data, sizeof(sensor_data)))
+			{
+				err = -EFAULT;
+				break;	  
+			}
+			if(atomic_read(&obj->suspend))
+			{
+				GSE_ERR("Perform calibration in suspend state!!\n");
+				err = -EINVAL;
+			}
+			else
+			{
+			#if 0
+				cali[BMA255_AXIS_X] = sensor_data.x * obj->reso->sensitivity / GRAVITY_EARTH_1000;
+				cali[BMA255_AXIS_Y] = sensor_data.y * obj->reso->sensitivity / GRAVITY_EARTH_1000;
+				cali[BMA255_AXIS_Z] = sensor_data.z * obj->reso->sensitivity / GRAVITY_EARTH_1000;			  
+			#else
+			cali[BMA255_AXIS_X] = sensor_data.x;
+			cali[BMA255_AXIS_Y] = sensor_data.y;
+			cali[BMA255_AXIS_Z] = sensor_data.z;			  
+						
+
+			#endif
+				err = BMA255_WriteCalibration(client, cali);			 
+			}
+			break;
+
+		case GSENSOR_IOCTL_CLR_CALI:
+			err = BMA255_ResetCalibration(client);
+			break;
+
+		case GSENSOR_IOCTL_GET_CALI:
+			data = (void __user*)arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}
+			if((err = BMA255_ReadCalibration(client, cali)))
+			{
+				break;
+			}
+			#if 0
+			sensor_data.x = cali[BMA255_AXIS_X] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+			sensor_data.y = cali[BMA255_AXIS_Y] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+			sensor_data.z = cali[BMA255_AXIS_Z] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+			#else
+			sensor_data.x = cali[BMA255_AXIS_X];
+			sensor_data.y = cali[BMA255_AXIS_Y];
+			sensor_data.z = cali[BMA255_AXIS_Z];
+						
+
+			#endif
+			if(copy_to_user(data, &sensor_data, sizeof(sensor_data)))
+			{
+				err = -EFAULT;
+				break;
+			}		
+			break;
+		
+
+		default:
+			GSE_ERR("unknown IOCTL: 0x%08x\n", cmd);
+			err = -ENOIOCTLCMD;
+			break;
+			
+	}
+
+	return err;
+}
+
+
+/*----------------------------------------------------------------------------*/
+static struct file_operations bma255_fops = {
+	//.owner = THIS_MODULE,
+	.open = bma255_open,
+	.release = bma255_release,
+	.unlocked_ioctl = bma255_unlocked_ioctl,
+};
+/*----------------------------------------------------------------------------*/
+static struct miscdevice bma255_device = {
+	.minor = MISC_DYNAMIC_MINOR,
+	.name = "gsensor",
+	.fops = &bma255_fops,
+};
+/*----------------------------------------------------------------------------*/
+#ifndef USE_EARLY_SUSPEND
+/*----------------------------------------------------------------------------*/
+static int bma255_suspend(struct i2c_client *client, pm_message_t msg) 
+{
+	struct bma255_i2c_data *obj = i2c_get_clientdata(client);    
+	int err = 0;
+	
+	GSE_FUN();    
+    mutex_lock(&bma_op_mutex);
+	if(msg.event == PM_EVENT_SUSPEND)
+	{   
+		if(obj == NULL)
+		{
+			GSE_ERR("null pointer!!\n");
+			mutex_unlock(&bma_op_mutex);
+			return -EINVAL;
+		}
+		atomic_set(&obj->suspend, 1);
+		if((err = BMA255_SetPowerMode(obj->client, false)))
+		{
+			GSE_ERR("write power control fail!!\n");
+			mutex_unlock(&bma_op_mutex);
+			return -EINVAL;
+		}       
+		sensor_power = false;
+		BMA255_power(obj->hw, 0);
+	}
+	mutex_unlock(&bma_op_mutex);
+	return err;
+}
+/*----------------------------------------------------------------------------*/
+static int bma255_resume(struct i2c_client *client)
+{
+	struct bma255_i2c_data *obj = i2c_get_clientdata(client);        
+	int err;
+	
+	GSE_FUN();
+
+	if(obj == NULL)
+	{
+		GSE_ERR("null pointer!!\n");
+		return -EINVAL;
+	}
+    mutex_lock(&bma_op_mutex);
+	BMA255_power(obj->hw, 1);
+	if((err = bma255_init_client(client, 0)))
+	{
+		GSE_ERR("initialize client fail!!\n");
+		mutex_unlock(&bma_op_mutex);
+		return err;        
+	}
+	atomic_set(&obj->suspend, 0);
+    mutex_unlock(&bma_op_mutex);
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+#else /*CONFIG_HAS_EARLY_SUSPEND is defined*/
+/*----------------------------------------------------------------------------*/
+static void bma255_early_suspend(struct early_suspend *h) 
+{
+	struct bma255_i2c_data *obj = container_of(h, struct bma255_i2c_data, early_drv);   
+	int err;
+	
+	GSE_FUN();    
+
+	if(obj == NULL)
+	{
+		GSE_ERR("null pointer!!\n");
+		return;
+	}
+	atomic_set(&obj->suspend, 1); 
+	if(err = BMA255_SetPowerMode(obj->client, false))
+	{
+		GSE_ERR("write power control fail!!\n");
+		return;
+	}
+
+	sensor_power = false;
+	
+	BMA255_power(obj->hw, 0);
+
+}
+/*----------------------------------------------------------------------------*/
+static void bma255_late_resume(struct early_suspend *h)
+{
+	struct bma255_i2c_data *obj = container_of(h, struct bma255_i2c_data, early_drv);         
+	int err;
+	
+	GSE_FUN();
+
+	if(obj == NULL)
+	{
+		GSE_ERR("null pointer!!\n");
+		return;
+	}
+
+	BMA255_power(obj->hw, 1);
+	if(err = bma255_init_client(obj->client, 0))
+	{
+		GSE_ERR("initialize client fail!!\n");
+		return;        
+	}
+	atomic_set(&obj->suspend, 0);    
+}
+/*----------------------------------------------------------------------------*/
+#endif /*USE_EARLY_SUSPEND*/
+/*----------------------------------------------------------------------------*/
+static int bma255_i2c_detect(struct i2c_client *client, struct i2c_board_info *info) 
+{    
+	strcpy(info->type, BMA255_DEV_NAME);
+	return 0;
+}
+
+/*----------------------------------------------------------------------------*/
+static int bma255_i2c_probe(struct i2c_client *client, const struct i2c_device_id *id)
+{
+	struct i2c_client *new_client;
+	struct bma255_i2c_data *obj;
+	struct hwmsen_object sobj;
+	int err = 0;
+	int retry = 0;
+	GSE_FUN();
+
+	if(!(obj = kzalloc(sizeof(*obj), GFP_KERNEL)))
+	{
+		err = -ENOMEM;
+		goto exit;
+	}
+	
+	memset(obj, 0, sizeof(struct bma255_i2c_data));
+
+	obj->hw = get_cust_acc_hw();
+	
+	if((err = hwmsen_get_convert(obj->hw->direction, &obj->cvt)))
+	{
+		GSE_ERR("invalid direction: %d\n", obj->hw->direction);
+		goto exit;
+	}
+
+	obj_i2c_data = obj;
+	obj->client = client;
+	new_client = obj->client;
+	i2c_set_clientdata(new_client,obj);
+	
+	atomic_set(&obj->trace, 0);
+	atomic_set(&obj->suspend, 0);
+	
+#ifdef CONFIG_BMA255_LOWPASS
+	if(obj->hw->firlen > C_MAX_FIR_LENGTH)
+	{
+		atomic_set(&obj->firlen, C_MAX_FIR_LENGTH);
+	}	
+	else
+	{
+		atomic_set(&obj->firlen, obj->hw->firlen);
+	}
+	
+	if(atomic_read(&obj->firlen) > 0)
+	{
+		atomic_set(&obj->fir_en, 1);
+	}
+	
+#endif
+
+	bma255_i2c_client = new_client;	
+
+	for(retry = 0; retry < 3; retry++){
+		if((err = bma255_init_client(new_client, 1)))
+	{
+			GSE_ERR("bma056_device init cilent fail time: %d\n", retry);
+			continue;
+	}
+	}
+	if(err != 0)
+		goto exit_init_failed;
+
+	if((err = misc_register(&bma255_device)))
+	{
+		GSE_ERR("bma255_device register failed\n");
+		goto exit_misc_device_register_failed;
+	}
+
+	if((err = bma255_create_attr(&bma255_gsensor_driver.driver)))
+	{
+		GSE_ERR("create attribute err = %d\n", err);
+		goto exit_create_attr_failed;
+	}
+
+	sobj.self = obj;
+    sobj.polling = 1;
+    sobj.sensor_operate = gsensor_operate;
+	if((err = hwmsen_attach(ID_ACCELEROMETER, &sobj)))
+	{
+		GSE_ERR("attach fail = %d\n", err);
+		goto exit_kfree;
+	}
+
+#ifdef USE_EARLY_SUSPEND
+	obj->early_drv.level    = EARLY_SUSPEND_LEVEL_DISABLE_FB - 1,
+	obj->early_drv.suspend  = bma255_early_suspend,
+	obj->early_drv.resume   = bma255_late_resume,    
+	register_early_suspend(&obj->early_drv);
+#endif 
+
+	GSE_LOG("%s: OK\n", __func__);    
+	return 0;
+
+	exit_create_attr_failed:
+	misc_deregister(&bma255_device);
+	exit_misc_device_register_failed:
+	exit_init_failed:
+	//i2c_detach_client(new_client);
+	exit_kfree:
+	kfree(obj);
+	exit:
+	GSE_ERR("%s: err = %d\n", __func__, err);        
+	return err;
+}
+
+/*----------------------------------------------------------------------------*/
+static int bma255_i2c_remove(struct i2c_client *client)
+{
+	int err = 0;	
+	
+	if((err = bma255_delete_attr(&bma255_gsensor_driver.driver)))
+	{
+		GSE_ERR("bma150_delete_attr fail: %d\n", err);
+	}
+	
+	if((err = misc_deregister(&bma255_device)))
+	{
+		GSE_ERR("misc_deregister fail: %d\n", err);
+	}
+
+	if((err = hwmsen_detach(ID_ACCELEROMETER)))
+	    
+
+	bma255_i2c_client = NULL;
+	i2c_unregister_device(client);
+	kfree(i2c_get_clientdata(client));
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int bma255_probe(struct platform_device *pdev) 
+{
+	struct acc_hw *hw = get_cust_acc_hw();
+	
+	GSE_FUN();
+
+	BMA255_power(hw, 1);
+	if(i2c_add_driver(&bma255_i2c_driver))
+	{
+		GSE_ERR("add driver error\n");
+		return -1;
+	}
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int bma255_remove(struct platform_device *pdev)
+{
+    struct acc_hw *hw = get_cust_acc_hw();
+
+    GSE_FUN();    
+    BMA255_power(hw, 0);    
+    i2c_del_driver(&bma255_i2c_driver);
+    return 0;
+}
+/*----------------------------------------------------------------------------*/
+#if 1
+#ifdef CONFIG_OF
+static const struct of_device_id gsensor_of_match[] = {
+	{ .compatible = "mediatek,gsensor", },
+	{},
+};
+#endif
+
+static struct platform_driver bma255_gsensor_driver = {
+	.probe      = bma255_probe,
+	.remove     = bma255_remove,    
+	.driver     = 
+	{
+		.name  = "gsensor",
+	//	.owner  = THIS_MODULE,
+        #ifdef CONFIG_OF
+		.of_match_table = gsensor_of_match,
+		#endif
+	}
+};
+#else
+
+static struct platform_driver bma255_gsensor_driver = {
+	.probe      = bma255_probe,
+	.remove     = bma255_remove,    
+	.driver     = {
+		.name  = "gsensor",
+	}
+};
+#endif
+/*----------------------------------------------------------------------------*/
+static int __init bma255_init(void)
+{
+	struct acc_hw *hw = get_cust_acc_hw();
+	GSE_LOG("%s: i2c_number=%d\n", __func__,hw->i2c_num); 
+	i2c_register_board_info(hw->i2c_num, &bma255_i2c_info, 1);
+	if(platform_driver_register(&bma255_gsensor_driver))
+	{
+		GSE_ERR("failed to register driver");
+		return -ENODEV;
+	}
+	return 0;    
+}
+/*----------------------------------------------------------------------------*/
+static void __exit bma255_exit(void)
+{
+	GSE_FUN();
+	platform_driver_unregister(&bma255_gsensor_driver);
+}
+/*----------------------------------------------------------------------------*/
+module_init(bma255_init);
+module_exit(bma255_exit);
+/*----------------------------------------------------------------------------*/
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("BMA255 I2C driver");
+MODULE_AUTHOR("hongji.zhou@bosch-sensortec.com");
diff --git a/drivers/misc/mediatek/accelerometer/bma056/bma056.h b/drivers/misc/mediatek/accelerometer/bma056/bma056.h
new file mode 100644
index 000000000..230748223
--- /dev/null
+++ b/drivers/misc/mediatek/accelerometer/bma056/bma056.h
@@ -0,0 +1,59 @@
+/* BMA255 motion sensor driver
+ *
+ *
+ * This software program is licensed subject to the GNU General Public License
+ * (GPL).Version 2,June 1991, available at http://www.fsf.org/copyleft/gpl.html
+
+ * (C) Copyright 2011 Bosch Sensortec GmbH
+ * All Rights Reserved
+ */
+
+#ifndef BMA255_H
+#define BMA255_H
+	 
+#include <linux/ioctl.h>
+
+/* 7-bit addr: 0x10 (SDO connected to GND); 0x11 (SDO connected to VDDIO) */
+#define BMA255_I2C_ADDR				0x11
+/* chip ID */
+#define BMA255_FIXED_DEVID			0xFA
+
+ /* BMA255 Register Map  (Please refer to BMA255 Specifications) */
+#define BMA255_REG_DEVID			0x00
+#define BMA255_REG_OFSX				0x16
+#define BMA255_REG_OFSX_HIGH		0x1A
+#define BMA255_REG_BW_RATE			0x10
+#define BMA255_BW_MASK				0x1f
+#define BMA255_BW_200HZ				0x0d
+#define BMA255_BW_100HZ				0x0c
+#define BMA255_BW_50HZ				0x0b
+#define BMA255_BW_25HZ				0x0a
+#define BMA255_REG_POWER_CTL		0x11		
+#define BMA255_REG_DATA_FORMAT		0x0f
+#define BMA255_RANGE_MASK			0x0f
+#define BMA255_RANGE_2G				0x03
+#define BMA255_RANGE_4G				0x05
+#define BMA255_RANGE_8G				0x08
+#define BMA255_REG_DATAXLOW			0x02
+#define BMA255_REG_DATA_RESOLUTION	0x14
+#define BMA255_MEASURE_MODE			0x80	
+#define BMA255_SELF_TEST           	0x32
+#define BMA255_SELF_TEST_AXIS_X		0x01
+#define BMA255_SELF_TEST_AXIS_Y		0x02
+#define BMA255_SELF_TEST_AXIS_Z		0x03
+#define BMA255_SELF_TEST_POSITIVE	0x00
+#define BMA255_SELF_TEST_NEGATIVE	0x04
+#define BMA255_INT_REG_1           	0x16
+#define BMA255_INT_REG_2          	0x17
+
+#define BMA255_SUCCESS					0
+#define BMA255_ERR_I2C					-1
+#define BMA255_ERR_STATUS				-3
+#define BMA255_ERR_SETUP_FAILURE		-4
+#define BMA255_ERR_GETGSENSORDATA		-5
+#define BMA255_ERR_IDENTIFICATION		-6
+	 
+#define BMA255_BUFSIZE					256
+	 
+#endif
+
diff --git a/drivers/misc/mediatek/accelerometer/bma222E-new/Makefile b/drivers/misc/mediatek/accelerometer/bma222E-new/Makefile
new file mode 100644
index 000000000..7c0399b38
--- /dev/null
+++ b/drivers/misc/mediatek/accelerometer/bma222E-new/Makefile
@@ -0,0 +1,4 @@
+include $(srctree)/drivers/misc/mediatek/Makefile.custom
+
+obj-y	:=  bma222E.o
+
diff --git a/drivers/misc/mediatek/accelerometer/bma222E-new/bma222E.c b/drivers/misc/mediatek/accelerometer/bma222E-new/bma222E.c
new file mode 100644
index 000000000..216ef86bd
--- /dev/null
+++ b/drivers/misc/mediatek/accelerometer/bma222E-new/bma222E.c
@@ -0,0 +1,2383 @@
+/* BMA150 motion sensor driver
+ *
+ *
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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.
+ *
+ */
+
+#include <linux/interrupt.h>
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include <linux/irq.h>
+#include <linux/miscdevice.h>
+#include <asm/uaccess.h>
+#include <linux/delay.h>
+#include <linux/input.h>
+#include <linux/workqueue.h>
+#include <linux/kobject.h>
+#include <linux/earlysuspend.h>
+#include <linux/platform_device.h>
+#include <asm/atomic.h>
+
+#include <mach/mt_typedefs.h>
+#include <mach/mt_gpio.h>
+#include <mach/mt_pm_ldo.h>
+
+#define POWER_NONE_MACRO MT65XX_POWER_NONE
+
+#include <cust_acc.h>
+#include <linux/hwmsensor.h>
+#include <linux/hwmsen_dev.h>
+#include <linux/sensors_io.h>
+#include "bma222E.h"
+#include <linux/hwmsen_helper.h>
+
+#include <accel.h>
+#include <linux/batch.h>
+#ifdef CUSTOM_KERNEL_SENSORHUB
+#include <SCP_sensorHub.h>
+#endif//#ifdef CUSTOM_KERNEL_SENSORHUB
+
+/*----------------------------------------------------------------------------*/
+#define I2C_DRIVERID_BMA222 222
+/*----------------------------------------------------------------------------*/
+#define DEBUG 1
+/*----------------------------------------------------------------------------*/
+     
+#define SW_CALIBRATION
+
+/*----------------------------------------------------------------------------*/
+#define BMA222_AXIS_X          0
+#define BMA222_AXIS_Y          1
+#define BMA222_AXIS_Z          2
+#define BMA222_DATA_LEN        6
+#define BMA222_DEV_NAME        "BMA222"
+/*----------------------------------------------------------------------------*/
+
+/*********/
+/*----------------------------------------------------------------------------*/
+static const struct i2c_device_id bma222_i2c_id[] = {{BMA222_DEV_NAME,0},{}};
+static struct i2c_board_info __initdata i2c_BMA222={ I2C_BOARD_INFO(BMA222_DEV_NAME, 0x18)};
+
+
+/*----------------------------------------------------------------------------*/
+static int bma222_i2c_probe(struct i2c_client *client, const struct i2c_device_id *id); 
+static int bma222_i2c_remove(struct i2c_client *client);
+#if !defined(CONFIG_HAS_EARLYSUSPEND) || !defined(USE_EARLY_SUSPEND)
+static int bma222_suspend(struct i2c_client *client, pm_message_t msg);
+static int bma222_resume(struct i2c_client *client);
+#endif
+
+static int gsensor_local_init(void);
+static int  gsensor_remove(void);
+#ifdef CUSTOM_KERNEL_SENSORHUB
+static int gsensor_setup_irq(void);
+#endif//#ifdef CUSTOM_KERNEL_SENSORHUB
+static int gsensor_set_delay(u64 ns);
+/*----------------------------------------------------------------------------*/
+typedef enum {
+    ADX_TRC_FILTER  = 0x01,
+    ADX_TRC_RAWDATA = 0x02,
+    ADX_TRC_IOCTL   = 0x04,
+    ADX_TRC_CALI	= 0X08,
+    ADX_TRC_INFO	= 0X10,
+} ADX_TRC;
+/*----------------------------------------------------------------------------*/
+struct scale_factor{
+    u8  whole;
+    u8  fraction;
+};
+/*----------------------------------------------------------------------------*/
+struct data_resolution {
+    struct scale_factor scalefactor;
+    int                 sensitivity;
+};
+/*----------------------------------------------------------------------------*/
+#define C_MAX_FIR_LENGTH (32)
+/*----------------------------------------------------------------------------*/
+struct data_filter {
+    s16 raw[C_MAX_FIR_LENGTH][BMA222_AXES_NUM];
+    int sum[BMA222_AXES_NUM];
+    int num;
+    int idx;
+};
+/*----------------------------------------------------------------------------*/
+struct bma222_i2c_data {
+    struct i2c_client *client;
+    struct acc_hw *hw;
+    struct hwmsen_convert   cvt;
+#ifdef CUSTOM_KERNEL_SENSORHUB
+    struct work_struct	irq_work;
+#endif//#ifdef CUSTOM_KERNEL_SENSORHUB
+    
+    /*misc*/
+    struct data_resolution *reso;
+    atomic_t                trace;
+    atomic_t                suspend;
+    atomic_t                selftest;
+	atomic_t				filter;
+    s16                     cali_sw[BMA222_AXES_NUM+1];
+
+    /*data*/
+    s8                      offset[BMA222_AXES_NUM+1];  /*+1: for 4-byte alignment*/
+    s16                     data[BMA222_AXES_NUM+1];
+
+#ifdef CUSTOM_KERNEL_SENSORHUB
+    int                     SCP_init_done;
+#endif//#ifdef CUSTOM_KERNEL_SENSORHUB
+
+
+#if defined(CONFIG_BMA222_LOWPASS)
+    atomic_t                firlen;
+    atomic_t                fir_en;
+    struct data_filter      fir;
+#endif 
+    /*early suspend*/
+#if defined(CONFIG_HAS_EARLYSUSPEND) && defined(USE_EARLY_SUSPEND)
+    struct early_suspend    early_drv;
+#endif   
+    u8                      bandwidth;
+};
+/*----------------------------------------------------------------------------*/
+static struct i2c_driver bma222_i2c_driver = {
+    .driver = {
+//        .owner          = THIS_MODULE,
+        .name           = BMA222_DEV_NAME,
+    },
+	.probe      		= bma222_i2c_probe,
+	.remove    			= bma222_i2c_remove,
+#if defined(CONFIG_HAS_EARLYSUSPEND) && defined(USE_EARLY_SUSPEND)   
+    .suspend            = bma222_suspend,
+    .resume             = bma222_resume,
+#endif
+	.id_table = bma222_i2c_id,
+//	.address_data = &bma222_addr_data,
+};
+
+/*----------------------------------------------------------------------------*/
+static struct i2c_client *bma222_i2c_client = NULL;
+static struct bma222_i2c_data *obj_i2c_data = NULL;
+static bool sensor_power = true;
+static int sensor_suspend = 0;
+static GSENSOR_VECTOR3D gsensor_gain;
+//static char selftestRes[8]= {0}; 
+static DEFINE_MUTEX(gsensor_mutex);
+static DEFINE_MUTEX(gsensor_scp_en_mutex);
+
+
+static bool enable_status = false;
+
+static int gsensor_init_flag =-1; // 0<==>OK -1 <==> fail
+static struct acc_init_info bma222_init_info = {
+    .name = BMA222_DEV_NAME,
+    .init = gsensor_local_init,
+    .uninit = gsensor_remove,
+};
+
+/*----------------------------------------------------------------------------*/
+#ifdef GSE_DUBUG
+#define GSE_TAG                  "[Gsensor] "
+#define GSE_FUN(f)               printk(GSE_TAG"%s\n", __FUNCTION__)
+#define GSE_ERR(fmt, args...)    printk(GSE_TAG"%s %d : "fmt, __FUNCTION__, __LINE__, ##args)
+#define GSE_LOG(fmt, args...)    printk(GSE_TAG fmt, ##args)
+#else 
+#define GSE_TAG                 
+#define GSE_FUN(f)               
+#define GSE_ERR(fmt, args...)    
+#define GSE_LOG(fmt, args...) 
+#endif
+
+/*----------------------------------------------------------------------------*/
+static struct data_resolution bma222_data_resolution[1] = {
+ /* combination by {FULL_RES,RANGE}*/
+    {{ 15, 6}, 64},   // dataformat +/-2g  in 8-bit resolution;  { 15, 6} = 15.6= (2*2*1000)/(2^8);  64 = (2^8)/(2*2)          
+};
+/*----------------------------------------------------------------------------*/
+static struct data_resolution bma222_offset_resolution = {{15, 6}, 64};
+
+/*----------------------------------------------------------------------------*/
+static int bma_i2c_read_block(struct i2c_client *client, u8 addr, u8 *data, u8 len)
+{
+    u8 beg = addr;
+	int err;
+	struct i2c_msg msgs[2]={{0},{0}};
+	
+	mutex_lock(&gsensor_mutex);
+	
+	msgs[0].addr = client->addr;
+	msgs[0].flags = 0;
+	msgs[0].len =1;
+	msgs[0].buf = &beg;
+
+	msgs[1].addr = client->addr;
+	msgs[1].flags = I2C_M_RD;
+	msgs[1].len =len;
+	msgs[1].buf = data;
+	
+	if (!client)
+	{
+	    mutex_unlock(&gsensor_mutex);
+		return -EINVAL;
+	}
+	else if (len > C_I2C_FIFO_SIZE) 
+	{
+		GSE_ERR(" length %d exceeds %d\n", len, C_I2C_FIFO_SIZE);
+		mutex_unlock(&gsensor_mutex);
+		return -EINVAL;
+	}
+	err = i2c_transfer(client->adapter, msgs, sizeof(msgs)/sizeof(msgs[0]));
+	if (err != 2) 
+	{
+		GSE_ERR("i2c_transfer error: (%d %p %d) %d\n",addr, data, len, err);
+		err = -EIO;
+	} 
+	else 
+	{
+		err = 0;
+	}
+	mutex_unlock(&gsensor_mutex);
+	return err;
+
+}
+EXPORT_SYMBOL(bma_i2c_read_block);
+static int bma_i2c_write_block(struct i2c_client *client, u8 addr, u8 *data, u8 len)
+{   /*because address also occupies one byte, the maximum length for write is 7 bytes*/
+    int err, idx, num;
+    char buf[C_I2C_FIFO_SIZE];
+    err =0;
+	mutex_lock(&gsensor_mutex);
+    if (!client)
+    {
+        mutex_unlock(&gsensor_mutex);
+        return -EINVAL;
+    }
+    else if (len >= C_I2C_FIFO_SIZE) 
+	{        
+        GSE_ERR(" length %d exceeds %d\n", len, C_I2C_FIFO_SIZE);
+		mutex_unlock(&gsensor_mutex);
+        return -EINVAL;
+    }    
+
+    num = 0;
+    buf[num++] = addr;
+    for (idx = 0; idx < len; idx++)
+    {
+        buf[num++] = data[idx];
+    }
+
+    err = i2c_master_send(client, buf, num);
+    if (err < 0)
+	{
+        GSE_ERR("send command error!!\n");
+		mutex_unlock(&gsensor_mutex);
+        return -EFAULT;
+    } 
+	mutex_unlock(&gsensor_mutex);
+    return err;
+}
+EXPORT_SYMBOL(bma_i2c_write_block);
+
+/*----------------------------------------------------------------------------*/
+/*--------------------Add by Susan----------------------------------*/
+#ifdef CUSTOM_KERNEL_SENSORHUB
+int BMA222_SCP_SetPowerMode(bool enable, int sensorType)
+{
+   static bool gsensor_scp_en_status = false;
+   static unsigned int gsensor_scp_en_map = 0;
+   SCP_SENSOR_HUB_DATA req;
+   int len;
+   int err = 0;
+
+   mutex_lock(&gsensor_scp_en_mutex);
+
+   if (sensorType >= 32)
+   {
+      GSE_ERR("Out of index!\n");
+	  return -1;
+   }
+
+   if (true == enable)
+   {
+      gsensor_scp_en_map |= (1<<sensorType);
+   }
+   else
+   {
+      gsensor_scp_en_map &= ~(1<<sensorType);
+   }
+
+   if (0 == gsensor_scp_en_map)
+   	   enable = false;
+   else
+   	   enable = true;
+
+   if (gsensor_scp_en_status != enable)
+   	{
+   	   gsensor_scp_en_status = enable;
+
+       req.activate_req.sensorType = ID_ACCELEROMETER;
+	   req.activate_req.action = SENSOR_HUB_ACTIVATE;
+	   req.activate_req.enable = enable;
+	   len = sizeof(req.activate_req);
+	   err = SCP_sensorHub_req_send(&req, &len, 1);
+	   if (err)
+	   	{
+	   	   GSE_ERR("SCP_sensorHub_req_send fail\n");
+		}
+    }
+
+	 mutex_unlock(&gsensor_scp_en_mutex);
+
+	 return err;
+}
+EXPORT_SYMBOL(BMA222_SCP_SetPowerMode);
+#endif 
+/*----------------------------------------------------------------------------*/
+/*--------------------BMA222 power control function----------------------------------*/
+static void BMA222_power(struct acc_hw *hw, unsigned int on) 
+{
+#ifdef __USE_LINUX_REGULATOR_FRAMEWORK__
+#else
+#ifndef FPGA_EARLY_PORTING
+	static unsigned int power_on = 0;
+
+	if(hw->power_id != POWER_NONE_MACRO)		// have externel LDO
+	{        
+		GSE_LOG("power %s\n", on ? "on" : "off");
+		if(power_on == on)	// power status not change
+		{
+			GSE_LOG("ignore power control: %d\n", on);
+		}
+		else if(on)	// power on
+		{
+			if(!hwPowerOn(hw->power_id, hw->power_vol, "BMA222"))
+			{
+				GSE_ERR("power on fails!!\n");
+			}
+		}
+		else	// power off
+		{
+			if (!hwPowerDown(hw->power_id, "BMA222"))
+			{
+				GSE_ERR("power off fail!!\n");
+			}			  
+		}
+	}
+	power_on = on;
+#endif //#ifndef FPGA_EARLY_PORTING
+#endif //__USE_LINUX_REGULATOR_FRAMEWORK__
+}
+/*----------------------------------------------------------------------------*/
+
+/*----------------------------------------------------------------------------*/
+static int BMA222_SetDataResolution(struct bma222_i2c_data *obj)
+{
+
+/*set g sensor dataresolution here*/
+
+/*BMA222 only can set to 10-bit dataresolution, so do nothing in bma222 driver here*/
+
+/*end of set dataresolution*/
+
+
+ 
+ /*we set measure range from -2g to +2g in BMA150_SetDataFormat(client, BMA150_RANGE_2G), 
+                                                    and set 10-bit dataresolution BMA150_SetDataResolution()*/
+                                                    
+ /*so bma222_data_resolution[0] set value as {{ 3, 9}, 256} when declaration, and assign the value to obj->reso here*/  
+
+ 	obj->reso = &bma222_data_resolution[0];
+	return 0;
+	
+/*if you changed the measure range, for example call: BMA222_SetDataFormat(client, BMA150_RANGE_4G), 
+you must set the right value to bma222_data_resolution*/
+
+}
+/*----------------------------------------------------------------------------*/
+static int BMA222_ReadData(struct i2c_client *client, s16 data[BMA222_AXES_NUM])
+{
+	struct bma222_i2c_data *priv = i2c_get_clientdata(client);        
+	int err = 0;
+#if 0 //CUSTOM_KERNEL_SENSORHUB
+    SCP_SENSOR_HUB_DATA req;
+    int len;
+#else //#ifdef CUSTOM_KERNEL_SENSORHUB
+    u8 addr = BMA222_REG_DATAXLOW;
+    u8 buf[BMA222_DATA_LEN] = {0};
+#endif //#ifdef CUSTOM_KERNEL_SENSORHUB
+
+#if 0 // CUSTOM_KERNEL_SENSORHUB
+    req.get_data_req.sensorType = ID_ACCELEROMETER;
+    req.get_data_req.action = SENSOR_HUB_GET_DATA;
+    len = sizeof(req.get_data_req);
+    err = SCP_sensorHub_req_send(&req, &len, 1);
+    if (err)
+    {
+        GSE_ERR("SCP_sensorHub_req_send!\n");
+        return err;
+    }
+
+    if (ID_ACCELEROMETER != req.get_data_rsp.sensorType ||
+        SENSOR_HUB_GET_DATA != req.get_data_rsp.action ||
+        0 != req.get_data_rsp.errCode)
+    {
+        GSE_ERR("error : %d\n", req.get_data_rsp.errCode);
+        return req.get_data_rsp.errCode;
+    }
+
+    len -= offsetof(SCP_SENSOR_HUB_GET_DATA_RSP, int8_Data);
+
+    if (6 == len)
+    {
+        data[BMA222_AXIS_X] = req.get_data_rsp.int16_Data[0];
+        data[BMA222_AXIS_Y] = req.get_data_rsp.int16_Data[1];
+        data[BMA222_AXIS_Z] = req.get_data_rsp.int16_Data[2];
+    }
+    else
+    {
+        GSE_ERR("data length fail : %d\n", len);
+    }
+
+    if(atomic_read(&priv->trace) & ADX_TRC_RAWDATA)
+	{
+        //show data
+	}
+#endif//#ifdef CUSTOM_KERNEL_SENSORHUB
+	if(NULL == client)
+	{
+		err = -EINVAL;
+	}
+	else if((err = bma_i2c_read_block(client, addr, buf, 0x05))!=0)
+	{
+		GSE_ERR("error: %d\n", err);
+	}
+	else
+	{
+		data[BMA222_AXIS_X] = (s16)buf[BMA222_AXIS_X*2] ;
+		data[BMA222_AXIS_Y] = (s16)buf[BMA222_AXIS_Y*2];
+		data[BMA222_AXIS_Z] = (s16)buf[BMA222_AXIS_Z*2] ;
+		if(atomic_read(&priv->trace) & ADX_TRC_RAWDATA)
+		{
+			GSE_LOG("[%08X %08X %08X] => [%5d %5d %5d] before\n", data[BMA222_AXIS_X], data[BMA222_AXIS_Y], data[BMA222_AXIS_Z],
+		                               data[BMA222_AXIS_X], data[BMA222_AXIS_Y], data[BMA222_AXIS_Z]);
+		}
+
+		if(data[BMA222_AXIS_X]&0x80)
+		{
+				data[BMA222_AXIS_X] = ~data[BMA222_AXIS_X];
+				data[BMA222_AXIS_X] &= 0xff;
+				data[BMA222_AXIS_X]+=1;
+				data[BMA222_AXIS_X] = -data[BMA222_AXIS_X];
+		}
+		if(data[BMA222_AXIS_Y]&0x80)
+		{
+				data[BMA222_AXIS_Y] = ~data[BMA222_AXIS_Y];
+				data[BMA222_AXIS_Y] &= 0xff;
+				data[BMA222_AXIS_Y]+=1;
+				data[BMA222_AXIS_Y] = -data[BMA222_AXIS_Y];
+		}
+		if(data[BMA222_AXIS_Z]&0x80)
+		{
+				data[BMA222_AXIS_Z] = ~data[BMA222_AXIS_Z];
+				data[BMA222_AXIS_Z] &= 0xff;
+				data[BMA222_AXIS_Z]+=1;
+				data[BMA222_AXIS_Z] = -data[BMA222_AXIS_Z];
+		}
+
+		if(atomic_read(&priv->trace) & ADX_TRC_RAWDATA)
+		{
+			GSE_LOG("[%08X %08X %08X] => [%5d %5d %5d] after\n", data[BMA222_AXIS_X], data[BMA222_AXIS_Y], data[BMA222_AXIS_Z],
+		                               data[BMA222_AXIS_X], data[BMA222_AXIS_Y], data[BMA222_AXIS_Z]);
+		}
+#ifdef CONFIG_BMA222_LOWPASS
+		if(atomic_read(&priv->filter))
+		{
+			if(atomic_read(&priv->fir_en) && !atomic_read(&priv->suspend))
+			{
+				int idx, firlen = atomic_read(&priv->firlen);   
+				if(priv->fir.num < firlen)
+				{                
+					priv->fir.raw[priv->fir.num][BMA222_AXIS_X] = data[BMA222_AXIS_X];
+					priv->fir.raw[priv->fir.num][BMA222_AXIS_Y] = data[BMA222_AXIS_Y];
+					priv->fir.raw[priv->fir.num][BMA222_AXIS_Z] = data[BMA222_AXIS_Z];
+					priv->fir.sum[BMA222_AXIS_X] += data[BMA222_AXIS_X];
+					priv->fir.sum[BMA222_AXIS_Y] += data[BMA222_AXIS_Y];
+					priv->fir.sum[BMA222_AXIS_Z] += data[BMA222_AXIS_Z];
+					if(atomic_read(&priv->trace) & ADX_TRC_FILTER)
+					{
+						GSE_LOG("add [%2d] [%5d %5d %5d] => [%5d %5d %5d]\n", priv->fir.num,
+							priv->fir.raw[priv->fir.num][BMA222_AXIS_X], priv->fir.raw[priv->fir.num][BMA222_AXIS_Y], priv->fir.raw[priv->fir.num][BMA222_AXIS_Z],
+							priv->fir.sum[BMA222_AXIS_X], priv->fir.sum[BMA222_AXIS_Y], priv->fir.sum[BMA222_AXIS_Z]);
+					}
+					priv->fir.num++;
+					priv->fir.idx++;
+				}
+				else
+				{
+					idx = priv->fir.idx % firlen;
+					priv->fir.sum[BMA222_AXIS_X] -= priv->fir.raw[idx][BMA222_AXIS_X];
+					priv->fir.sum[BMA222_AXIS_Y] -= priv->fir.raw[idx][BMA222_AXIS_Y];
+					priv->fir.sum[BMA222_AXIS_Z] -= priv->fir.raw[idx][BMA222_AXIS_Z];
+					priv->fir.raw[idx][BMA222_AXIS_X] = data[BMA222_AXIS_X];
+					priv->fir.raw[idx][BMA222_AXIS_Y] = data[BMA222_AXIS_Y];
+					priv->fir.raw[idx][BMA222_AXIS_Z] = data[BMA222_AXIS_Z];
+					priv->fir.sum[BMA222_AXIS_X] += data[BMA222_AXIS_X];
+					priv->fir.sum[BMA222_AXIS_Y] += data[BMA222_AXIS_Y];
+					priv->fir.sum[BMA222_AXIS_Z] += data[BMA222_AXIS_Z];
+					priv->fir.idx++;
+					data[BMA222_AXIS_X] = priv->fir.sum[BMA222_AXIS_X]/firlen;
+					data[BMA222_AXIS_Y] = priv->fir.sum[BMA222_AXIS_Y]/firlen;
+					data[BMA222_AXIS_Z] = priv->fir.sum[BMA222_AXIS_Z]/firlen;
+					if(atomic_read(&priv->trace) & ADX_TRC_FILTER)
+					{
+						GSE_LOG("add [%2d] [%5d %5d %5d] => [%5d %5d %5d] : [%5d %5d %5d]\n", idx,
+						priv->fir.raw[idx][BMA222_AXIS_X], priv->fir.raw[idx][BMA222_AXIS_Y], priv->fir.raw[idx][BMA222_AXIS_Z],
+						priv->fir.sum[BMA222_AXIS_X], priv->fir.sum[BMA222_AXIS_Y], priv->fir.sum[BMA222_AXIS_Z],
+						data[BMA222_AXIS_X], data[BMA222_AXIS_Y], data[BMA222_AXIS_Z]);
+					}
+				}
+			}
+		}	
+#endif         
+	}
+
+	return err;
+}
+/*----------------------------------------------------------------------------*/
+
+static int BMA222_ReadOffset(struct i2c_client *client, s8 ofs[BMA222_AXES_NUM])
+{    
+	int err;
+	err = 0;
+
+	
+#ifdef SW_CALIBRATION
+	ofs[0]=ofs[1]=ofs[2]=0x0;
+#else
+	if((err = bma_i2c_read_block(client, BMA222_REG_OFSX, ofs, BMA222_AXES_NUM)))
+	{
+		GSE_ERR("error: %d\n", err);
+	}
+#endif
+	//printk("offesx=%x, y=%x, z=%x",ofs[0],ofs[1],ofs[2]);
+	
+	return err;    
+}
+
+/*----------------------------------------------------------------------------*/
+static int BMA222_ResetCalibration(struct i2c_client *client)
+{
+	struct bma222_i2c_data *obj = i2c_get_clientdata(client);
+	//u8 ofs[4]={0,0,0,0};
+	int err = 0;
+#ifdef CUSTOM_KERNEL_SENSORHUB
+    SCP_SENSOR_HUB_DATA data;
+    BMA222_CUST_DATA *pCustData;
+    unsigned int len;
+#endif
+
+#ifdef GSENSOR_UT
+		GSE_FUN();
+#endif
+
+#ifdef CUSTOM_KERNEL_SENSORHUB
+    if (0 != obj->SCP_init_done)
+    {
+    pCustData = (BMA222_CUST_DATA *)&data.set_cust_req.custData;
+
+    data.set_cust_req.sensorType = ID_ACCELEROMETER;
+    data.set_cust_req.action = SENSOR_HUB_SET_CUST;
+    pCustData->resetCali.action = BMA222_CUST_ACTION_RESET_CALI;
+    len = offsetof(SCP_SENSOR_HUB_SET_CUST_REQ, custData) + sizeof(pCustData->resetCali);
+    SCP_sensorHub_req_send(&data, &len, 1);
+    }
+#endif
+
+	memset(obj->cali_sw, 0x00, sizeof(obj->cali_sw));
+	memset(obj->offset, 0x00, sizeof(obj->offset));
+	return err;    
+}
+/*----------------------------------------------------------------------------*/
+static int BMA222_ReadCalibration(struct i2c_client *client, int dat[BMA222_AXES_NUM])
+{
+    struct bma222_i2c_data *obj = i2c_get_clientdata(client);
+    int  err = 0;
+    int mul;
+
+    GSE_FUN();
+	#ifdef SW_CALIBRATION
+		mul = 0;//only SW Calibration, disable HW Calibration
+	#else
+	    if ((err = BMA222_ReadOffset(client, obj->offset))) {
+        GSE_ERR("read offset fail, %d\n", err);
+        return err;
+    	}    
+    	mul = obj->reso->sensitivity/bma222_offset_resolution.sensitivity;
+	#endif
+
+    dat[obj->cvt.map[BMA222_AXIS_X]] = obj->cvt.sign[BMA222_AXIS_X]*(obj->offset[BMA222_AXIS_X]*mul + obj->cali_sw[BMA222_AXIS_X]);
+    dat[obj->cvt.map[BMA222_AXIS_Y]] = obj->cvt.sign[BMA222_AXIS_Y]*(obj->offset[BMA222_AXIS_Y]*mul + obj->cali_sw[BMA222_AXIS_Y]);
+    dat[obj->cvt.map[BMA222_AXIS_Z]] = obj->cvt.sign[BMA222_AXIS_Z]*(obj->offset[BMA222_AXIS_Z]*mul + obj->cali_sw[BMA222_AXIS_Z]);                        
+                                       
+    return err;
+}
+/*----------------------------------------------------------------------------*/
+static int BMA222_ReadCalibrationEx(struct i2c_client *client, int act[BMA222_AXES_NUM], int raw[BMA222_AXES_NUM])
+{  
+	/*raw: the raw calibration data; act: the actual calibration data*/
+	struct bma222_i2c_data *obj = i2c_get_clientdata(client);
+	int err;
+	int mul;
+	err = 0;
+
+
+	#ifdef SW_CALIBRATION
+		mul = 0;//only SW Calibration, disable HW Calibration
+	#else
+		if((err = BMA222_ReadOffset(client, obj->offset)))
+		{
+			GSE_ERR("read offset fail, %d\n", err);
+			return err;
+		}   
+		mul = obj->reso->sensitivity/bma222_offset_resolution.sensitivity;
+	#endif
+	
+	raw[BMA222_AXIS_X] = obj->offset[BMA222_AXIS_X]*mul + obj->cali_sw[BMA222_AXIS_X];
+	raw[BMA222_AXIS_Y] = obj->offset[BMA222_AXIS_Y]*mul + obj->cali_sw[BMA222_AXIS_Y];
+	raw[BMA222_AXIS_Z] = obj->offset[BMA222_AXIS_Z]*mul + obj->cali_sw[BMA222_AXIS_Z];
+
+	act[obj->cvt.map[BMA222_AXIS_X]] = obj->cvt.sign[BMA222_AXIS_X]*raw[BMA222_AXIS_X];
+	act[obj->cvt.map[BMA222_AXIS_Y]] = obj->cvt.sign[BMA222_AXIS_Y]*raw[BMA222_AXIS_Y];
+	act[obj->cvt.map[BMA222_AXIS_Z]] = obj->cvt.sign[BMA222_AXIS_Z]*raw[BMA222_AXIS_Z];                        
+	                       
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int BMA222_WriteCalibration(struct i2c_client *client, int dat[BMA222_AXES_NUM])
+{
+	struct bma222_i2c_data *obj = i2c_get_clientdata(client);
+	int err = 0;
+	int cali[BMA222_AXES_NUM], raw[BMA222_AXES_NUM];
+#ifdef CUSTOM_KERNEL_SENSORHUB
+    SCP_SENSOR_HUB_DATA data;
+    BMA222_CUST_DATA *pCustData;
+    unsigned int len;
+#endif
+
+
+	if(0 != (err = BMA222_ReadCalibrationEx(client, cali, raw)))	/*offset will be updated in obj->offset*/
+	{ 
+		GSE_ERR("read offset fail, %d\n", err);
+		return err;
+	}
+
+	GSE_LOG("OLDOFF: (%+3d %+3d %+3d): (%+3d %+3d %+3d) / (%+3d %+3d %+3d)\n", 
+		raw[BMA222_AXIS_X], raw[BMA222_AXIS_Y], raw[BMA222_AXIS_Z],
+		obj->offset[BMA222_AXIS_X], obj->offset[BMA222_AXIS_Y], obj->offset[BMA222_AXIS_Z],
+		obj->cali_sw[BMA222_AXIS_X], obj->cali_sw[BMA222_AXIS_Y], obj->cali_sw[BMA222_AXIS_Z]);
+
+#ifdef CUSTOM_KERNEL_SENSORHUB
+    pCustData = (BMA222_CUST_DATA *)data.set_cust_req.custData;
+    data.set_cust_req.sensorType = ID_ACCELEROMETER;
+    data.set_cust_req.action = SENSOR_HUB_SET_CUST;
+    pCustData->setCali.action = BMA222_CUST_ACTION_SET_CALI;
+    pCustData->setCali.data[BMA222_AXIS_X] = dat[BMA222_AXIS_X];
+    pCustData->setCali.data[BMA222_AXIS_Y] = dat[BMA222_AXIS_Y];
+    pCustData->setCali.data[BMA222_AXIS_Z] = dat[BMA222_AXIS_Z];
+    len = offsetof(SCP_SENSOR_HUB_SET_CUST_REQ, custData) + sizeof(pCustData->setCali);
+    SCP_sensorHub_req_send(&data, &len, 1);
+#endif
+
+	/*calculate the real offset expected by caller*/
+	cali[BMA222_AXIS_X] += dat[BMA222_AXIS_X];
+	cali[BMA222_AXIS_Y] += dat[BMA222_AXIS_Y];
+	cali[BMA222_AXIS_Z] += dat[BMA222_AXIS_Z];
+
+	GSE_LOG("UPDATE: (%+3d %+3d %+3d)\n", 
+		dat[BMA222_AXIS_X], dat[BMA222_AXIS_Y], dat[BMA222_AXIS_Z]);
+
+#ifdef SW_CALIBRATION
+	obj->cali_sw[BMA222_AXIS_X] = obj->cvt.sign[BMA222_AXIS_X]*(cali[obj->cvt.map[BMA222_AXIS_X]]);
+	obj->cali_sw[BMA222_AXIS_Y] = obj->cvt.sign[BMA222_AXIS_Y]*(cali[obj->cvt.map[BMA222_AXIS_Y]]);
+	obj->cali_sw[BMA222_AXIS_Z] = obj->cvt.sign[BMA222_AXIS_Z]*(cali[obj->cvt.map[BMA222_AXIS_Z]]);	
+#else
+	int divisor = obj->reso->sensitivity/lsb;//modified
+	obj->offset[BMA222_AXIS_X] = (s8)(obj->cvt.sign[BMA222_AXIS_X]*(cali[obj->cvt.map[BMA222_AXIS_X]])/(divisor));
+	obj->offset[BMA222_AXIS_Y] = (s8)(obj->cvt.sign[BMA222_AXIS_Y]*(cali[obj->cvt.map[BMA222_AXIS_Y]])/(divisor));
+	obj->offset[BMA222_AXIS_Z] = (s8)(obj->cvt.sign[BMA222_AXIS_Z]*(cali[obj->cvt.map[BMA222_AXIS_Z]])/(divisor));
+
+	/*convert software calibration using standard calibration*/
+	obj->cali_sw[BMA222_AXIS_X] = obj->cvt.sign[BMA222_AXIS_X]*(cali[obj->cvt.map[BMA222_AXIS_X]])%(divisor);
+	obj->cali_sw[BMA222_AXIS_Y] = obj->cvt.sign[BMA222_AXIS_Y]*(cali[obj->cvt.map[BMA222_AXIS_Y]])%(divisor);
+	obj->cali_sw[BMA222_AXIS_Z] = obj->cvt.sign[BMA222_AXIS_Z]*(cali[obj->cvt.map[BMA222_AXIS_Z]])%(divisor);
+
+	GSE_LOG("NEWOFF: (%+3d %+3d %+3d): (%+3d %+3d %+3d) / (%+3d %+3d %+3d)\n", 
+		obj->offset[BMA222_AXIS_X]*divisor + obj->cali_sw[BMA222_AXIS_X], 
+		obj->offset[BMA222_AXIS_Y]*divisor + obj->cali_sw[BMA222_AXIS_Y], 
+		obj->offset[BMA222_AXIS_Z]*divisor + obj->cali_sw[BMA222_AXIS_Z], 
+		obj->offset[BMA222_AXIS_X], obj->offset[BMA222_AXIS_Y], obj->offset[BMA222_AXIS_Z],
+		obj->cali_sw[BMA222_AXIS_X], obj->cali_sw[BMA222_AXIS_Y], obj->cali_sw[BMA222_AXIS_Z]);
+
+	if((err = hwmsen_write_block(obj->client, BMA222_REG_OFSX, obj->offset, BMA222_AXES_NUM)))
+	{
+		GSE_ERR("write offset fail: %d\n", err);
+		return err;
+	}
+#endif
+	mdelay(1);
+	return err;
+}
+/*----------------------------------------------------------------------------*/
+static int BMA222_CheckDeviceID(struct i2c_client *client)
+{
+	u8 databuf[2]={0};    
+	int res = 0;
+	
+	
+	res = bma_i2c_read_block(client,BMA222_REG_DEVID,databuf,0x1);
+	if(res < 0)
+	{
+		goto exit_BMA222_CheckDeviceID;
+	}
+	
+
+	GSE_LOG("BMA222_CheckDeviceID %d done!\n ", databuf[0]);
+
+	exit_BMA222_CheckDeviceID:
+	if (res < 0)
+	{
+		GSE_ERR("BMA222_CheckDeviceID %d failt!\n ", BMA222_ERR_I2C);
+		return BMA222_ERR_I2C;
+	}
+	mdelay(1);
+	return BMA222_SUCCESS;
+}
+/*----------------------------------------------------------------------------*/
+static int BMA222_SetPowerMode(struct i2c_client *client, bool enable)
+{
+    struct bma222_i2c_data *obj = i2c_get_clientdata(client);
+	int res = 0;
+	
+    u8 databuf[2];
+	u8 addr = BMA222_REG_POWER_CTL;
+
+	if(enable == sensor_power )
+	{
+		GSE_LOG("Sensor power status is newest!\n");
+		return BMA222_SUCCESS;
+	}
+
+	if(bma_i2c_read_block(client, addr, databuf, 0x01))
+	{
+		GSE_ERR("read power ctl register err!\n");
+		return BMA222_ERR_I2C;
+	}
+	GSE_LOG("set power mode value = 0x%x!\n",databuf[0]);
+	mdelay(1);
+	if(enable == TRUE)
+	{
+		databuf[0] &= ~BMA222_MEASURE_MODE;
+	}
+	else
+	{
+		databuf[0] |= BMA222_MEASURE_MODE;
+	}
+	
+	res = bma_i2c_write_block(client,BMA222_REG_POWER_CTL,databuf,0x1);
+	if(res < 0)
+	{
+		GSE_LOG("set power mode failed!\n");
+		return BMA222_ERR_I2C;
+	}
+	else if(atomic_read(&obj->trace) & ADX_TRC_INFO)
+	{
+		GSE_LOG("set power mode ok %d!\n", databuf[1]);
+	}
+
+	
+	sensor_power = enable;
+	mdelay(1);
+	//GSE_LOG("leave Sensor power status is sensor_power = %d\n",sensor_power);
+	return BMA222_SUCCESS;    
+}
+/*----------------------------------------------------------------------------*/
+static int BMA222_SetDataFormat(struct i2c_client *client, u8 dataformat)
+{
+	struct bma222_i2c_data *obj = i2c_get_clientdata(client);
+	u8 databuf[10]={0};    
+	int res = 0;
+   
+	if(bma_i2c_read_block(client, BMA222_REG_DATA_FORMAT, databuf, 0x01))
+	{
+		printk("bma222 read Dataformat failt \n");
+		return BMA222_ERR_I2C;
+	}
+	mdelay(1);
+	databuf[0] &= ~BMA222_RANGE_MASK;
+	databuf[0] |= dataformat;
+	
+	res = bma_i2c_write_block(client,BMA222_REG_DATA_FORMAT,databuf,0x1);
+	if(res < 0)
+	{
+		return BMA222_ERR_I2C;
+	}
+	
+	//printk("BMA222_SetDataFormat OK! \n");
+	mdelay(1);
+	return BMA222_SetDataResolution(obj);    
+}
+/*----------------------------------------------------------------------------*/
+static int BMA222_SetBWRate(struct i2c_client *client, u8 bwrate)
+{
+	u8 databuf[10]={0};    
+	int res = 0;
+
+	if(bma_i2c_read_block(client, BMA222_REG_BW_RATE, databuf, 0x01))
+	{
+		printk("bma222 read rate failt \n");
+		return BMA222_ERR_I2C;
+	}
+	mdelay(1);
+	databuf[0] &= ~BMA222_BW_MASK;
+	databuf[0] |= bwrate;
+	
+
+    res = bma_i2c_write_block(client,BMA222_REG_BW_RATE,databuf,0x1);
+	if(res < 0)
+	{
+		return BMA222_ERR_I2C;
+	}
+	mdelay(1);
+	//printk("BMA222_SetBWRate OK! \n");
+	
+	return BMA222_SUCCESS;    
+}
+/*----------------------------------------------------------------------------*/
+static int BMA222_SetIntEnable(struct i2c_client *client, u8 intenable)
+{
+			//u8 databuf[10];    
+			int res = 0;
+		
+			res = hwmsen_write_byte(client, BMA222_INT_REG_1, 0x00);
+			if(res != BMA222_SUCCESS) 
+			{
+				return res;
+			}
+			mdelay(1);
+			res = hwmsen_write_byte(client, BMA222_INT_REG_2, 0x00);
+			if(res != BMA222_SUCCESS) 
+			{
+				return res;
+			}
+			//printk("BMA222 disable interrupt ...\n");
+		
+			/*for disable interrupt function*/
+			mdelay(1);
+			return BMA222_SUCCESS;	  
+}
+
+/*----------------------------------------------------------------------------*/
+static int bma222_init_client(struct i2c_client *client, int reset_cali)
+{
+	struct bma222_i2c_data *obj = i2c_get_clientdata(client);
+	int res = 0;
+
+    GSE_FUN();
+
+
+	res = BMA222_CheckDeviceID(client); 
+	if(res != BMA222_SUCCESS)
+	{
+		return res;
+	}	
+	//printk("BMA222_CheckDeviceID ok \n");
+	
+	res = BMA222_SetBWRate(client, BMA222_BW_100HZ);
+	if(res != BMA222_SUCCESS ) 
+	{
+		return res;
+	}
+	//printk("BMA222_SetBWRate OK!\n");
+	
+	res = BMA222_SetDataFormat(client, BMA222_RANGE_2G);
+	if(res != BMA222_SUCCESS) 
+	{
+		return res;
+	}
+	//printk("BMA222_SetDataFormat OK!\n");
+
+	gsensor_gain.x = gsensor_gain.y = gsensor_gain.z = obj->reso->sensitivity;
+
+#ifdef CUSTOM_KERNEL_SENSORHUB
+    res = gsensor_setup_irq();
+    if(res != BMA222_SUCCESS)
+	{
+		return res;
+	}
+#endif//#ifdef CUSTOM_KERNEL_SENSORHUB
+
+	res = BMA222_SetIntEnable(client, 0x00);        
+	if(res != BMA222_SUCCESS)
+	{
+		return res;
+	}
+	//printk("BMA222 disable interrupt function!\n");
+	
+	res = BMA222_SetPowerMode(client, enable_status);//false);//
+		if(res != BMA222_SUCCESS)
+		{
+			return res;
+		}
+	//printk("BMA222_SetPowerMode OK!\n");
+
+
+	if(0 != reset_cali)
+	{ 
+		/*reset calibration only in power on*/
+		res = BMA222_ResetCalibration(client);
+		if(res != BMA222_SUCCESS)
+		{
+			return res;
+		}
+	}
+	GSE_LOG("bma222_init_client OK!\n");
+#ifdef CONFIG_BMA222_LOWPASS
+	memset(&obj->fir, 0x00, sizeof(obj->fir));  
+#endif
+	return BMA222_SUCCESS;
+}
+/*----------------------------------------------------------------------------*/
+static int BMA222_ReadChipInfo(struct i2c_client *client, char *buf, int bufsize)
+{
+	u8 databuf[10];    
+
+	memset(databuf, 0, sizeof(u8)*10);
+
+	if((NULL == buf)||(bufsize<=30))
+	{
+		return -1;
+	}
+	
+	if(NULL == client)
+	{
+		*buf = 0;
+		return -2;
+	}
+
+	sprintf(buf, "BMA222 Chip");
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int BMA222_ReadSensorData(struct i2c_client *client, char *buf, int bufsize)
+{
+	struct bma222_i2c_data *obj = (struct bma222_i2c_data*)i2c_get_clientdata(client);
+	u8 databuf[20];
+	int acc[BMA222_AXES_NUM];
+	int res = 0;
+	memset(databuf, 0, sizeof(u8)*10);
+
+	if(NULL == buf)
+	{
+		return -1;
+	}
+	if(NULL == client)
+	{
+		*buf = 0;
+		return -2;
+	}
+		
+	if(sensor_suspend == 1)
+	{
+		//GSE_LOG("sensor in suspend read not data!\n");
+		return 0;
+	}
+
+	if((res = BMA222_ReadData(client, obj->data))!=0)
+	{        
+		GSE_ERR("I2C error: ret value=%d", res);
+		return -3;
+	}
+	else
+	{
+#if 0 // CUSTOM_KERNEL_SENSORHUB
+        acc[BMA222_AXIS_X] = obj->data[BMA222_AXIS_X];
+		acc[BMA222_AXIS_Y] = obj->data[BMA222_AXIS_Y];
+		acc[BMA222_AXIS_Z] = obj->data[BMA222_AXIS_Z];
+        //data has been calibrated in SCP side.
+#else //#ifdef CUSTOM_KERNEL_SENSORHUB
+		//GSE_LOG("raw data x=%d, y=%d, z=%d \n",obj->data[BMA222_AXIS_X],obj->data[BMA222_AXIS_Y],obj->data[BMA222_AXIS_Z]);
+		obj->data[BMA222_AXIS_X] += obj->cali_sw[BMA222_AXIS_X];
+		obj->data[BMA222_AXIS_Y] += obj->cali_sw[BMA222_AXIS_Y];
+		obj->data[BMA222_AXIS_Z] += obj->cali_sw[BMA222_AXIS_Z];
+		
+		//printk("cali_sw x=%d, y=%d, z=%d \n",obj->cali_sw[BMA150_AXIS_X],obj->cali_sw[BMA150_AXIS_Y],obj->cali_sw[BMA150_AXIS_Z]);
+		
+		/*remap coordinate*/
+		acc[obj->cvt.map[BMA222_AXIS_X]] = obj->cvt.sign[BMA222_AXIS_X]*obj->data[BMA222_AXIS_X];
+		acc[obj->cvt.map[BMA222_AXIS_Y]] = obj->cvt.sign[BMA222_AXIS_Y]*obj->data[BMA222_AXIS_Y];
+		acc[obj->cvt.map[BMA222_AXIS_Z]] = obj->cvt.sign[BMA222_AXIS_Z]*obj->data[BMA222_AXIS_Z];
+		//printk("cvt x=%d, y=%d, z=%d \n",obj->cvt.sign[BMA150_AXIS_X],obj->cvt.sign[BMA150_AXIS_Y],obj->cvt.sign[BMA150_AXIS_Z]);
+
+
+		//GSE_LOG("Mapped gsensor data: %d, %d, %d!\n", acc[BMA150_AXIS_X], acc[BMA150_AXIS_Y], acc[BMA150_AXIS_Z]);
+
+		//Out put the mg
+		//printk("mg acc=%d, GRAVITY=%d, sensityvity=%d \n",acc[BMA150_AXIS_X],GRAVITY_EARTH_1000,obj->reso->sensitivity);
+		acc[BMA222_AXIS_X] = acc[BMA222_AXIS_X] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+		acc[BMA222_AXIS_Y] = acc[BMA222_AXIS_Y] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+		acc[BMA222_AXIS_Z] = acc[BMA222_AXIS_Z] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;		
+#endif //#ifdef CUSTOM_KERNEL_SENSORHUB
+	
+
+		sprintf(buf, "%04x %04x %04x", acc[BMA222_AXIS_X], acc[BMA222_AXIS_Y], acc[BMA222_AXIS_Z]);
+		if(atomic_read(&obj->trace) & ADX_TRC_IOCTL)
+		{
+			GSE_LOG("gsensor data: %s!\n", buf);
+		}
+	}
+	
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int BMA222_ReadRawData(struct i2c_client *client, char *buf)
+{
+	struct bma222_i2c_data *obj = (struct bma222_i2c_data*)i2c_get_clientdata(client);
+	int res = 0;
+
+	if (!buf || !client)
+	{
+		return EINVAL;
+	}
+	
+	if(0 != (res = BMA222_ReadData(client, obj->data)))
+	{        
+		GSE_ERR("I2C error: ret value=%d", res);
+		return EIO;
+	}
+	else
+	{
+		sprintf(buf, "BMA222_ReadRawData %04x %04x %04x", obj->data[BMA222_AXIS_X], 
+			obj->data[BMA222_AXIS_Y], obj->data[BMA222_AXIS_Z]);
+	
+	}
+	
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t show_chipinfo_value(struct device_driver *ddri, char *buf)
+{
+	struct i2c_client *client = bma222_i2c_client;
+	char strbuf[BMA222_BUFSIZE];
+	if(NULL == client)
+	{
+		GSE_ERR("i2c client is null!!\n");
+		return 0;
+	}
+	
+	BMA222_ReadChipInfo(client, strbuf, BMA222_BUFSIZE);
+	return snprintf(buf, PAGE_SIZE, "%s\n", strbuf);        
+}
+
+#if 0
+static ssize_t gsensor_init(struct device_driver *ddri, char *buf, size_t count)
+	{
+		struct i2c_client *client = bma222_i2c_client;
+		char strbuf[BMA222_BUFSIZE];
+		
+		if(NULL == client)
+		{
+			GSE_ERR("i2c client is null!!\n");
+			return 0;
+		}
+		bma222_init_client(client, 1);
+		return snprintf(buf, PAGE_SIZE, "%s\n", strbuf);			
+	}
+#endif
+
+
+/*----------------------------------------------------------------------------*/
+static ssize_t show_sensordata_value(struct device_driver *ddri, char *buf)
+{
+	struct i2c_client *client = bma222_i2c_client;
+	char strbuf[BMA222_BUFSIZE];
+	
+	if(NULL == client)
+	{
+		GSE_ERR("i2c client is null!!\n");
+		return 0;
+	}
+	BMA222_ReadSensorData(client, strbuf, BMA222_BUFSIZE);
+	//BMA150_ReadRawData(client, strbuf);
+	return snprintf(buf, PAGE_SIZE, "%s\n", strbuf);            
+}
+
+#if 0
+static ssize_t show_sensorrawdata_value(struct device_driver *ddri, char *buf, size_t count)
+	{
+		struct i2c_client *client = bma222_i2c_client;
+		char strbuf[BMA222_BUFSIZE];
+		
+		if(NULL == client)
+		{
+			GSE_ERR("i2c client is null!!\n");
+			return 0;
+		}
+		//BMA150_ReadSensorData(client, strbuf, BMA150_BUFSIZE);
+		BMA222_ReadRawData(client, strbuf);
+		return snprintf(buf, PAGE_SIZE, "%s\n", strbuf);			
+	}
+#endif
+
+/*----------------------------------------------------------------------------*/
+#if 1
+static ssize_t show_cali_value(struct device_driver *ddri, char *buf)
+{
+	struct i2c_client *client = bma222_i2c_client;
+	struct bma222_i2c_data *obj;
+	int err, len = 0, mul;
+	int tmp[BMA222_AXES_NUM];
+
+	if(NULL == client)
+	{
+		GSE_ERR("i2c client is null!!\n");
+		return 0;
+	}
+
+	obj = i2c_get_clientdata(client);
+
+
+
+	if(0 != (err = BMA222_ReadOffset(client, obj->offset)))
+	{
+		return -EINVAL;
+	}
+	else if(0 != (err = BMA222_ReadCalibration(client, tmp)))
+	{
+		return -EINVAL;
+	}
+	else
+	{    
+		mul = obj->reso->sensitivity/bma222_offset_resolution.sensitivity;
+		len += snprintf(buf+len, PAGE_SIZE-len, "[HW ][%d] (%+3d, %+3d, %+3d) : (0x%02X, 0x%02X, 0x%02X)\n", mul,                        
+			obj->offset[BMA222_AXIS_X], obj->offset[BMA222_AXIS_Y], obj->offset[BMA222_AXIS_Z],
+			obj->offset[BMA222_AXIS_X], obj->offset[BMA222_AXIS_Y], obj->offset[BMA222_AXIS_Z]);
+		len += snprintf(buf+len, PAGE_SIZE-len, "[SW ][%d] (%+3d, %+3d, %+3d)\n", 1, 
+			obj->cali_sw[BMA222_AXIS_X], obj->cali_sw[BMA222_AXIS_Y], obj->cali_sw[BMA222_AXIS_Z]);
+
+		len += snprintf(buf+len, PAGE_SIZE-len, "[ALL]    (%+3d, %+3d, %+3d) : (%+3d, %+3d, %+3d)\n", 
+			obj->offset[BMA222_AXIS_X]*mul + obj->cali_sw[BMA222_AXIS_X],
+			obj->offset[BMA222_AXIS_Y]*mul + obj->cali_sw[BMA222_AXIS_Y],
+			obj->offset[BMA222_AXIS_Z]*mul + obj->cali_sw[BMA222_AXIS_Z],
+			tmp[BMA222_AXIS_X], tmp[BMA222_AXIS_Y], tmp[BMA222_AXIS_Z]);
+		
+		return len;
+    }
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t store_cali_value(struct device_driver *ddri, const char *buf, size_t count)
+{
+	struct i2c_client *client = bma222_i2c_client;  
+	int err, x, y, z;
+	int dat[BMA222_AXES_NUM];
+
+	if(!strncmp(buf, "rst", 3))
+	{
+		if(0 != (err = BMA222_ResetCalibration(client)))
+		{
+			GSE_ERR("reset offset err = %d\n", err);
+		}	
+	}
+	else if(3 == sscanf(buf, "0x%02X 0x%02X 0x%02X", &x, &y, &z))
+	{
+		dat[BMA222_AXIS_X] = x;
+		dat[BMA222_AXIS_Y] = y;
+		dat[BMA222_AXIS_Z] = z;
+		if(0 != (err = BMA222_WriteCalibration(client, dat)))
+		{
+			GSE_ERR("write calibration err = %d\n", err);
+		}		
+	}
+	else
+	{
+		GSE_ERR("invalid format\n");
+	}
+	
+	return count;
+}
+#endif
+
+/*----------------------------------------------------------------------------*/
+static ssize_t show_firlen_value(struct device_driver *ddri, char *buf)
+{
+#ifdef CONFIG_BMA222_LOWPASS
+	struct i2c_client *client = bma222_i2c_client;
+	struct bma222_i2c_data *obj = i2c_get_clientdata(client);
+	if(atomic_read(&obj->firlen))
+	{
+		int idx, len = atomic_read(&obj->firlen);
+		GSE_LOG("len = %2d, idx = %2d\n", obj->fir.num, obj->fir.idx);
+
+		for(idx = 0; idx < len; idx++)
+		{
+			GSE_LOG("[%5d %5d %5d]\n", obj->fir.raw[idx][BMA222_AXIS_X], obj->fir.raw[idx][BMA222_AXIS_Y], obj->fir.raw[idx][BMA222_AXIS_Z]);
+		}
+		
+		GSE_LOG("sum = [%5d %5d %5d]\n", obj->fir.sum[BMA222_AXIS_X], obj->fir.sum[BMA222_AXIS_Y], obj->fir.sum[BMA222_AXIS_Z]);
+		GSE_LOG("avg = [%5d %5d %5d]\n", obj->fir.sum[BMA222_AXIS_X]/len, obj->fir.sum[BMA222_AXIS_Y]/len, obj->fir.sum[BMA222_AXIS_Z]/len);
+	}
+	return snprintf(buf, PAGE_SIZE, "%d\n", atomic_read(&obj->firlen));
+#else
+	return snprintf(buf, PAGE_SIZE, "not support\n");
+#endif
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t store_firlen_value(struct device_driver *ddri, const char *buf, size_t count)
+{
+#ifdef CONFIG_BMA222_LOWPASS
+	struct i2c_client *client = bma222_i2c_client;  
+	struct bma222_i2c_data *obj = i2c_get_clientdata(client);
+	int firlen;
+
+	if(1 != sscanf(buf, "%d", &firlen))
+	{
+		GSE_ERR("invallid format\n");
+	}
+	else if(firlen > C_MAX_FIR_LENGTH)
+	{
+		GSE_ERR("exceeds maximum filter length\n");
+	}
+	else
+	{ 
+		atomic_set(&obj->firlen, firlen);
+		if(NULL == firlen)
+		{
+			atomic_set(&obj->fir_en, 0);
+		}
+		else
+		{
+			memset(&obj->fir, 0x00, sizeof(obj->fir));
+			atomic_set(&obj->fir_en, 1);
+		}
+	}
+#endif    
+	return count;
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t show_trace_value(struct device_driver *ddri, char *buf)
+{
+	ssize_t res;
+	struct bma222_i2c_data *obj = obj_i2c_data;
+	if (obj == NULL)
+	{
+		GSE_ERR("i2c_data obj is null!!\n");
+		return 0;
+	}
+	
+	res = snprintf(buf, PAGE_SIZE, "0x%04X\n", atomic_read(&obj->trace));     
+	return res;    
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t store_trace_value(struct device_driver *ddri, const char *buf, size_t count)
+{
+	struct bma222_i2c_data *obj = obj_i2c_data;
+	int trace;
+	if (obj == NULL)
+	{
+		GSE_ERR("i2c_data obj is null!!\n");
+		return 0;
+	}
+	
+	if(1 == sscanf(buf, "0x%x", &trace))
+	{
+		atomic_set(&obj->trace, trace);
+	}	
+	else
+	{
+		GSE_ERR("invalid content: '%s', length = %d\n", buf, (int)count);
+	}
+	
+	return count;    
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t show_status_value(struct device_driver *ddri, char *buf)
+{
+	ssize_t len = 0;    
+	struct bma222_i2c_data *obj = obj_i2c_data;
+	if (obj == NULL)
+	{
+		GSE_ERR("i2c_data obj is null!!\n");
+		return 0;
+	}	
+	
+	if(obj->hw)
+	{
+		len += snprintf(buf+len, PAGE_SIZE-len, "CUST: %d %d (%d %d)\n", 
+	            obj->hw->i2c_num, obj->hw->direction, obj->hw->power_id, obj->hw->power_vol);   
+	}
+	else
+	{
+		len += snprintf(buf+len, PAGE_SIZE-len, "CUST: NULL\n");
+	}
+	return len;    
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t show_power_status_value(struct device_driver *ddri, char *buf)
+{
+	
+	u8 databuf[2];    
+	//int res = 0;
+	u8 addr = BMA222_REG_POWER_CTL;
+	struct bma222_i2c_data *obj = obj_i2c_data;
+	if(bma_i2c_read_block(obj->client, addr, databuf, 0x01))
+	{
+		GSE_ERR("read power ctl register err!\n");
+		return 1;
+	}
+	
+	if(sensor_power)
+		GSE_LOG("G sensor is in work mode, sensor_power = %d\n", sensor_power);
+	else
+		GSE_LOG("G sensor is in standby mode, sensor_power = %d\n", sensor_power);
+
+	return snprintf(buf, PAGE_SIZE, "%x\n", databuf[0]);
+}
+static ssize_t show_chip_orientation(struct device_driver *ddri, char *pbBuf)
+{
+    ssize_t          _tLength = 0;
+    struct acc_hw   *_ptAccelHw = get_cust_acc_hw();
+
+    GSE_LOG("[%s] default direction: %d\n", __FUNCTION__, _ptAccelHw->direction);
+
+    _tLength = snprintf(pbBuf, PAGE_SIZE, "default direction = %d\n", _ptAccelHw->direction);
+
+    return (_tLength);
+}
+
+
+static ssize_t store_chip_orientation(struct device_driver *ddri, const char *pbBuf, size_t tCount)
+{
+    int                       _nDirection = 0;
+    struct bma222_i2c_data   *_pt_i2c_obj = obj_i2c_data;
+
+    if (NULL == _pt_i2c_obj)
+        return (0);
+
+    if (1 == sscanf(pbBuf, "%d", &_nDirection))
+    {
+        if (hwmsen_get_convert(_nDirection, &_pt_i2c_obj->cvt))
+            GSE_ERR("ERR: fail to set direction\n");
+    }
+
+    GSE_LOG("[%s] set direction: %d\n", __FUNCTION__, _nDirection);
+
+    return (tCount);
+}
+/*----------------------------------------------------------------------------*/
+static DRIVER_ATTR(chipinfo,   S_IWUSR | S_IRUGO, show_chipinfo_value,      NULL);
+static DRIVER_ATTR(sensordata, S_IWUSR | S_IRUGO, show_sensordata_value,    NULL);
+static DRIVER_ATTR(cali,       S_IWUSR | S_IRUGO, show_cali_value,          store_cali_value);
+static DRIVER_ATTR(firlen,     S_IWUSR | S_IRUGO, show_firlen_value,        store_firlen_value);
+static DRIVER_ATTR(trace,      S_IWUSR | S_IRUGO, show_trace_value,         store_trace_value);
+static DRIVER_ATTR(status,     S_IRUGO, show_status_value,        NULL);
+static DRIVER_ATTR(powerstatus,               S_IRUGO, show_power_status_value,        NULL);
+static DRIVER_ATTR(orientation, S_IWUSR | S_IRUGO, show_chip_orientation, store_chip_orientation);
+
+/*----------------------------------------------------------------------------*/
+static struct driver_attribute *bma222_attr_list[] = {
+	&driver_attr_chipinfo,     /*chip information*/
+	&driver_attr_sensordata,   /*dump sensor data*/
+	&driver_attr_cali,         /*show calibration data*/
+	&driver_attr_firlen,       /*filter length: 0: disable, others: enable*/
+	&driver_attr_trace,        /*trace log*/
+	&driver_attr_status,
+	&driver_attr_powerstatus,
+	&driver_attr_orientation,
+};
+/*----------------------------------------------------------------------------*/
+static int bma222_create_attr(struct device_driver *driver) 
+{
+	int idx, err = 0;
+	int num = (int)(sizeof(bma222_attr_list)/sizeof(bma222_attr_list[0]));
+	if (driver == NULL)
+	{
+		return -EINVAL;
+	}
+
+	for(idx = 0; idx < num; idx++)
+	{
+		if(0 != (err = driver_create_file(driver, bma222_attr_list[idx])))
+		{            
+			GSE_ERR("driver_create_file (%s) = %d\n", bma222_attr_list[idx]->attr.name, err);
+			break;
+		}
+	}    
+	return err;
+}
+/*----------------------------------------------------------------------------*/
+static int bma222_delete_attr(struct device_driver *driver)
+{
+	int idx ,err = 0;
+	int num = (int)(sizeof(bma222_attr_list)/sizeof(bma222_attr_list[0]));
+
+	if(driver == NULL)
+	{
+		return -EINVAL;
+	}
+	
+
+	for(idx = 0; idx < num; idx++)
+	{
+		driver_remove_file(driver, bma222_attr_list[idx]);
+	}
+	
+
+	return err;
+}
+
+/*----------------------------------------------------------------------------*/
+#ifdef CUSTOM_KERNEL_SENSORHUB
+static void gsensor_irq_work(struct work_struct *work)
+{
+    struct bma222_i2c_data *obj = obj_i2c_data;
+    struct scp_acc_hw scp_hw;
+    BMA222_CUST_DATA *p_cust_data;
+    SCP_SENSOR_HUB_DATA data;
+    int max_cust_data_size_per_packet;
+    int i;
+    uint sizeOfCustData;
+    uint len;
+    char *p = (char *)&scp_hw;
+
+	GSE_FUN();
+
+    scp_hw.i2c_num = obj->hw->i2c_num;
+    scp_hw.direction = obj->hw->direction;
+    scp_hw.power_id = obj->hw->power_id;
+    scp_hw.power_vol = obj->hw->power_vol;
+    scp_hw.firlen = obj->hw->firlen;
+    memcpy(scp_hw.i2c_addr, obj->hw->i2c_addr, sizeof(obj->hw->i2c_addr));
+    scp_hw.power_vio_id = obj->hw->power_vio_id;
+    scp_hw.power_vio_vol = obj->hw->power_vio_vol;
+    scp_hw.is_batch_supported = obj->hw->is_batch_supported;
+
+    p_cust_data = (BMA222_CUST_DATA *)data.set_cust_req.custData;
+    sizeOfCustData = sizeof(scp_hw);
+    max_cust_data_size_per_packet = sizeof(data.set_cust_req.custData) - offsetof(BMA222_SET_CUST, data);
+    
+	GSE_ERR("sizeOfCustData = %d, max_cust_data_size_per_packet = %d\n", sizeOfCustData, max_cust_data_size_per_packet);
+    GSE_ERR("offset %d\n", offsetof(BMA222_SET_CUST, data));
+
+	for (i=0;sizeOfCustData>0;i++)
+    {
+        data.set_cust_req.sensorType = ID_ACCELEROMETER;
+        data.set_cust_req.action = SENSOR_HUB_SET_CUST;
+        p_cust_data->setCust.action = BMA222_CUST_ACTION_SET_CUST;
+        p_cust_data->setCust.part = i;
+        if (sizeOfCustData > max_cust_data_size_per_packet)
+        {
+            len = max_cust_data_size_per_packet;
+        }
+        else
+        {
+            len = sizeOfCustData;
+        }
+
+        memcpy(p_cust_data->setCust.data, p, len);
+        sizeOfCustData -= len;
+        p += len;
+		
+        GSE_ERR("i= %d, sizeOfCustData = %d, len = %d \n", i, sizeOfCustData, len);        
+        len += offsetof(SCP_SENSOR_HUB_SET_CUST_REQ, custData) + offsetof(BMA222_SET_CUST, data);
+        GSE_ERR("data.set_cust_req.sensorType= %d \n",  data.set_cust_req.sensorType);  		
+        SCP_sensorHub_req_send(&data, &len, 1);
+		
+    }
+		p_cust_data = (BMA222_CUST_DATA *)&data.set_cust_req.custData;
+		data.set_cust_req.sensorType = ID_ACCELEROMETER;
+		data.set_cust_req.action = SENSOR_HUB_SET_CUST;
+		p_cust_data->resetCali.action = BMA222_CUST_ACTION_RESET_CALI;
+		len = offsetof(SCP_SENSOR_HUB_SET_CUST_REQ, custData) + sizeof(p_cust_data->resetCali);
+		SCP_sensorHub_req_send(&data, &len, 1);
+		obj->SCP_init_done = 1;
+}
+/*----------------------------------------------------------------------------*/
+static int gsensor_irq_handler(void* data, uint len)
+{
+	struct bma222_i2c_data *obj = obj_i2c_data;
+    SCP_SENSOR_HUB_DATA_P rsp = (SCP_SENSOR_HUB_DATA_P)data;
+    
+	GSE_ERR("gsensor_irq_handler len = %d, type = %d, action = %d, errCode = %d\n", len, rsp->rsp.sensorType, rsp->rsp.action, rsp->rsp.errCode);
+	if(!obj)
+	{
+		return -1;
+	}
+
+    switch(rsp->rsp.action)
+    {
+        case SENSOR_HUB_NOTIFY:
+            switch(rsp->notify_rsp.event)
+            {
+                case SCP_INIT_DONE:
+                    schedule_work(&obj->irq_work);
+					GSE_ERR("OK sensor hub notify\n");
+                    break;
+                default:
+                    GSE_ERR("Error sensor hub notify\n");
+                    break;
+            }
+            break;
+        default:
+            GSE_ERR("Error sensor hub action\n");
+            break;
+    }
+
+    return 0;
+}
+
+static int gsensor_setup_irq()
+{
+    int err = 0;
+
+
+
+    err = SCP_sensorHub_rsp_registration(ID_ACCELEROMETER, gsensor_irq_handler);
+    
+	return err;
+}
+#endif//#ifdef CUSTOM_KERNEL_SENSORHUB
+/****************************************************************************** 
+ * Function Configuration
+******************************************************************************/
+static int bma222_open(struct inode *inode, struct file *file)
+{
+	file->private_data = bma222_i2c_client;
+
+	if(file->private_data == NULL)
+	{
+		GSE_ERR("null pointer!!\n");
+		return -EINVAL;
+	}
+	return nonseekable_open(inode, file);
+}
+/*----------------------------------------------------------------------------*/
+static int bma222_release(struct inode *inode, struct file *file)
+{
+	file->private_data = NULL;
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+//static int bma222_ioctl(struct inode *inode, struct file *file, unsigned int cmd,
+//       unsigned long arg)
+static long bma222_unlocked_ioctl(struct file *file, unsigned int cmd,
+       unsigned long arg)
+
+{
+	struct i2c_client *client = (struct i2c_client*)file->private_data;
+	struct bma222_i2c_data *obj = (struct bma222_i2c_data*)i2c_get_clientdata(client);	
+	char strbuf[BMA222_BUFSIZE];
+	void __user *data;
+	SENSOR_DATA sensor_data;
+	long err = 0;
+	int cali[3];
+
+	//GSE_FUN(f);
+	if(_IOC_DIR(cmd) & _IOC_READ)
+	{
+		err = !access_ok(VERIFY_WRITE, (void __user *)arg, _IOC_SIZE(cmd));
+	}
+	else if(_IOC_DIR(cmd) & _IOC_WRITE)
+	{
+		err = !access_ok(VERIFY_READ, (void __user *)arg, _IOC_SIZE(cmd));
+	}
+
+	if(err)
+	{
+		GSE_ERR("access error: %08X, (%2d, %2d)\n", cmd, _IOC_DIR(cmd), _IOC_SIZE(cmd));
+		return -EFAULT;
+	}
+
+	switch(cmd)
+	{
+		case GSENSOR_IOCTL_INIT:
+			bma222_init_client(client, 0);	
+			break;
+
+		case GSENSOR_IOCTL_READ_CHIPINFO:
+			data = (void __user *) arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}
+			
+			BMA222_ReadChipInfo(client, strbuf, BMA222_BUFSIZE);
+			if(copy_to_user(data, strbuf, strlen(strbuf)+1))
+			{
+				err = -EFAULT;
+				break;
+			}				 
+			break;	  
+
+		case GSENSOR_IOCTL_READ_SENSORDATA:
+			data = (void __user *) arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}
+			BMA222_SetPowerMode(client,true);	
+			BMA222_ReadSensorData(client, strbuf, BMA222_BUFSIZE);
+			if(copy_to_user(data, strbuf, strlen(strbuf)+1))
+			{
+				err = -EFAULT;
+				break;	  
+			}				 
+			break;
+
+		case GSENSOR_IOCTL_READ_GAIN:
+			data = (void __user *) arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}			
+			
+			if(copy_to_user(data, &gsensor_gain, sizeof(GSENSOR_VECTOR3D)))
+			{
+				err = -EFAULT;
+				break;
+			}				 
+			break;
+
+		case GSENSOR_IOCTL_READ_RAW_DATA:
+			data = (void __user *) arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}
+			BMA222_ReadRawData(client, strbuf);
+			if(copy_to_user(data, &strbuf, strlen(strbuf)+1))
+			{
+				err = -EFAULT;
+				break;	  
+			}
+			break;	  
+
+		case GSENSOR_IOCTL_SET_CALI:
+			data = (void __user*)arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}
+			if(copy_from_user(&sensor_data, data, sizeof(sensor_data)))
+			{
+				err = -EFAULT;
+				break;	  
+			}
+			if(atomic_read(&obj->suspend))
+			{
+				GSE_ERR("Perform calibration in suspend state!!\n");
+				err = -EINVAL;
+			}
+			else
+			{
+				cali[BMA222_AXIS_X] = sensor_data.x * obj->reso->sensitivity / GRAVITY_EARTH_1000;
+				cali[BMA222_AXIS_Y] = sensor_data.y * obj->reso->sensitivity / GRAVITY_EARTH_1000;
+				cali[BMA222_AXIS_Z] = sensor_data.z * obj->reso->sensitivity / GRAVITY_EARTH_1000;			  
+				err = BMA222_WriteCalibration(client, cali);			 
+			}
+			break;
+
+		case GSENSOR_IOCTL_CLR_CALI:
+			err = BMA222_ResetCalibration(client);
+			break;
+
+		case GSENSOR_IOCTL_GET_CALI:
+			data = (void __user*)arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}
+			if(0 != (err = BMA222_ReadCalibration(client, cali)))
+			{
+				break;
+			}
+			
+			sensor_data.x = cali[BMA222_AXIS_X] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+			sensor_data.y = cali[BMA222_AXIS_Y] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+			sensor_data.z = cali[BMA222_AXIS_Z] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+			if(copy_to_user(data, &sensor_data, sizeof(sensor_data)))
+			{
+				err = -EFAULT;
+				break;
+			}		
+			break;
+		
+
+		default:
+			GSE_ERR("unknown IOCTL: 0x%08x\n", cmd);
+			err = -ENOIOCTLCMD;
+			break;
+			
+	}
+
+	return err;
+}
+
+
+#ifdef CONFIG_COMPAT
+static long bma222_compat_ioctl(struct file *file, unsigned int cmd,
+       unsigned long arg)
+{
+    long err = 0;
+
+	void __user *arg32 = compat_ptr(arg);
+	
+	if (!file->f_op || !file->f_op->unlocked_ioctl)
+		return -ENOTTY;
+	
+    switch (cmd)
+    {
+        case COMPAT_GSENSOR_IOCTL_READ_SENSORDATA:
+            if (arg32 == NULL)
+            {
+                err = -EINVAL;
+                break;    
+            }
+		
+		    err = file->f_op->unlocked_ioctl(file, GSENSOR_IOCTL_READ_SENSORDATA, (unsigned long)arg32);
+		    if (err){
+		        GSE_ERR("GSENSOR_IOCTL_READ_SENSORDATA unlocked_ioctl failed.");
+		        return err;
+		    }
+        break;
+        case COMPAT_GSENSOR_IOCTL_SET_CALI:
+            if (arg32 == NULL)
+            {
+                err = -EINVAL;
+                break;    
+            }
+		
+		    err = file->f_op->unlocked_ioctl(file, GSENSOR_IOCTL_SET_CALI, (unsigned long)arg32);
+		    if (err){
+		        GSE_ERR("GSENSOR_IOCTL_SET_CALI unlocked_ioctl failed.");
+		        return err;
+		    }
+        break;
+        case COMPAT_GSENSOR_IOCTL_GET_CALI:
+            if (arg32 == NULL)
+            {
+                err = -EINVAL;
+                break;    
+            }
+		
+		    err = file->f_op->unlocked_ioctl(file, GSENSOR_IOCTL_GET_CALI, (unsigned long)arg32);
+		    if (err){
+		        GSE_ERR("GSENSOR_IOCTL_GET_CALI unlocked_ioctl failed.");
+		        return err;
+		    }
+        break;
+        case COMPAT_GSENSOR_IOCTL_CLR_CALI:
+            if (arg32 == NULL)
+            {
+                err = -EINVAL;
+                break;    
+            }
+		
+		    err = file->f_op->unlocked_ioctl(file, GSENSOR_IOCTL_CLR_CALI, (unsigned long)arg32);
+		    if (err){
+		        GSE_ERR("GSENSOR_IOCTL_CLR_CALI unlocked_ioctl failed.");
+		        return err;
+		    }
+        break;
+
+        default:
+            GSE_ERR("unknown IOCTL: 0x%08x\n", cmd);
+            err = -ENOIOCTLCMD;
+        break;
+
+    }
+
+    return err;
+}
+#endif
+/*----------------------------------------------------------------------------*/
+static struct file_operations bma222_fops = {
+	.owner = THIS_MODULE,
+	.open = bma222_open,
+	.release = bma222_release,
+	.unlocked_ioctl = bma222_unlocked_ioctl,
+	#ifdef CONFIG_COMPAT
+	.compat_ioctl = bma222_compat_ioctl,
+	#endif
+};
+/*----------------------------------------------------------------------------*/
+static struct miscdevice bma222_device = {
+	.minor = MISC_DYNAMIC_MINOR,
+	.name = "gsensor",
+	.fops = &bma222_fops,
+};
+/*----------------------------------------------------------------------------*/
+#ifndef USE_EARLY_SUSPEND
+/*----------------------------------------------------------------------------*/
+static int bma222_suspend(struct i2c_client *client, pm_message_t msg) 
+{
+	struct bma222_i2c_data *obj = i2c_get_clientdata(client);    
+	int err = 0;  
+    mutex_lock(&gsensor_scp_en_mutex);
+	if(msg.event == PM_EVENT_SUSPEND)
+	{   
+		if(obj == NULL)
+		{
+			GSE_ERR("null pointer!!\n");
+			mutex_unlock(&gsensor_scp_en_mutex);
+			return -EINVAL;
+		}
+		atomic_set(&obj->suspend, 1);
+#ifdef CUSTOM_KERNEL_SENSORHUB
+	   if(0 != (err = BMA222_SCP_SetPowerMode(false, ID_ACCELEROMETER)))
+#else
+		if(0 != (err = BMA222_SetPowerMode(obj->client, false)))
+#endif
+		{
+			GSE_ERR("write power control fail!!\n");
+			mutex_unlock(&gsensor_scp_en_mutex);
+			return -EINVAL;
+		} 
+#ifndef CUSTOM_KERNEL_SENSORHUB
+		BMA222_power(obj->hw, 0);
+#endif
+	}
+	mutex_unlock(&gsensor_scp_en_mutex);
+	return err;
+}
+/*----------------------------------------------------------------------------*/
+static int bma222_resume(struct i2c_client *client)
+{
+	struct bma222_i2c_data *obj = i2c_get_clientdata(client);        
+	int err;
+
+	if(obj == NULL)
+	{
+		GSE_ERR("null pointer!!\n");
+		return -EINVAL;
+	}
+	
+#ifndef CUSTOM_KERNEL_SENSORHUB
+	BMA222_power(obj->hw, 1);
+#endif
+
+#ifndef CUSTOM_KERNEL_SENSORHUB
+	if(0 != (err = bma222_init_client(client, 0)))
+#else
+    if(0 != (err = BMA222_SCP_SetPowerMode(enable_status, ID_ACCELEROMETER)))
+#endif
+	{
+		GSE_ERR("initialize client fail!!\n");
+		
+		return err;        
+	}
+	atomic_set(&obj->suspend, 0);
+
+	
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+#else /*CONFIG_HAS_EARLY_SUSPEND is defined*/
+/*----------------------------------------------------------------------------*/
+static void bma222_early_suspend(struct early_suspend *h) 
+{
+	struct bma222_i2c_data *obj = container_of(h, struct bma222_i2c_data, early_drv);   
+	int err;
+		
+	if(obj == NULL)
+	{
+		GSE_ERR("null pointer!!\n");
+		return;
+	}
+	atomic_set(&obj->suspend, 1); 
+	
+	GSE_FUN();  
+	u8 databuf[2]; //for debug read power control register to see the value is OK
+	if(bma_i2c_read_block(obj->client, BMA222_REG_POWER_CTL, databuf, 0x01))
+	{
+		GSE_ERR("read power ctl register err!\n");
+		return BMA222_ERR_I2C;
+	}
+	if(databuf[0]==0xff)//if the value is ff the gsensor will not work anymore, any i2c operations won't be vaild
+		GSE_LOG("before BMA222_SetPowerMode in suspend databuf = 0x%x\n",databuf[0]);
+#ifndef CUSTOM_KERNEL_SENSORHUB
+	if((err = BMA222_SetPowerMode(obj->client, false)))
+#else
+    if((err = BMA222_SCP_SetPowerMode(false, ID_ACCELEROMETER)))
+#endif
+	{
+		GSE_ERR("write power control fail!!\n");
+
+		return;
+	}
+	if(bma_i2c_read_block(obj->client, BMA222_REG_POWER_CTL, databuf, 0x01)) //for debug read power control register to see the value is OK
+	{
+		GSE_ERR("read power ctl register err!\n");
+
+		return BMA222_ERR_I2C;
+	}
+	if(databuf[0]==0xff)//if the value is ff the gsensor will not work anymore, any i2c operations won't be vaild
+		GSE_LOG("after BMA222_SetPowerMode suspend err databuf = 0x%x\n",databuf[0]);
+	sensor_suspend = 1;
+	
+#ifndef CUSTOM_KERNEL_SENSORHUB
+	BMA222_power(obj->hw, 0);
+#endif
+
+}
+/*----------------------------------------------------------------------------*/
+static void bma222_late_resume(struct early_suspend *h)
+{
+	struct bma222_i2c_data *obj = container_of(h, struct bma222_i2c_data, early_drv);         
+	int err;
+	
+	if(obj == NULL)
+	{
+		GSE_ERR("null pointer!!\n");
+		return;
+	}
+
+#ifndef CUSTOM_KERNEL_SENSORHUB
+	BMA222_power(obj->hw, 1);
+
+#endif
+
+
+
+	u8 databuf[2];//for debug read power control register to see the value is OK
+	if(bma_i2c_read_block(obj->client, BMA222_REG_POWER_CTL, databuf, 0x01))
+	{
+		GSE_ERR("read power ctl register err!\n");		
+
+		return BMA222_ERR_I2C;
+		
+	}
+	if(databuf[0]==0xff)//if the value is ff the gsensor will not work anymore, any i2c operations won't be vaild
+	
+		GSE_LOG("before bma222_init_client databuf = 0x%x\n",databuf[0]);
+#ifndef CUSTOM_KERNEL_SENSORHUB	
+	if((err = bma222_init_client(obj->client, 0)))
+#else
+    if((err = BMA222_SCP_SetPowerMode(enable_status, ID_ACCELEROMETER)))
+#endif
+	{
+		GSE_ERR("initialize client fail!!\n");
+
+		return;        
+	}
+	
+	if(bma_i2c_read_block(obj->client, BMA222_REG_POWER_CTL, databuf, 0x01)) //for debug read power control register to see the value is OK
+	{
+		GSE_ERR("read power ctl register err!\n");
+
+		return BMA222_ERR_I2C;
+	}
+	
+	if(databuf[0]==0xff)//if the value is ff the gsensor will not work anymore, any i2c operations won't be vaild
+		GSE_LOG("after bma222_init_client databuf = 0x%x\n",databuf[0]);
+	sensor_suspend = 0;
+
+	atomic_set(&obj->suspend, 0);    
+}
+/*----------------------------------------------------------------------------*/
+#endif /*USE_EARLY_SUSPEND*/
+/*----------------------------------------------------------------------------*/
+// if use  this typ of enable , Gsensor should report inputEvent(x, y, z ,stats, div) to HAL
+static int gsensor_open_report_data(int open)
+{
+	//should queuq work to report event if  is_report_input_direct=true
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+// if use  this typ of enable , Gsensor only enabled but not report inputEvent to HAL
+static int gsensor_enable_nodata(int en)
+{
+    int err = 0;
+
+	
+    
+	if(((en == 0) && (sensor_power == false)) ||((en == 1) && (sensor_power == true)))
+	{
+		enable_status = sensor_power;
+		GSE_LOG("Gsensor device have updated!\n");
+	}
+	else
+	{
+		enable_status = !sensor_power;
+		if (atomic_read(&obj_i2c_data->suspend) == 0)
+		{
+#ifdef CUSTOM_KERNEL_SENSORHUB
+            err = BMA222_SCP_SetPowerMode(enable_status, ID_ACCELEROMETER);
+            if (0 == err)
+            {
+               sensor_power = enable_status;
+			}
+#else
+			err = BMA222_SetPowerMode(obj_i2c_data->client, enable_status);
+#endif
+			GSE_LOG("Gsensor not in suspend BMA222_SetPowerMode!, enable_status = %d\n",enable_status);
+		}
+		else
+		{
+			GSE_LOG("Gsensor in suspend and can not enable or disable!enable_status = %d\n",enable_status);
+		}
+	}
+
+
+    if(err != BMA222_SUCCESS)
+	{
+		GSE_ERR("gsensor_enable_nodata fail!\n");
+		return -1;
+	}
+
+    GSE_ERR("gsensor_enable_nodata OK!\n");
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int gsensor_set_delay(u64 ns)
+{
+    int err = 0;
+    int value;
+#ifdef CUSTOM_KERNEL_SENSORHUB
+    SCP_SENSOR_HUB_DATA req;
+    int len;
+#else//#ifdef CUSTOM_KERNEL_SENSORHUB
+	int sample_delay;
+#endif//#ifdef CUSTOM_KERNEL_SENSORHUB
+
+    value = (int)ns/1000/1000;
+
+#ifdef CUSTOM_KERNEL_SENSORHUB
+    req.set_delay_req.sensorType = ID_ACCELEROMETER;
+    req.set_delay_req.action = SENSOR_HUB_SET_DELAY;
+    req.set_delay_req.delay = value;
+    len = sizeof(req.activate_req);
+    err = SCP_sensorHub_req_send(&req, &len, 1);
+    if (err)
+    {
+        GSE_ERR("SCP_sensorHub_req_send!\n");
+        return err;
+    }
+#else//#ifdef CUSTOM_KERNEL_SENSORHUB    
+	if(value <= 5)
+	{
+		sample_delay = BMA222_BW_200HZ;
+	}
+	else if(value <= 10)
+	{
+		sample_delay = BMA222_BW_100HZ;
+	}
+	else
+	{
+		sample_delay = BMA222_BW_100HZ;
+	}
+
+	mutex_lock(&gsensor_scp_en_mutex);
+	err = BMA222_SetBWRate(obj_i2c_data->client, sample_delay);
+	mutex_unlock(&gsensor_scp_en_mutex);
+	if(err != BMA222_SUCCESS ) //0x2C->BW=100Hz
+	{
+		GSE_ERR("Set delay parameter error!\n");
+        return -1;
+	}
+
+	if(value >= 50)
+	{
+		atomic_set(&obj_i2c_data->filter, 0);
+	}
+	else
+	{	
+	#if defined(CONFIG_BMA222_LOWPASS)
+		priv->fir.num = 0;
+		priv->fir.idx = 0;
+		priv->fir.sum[BMA222_AXIS_X] = 0;
+		priv->fir.sum[BMA222_AXIS_Y] = 0;
+		priv->fir.sum[BMA222_AXIS_Z] = 0;
+		atomic_set(&priv->filter, 1);
+	#endif
+	}
+#endif//#ifdef CUSTOM_KERNEL_SENSORHUB
+    
+    GSE_LOG("gsensor_set_delay (%d)\n",value);
+
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+/*----------------------------------------------------------------------------*/
+static int gsensor_get_data(int* x ,int* y,int* z, int* status)
+{
+#ifdef CUSTOM_KERNEL_SENSORHUB
+    SCP_SENSOR_HUB_DATA req;
+    int len;
+	int err = 0;
+#else
+	char buff[BMA222_BUFSIZE];
+#endif
+
+#ifdef CUSTOM_KERNEL_SENSORHUB
+    req.get_data_req.sensorType = ID_ACCELEROMETER;
+    req.get_data_req.action = SENSOR_HUB_GET_DATA;
+	len = sizeof(req.get_data_req);
+	err = SCP_sensorHub_req_send(&req, &len, 1);
+	if (err)
+	{
+	    GSE_ERR("SCP_sensorHub_req_send!\n");
+		return err;
+	}
+	
+	if (ID_ACCELEROMETER != req.get_data_rsp.sensorType ||
+		SENSOR_HUB_GET_DATA != req.get_data_rsp.action ||
+		0 != req.get_data_rsp.errCode)
+	{
+	    GSE_ERR("error : %d\n", req.get_data_rsp.errCode);
+		return req.get_data_rsp.errCode;
+	}
+
+	*x = (int)req.get_data_rsp.int16_Data[0]*GRAVITY_EARTH_1000/1000;
+	*y = (int)req.get_data_rsp.int16_Data[1]*GRAVITY_EARTH_1000/1000;
+	*z = (int)req.get_data_rsp.int16_Data[2]*GRAVITY_EARTH_1000/1000;
+	GSE_ERR("x = %d, y = %d, z = %d\n", *x, *y, *z);
+
+	*status = SENSOR_STATUS_ACCURACY_MEDIUM;
+#else
+    mutex_lock(&gsensor_scp_en_mutex);
+	BMA222_ReadSensorData(obj_i2c_data->client, buff, BMA222_BUFSIZE);
+	mutex_unlock(&gsensor_scp_en_mutex);
+	sscanf(buff, "%x %x %x", x, y, z);				
+	*status = SENSOR_STATUS_ACCURACY_MEDIUM;
+  #endif
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int bma222_i2c_probe(struct i2c_client *client, const struct i2c_device_id *id)
+{
+	struct i2c_client *new_client;
+	struct bma222_i2c_data *obj;
+    struct acc_control_path ctl={0};
+    struct acc_data_path data={0};
+	int err = 0;
+	int retry = 0;
+	GSE_FUN();
+
+	if(!(obj = kzalloc(sizeof(*obj), GFP_KERNEL)))
+	{
+		err = -ENOMEM;
+		goto exit;
+	}
+	
+	memset(obj, 0, sizeof(struct bma222_i2c_data));
+
+	obj->hw = get_cust_acc_hw();
+	
+	if(0 != (err = hwmsen_get_convert(obj->hw->direction, &obj->cvt)))
+	{
+		GSE_ERR("invalid direction: %d\n", obj->hw->direction);
+		goto exit;
+	}
+
+#ifdef CUSTOM_KERNEL_SENSORHUB
+    INIT_WORK(&obj->irq_work, gsensor_irq_work);
+#endif//#ifdef CUSTOM_KERNEL_SENSORHUB
+
+	obj_i2c_data = obj;
+	obj->client = client;
+#ifdef FPGA_EARLY_PORTING
+    obj->client->timing = 100; 
+#else
+    obj->client->timing = 400;
+#endif
+	new_client = obj->client;
+	i2c_set_clientdata(new_client,obj);
+	
+	atomic_set(&obj->trace, 0);
+	atomic_set(&obj->suspend, 0);
+	
+#ifdef CONFIG_BMA222_LOWPASS
+	if(obj->hw->firlen > C_MAX_FIR_LENGTH)
+	{
+		atomic_set(&obj->firlen, C_MAX_FIR_LENGTH);
+	}	
+	else
+	{
+		atomic_set(&obj->firlen, obj->hw->firlen);
+	}
+	
+	if(atomic_read(&obj->firlen) > 0)
+	{
+		atomic_set(&obj->fir_en, 1);
+	}
+	
+#endif
+
+	bma222_i2c_client = new_client;	
+
+	for(retry = 0; retry < 3; retry++){
+		if(0 != (err = bma222_init_client(new_client, 1)))
+		{
+			GSE_ERR("bma222_device init cilent fail time: %d\n", retry);
+			continue;
+		}
+	}
+	if(err != 0)
+		goto exit_init_failed;
+	
+
+	if(0 != (err = misc_register(&bma222_device)))
+	{
+		GSE_ERR("bma222_device register failed\n");
+		goto exit_misc_device_register_failed;
+	}
+
+	if(0 != (err = bma222_create_attr(&bma222_init_info.platform_diver_addr->driver)))
+	{
+		GSE_ERR("create attribute err = %d\n", err);
+		goto exit_create_attr_failed;
+	}
+
+    ctl.open_report_data= gsensor_open_report_data;
+    ctl.enable_nodata = gsensor_enable_nodata;
+    ctl.set_delay  = gsensor_set_delay;
+    //ctl.batch = gsensor_set_batch;
+    ctl.is_report_input_direct = false;
+
+#ifdef CUSTOM_KERNEL_SENSORHUB
+    ctl.is_support_batch = obj->hw->is_batch_supported;
+#else
+    ctl.is_support_batch = false;
+#endif
+
+    err = acc_register_control_path(&ctl);
+    if(err)
+    {
+        GSE_ERR("register acc control path err\n");
+        goto exit_kfree;
+    }
+
+    data.get_data = gsensor_get_data;
+    data.vender_div = 1000;
+    err = acc_register_data_path(&data);
+    if(err)
+    {
+        GSE_ERR("register acc data path err\n");
+        goto exit_kfree;
+    }
+
+    err = batch_register_support_info(ID_ACCELEROMETER,ctl.is_support_batch, 102, 0); //divisor is 1000/9.8
+    if(err)
+    {
+	    GSE_ERR("register gsensor batch support err = %d\n", err);
+	     goto exit_create_attr_failed;
+     }
+
+#ifdef USE_EARLY_SUSPEND
+	obj->early_drv.level    = EARLY_SUSPEND_LEVEL_STOP_DRAWING - 2,
+	obj->early_drv.suspend  = bma222_early_suspend,
+	obj->early_drv.resume   = bma222_late_resume,    
+	register_early_suspend(&obj->early_drv);
+#endif 
+
+    gsensor_init_flag =0;
+	GSE_LOG("%s: OK\n", __func__);    
+	return 0;
+
+	exit_create_attr_failed:
+	misc_deregister(&bma222_device);
+	exit_misc_device_register_failed:
+	exit_init_failed:
+	//i2c_detach_client(new_client);
+	exit_kfree:
+	kfree(obj);
+	exit:
+	GSE_ERR("%s: err = %d\n", __func__, err);
+    gsensor_init_flag = -1;
+	return err;
+}
+
+/*----------------------------------------------------------------------------*/
+static int bma222_i2c_remove(struct i2c_client *client)
+{
+	int err = 0;	
+	
+	if(0 != (err = bma222_delete_attr(&bma222_init_info.platform_diver_addr->driver)))
+	{
+		GSE_ERR("bma150_delete_attr fail: %d\n", err);
+	}
+	
+	if(0 != (err = misc_deregister(&bma222_device)))
+	{
+		GSE_ERR("misc_deregister fail: %d\n", err);
+	}
+    
+	bma222_i2c_client = NULL;
+	i2c_unregister_device(client);
+	kfree(i2c_get_clientdata(client));
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int  gsensor_local_init(void)
+{
+    struct acc_hw *hw = get_cust_acc_hw();
+
+    GSE_FUN();
+	
+	BMA222_power(hw, 1);
+	if(i2c_add_driver(&bma222_i2c_driver))
+	{
+		GSE_ERR("add driver error\n");
+		return -1;
+	}
+	if(-1 == gsensor_init_flag)
+	{
+	   return -1;
+	}
+	//printk("fwq loccal init---\n");
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int gsensor_remove()
+{
+    struct acc_hw *hw = get_cust_acc_hw();
+
+    GSE_FUN();    
+    BMA222_power(hw, 0);    
+    i2c_del_driver(&bma222_i2c_driver);
+    return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int __init bma222_init(void)
+{
+	GSE_FUN();
+	struct acc_hw *hw = get_cust_acc_hw();
+	GSE_LOG("%s: i2c_number=%d\n", __func__,hw->i2c_num);
+	i2c_register_board_info(hw->i2c_num, &i2c_BMA222, 1);
+    acc_driver_add(&bma222_init_info);
+	return 0;    
+}
+/*----------------------------------------------------------------------------*/
+static void __exit bma222_exit(void)
+{
+	GSE_FUN();
+}
+/*----------------------------------------------------------------------------*/
+module_init(bma222_init);
+module_exit(bma222_exit);
+/*----------------------------------------------------------------------------*/
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("BMA222 I2C driver");
+MODULE_AUTHOR("Xiaoli.li@mediatek.com");
diff --git a/drivers/misc/mediatek/accelerometer/bma222E-new/bma222E.h b/drivers/misc/mediatek/accelerometer/bma222E-new/bma222E.h
new file mode 100644
index 000000000..13ccc28e7
--- /dev/null
+++ b/drivers/misc/mediatek/accelerometer/bma222E-new/bma222E.h
@@ -0,0 +1,110 @@
+/* linux/drivers/hwmon/adxl345.c
+ *
+ * (C) Copyright 2008 
+ * MediaTek <www.mediatek.com>
+ *
+ * BMA150 driver for MT6516
+ *
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA  BMA150
+ */
+#ifndef BMA150_H
+#define BMA150_H
+	 
+#include <linux/ioctl.h>
+	 
+	#define BMA222_I2C_SLAVE_WRITE_ADDR		0x10
+	 
+	 /* BMA222 Register Map  (Please refer to BMA150 Specifications) */
+	#define BMA222_REG_DEVID				0x00
+	#define BMA222_FIXED_DEVID			0x03
+	#define BMA222_REG_OFSX				0x16
+	#define BMA222_REG_OFSX_HIGH			0x1A
+	#define BMA222_REG_BW_RATE			0x10
+	#define BMA222_BW_MASK				0x1f
+	#define BMA222_BW_200HZ				0x0d
+	#define BMA222_BW_100HZ				0x0c
+	#define BMA222_BW_50HZ				0x0b
+	#define BMA222_BW_25HZ				0x0a
+	#define BMA222_REG_POWER_CTL		0x11		
+	#define BMA222_REG_DATA_FORMAT		0x0f
+	#define BMA222_RANGE_MASK			0x0f
+	#define BMA222_RANGE_2G				0x03
+	#define BMA222_RANGE_4G				0x05
+	#define BMA222_RANGE_8G				0x08
+	#define BMA222_REG_DATAXLOW			0x03
+	#define BMA222_REG_DATA_RESOLUTION	0x14
+	#define BMA222_MEASURE_MODE			0x80	
+	#define BMA222_SELF_TEST           			0x32
+	#define BMA222_SELF_TEST_AXIS_X		0x01
+	#define BMA222_SELF_TEST_AXIS_Y		0x02
+	#define BMA222_SELF_TEST_AXIS_Z		0x03
+	#define BMA222_SELF_TEST_POSITIVE	0x00
+	#define BMA222_SELF_TEST_NEGATIVE	0x04
+	#define BMA222_INT_REG_1           			0x16
+	#define BMA222_INT_REG_2          		 	0x17
+
+	
+#define BMA222_SUCCESS						0
+#define BMA222_ERR_I2C						-1
+#define BMA222_ERR_STATUS					-3
+#define BMA222_ERR_SETUP_FAILURE			-4
+#define BMA222_ERR_GETGSENSORDATA			-5
+#define BMA222_ERR_IDENTIFICATION			-6
+	 
+	 
+	 
+#define BMA222_BUFSIZE				256
+
+#define BMA222_AXES_NUM        3
+
+/*----------------------------------------------------------------------------*/
+typedef enum{
+    BMA222_CUST_ACTION_SET_CUST = 1,
+    BMA222_CUST_ACTION_SET_CALI,
+    BMA222_CUST_ACTION_RESET_CALI
+}CUST_ACTION;
+/*----------------------------------------------------------------------------*/
+typedef struct
+{
+    uint16_t    action;
+}BMA222_CUST;
+/*----------------------------------------------------------------------------*/
+typedef struct
+{
+    uint16_t    action;
+    uint16_t    part;
+    int32_t     data[0];
+}BMA222_SET_CUST;
+/*----------------------------------------------------------------------------*/
+typedef struct
+{
+    uint16_t    action;
+    int32_t     data[BMA222_AXES_NUM];
+}BMA222_SET_CALI;
+/*----------------------------------------------------------------------------*/
+typedef BMA222_CUST BMA222_RESET_CALI;
+/*----------------------------------------------------------------------------*/
+typedef union
+{
+    uint32_t                data[10];
+    BMA222_CUST         cust;
+    BMA222_SET_CUST     setCust;
+    BMA222_SET_CALI     setCali;
+    BMA222_RESET_CALI   resetCali;
+}BMA222_CUST_DATA;
+/*----------------------------------------------------------------------------*/
+	 
+#endif
+
diff --git a/drivers/misc/mediatek/accelerometer/bma222E/Makefile b/drivers/misc/mediatek/accelerometer/bma222E/Makefile
new file mode 100644
index 000000000..7c0399b38
--- /dev/null
+++ b/drivers/misc/mediatek/accelerometer/bma222E/Makefile
@@ -0,0 +1,4 @@
+include $(srctree)/drivers/misc/mediatek/Makefile.custom
+
+obj-y	:=  bma222E.o
+
diff --git a/drivers/misc/mediatek/accelerometer/bma222E/bma222E.c b/drivers/misc/mediatek/accelerometer/bma222E/bma222E.c
new file mode 100644
index 000000000..e0be0c2a1
--- /dev/null
+++ b/drivers/misc/mediatek/accelerometer/bma222E/bma222E.c
@@ -0,0 +1,1945 @@
+/* BMA150 motion sensor driver
+ *
+ *
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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.
+ *
+ */
+
+#include <linux/interrupt.h>
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include <linux/irq.h>
+#include <linux/miscdevice.h>
+#include <asm/uaccess.h>
+#include <linux/delay.h>
+#include <linux/input.h>
+#include <linux/workqueue.h>
+#include <linux/kobject.h>
+#include <linux/earlysuspend.h>
+#include <linux/platform_device.h>
+#include <asm/atomic.h>
+
+#include <mach/mt_typedefs.h>
+#include <mach/mt_gpio.h>
+#include <mach/mt_pm_ldo.h>
+
+#include <cust_acc.h>
+#include <linux/hwmsensor.h>
+#include <linux/hwmsen_dev.h>
+#include <linux/sensors_io.h>
+#include "bma222E.h"
+#include <linux/hwmsen_helper.h>
+
+
+#define POWER_NONE_MACRO MT65XX_POWER_NONE
+
+/*----------------------------------------------------------------------------*/
+#define I2C_DRIVERID_BMA222 222
+/*----------------------------------------------------------------------------*/
+#define DEBUG 1
+/*----------------------------------------------------------------------------*/
+//#define CONFIG_BMA150_LOWPASS   /*apply low pass filter on output*/       
+#define SW_CALIBRATION
+
+/*----------------------------------------------------------------------------*/
+#define BMA222_AXIS_X          0
+#define BMA222_AXIS_Y          1
+#define BMA222_AXIS_Z          2
+#define BMA222_AXES_NUM        3
+#define BMA222_DATA_LEN        6
+#define BMA222_DEV_NAME        "BMA222"
+/*----------------------------------------------------------------------------*/
+
+/*********/
+/*----------------------------------------------------------------------------*/
+static const struct i2c_device_id bma222_i2c_id[] = {{BMA222_DEV_NAME,0},{}};
+static struct i2c_board_info __initdata i2c_BMA222={ I2C_BOARD_INFO("BMA222", 0x18)};
+/*the adapter id will be available in customization*/
+//static unsigned short bma222_force[] = {0x00, BMA222_I2C_SLAVE_WRITE_ADDR, I2C_CLIENT_END, I2C_CLIENT_END};
+//static const unsigned short *const bma222_forces[] = { bma222_force, NULL };
+//static struct i2c_client_address_data bma222_addr_data = { .forces = bma222_forces,};
+
+/*----------------------------------------------------------------------------*/
+static int bma222_i2c_probe(struct i2c_client *client, const struct i2c_device_id *id); 
+static int bma222_i2c_remove(struct i2c_client *client);
+static int bma222_i2c_detect(struct i2c_client *client, struct i2c_board_info *info);
+#ifndef USE_EARLY_SUSPEND
+static int bma222_suspend(struct i2c_client *client, pm_message_t msg);
+static int bma222_resume(struct i2c_client *client);
+#endif
+/*----------------------------------------------------------------------------*/
+typedef enum {
+    ADX_TRC_FILTER  = 0x01,
+    ADX_TRC_RAWDATA = 0x02,
+    ADX_TRC_IOCTL   = 0x04,
+    ADX_TRC_CALI	= 0X08,
+    ADX_TRC_INFO	= 0X10,
+} ADX_TRC;
+/*----------------------------------------------------------------------------*/
+struct scale_factor{
+    u8  whole;
+    u8  fraction;
+};
+/*----------------------------------------------------------------------------*/
+struct data_resolution {
+    struct scale_factor scalefactor;
+    int                 sensitivity;
+};
+/*----------------------------------------------------------------------------*/
+#define C_MAX_FIR_LENGTH (32)
+/*----------------------------------------------------------------------------*/
+struct data_filter {
+    s16 raw[C_MAX_FIR_LENGTH][BMA222_AXES_NUM];
+    int sum[BMA222_AXES_NUM];
+    int num;
+    int idx;
+};
+/*----------------------------------------------------------------------------*/
+struct bma222_i2c_data {
+    struct i2c_client *client;
+    struct acc_hw *hw;
+    struct hwmsen_convert   cvt;
+    
+    /*misc*/
+    struct data_resolution *reso;
+    atomic_t                trace;
+    atomic_t                suspend;
+    atomic_t                selftest;
+	atomic_t				filter;
+    s16                     cali_sw[BMA222_AXES_NUM+1];
+
+    /*data*/
+    s8                      offset[BMA222_AXES_NUM+1];  /*+1: for 4-byte alignment*/
+    s16                     data[BMA222_AXES_NUM+1];
+
+#if defined(CONFIG_BMA222_LOWPASS)
+    atomic_t                firlen;
+    atomic_t                fir_en;
+    struct data_filter      fir;
+#endif 
+    /*early suspend*/
+#ifdef USE_EARLY_SUSPEND
+    struct early_suspend    early_drv;
+#endif     
+};
+/*----------------------------------------------------------------------------*/
+static struct i2c_driver bma222_i2c_driver = {
+    .driver = {
+        .name           = BMA222_DEV_NAME,
+    },
+	.probe      		= bma222_i2c_probe,
+	.remove    			= bma222_i2c_remove,
+#if !defined(USE_EARLY_SUSPEND)    
+    .suspend            = bma222_suspend,
+    .resume             = bma222_resume,
+#endif
+	.id_table = bma222_i2c_id,
+};
+
+/*----------------------------------------------------------------------------*/
+static struct i2c_client *bma222_i2c_client = NULL;
+static struct platform_driver bma222_gsensor_driver;
+static struct bma222_i2c_data *obj_i2c_data = NULL;
+static bool sensor_power = true;
+static int sensor_suspend = 0;
+static GSENSOR_VECTOR3D gsensor_gain;
+//static char selftestRes[8]= {0}; 
+static DEFINE_MUTEX(bma222_i2c_mutex);
+static DEFINE_MUTEX(bma222_op_mutex);
+
+static bool enable_status = false;
+
+
+/*----------------------------------------------------------------------------*/
+#define GSE_TAG                  "[Gsensor] "
+#define GSE_FUN(f)               printk(KERN_INFO GSE_TAG"%s\n", __FUNCTION__)
+#define GSE_ERR(fmt, args...)    printk(KERN_ERR GSE_TAG"%s %d : "fmt, __FUNCTION__, __LINE__, ##args)
+#define GSE_LOG(fmt, args...)    printk(KERN_INFO GSE_TAG fmt, ##args)
+/*----------------------------------------------------------------------------*/
+static struct data_resolution bma222_data_resolution[1] = {
+ /* combination by {FULL_RES,RANGE}*/
+    {{ 15, 6}, 64},   // dataformat +/-2g  in 8-bit resolution;  { 15, 6} = 15.6= (2*2*1000)/(2^8);  64 = (2^8)/(2*2)          
+};
+/*----------------------------------------------------------------------------*/
+static struct data_resolution bma222_offset_resolution = {{15, 6}, 64};
+
+/*----------------------------------------------------------------------------*/
+static int bma_i2c_read_block(struct i2c_client *client, u8 addr, u8 *data, u8 len)
+{
+    u8 beg = addr;
+	int err;
+	struct i2c_msg msgs[2]={{0},{0}};
+	
+	mutex_lock(&bma222_i2c_mutex);
+	
+	msgs[0].addr = client->addr;
+	msgs[0].flags = 0;
+	msgs[0].len =1;
+	msgs[0].buf = &beg;
+
+	msgs[1].addr = client->addr;
+	msgs[1].flags = I2C_M_RD;
+	msgs[1].len =len;
+	msgs[1].buf = data;
+	
+	if (!client)
+	{
+	    mutex_unlock(&bma222_i2c_mutex);
+		return -EINVAL;
+	}
+	else if (len > C_I2C_FIFO_SIZE) 
+	{
+		GSE_ERR(" length %d exceeds %d\n", len, C_I2C_FIFO_SIZE);
+		mutex_unlock(&bma222_i2c_mutex);
+		return -EINVAL;
+	}
+	err = i2c_transfer(client->adapter, msgs, sizeof(msgs)/sizeof(msgs[0]));
+	if (err != 2) 
+	{
+		GSE_ERR("i2c_transfer error: (%d %p %d) %d\n",addr, data, len, err);
+		err = -EIO;
+	} 
+	else 
+	{
+		err = 0;
+	}
+	mutex_unlock(&bma222_i2c_mutex);
+	return err;
+
+}
+
+static int bma_i2c_write_block(struct i2c_client *client, u8 addr, u8 *data, u8 len)
+{   /*because address also occupies one byte, the maximum length for write is 7 bytes*/
+    int err, idx, num;
+    char buf[C_I2C_FIFO_SIZE];
+    err =0;
+	mutex_lock(&bma222_i2c_mutex);
+    if (!client)
+    {
+        mutex_unlock(&bma222_i2c_mutex);
+        return -EINVAL;
+    }
+    else if (len >= C_I2C_FIFO_SIZE) 
+	{        
+        GSE_ERR(" length %d exceeds %d\n", len, C_I2C_FIFO_SIZE);
+		mutex_unlock(&bma222_i2c_mutex);
+        return -EINVAL;
+    }    
+
+    num = 0;
+    buf[num++] = addr;
+    for (idx = 0; idx < len; idx++)
+    {
+        buf[num++] = data[idx];
+    }
+
+    err = i2c_master_send(client, buf, num);
+    if (err < 0)
+	{
+        GSE_ERR("send command error!!\n");
+		mutex_unlock(&bma222_i2c_mutex);
+        return -EFAULT;
+    } 
+	mutex_unlock(&bma222_i2c_mutex);
+    return err;
+}
+
+
+/*----------------------------------------------------------------------------*/
+/*--------------------BMA222 power control function----------------------------------*/
+static void BMA222_power(struct acc_hw *hw, unsigned int on) 
+{
+	static unsigned int power_on = 0;
+
+	if(hw->power_id != POWER_NONE_MACRO)		// have externel LDO
+	{        
+		GSE_LOG("power %s\n", on ? "on" : "off");
+		if(power_on == on)	// power status not change
+		{
+			GSE_LOG("ignore power control: %d\n", on);
+		}
+		else if(on)	// power on
+		{
+			if(!hwPowerOn(hw->power_id, hw->power_vol, "BMA222"))
+			{
+				GSE_ERR("power on fails!!\n");
+			}
+		}
+		else	// power off
+		{
+			if (!hwPowerDown(hw->power_id, "BMA222"))
+			{
+				GSE_ERR("power off fail!!\n");
+			}			  
+		}
+	}
+	power_on = on;    
+}
+/*----------------------------------------------------------------------------*/
+
+/*----------------------------------------------------------------------------*/
+static int BMA222_SetDataResolution(struct bma222_i2c_data *obj)
+{
+
+/*set g sensor dataresolution here*/
+
+/*BMA222 only can set to 10-bit dataresolution, so do nothing in bma222 driver here*/
+
+/*end of set dataresolution*/
+
+
+ 
+ /*we set measure range from -2g to +2g in BMA150_SetDataFormat(client, BMA150_RANGE_2G), 
+                                                    and set 10-bit dataresolution BMA150_SetDataResolution()*/
+                                                    
+ /*so bma222_data_resolution[0] set value as {{ 3, 9}, 256} when declaration, and assign the value to obj->reso here*/  
+
+ 	obj->reso = &bma222_data_resolution[0];
+	return 0;
+	
+/*if you changed the measure range, for example call: BMA222_SetDataFormat(client, BMA150_RANGE_4G), 
+you must set the right value to bma222_data_resolution*/
+
+}
+/*----------------------------------------------------------------------------*/
+static int BMA222_ReadData(struct i2c_client *client, s16 data[BMA222_AXES_NUM])
+{
+	struct bma222_i2c_data *priv = i2c_get_clientdata(client);        
+	u8 addr = BMA222_REG_DATAXLOW;
+	u8 buf[BMA222_DATA_LEN] = {0};
+	int err = 0;
+
+	if(NULL == client)
+	{
+		err = -EINVAL;
+	}
+	else if((err = bma_i2c_read_block(client, addr, buf, 0x05))!=0)
+	{
+		GSE_ERR("error: %d\n", err);
+	}
+	else
+	{
+		data[BMA222_AXIS_X] = (s16)buf[BMA222_AXIS_X*2] ;
+		data[BMA222_AXIS_Y] = (s16)buf[BMA222_AXIS_Y*2];
+		data[BMA222_AXIS_Z] = (s16)buf[BMA222_AXIS_Z*2] ;
+		if(atomic_read(&priv->trace) & ADX_TRC_RAWDATA)
+		{
+			GSE_LOG("[%08X %08X %08X] => [%5d %5d %5d] before\n", data[BMA222_AXIS_X], data[BMA222_AXIS_Y], data[BMA222_AXIS_Z],
+		                               data[BMA222_AXIS_X], data[BMA222_AXIS_Y], data[BMA222_AXIS_Z]);
+		}
+
+		if(data[BMA222_AXIS_X]&0x80)
+		{
+				data[BMA222_AXIS_X] = ~data[BMA222_AXIS_X];
+				data[BMA222_AXIS_X] &= 0xff;
+				data[BMA222_AXIS_X]+=1;
+				data[BMA222_AXIS_X] = -data[BMA222_AXIS_X];
+		}
+		if(data[BMA222_AXIS_Y]&0x80)
+		{
+				data[BMA222_AXIS_Y] = ~data[BMA222_AXIS_Y];
+				data[BMA222_AXIS_Y] &= 0xff;
+				data[BMA222_AXIS_Y]+=1;
+				data[BMA222_AXIS_Y] = -data[BMA222_AXIS_Y];
+		}
+		if(data[BMA222_AXIS_Z]&0x80)
+		{
+				data[BMA222_AXIS_Z] = ~data[BMA222_AXIS_Z];
+				data[BMA222_AXIS_Z] &= 0xff;
+				data[BMA222_AXIS_Z]+=1;
+				data[BMA222_AXIS_Z] = -data[BMA222_AXIS_Z];
+		}
+
+		if(atomic_read(&priv->trace) & ADX_TRC_RAWDATA)
+		{
+			GSE_LOG("[%08X %08X %08X] => [%5d %5d %5d] after\n", data[BMA222_AXIS_X], data[BMA222_AXIS_Y], data[BMA222_AXIS_Z],
+		                               data[BMA222_AXIS_X], data[BMA222_AXIS_Y], data[BMA222_AXIS_Z]);
+		}
+#ifdef CONFIG_BMA222_LOWPASS
+		if(atomic_read(&priv->filter))
+		{
+			if(atomic_read(&priv->fir_en) && !atomic_read(&priv->suspend))
+			{
+				int idx, firlen = atomic_read(&priv->firlen);   
+				if(priv->fir.num < firlen)
+				{                
+					priv->fir.raw[priv->fir.num][BMA222_AXIS_X] = data[BMA222_AXIS_X];
+					priv->fir.raw[priv->fir.num][BMA222_AXIS_Y] = data[BMA222_AXIS_Y];
+					priv->fir.raw[priv->fir.num][BMA222_AXIS_Z] = data[BMA222_AXIS_Z];
+					priv->fir.sum[BMA222_AXIS_X] += data[BMA222_AXIS_X];
+					priv->fir.sum[BMA222_AXIS_Y] += data[BMA222_AXIS_Y];
+					priv->fir.sum[BMA222_AXIS_Z] += data[BMA222_AXIS_Z];
+					if(atomic_read(&priv->trace) & ADX_TRC_FILTER)
+					{
+						GSE_LOG("add [%2d] [%5d %5d %5d] => [%5d %5d %5d]\n", priv->fir.num,
+							priv->fir.raw[priv->fir.num][BMA222_AXIS_X], priv->fir.raw[priv->fir.num][BMA222_AXIS_Y], priv->fir.raw[priv->fir.num][BMA222_AXIS_Z],
+							priv->fir.sum[BMA222_AXIS_X], priv->fir.sum[BMA222_AXIS_Y], priv->fir.sum[BMA222_AXIS_Z]);
+					}
+					priv->fir.num++;
+					priv->fir.idx++;
+				}
+				else
+				{
+					idx = priv->fir.idx % firlen;
+					priv->fir.sum[BMA222_AXIS_X] -= priv->fir.raw[idx][BMA222_AXIS_X];
+					priv->fir.sum[BMA222_AXIS_Y] -= priv->fir.raw[idx][BMA222_AXIS_Y];
+					priv->fir.sum[BMA222_AXIS_Z] -= priv->fir.raw[idx][BMA222_AXIS_Z];
+					priv->fir.raw[idx][BMA222_AXIS_X] = data[BMA222_AXIS_X];
+					priv->fir.raw[idx][BMA222_AXIS_Y] = data[BMA222_AXIS_Y];
+					priv->fir.raw[idx][BMA222_AXIS_Z] = data[BMA222_AXIS_Z];
+					priv->fir.sum[BMA222_AXIS_X] += data[BMA222_AXIS_X];
+					priv->fir.sum[BMA222_AXIS_Y] += data[BMA222_AXIS_Y];
+					priv->fir.sum[BMA222_AXIS_Z] += data[BMA222_AXIS_Z];
+					priv->fir.idx++;
+					data[BMA222_AXIS_X] = priv->fir.sum[BMA222_AXIS_X]/firlen;
+					data[BMA222_AXIS_Y] = priv->fir.sum[BMA222_AXIS_Y]/firlen;
+					data[BMA222_AXIS_Z] = priv->fir.sum[BMA222_AXIS_Z]/firlen;
+					if(atomic_read(&priv->trace) & ADX_TRC_FILTER)
+					{
+						GSE_LOG("add [%2d] [%5d %5d %5d] => [%5d %5d %5d] : [%5d %5d %5d]\n", idx,
+						priv->fir.raw[idx][BMA222_AXIS_X], priv->fir.raw[idx][BMA222_AXIS_Y], priv->fir.raw[idx][BMA222_AXIS_Z],
+						priv->fir.sum[BMA222_AXIS_X], priv->fir.sum[BMA222_AXIS_Y], priv->fir.sum[BMA222_AXIS_Z],
+						data[BMA222_AXIS_X], data[BMA222_AXIS_Y], data[BMA222_AXIS_Z]);
+					}
+				}
+			}
+		}	
+#endif         
+	}
+	return err;
+}
+/*----------------------------------------------------------------------------*/
+
+static int BMA222_ReadOffset(struct i2c_client *client, s8 ofs[BMA222_AXES_NUM])
+{    
+	int err;
+	err = 0;
+#ifdef SW_CALIBRATION
+	ofs[0]=ofs[1]=ofs[2]=0x0;
+#else
+	if((err = bma_i2c_read_block(client, BMA222_REG_OFSX, ofs, BMA222_AXES_NUM)))
+	{
+		GSE_ERR("error: %d\n", err);
+	}
+#endif
+	//printk("offesx=%x, y=%x, z=%x",ofs[0],ofs[1],ofs[2]);
+	
+	return err;    
+}
+
+/*----------------------------------------------------------------------------*/
+static int BMA222_ResetCalibration(struct i2c_client *client)
+{
+	struct bma222_i2c_data *obj = i2c_get_clientdata(client);
+	//u8 ofs[4]={0,0,0,0};
+	int err;
+	err = 0;
+	
+	#ifdef SW_CALIBRATION
+		
+	#else
+		if((err = hwmsen_write_block(client, BMA222_REG_OFSX, ofs, 4)))
+		{
+			GSE_ERR("error: %d\n", err);
+		}
+	#endif
+
+	memset(obj->cali_sw, 0x00, sizeof(obj->cali_sw));
+	memset(obj->offset, 0x00, sizeof(obj->offset));
+	return err;    
+}
+/*----------------------------------------------------------------------------*/
+static int BMA222_ReadCalibration(struct i2c_client *client, int dat[BMA222_AXES_NUM])
+{
+    struct bma222_i2c_data *obj = i2c_get_clientdata(client);
+    int  err = 0;
+    int mul;
+
+	#ifdef SW_CALIBRATION
+		mul = 0;//only SW Calibration, disable HW Calibration
+	#else
+	    if ((err = BMA222_ReadOffset(client, obj->offset))) {
+        GSE_ERR("read offset fail, %d\n", err);
+        return err;
+    	}    
+    	mul = obj->reso->sensitivity/bma222_offset_resolution.sensitivity;
+	#endif
+
+    dat[obj->cvt.map[BMA222_AXIS_X]] = obj->cvt.sign[BMA222_AXIS_X]*(obj->offset[BMA222_AXIS_X]*mul * GRAVITY_EARTH_1000/obj->reso->sensitivity + obj->cali_sw[BMA222_AXIS_X]);
+    dat[obj->cvt.map[BMA222_AXIS_Y]] = obj->cvt.sign[BMA222_AXIS_Y]*(obj->offset[BMA222_AXIS_Y]*mul * GRAVITY_EARTH_1000/obj->reso->sensitivity + obj->cali_sw[BMA222_AXIS_Y]);
+    dat[obj->cvt.map[BMA222_AXIS_Z]] = obj->cvt.sign[BMA222_AXIS_Z]*(obj->offset[BMA222_AXIS_Z]*mul * GRAVITY_EARTH_1000/obj->reso->sensitivity + obj->cali_sw[BMA222_AXIS_Z]);                        
+                                       
+    return err;
+}
+/*----------------------------------------------------------------------------*/
+static int BMA222_ReadCalibrationEx(struct i2c_client *client, int act[BMA222_AXES_NUM], int raw[BMA222_AXES_NUM])
+{  
+	/*raw: the raw calibration data; act: the actual calibration data*/
+	struct bma222_i2c_data *obj = i2c_get_clientdata(client);
+	int err;
+	int mul;
+	err = 0;
+ 
+
+	#ifdef SW_CALIBRATION
+		mul = 0;//only SW Calibration, disable HW Calibration
+	#else
+		if((err = BMA222_ReadOffset(client, obj->offset)))
+		{
+			GSE_ERR("read offset fail, %d\n", err);
+			return err;
+		}   
+		mul = obj->reso->sensitivity/bma222_offset_resolution.sensitivity;
+	#endif
+	
+	raw[BMA222_AXIS_X] = obj->offset[BMA222_AXIS_X]*mul * GRAVITY_EARTH_1000/obj->reso->sensitivity + obj->cali_sw[BMA222_AXIS_X];
+	raw[BMA222_AXIS_Y] = obj->offset[BMA222_AXIS_Y]*mul * GRAVITY_EARTH_1000/obj->reso->sensitivity + obj->cali_sw[BMA222_AXIS_Y];
+	raw[BMA222_AXIS_Z] = obj->offset[BMA222_AXIS_Z]*mul * GRAVITY_EARTH_1000/obj->reso->sensitivity + obj->cali_sw[BMA222_AXIS_Z];
+
+	act[obj->cvt.map[BMA222_AXIS_X]] = obj->cvt.sign[BMA222_AXIS_X]*raw[BMA222_AXIS_X];
+	act[obj->cvt.map[BMA222_AXIS_Y]] = obj->cvt.sign[BMA222_AXIS_Y]*raw[BMA222_AXIS_Y];
+	act[obj->cvt.map[BMA222_AXIS_Z]] = obj->cvt.sign[BMA222_AXIS_Z]*raw[BMA222_AXIS_Z];                        
+	                       
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int BMA222_WriteCalibration(struct i2c_client *client, int dat[BMA222_AXES_NUM])
+{
+	struct bma222_i2c_data *obj = i2c_get_clientdata(client);
+	int err = 0;
+	int cali[BMA222_AXES_NUM], raw[BMA222_AXES_NUM];
+	int lsb; 
+	lsb = bma222_offset_resolution.sensitivity;
+	//int divisor = obj->reso->sensitivity/lsb;
+
+	if((err = BMA222_ReadCalibrationEx(client, cali, raw)))	/*offset will be updated in obj->offset*/
+	{ 
+		GSE_ERR("read offset fail, %d\n", err);
+		return err;
+	}
+
+	GSE_LOG("OLDOFF: (%+3d %+3d %+3d): (%+3d %+3d %+3d) / (%+3d %+3d %+3d)\n", 
+		raw[BMA222_AXIS_X], raw[BMA222_AXIS_Y], raw[BMA222_AXIS_Z],
+		obj->offset[BMA222_AXIS_X], obj->offset[BMA222_AXIS_Y], obj->offset[BMA222_AXIS_Z],
+		obj->cali_sw[BMA222_AXIS_X], obj->cali_sw[BMA222_AXIS_Y], obj->cali_sw[BMA222_AXIS_Z]);
+
+	/*calculate the real offset expected by caller*/
+	cali[BMA222_AXIS_X] += dat[BMA222_AXIS_X];
+	cali[BMA222_AXIS_Y] += dat[BMA222_AXIS_Y];
+	cali[BMA222_AXIS_Z] += dat[BMA222_AXIS_Z];
+
+	GSE_LOG("UPDATE: (%+3d %+3d %+3d)\n", 
+		dat[BMA222_AXIS_X], dat[BMA222_AXIS_Y], dat[BMA222_AXIS_Z]);
+
+#ifdef SW_CALIBRATION
+	obj->cali_sw[BMA222_AXIS_X] = obj->cvt.sign[BMA222_AXIS_X]*(cali[obj->cvt.map[BMA222_AXIS_X]]);
+	obj->cali_sw[BMA222_AXIS_Y] = obj->cvt.sign[BMA222_AXIS_Y]*(cali[obj->cvt.map[BMA222_AXIS_Y]]);
+	obj->cali_sw[BMA222_AXIS_Z] = obj->cvt.sign[BMA222_AXIS_Z]*(cali[obj->cvt.map[BMA222_AXIS_Z]]);	
+#else
+	int divisor = obj->reso->sensitivity/lsb;//modified
+	obj->offset[BMA222_AXIS_X] = (s8)(obj->cvt.sign[BMA222_AXIS_X]*(cali[obj->cvt.map[BMA222_AXIS_X]]) * obj->reso->sensitivity / GRAVITY_EARTH_1000/(divisor));
+	obj->offset[BMA222_AXIS_Y] = (s8)(obj->cvt.sign[BMA222_AXIS_Y]*(cali[obj->cvt.map[BMA222_AXIS_Y]]) * obj->reso->sensitivity / GRAVITY_EARTH_1000/(divisor));
+	obj->offset[BMA222_AXIS_Z] = (s8)(obj->cvt.sign[BMA222_AXIS_Z]*(cali[obj->cvt.map[BMA222_AXIS_Z]]) * obj->reso->sensitivity / GRAVITY_EARTH_1000/(divisor));
+
+	/*convert software calibration using standard calibration*/
+	obj->cali_sw[BMA222_AXIS_X] = obj->cvt.sign[BMA222_AXIS_X]*(cali[obj->cvt.map[BMA222_AXIS_X]])%(divisor);
+	obj->cali_sw[BMA222_AXIS_Y] = obj->cvt.sign[BMA222_AXIS_Y]*(cali[obj->cvt.map[BMA222_AXIS_Y]])%(divisor);
+	obj->cali_sw[BMA222_AXIS_Z] = obj->cvt.sign[BMA222_AXIS_Z]*(cali[obj->cvt.map[BMA222_AXIS_Z]])%(divisor);
+
+	GSE_LOG("NEWOFF: (%+3d %+3d %+3d): (%+3d %+3d %+3d) / (%+3d %+3d %+3d)\n", 
+		obj->offset[BMA222_AXIS_X]*divisor + obj->cali_sw[BMA222_AXIS_X], 
+		obj->offset[BMA222_AXIS_Y]*divisor + obj->cali_sw[BMA222_AXIS_Y], 
+		obj->offset[BMA222_AXIS_Z]*divisor + obj->cali_sw[BMA222_AXIS_Z], 
+		obj->offset[BMA222_AXIS_X], obj->offset[BMA222_AXIS_Y], obj->offset[BMA222_AXIS_Z],
+		obj->cali_sw[BMA222_AXIS_X], obj->cali_sw[BMA222_AXIS_Y], obj->cali_sw[BMA222_AXIS_Z]);
+
+	if((err = hwmsen_write_block(obj->client, BMA222_REG_OFSX, obj->offset, BMA222_AXES_NUM)))
+	{
+		GSE_ERR("write offset fail: %d\n", err);
+		return err;
+	}
+#endif
+	mdelay(1);
+	return err;
+}
+/*----------------------------------------------------------------------------*/
+static int BMA222_CheckDeviceID(struct i2c_client *client)
+{
+	u8 databuf[2]={0};    
+	int res = 0;
+	
+	res = bma_i2c_read_block(client,BMA222_REG_DEVID,databuf,0x1);
+	if(res < 0)
+	{
+		goto exit_BMA222_CheckDeviceID;
+	}
+	
+
+	GSE_LOG("BMA222_CheckDeviceID %d done!\n ", databuf[0]);
+
+	#if 0
+	if(databuf[0]!=BMA222_FIXED_DEVID)
+	{
+		GSE_LOG("BMA222_CheckDeviceID %d failt!\n ", databuf[0]);
+		return BMA222_ERR_IDENTIFICATION;
+	}
+	else
+	{
+		GSE_LOG("BMA222_CheckDeviceID %d pass!\n ", databuf[0]);
+	}
+	#endif
+
+	exit_BMA222_CheckDeviceID:
+	if (res < 0)
+	{
+		GSE_ERR("BMA222_CheckDeviceID %d failt!\n ", BMA222_ERR_I2C);
+		return BMA222_ERR_I2C;
+	}
+	mdelay(1);
+	return BMA222_SUCCESS;
+}
+/*----------------------------------------------------------------------------*/
+static int BMA222_SetPowerMode(struct i2c_client *client, bool enable)
+{
+	u8 databuf[2];    
+	int res = 0;
+	u8 addr = BMA222_REG_POWER_CTL;
+	struct bma222_i2c_data *obj = i2c_get_clientdata(client);
+	
+	//GSE_LOG("enter Sensor power status is sensor_power = %d\n",sensor_power);
+
+	if(enable == sensor_power )
+	{
+		GSE_LOG("Sensor power status is newest!\n");
+		return BMA222_SUCCESS;
+	}
+
+	if(bma_i2c_read_block(client, addr, databuf, 0x01))
+	{
+		GSE_ERR("read power ctl register err!\n");
+		return BMA222_ERR_I2C;
+	}
+	GSE_LOG("set power mode value = 0x%x!\n",databuf[0]);
+	mdelay(1);
+	if(enable == TRUE)
+	{
+		databuf[0] &= ~BMA222_MEASURE_MODE;
+	}
+	else
+	{
+		databuf[0] |= BMA222_MEASURE_MODE;
+	}
+	
+	res = bma_i2c_write_block(client,BMA222_REG_POWER_CTL,databuf,0x1);
+	if(res < 0)
+	{
+		GSE_LOG("set power mode failed!\n");
+		return BMA222_ERR_I2C;
+	}
+	else if(atomic_read(&obj->trace) & ADX_TRC_INFO)
+	{
+		GSE_LOG("set power mode ok %d!\n", databuf[1]);
+	}
+	
+	sensor_power = enable;
+	mdelay(1);
+	//GSE_LOG("leave Sensor power status is sensor_power = %d\n",sensor_power);
+	return BMA222_SUCCESS;    
+}
+/*----------------------------------------------------------------------------*/
+static int BMA222_SetDataFormat(struct i2c_client *client, u8 dataformat)
+{
+	struct bma222_i2c_data *obj = i2c_get_clientdata(client);
+	u8 databuf[10]={0};    
+	int res = 0;
+   
+	if(bma_i2c_read_block(client, BMA222_REG_DATA_FORMAT, databuf, 0x01))
+	{
+		printk("bma222 read Dataformat failt \n");
+		return BMA222_ERR_I2C;
+	}
+	mdelay(1);
+	databuf[0] &= ~BMA222_RANGE_MASK;
+	databuf[0] |= dataformat;
+	
+	res = bma_i2c_write_block(client,BMA222_REG_DATA_FORMAT,databuf,0x1);
+	if(res < 0)
+	{
+		return BMA222_ERR_I2C;
+	}
+	
+	//printk("BMA222_SetDataFormat OK! \n");
+	mdelay(1);
+	return BMA222_SetDataResolution(obj);    
+}
+/*----------------------------------------------------------------------------*/
+static int BMA222_SetBWRate(struct i2c_client *client, u8 bwrate)
+{
+	u8 databuf[10]={0};    
+	int res = 0;
+
+	if(bma_i2c_read_block(client, BMA222_REG_BW_RATE, databuf, 0x01))
+	{
+		printk("bma222 read rate failt \n");
+		return BMA222_ERR_I2C;
+	}
+	mdelay(1);
+	databuf[0] &= ~BMA222_BW_MASK;
+	databuf[0] |= bwrate;
+	
+
+    res = bma_i2c_write_block(client,BMA222_REG_BW_RATE,databuf,0x1);
+	if(res < 0)
+	{
+		return BMA222_ERR_I2C;
+	}
+	mdelay(1);
+	//printk("BMA222_SetBWRate OK! \n");
+	
+	return BMA222_SUCCESS;    
+}
+/*----------------------------------------------------------------------------*/
+static int BMA222_SetIntEnable(struct i2c_client *client, u8 intenable)
+{
+			//u8 databuf[10];    
+			int res = 0;
+		
+			res = hwmsen_write_byte(client, BMA222_INT_REG_1, 0x00);
+			if(res != BMA222_SUCCESS) 
+			{
+				return res;
+			}
+			mdelay(1);
+			res = hwmsen_write_byte(client, BMA222_INT_REG_2, 0x00);
+			if(res != BMA222_SUCCESS) 
+			{
+				return res;
+			}
+			//printk("BMA222 disable interrupt ...\n");
+		
+			/*for disable interrupt function*/
+			mdelay(1);
+			return BMA222_SUCCESS;	  
+}
+
+/*----------------------------------------------------------------------------*/
+static int bma222_init_client(struct i2c_client *client, int reset_cali)
+{
+	struct bma222_i2c_data *obj = i2c_get_clientdata(client);
+	int res = 0;
+	printk("bma222_init_client \n");
+
+	res = BMA222_CheckDeviceID(client); 
+	if(res != BMA222_SUCCESS)
+	{
+		return res;
+	}	
+	//printk("BMA222_CheckDeviceID ok \n");
+	
+	res = BMA222_SetBWRate(client, BMA222_BW_25HZ);
+	if(res != BMA222_SUCCESS ) 
+	{
+		return res;
+	}
+	//printk("BMA222_SetBWRate OK!\n");
+	
+	res = BMA222_SetDataFormat(client, BMA222_RANGE_2G);
+	if(res != BMA222_SUCCESS) 
+	{
+		return res;
+	}
+	//printk("BMA222_SetDataFormat OK!\n");
+
+	gsensor_gain.x = gsensor_gain.y = gsensor_gain.z = obj->reso->sensitivity;
+
+
+	res = BMA222_SetIntEnable(client, 0x00);        
+	if(res != BMA222_SUCCESS)
+	{
+		return res;
+	}
+	//printk("BMA222 disable interrupt function!\n");
+	
+	res = BMA222_SetPowerMode(client, enable_status);//false);//
+		if(res != BMA222_SUCCESS)
+		{
+			return res;
+		}
+	//printk("BMA222_SetPowerMode OK!\n");
+
+
+	if(0 != reset_cali)
+	{ 
+		/*reset calibration only in power on*/
+		res = BMA222_ResetCalibration(client);
+		if(res != BMA222_SUCCESS)
+		{
+			return res;
+		}
+	}
+	GSE_LOG("bma222_init_client OK!\n");
+#ifdef CONFIG_BMA222_LOWPASS
+	memset(&obj->fir, 0x00, sizeof(obj->fir));  
+#endif
+	return BMA222_SUCCESS;
+}
+/*----------------------------------------------------------------------------*/
+static int BMA222_ReadChipInfo(struct i2c_client *client, char *buf, int bufsize)
+{
+	u8 databuf[10];    
+
+	memset(databuf, 0, sizeof(u8)*10);
+
+	if((NULL == buf)||(bufsize<=30))
+	{
+		return -1;
+	}
+	
+	if(NULL == client)
+	{
+		*buf = 0;
+		return -2;
+	}
+
+	sprintf(buf, "BMA222 Chip");
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int BMA222_ReadSensorData(struct i2c_client *client, char *buf, int bufsize)
+{
+	struct bma222_i2c_data *obj = (struct bma222_i2c_data*)i2c_get_clientdata(client);
+	u8 databuf[20];
+	int acc[BMA222_AXES_NUM];
+	int res = 0;
+	memset(databuf, 0, sizeof(u8)*10);
+
+	if(NULL == buf)
+	{
+		return -1;
+	}
+	if(NULL == client)
+	{
+		*buf = 0;
+		return -2;
+	}
+		
+	if(sensor_suspend == 1)
+	{
+		//GSE_LOG("sensor in suspend read not data!\n");
+		return 0;
+	}
+	#if 0 //wrong operation marked
+	mutex_lock(&bma222_mutex);
+	if(sensor_power == FALSE)
+	{
+		GSE_ERR("BMA222_ReadSensorData bad operation sensor_power = %d!\n", sensor_power);
+		res = BMA222_SetPowerMode(client, true);
+		if(res)
+		{
+			GSE_ERR("Power on bma222 error %d!\n", res);
+		}
+	}
+	mutex_unlock(&bma222_mutex);	
+	#endif
+	if((res = BMA222_ReadData(client, obj->data))!=0)
+	{        
+		GSE_ERR("I2C error: ret value=%d", res);
+		return -3;
+	}
+	else
+	{
+	#if 1
+		obj->data[BMA222_AXIS_X] = obj->data[BMA222_AXIS_X] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+		obj->data[BMA222_AXIS_Y] = obj->data[BMA222_AXIS_Y] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+		obj->data[BMA222_AXIS_Z] = obj->data[BMA222_AXIS_Z] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;		
+	#endif	
+		//GSE_LOG("raw data x=%d, y=%d, z=%d \n",obj->data[BMA222_AXIS_X],obj->data[BMA222_AXIS_Y],obj->data[BMA222_AXIS_Z]);
+		obj->data[BMA222_AXIS_X] += obj->cali_sw[BMA222_AXIS_X];
+		obj->data[BMA222_AXIS_Y] += obj->cali_sw[BMA222_AXIS_Y];
+		obj->data[BMA222_AXIS_Z] += obj->cali_sw[BMA222_AXIS_Z];
+		
+		//printk("cali_sw x=%d, y=%d, z=%d \n",obj->cali_sw[BMA150_AXIS_X],obj->cali_sw[BMA150_AXIS_Y],obj->cali_sw[BMA150_AXIS_Z]);
+		
+		/*remap coordinate*/
+		acc[obj->cvt.map[BMA222_AXIS_X]] = obj->cvt.sign[BMA222_AXIS_X]*obj->data[BMA222_AXIS_X];
+		acc[obj->cvt.map[BMA222_AXIS_Y]] = obj->cvt.sign[BMA222_AXIS_Y]*obj->data[BMA222_AXIS_Y];
+		acc[obj->cvt.map[BMA222_AXIS_Z]] = obj->cvt.sign[BMA222_AXIS_Z]*obj->data[BMA222_AXIS_Z];
+		//printk("cvt x=%d, y=%d, z=%d \n",obj->cvt.sign[BMA150_AXIS_X],obj->cvt.sign[BMA150_AXIS_Y],obj->cvt.sign[BMA150_AXIS_Z]);
+
+
+		//GSE_LOG("Mapped gsensor data: %d, %d, %d!\n", acc[BMA150_AXIS_X], acc[BMA150_AXIS_Y], acc[BMA150_AXIS_Z]);
+
+		//Out put the mg
+		//printk("mg acc=%d, GRAVITY=%d, sensityvity=%d \n",acc[BMA150_AXIS_X],GRAVITY_EARTH_1000,obj->reso->sensitivity);
+#if 0
+		acc[BMA222_AXIS_X] = acc[BMA222_AXIS_X] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+		acc[BMA222_AXIS_Y] = acc[BMA222_AXIS_Y] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+		acc[BMA222_AXIS_Z] = acc[BMA222_AXIS_Z] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;		
+	#endif	
+	
+
+		sprintf(buf, "%04x %04x %04x", acc[BMA222_AXIS_X], acc[BMA222_AXIS_Y], acc[BMA222_AXIS_Z]);
+		if(atomic_read(&obj->trace) & ADX_TRC_IOCTL)
+		{
+			GSE_LOG("gsensor data: %s!\n", buf);
+		}
+	}
+	
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int BMA222_ReadRawData(struct i2c_client *client, char *buf)
+{
+	struct bma222_i2c_data *obj = (struct bma222_i2c_data*)i2c_get_clientdata(client);
+	int res = 0;
+
+	if (!buf || !client)
+	{
+		return EINVAL;
+	}
+	
+	if((res = BMA222_ReadData(client, obj->data)))
+	{        
+		GSE_ERR("I2C error: ret value=%d", res);
+		return EIO;
+	}
+	else
+	{
+		sprintf(buf, "BMA222_ReadRawData %04x %04x %04x", obj->data[BMA222_AXIS_X], 
+			obj->data[BMA222_AXIS_Y], obj->data[BMA222_AXIS_Z]);
+	
+	}
+	
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t show_chipinfo_value(struct device_driver *ddri, char *buf)
+{
+	struct i2c_client *client = bma222_i2c_client;
+	char strbuf[BMA222_BUFSIZE];
+	if(NULL == client)
+	{
+		GSE_ERR("i2c client is null!!\n");
+		return 0;
+	}
+	
+	BMA222_ReadChipInfo(client, strbuf, BMA222_BUFSIZE);
+	return snprintf(buf, PAGE_SIZE, "%s\n", strbuf);        
+}
+
+#if 0
+static ssize_t gsensor_init(struct device_driver *ddri, char *buf, size_t count)
+	{
+		struct i2c_client *client = bma222_i2c_client;
+		char strbuf[BMA222_BUFSIZE];
+		
+		if(NULL == client)
+		{
+			GSE_ERR("i2c client is null!!\n");
+			return 0;
+		}
+		bma222_init_client(client, 1);
+		return snprintf(buf, PAGE_SIZE, "%s\n", strbuf);			
+	}
+#endif
+
+
+/*----------------------------------------------------------------------------*/
+static ssize_t show_sensordata_value(struct device_driver *ddri, char *buf)
+{
+	struct i2c_client *client = bma222_i2c_client;
+	char strbuf[BMA222_BUFSIZE];
+	
+	if(NULL == client)
+	{
+		GSE_ERR("i2c client is null!!\n");
+		return 0;
+	}
+	BMA222_ReadSensorData(client, strbuf, BMA222_BUFSIZE);
+	//BMA150_ReadRawData(client, strbuf);
+	return snprintf(buf, PAGE_SIZE, "%s\n", strbuf);            
+}
+
+#if 0
+static ssize_t show_sensorrawdata_value(struct device_driver *ddri, char *buf, size_t count)
+	{
+		struct i2c_client *client = bma222_i2c_client;
+		char strbuf[BMA222_BUFSIZE];
+		
+		if(NULL == client)
+		{
+			GSE_ERR("i2c client is null!!\n");
+			return 0;
+		}
+		//BMA150_ReadSensorData(client, strbuf, BMA150_BUFSIZE);
+		BMA222_ReadRawData(client, strbuf);
+		return snprintf(buf, PAGE_SIZE, "%s\n", strbuf);			
+	}
+#endif
+
+/*----------------------------------------------------------------------------*/
+#if 1
+static ssize_t show_cali_value(struct device_driver *ddri, char *buf)
+{
+	struct i2c_client *client = bma222_i2c_client;
+	struct bma222_i2c_data *obj;
+	int err, len = 0, mul;
+	int tmp[BMA222_AXES_NUM];
+
+	if(NULL == client)
+	{
+		GSE_ERR("i2c client is null!!\n");
+		return 0;
+	}
+
+	obj = i2c_get_clientdata(client);
+
+
+
+	if((err = BMA222_ReadOffset(client, obj->offset)))
+	{
+		return -EINVAL;
+	}
+	else if((err = BMA222_ReadCalibration(client, tmp)))
+	{
+		return -EINVAL;
+	}
+	else
+	{    
+		mul = obj->reso->sensitivity/bma222_offset_resolution.sensitivity;
+		len += snprintf(buf+len, PAGE_SIZE-len, "[HW ][%d] (%+3d, %+3d, %+3d) : (0x%02X, 0x%02X, 0x%02X)\n", mul,                        
+			obj->offset[BMA222_AXIS_X], obj->offset[BMA222_AXIS_Y], obj->offset[BMA222_AXIS_Z],
+			obj->offset[BMA222_AXIS_X], obj->offset[BMA222_AXIS_Y], obj->offset[BMA222_AXIS_Z]);
+		len += snprintf(buf+len, PAGE_SIZE-len, "[SW ][%d] (%+3d, %+3d, %+3d)\n", 1, 
+			obj->cali_sw[BMA222_AXIS_X], obj->cali_sw[BMA222_AXIS_Y], obj->cali_sw[BMA222_AXIS_Z]);
+
+		len += snprintf(buf+len, PAGE_SIZE-len, "[ALL]    (%+3d, %+3d, %+3d) : (%+3d, %+3d, %+3d)\n", 
+			obj->offset[BMA222_AXIS_X]*mul + obj->cali_sw[BMA222_AXIS_X],
+			obj->offset[BMA222_AXIS_Y]*mul + obj->cali_sw[BMA222_AXIS_Y],
+			obj->offset[BMA222_AXIS_Z]*mul + obj->cali_sw[BMA222_AXIS_Z],
+			tmp[BMA222_AXIS_X], tmp[BMA222_AXIS_Y], tmp[BMA222_AXIS_Z]);
+		
+		return len;
+    }
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t store_cali_value(struct device_driver *ddri, const char *buf, size_t count)
+{
+	struct i2c_client *client = bma222_i2c_client;  
+	int err, x, y, z;
+	int dat[BMA222_AXES_NUM];
+
+	if(!strncmp(buf, "rst", 3))
+	{
+		if((err = BMA222_ResetCalibration(client)))
+		{
+			GSE_ERR("reset offset err = %d\n", err);
+		}	
+	}
+	else if(3 == sscanf(buf, "0x%02X 0x%02X 0x%02X", &x, &y, &z))
+	{
+		dat[BMA222_AXIS_X] = x;
+		dat[BMA222_AXIS_Y] = y;
+		dat[BMA222_AXIS_Z] = z;
+		if((err = BMA222_WriteCalibration(client, dat)))
+		{
+			GSE_ERR("write calibration err = %d\n", err);
+		}		
+	}
+	else
+	{
+		GSE_ERR("invalid format\n");
+	}
+	
+	return count;
+}
+#endif
+
+/*----------------------------------------------------------------------------*/
+static ssize_t show_firlen_value(struct device_driver *ddri, char *buf)
+{
+#ifdef CONFIG_BMA222_LOWPASS
+	struct i2c_client *client = bma222_i2c_client;
+	struct bma222_i2c_data *obj = i2c_get_clientdata(client);
+	if(atomic_read(&obj->firlen))
+	{
+		int idx, len = atomic_read(&obj->firlen);
+		GSE_LOG("len = %2d, idx = %2d\n", obj->fir.num, obj->fir.idx);
+
+		for(idx = 0; idx < len; idx++)
+		{
+			GSE_LOG("[%5d %5d %5d]\n", obj->fir.raw[idx][BMA222_AXIS_X], obj->fir.raw[idx][BMA222_AXIS_Y], obj->fir.raw[idx][BMA222_AXIS_Z]);
+		}
+		
+		GSE_LOG("sum = [%5d %5d %5d]\n", obj->fir.sum[BMA222_AXIS_X], obj->fir.sum[BMA222_AXIS_Y], obj->fir.sum[BMA222_AXIS_Z]);
+		GSE_LOG("avg = [%5d %5d %5d]\n", obj->fir.sum[BMA222_AXIS_X]/len, obj->fir.sum[BMA222_AXIS_Y]/len, obj->fir.sum[BMA222_AXIS_Z]/len);
+	}
+	return snprintf(buf, PAGE_SIZE, "%d\n", atomic_read(&obj->firlen));
+#else
+	return snprintf(buf, PAGE_SIZE, "not support\n");
+#endif
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t store_firlen_value(struct device_driver *ddri, const char *buf, size_t count)
+{
+#ifdef CONFIG_BMA222_LOWPASS
+	struct i2c_client *client = bma222_i2c_client;  
+	struct bma222_i2c_data *obj = i2c_get_clientdata(client);
+	int firlen;
+
+	if(1 != sscanf(buf, "%d", &firlen))
+	{
+		GSE_ERR("invallid format\n");
+	}
+	else if(firlen > C_MAX_FIR_LENGTH)
+	{
+		GSE_ERR("exceeds maximum filter length\n");
+	}
+	else
+	{ 
+		atomic_set(&obj->firlen, firlen);
+		if(NULL == firlen)
+		{
+			atomic_set(&obj->fir_en, 0);
+		}
+		else
+		{
+			memset(&obj->fir, 0x00, sizeof(obj->fir));
+			atomic_set(&obj->fir_en, 1);
+		}
+	}
+#endif    
+	return count;
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t show_trace_value(struct device_driver *ddri, char *buf)
+{
+	ssize_t res;
+	struct bma222_i2c_data *obj = obj_i2c_data;
+	if (obj == NULL)
+	{
+		GSE_ERR("i2c_data obj is null!!\n");
+		return 0;
+	}
+	
+	res = snprintf(buf, PAGE_SIZE, "0x%04X\n", atomic_read(&obj->trace));     
+	return res;    
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t store_trace_value(struct device_driver *ddri, const char *buf, size_t count)
+{
+	struct bma222_i2c_data *obj = obj_i2c_data;
+	int trace;
+	if (obj == NULL)
+	{
+		GSE_ERR("i2c_data obj is null!!\n");
+		return 0;
+	}
+	
+	if(1 == sscanf(buf, "0x%x", &trace))
+	{
+		atomic_set(&obj->trace, trace);
+	}	
+	else
+	{
+		GSE_ERR("invalid content: '%s', length = %d\n", buf, (int)count);
+	}
+	
+	return count;    
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t show_status_value(struct device_driver *ddri, char *buf)
+{
+	ssize_t len = 0;    
+	struct bma222_i2c_data *obj = obj_i2c_data;
+	if (obj == NULL)
+	{
+		GSE_ERR("i2c_data obj is null!!\n");
+		return 0;
+	}	
+	
+	if(obj->hw)
+	{
+		len += snprintf(buf+len, PAGE_SIZE-len, "CUST: %d %d (%d %d)\n", 
+	            obj->hw->i2c_num, obj->hw->direction, obj->hw->power_id, obj->hw->power_vol);   
+	}
+	else
+	{
+		len += snprintf(buf+len, PAGE_SIZE-len, "CUST: NULL\n");
+	}
+	return len;    
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t show_power_status_value(struct device_driver *ddri, char *buf)
+{
+	
+	u8 databuf[2];    
+	//int res = 0;
+	u8 addr = BMA222_REG_POWER_CTL;
+	struct bma222_i2c_data *obj = obj_i2c_data;
+	if(bma_i2c_read_block(obj->client, addr, databuf, 0x01))
+	{
+		GSE_ERR("read power ctl register err!\n");
+		return 1;
+	}
+	
+	if(sensor_power)
+		GSE_LOG("G sensor is in work mode, sensor_power = %d\n", sensor_power);
+	else
+		GSE_LOG("G sensor is in standby mode, sensor_power = %d\n", sensor_power);
+
+	return snprintf(buf, PAGE_SIZE, "%x\n", databuf[0]);
+}
+/*----------------------------------------------------------------------------*/
+static DRIVER_ATTR(chipinfo,   S_IWUSR | S_IRUGO, show_chipinfo_value,      NULL);
+static DRIVER_ATTR(sensordata, S_IWUSR | S_IRUGO, show_sensordata_value,    NULL);
+static DRIVER_ATTR(cali,       S_IWUSR | S_IRUGO, show_cali_value,          store_cali_value);
+static DRIVER_ATTR(firlen,     S_IWUSR | S_IRUGO, show_firlen_value,        store_firlen_value);
+static DRIVER_ATTR(trace,      S_IWUSR | S_IRUGO, show_trace_value,         store_trace_value);
+static DRIVER_ATTR(status,     S_IRUGO, show_status_value,        NULL);
+static DRIVER_ATTR(powerstatus,               S_IRUGO, show_power_status_value,        NULL);
+
+/*----------------------------------------------------------------------------*/
+static struct driver_attribute *bma222_attr_list[] = {
+	&driver_attr_chipinfo,     /*chip information*/
+	&driver_attr_sensordata,   /*dump sensor data*/
+	&driver_attr_cali,         /*show calibration data*/
+	&driver_attr_firlen,       /*filter length: 0: disable, others: enable*/
+	&driver_attr_trace,        /*trace log*/
+	&driver_attr_status,
+	&driver_attr_powerstatus,
+};
+/*----------------------------------------------------------------------------*/
+static int bma222_create_attr(struct device_driver *driver) 
+{
+	int idx, err = 0;
+	int num = (int)(sizeof(bma222_attr_list)/sizeof(bma222_attr_list[0]));
+	if (driver == NULL)
+	{
+		return -EINVAL;
+	}
+
+	for(idx = 0; idx < num; idx++)
+	{
+		if((err = driver_create_file(driver, bma222_attr_list[idx])))
+		{            
+			GSE_ERR("driver_create_file (%s) = %d\n", bma222_attr_list[idx]->attr.name, err);
+			break;
+		}
+	}    
+	return err;
+}
+/*----------------------------------------------------------------------------*/
+static int bma222_delete_attr(struct device_driver *driver)
+{
+	int idx ,err = 0;
+	int num = (int)(sizeof(bma222_attr_list)/sizeof(bma222_attr_list[0]));
+
+	if(driver == NULL)
+	{
+		return -EINVAL;
+	}
+	
+
+	for(idx = 0; idx < num; idx++)
+	{
+		driver_remove_file(driver, bma222_attr_list[idx]);
+	}
+	
+
+	return err;
+}
+
+/*----------------------------------------------------------------------------*/
+int gsensor_operate(void* self, uint32_t command, void* buff_in, int size_in,
+		void* buff_out, int size_out, int* actualout)
+{
+	int err = 0;
+	int value, sample_delay;	
+	struct bma222_i2c_data *priv = (struct bma222_i2c_data*)self;
+	hwm_sensor_data* gsensor_data;
+	char buff[BMA222_BUFSIZE];
+	
+	//GSE_FUN(f);
+	switch (command)
+	{
+		case SENSOR_DELAY:
+			if((buff_in == NULL) || (size_in < sizeof(int)))
+			{
+				GSE_ERR("Set delay parameter error!\n");
+				err = -EINVAL;
+			}
+			else
+			{
+				value = *(int *)buff_in;
+				if(value <= 5)
+				{
+					sample_delay = BMA222_BW_200HZ;
+				}
+				else if(value <= 10)
+				{
+					sample_delay = BMA222_BW_100HZ;
+				}
+				else
+				{
+					sample_delay = BMA222_BW_50HZ;
+				}
+				mutex_lock(&bma222_op_mutex);
+				err = BMA222_SetBWRate(priv->client, sample_delay);
+				if(err != BMA222_SUCCESS ) //0x2C->BW=100Hz
+				{
+					GSE_ERR("Set delay parameter error!\n");
+				}
+				mutex_unlock(&bma222_op_mutex);
+				if(value >= 50)
+				{
+					atomic_set(&priv->filter, 0);
+				}
+				else
+				{	
+				#if defined(CONFIG_BMA222_LOWPASS)
+					priv->fir.num = 0;
+					priv->fir.idx = 0;
+					priv->fir.sum[BMA222_AXIS_X] = 0;
+					priv->fir.sum[BMA222_AXIS_Y] = 0;
+					priv->fir.sum[BMA222_AXIS_Z] = 0;
+					atomic_set(&priv->filter, 1);
+				#endif
+				}
+			}
+			break;
+
+		case SENSOR_ENABLE:
+			if((buff_in == NULL) || (size_in < sizeof(int)))
+			{
+				GSE_ERR("Enable sensor parameter error!\n");
+				err = -EINVAL;
+			}
+			else
+			{
+				value = *(int *)buff_in;
+				mutex_lock(&bma222_op_mutex);
+				GSE_LOG("Gsensor device enable function enable = %d, sensor_power = %d!\n",value,sensor_power);
+				if(((value == 0) && (sensor_power == false)) ||((value == 1) && (sensor_power == true)))
+				{
+					enable_status = sensor_power;
+					GSE_LOG("Gsensor device have updated !\n");
+				}
+				else
+				{
+					enable_status = !sensor_power;
+					err = BMA222_SetPowerMode( priv->client, !sensor_power);
+					GSE_LOG("Gsensor not in suspend BMA222_SetPowerMode!, enable_status = %d\n",enable_status);
+					
+				}
+				mutex_unlock(&bma222_op_mutex);
+			}
+			break;
+
+		case SENSOR_GET_DATA:
+			if((buff_out == NULL) || (size_out< sizeof(hwm_sensor_data)))
+			{
+				GSE_ERR("get sensor data parameter error!\n");
+				err = -EINVAL;
+			}
+			else
+			{
+				gsensor_data = (hwm_sensor_data *)buff_out;
+				BMA222_ReadSensorData(priv->client, buff, BMA222_BUFSIZE);
+				sscanf(buff, "%x %x %x", &gsensor_data->values[0], 
+					&gsensor_data->values[1], &gsensor_data->values[2]);				
+				gsensor_data->status = SENSOR_STATUS_ACCURACY_MEDIUM;				
+				gsensor_data->value_divide = 1000;
+			}
+			break;
+		default:
+			GSE_ERR("gsensor operate function no this parameter %d!\n", command);
+			err = -1;
+			break;
+	}
+	
+	return err;
+}
+
+/****************************************************************************** 
+ * Function Configuration
+******************************************************************************/
+static int bma222_open(struct inode *inode, struct file *file)
+{
+	file->private_data = bma222_i2c_client;
+
+	if(file->private_data == NULL)
+	{
+		GSE_ERR("null pointer!!\n");
+		return -EINVAL;
+	}
+	return nonseekable_open(inode, file);
+}
+/*----------------------------------------------------------------------------*/
+static int bma222_release(struct inode *inode, struct file *file)
+{
+	file->private_data = NULL;
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+//static int bma222_ioctl(struct inode *inode, struct file *file, unsigned int cmd,
+//       unsigned long arg)
+static long bma222_unlocked_ioctl(struct file *file, unsigned int cmd,
+       unsigned long arg)
+
+{
+	struct i2c_client *client = (struct i2c_client*)file->private_data;
+	struct bma222_i2c_data *obj = (struct bma222_i2c_data*)i2c_get_clientdata(client);	
+	char strbuf[BMA222_BUFSIZE];
+	void __user *data;
+	SENSOR_DATA sensor_data;
+	long err = 0;
+	int cali[3];
+
+	//GSE_FUN(f);
+	if(_IOC_DIR(cmd) & _IOC_READ)
+	{
+		err = !access_ok(VERIFY_WRITE, (void __user *)arg, _IOC_SIZE(cmd));
+	}
+	else if(_IOC_DIR(cmd) & _IOC_WRITE)
+	{
+		err = !access_ok(VERIFY_READ, (void __user *)arg, _IOC_SIZE(cmd));
+	}
+
+	if(err)
+	{
+		GSE_ERR("access error: %08X, (%2d, %2d)\n", cmd, _IOC_DIR(cmd), _IOC_SIZE(cmd));
+		return -EFAULT;
+	}
+
+	switch(cmd)
+	{
+		case GSENSOR_IOCTL_INIT:
+			bma222_init_client(client, 0);	
+			break;
+
+		case GSENSOR_IOCTL_READ_CHIPINFO:
+			data = (void __user *) arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}
+			
+			BMA222_ReadChipInfo(client, strbuf, BMA222_BUFSIZE);
+			if(copy_to_user(data, strbuf, strlen(strbuf)+1))
+			{
+				err = -EFAULT;
+				break;
+			}				 
+			break;	  
+
+		case GSENSOR_IOCTL_READ_SENSORDATA:
+			data = (void __user *) arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}
+			BMA222_SetPowerMode(client,true);	
+			BMA222_ReadSensorData(client, strbuf, BMA222_BUFSIZE);
+			if(copy_to_user(data, strbuf, strlen(strbuf)+1))
+			{
+				err = -EFAULT;
+				break;	  
+			}				 
+			break;
+
+		case GSENSOR_IOCTL_READ_GAIN:
+			data = (void __user *) arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}			
+			
+			if(copy_to_user(data, &gsensor_gain, sizeof(GSENSOR_VECTOR3D)))
+			{
+				err = -EFAULT;
+				break;
+			}				 
+			break;
+
+		case GSENSOR_IOCTL_READ_RAW_DATA:
+			data = (void __user *) arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}
+			BMA222_ReadRawData(client, strbuf);
+			if(copy_to_user(data, &strbuf, strlen(strbuf)+1))
+			{
+				err = -EFAULT;
+				break;	  
+			}
+			break;	  
+
+		case GSENSOR_IOCTL_SET_CALI:
+			data = (void __user*)arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}
+			if(copy_from_user(&sensor_data, data, sizeof(sensor_data)))
+			{
+				err = -EFAULT;
+				break;	  
+			}
+			if(atomic_read(&obj->suspend))
+			{
+				GSE_ERR("Perform calibration in suspend state!!\n");
+				err = -EINVAL;
+			}
+			else
+			{
+			#if 0
+				cali[BMA222_AXIS_X] = sensor_data.x * obj->reso->sensitivity / GRAVITY_EARTH_1000;
+				cali[BMA222_AXIS_Y] = sensor_data.y * obj->reso->sensitivity / GRAVITY_EARTH_1000;
+				cali[BMA222_AXIS_Z] = sensor_data.z * obj->reso->sensitivity / GRAVITY_EARTH_1000;			  
+			#else
+			cali[BMA222_AXIS_X] = sensor_data.x;
+			cali[BMA222_AXIS_Y] = sensor_data.y;
+			cali[BMA222_AXIS_Z] = sensor_data.z;			  
+					
+
+			#endif
+				err = BMA222_WriteCalibration(client, cali);			 
+			}
+			break;
+
+		case GSENSOR_IOCTL_CLR_CALI:
+			err = BMA222_ResetCalibration(client);
+			break;
+
+		case GSENSOR_IOCTL_GET_CALI:
+			data = (void __user*)arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}
+			if((err = BMA222_ReadCalibration(client, cali)))
+			{
+				break;
+			}
+			#if 0
+			sensor_data.x = cali[BMA222_AXIS_X] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+			sensor_data.y = cali[BMA222_AXIS_Y] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+			sensor_data.z = cali[BMA222_AXIS_Z] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+			#else
+			sensor_data.x = cali[BMA222_AXIS_X];
+			sensor_data.y = cali[BMA222_AXIS_Y];
+			sensor_data.z = cali[BMA222_AXIS_Z];
+						
+
+			#endif
+			if(copy_to_user(data, &sensor_data, sizeof(sensor_data)))
+			{
+				err = -EFAULT;
+				break;
+			}		
+			break;
+		
+
+		default:
+			GSE_ERR("unknown IOCTL: 0x%08x\n", cmd);
+			err = -ENOIOCTLCMD;
+			break;
+			
+	}
+
+	return err;
+}
+
+
+/*----------------------------------------------------------------------------*/
+static struct file_operations bma222_fops = {
+	.owner = THIS_MODULE,
+	.open = bma222_open,
+	.release = bma222_release,
+	.unlocked_ioctl = bma222_unlocked_ioctl,
+};
+/*----------------------------------------------------------------------------*/
+static struct miscdevice bma222_device = {
+	.minor = MISC_DYNAMIC_MINOR,
+	.name = "gsensor",
+	.fops = &bma222_fops,
+};
+/*----------------------------------------------------------------------------*/
+#ifndef USE_EARLY_SUSPEND
+/*----------------------------------------------------------------------------*/
+static int bma222_suspend(struct i2c_client *client, pm_message_t msg) 
+{
+	struct bma222_i2c_data *obj = i2c_get_clientdata(client);    
+	int err = 0;
+	GSE_FUN();    
+    mutex_lock(&bma222_op_mutex);
+	if(msg.event == PM_EVENT_SUSPEND)
+	{   
+		if(obj == NULL)
+		{
+			GSE_ERR("null pointer!!\n");
+			mutex_unlock(&bma222_op_mutex);
+			return -EINVAL;
+		}
+		atomic_set(&obj->suspend, 1);
+		if((err = BMA222_SetPowerMode(obj->client, false)))
+		{
+			GSE_ERR("write power control fail!!\n");
+			mutex_unlock(&bma222_op_mutex);
+			return -EINVAL;
+		}       
+		BMA222_power(obj->hw, 0);
+	}
+	mutex_unlock(&bma222_op_mutex);
+	return err;
+}
+/*----------------------------------------------------------------------------*/
+static int bma222_resume(struct i2c_client *client)
+{
+	struct bma222_i2c_data *obj = i2c_get_clientdata(client);        
+	int err;
+	GSE_FUN();
+
+	if(obj == NULL)
+	{
+		GSE_ERR("null pointer!!\n");
+		return -EINVAL;
+	}
+	mutex_lock(&bma222_op_mutex);
+	BMA222_power(obj->hw, 1);
+
+	if((err = bma222_init_client(client, 0)))
+	{
+		GSE_ERR("initialize client fail!!\n");
+		mutex_unlock(&bma222_op_mutex);
+		return err;        
+	}
+	atomic_set(&obj->suspend, 0);
+    mutex_unlock(&bma222_op_mutex);
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+#else /*CONFIG_HAS_EARLY_SUSPEND is defined*/
+/*----------------------------------------------------------------------------*/
+static void bma222_early_suspend(struct early_suspend *h) 
+{
+	struct bma222_i2c_data *obj = container_of(h, struct bma222_i2c_data, early_drv);   
+	int err;
+		
+	if(obj == NULL)
+	{
+		GSE_ERR("null pointer!!\n");
+		return;
+	}
+	atomic_set(&obj->suspend, 1); 
+	mutex_lock(&bma222_op_mutex);
+	GSE_FUN();  
+	u8 databuf[2]; //for debug read power control register to see the value is OK
+	if(bma_i2c_read_block(obj->client, BMA222_REG_POWER_CTL, databuf, 0x01))
+	{
+		GSE_ERR("read power ctl register err!\n");
+		mutex_unlock(&bma222_op_mutex);
+		return BMA222_ERR_I2C;
+	}
+	if(databuf[0]==0xff)//if the value is ff the gsensor will not work anymore, any i2c operations won't be vaild
+		GSE_LOG("before BMA222_SetPowerMode in suspend databuf = 0x%x\n",databuf[0]);
+	if((err = BMA222_SetPowerMode(obj->client, false)))
+	{
+		GSE_ERR("write power control fail!!\n");
+		mutex_unlock(&bma222_op_mutex);
+		return;
+	}
+	if(bma_i2c_read_block(obj->client, BMA222_REG_POWER_CTL, databuf, 0x01)) //for debug read power control register to see the value is OK
+	{
+		GSE_ERR("read power ctl register err!\n");
+		mutex_unlock(&bma222_op_mutex);
+		return BMA222_ERR_I2C;
+	}
+	if(databuf[0]==0xff)//if the value is ff the gsensor will not work anymore, any i2c operations won't be vaild
+		GSE_LOG("after BMA222_SetPowerMode suspend err databuf = 0x%x\n",databuf[0]);
+	sensor_suspend = 1;
+	mutex_unlock(&bma222_op_mutex);
+	BMA222_power(obj->hw, 0);
+}
+/*----------------------------------------------------------------------------*/
+static void bma222_late_resume(struct early_suspend *h)
+{
+	struct bma222_i2c_data *obj = container_of(h, struct bma222_i2c_data, early_drv);         
+	int err;
+	
+	if(obj == NULL)
+	{
+		GSE_ERR("null pointer!!\n");
+		return;
+	}
+
+	BMA222_power(obj->hw, 1);
+	mutex_lock(&bma222_op_mutex);
+	GSE_FUN();
+	u8 databuf[2];//for debug read power control register to see the value is OK
+	if(bma_i2c_read_block(obj->client, BMA222_REG_POWER_CTL, databuf, 0x01))
+	{
+		GSE_ERR("read power ctl register err!\n");
+		mutex_unlock(&bma222_op_mutex);
+		return BMA222_ERR_I2C;
+	}
+	if(databuf[0]==0xff)//if the value is ff the gsensor will not work anymore, any i2c operations won't be vaild
+		GSE_LOG("before bma222_init_client databuf = 0x%x\n",databuf[0]);
+	if((err = bma222_init_client(obj->client, 0)))
+	{
+		GSE_ERR("initialize client fail!!\n");
+		mutex_unlock(&bma222_op_mutex);
+		return;        
+	}
+	
+	if(bma_i2c_read_block(obj->client, BMA222_REG_POWER_CTL, databuf, 0x01)) //for debug read power control register to see the value is OK
+	{
+		GSE_ERR("read power ctl register err!\n");
+		mutex_unlock(&bma222_op_mutex);
+		return BMA222_ERR_I2C;
+	}
+	if(databuf[0]==0xff)//if the value is ff the gsensor will not work anymore, any i2c operations won't be vaild
+		GSE_LOG("after bma222_init_client databuf = 0x%x\n",databuf[0]);
+	sensor_suspend = 0;
+	mutex_unlock(&bma222_op_mutex);
+	atomic_set(&obj->suspend, 0);    
+}
+/*----------------------------------------------------------------------------*/
+#endif /*USE_EARLY_SUSPEND*/
+/*----------------------------------------------------------------------------*/
+static int bma222_i2c_detect(struct i2c_client *client, struct i2c_board_info *info) 
+{    
+	strcpy(info->type, BMA222_DEV_NAME);
+	return 0;
+}
+
+/*----------------------------------------------------------------------------*/
+static int bma222_i2c_probe(struct i2c_client *client, const struct i2c_device_id *id)
+{
+	struct i2c_client *new_client;
+	struct bma222_i2c_data *obj;
+	struct hwmsen_object sobj;
+	int err = 0;
+	int retry = 0;
+	GSE_FUN();
+
+	if(!(obj = kzalloc(sizeof(*obj), GFP_KERNEL)))
+	{
+		err = -ENOMEM;
+		goto exit;
+	}
+	
+	memset(obj, 0, sizeof(struct bma222_i2c_data));
+
+	obj->hw = get_cust_acc_hw();
+	
+	if((err = hwmsen_get_convert(obj->hw->direction, &obj->cvt)))
+	{
+		GSE_ERR("invalid direction: %d\n", obj->hw->direction);
+		goto exit;
+	}
+
+	obj_i2c_data = obj;
+	obj->client = client;
+	new_client = obj->client;
+	i2c_set_clientdata(new_client,obj);
+	
+	atomic_set(&obj->trace, 0);
+	atomic_set(&obj->suspend, 0);
+	
+#ifdef CONFIG_BMA222_LOWPASS
+	if(obj->hw->firlen > C_MAX_FIR_LENGTH)
+	{
+		atomic_set(&obj->firlen, C_MAX_FIR_LENGTH);
+	}	
+	else
+	{
+		atomic_set(&obj->firlen, obj->hw->firlen);
+	}
+	
+	if(atomic_read(&obj->firlen) > 0)
+	{
+		atomic_set(&obj->fir_en, 1);
+	}
+	
+#endif
+
+	bma222_i2c_client = new_client;	
+
+	for(retry = 0; retry < 3; retry++){
+		if((err = bma222_init_client(new_client, 1)))
+		{
+			GSE_ERR("bma222_device init cilent fail time: %d\n", retry);
+			continue;
+		}
+	}
+	if(err != 0)
+		goto exit_init_failed;
+	
+
+	if((err = misc_register(&bma222_device)))
+	{
+		GSE_ERR("bma222_device register failed\n");
+		goto exit_misc_device_register_failed;
+	}
+
+	if((err = bma222_create_attr(&bma222_gsensor_driver.driver)))
+	{
+		GSE_ERR("create attribute err = %d\n", err);
+		goto exit_create_attr_failed;
+	}
+
+	sobj.self = obj;
+    sobj.polling = 1;
+    sobj.sensor_operate = gsensor_operate;
+	if((err = hwmsen_attach(ID_ACCELEROMETER, &sobj)))
+	{
+		GSE_ERR("attach fail = %d\n", err);
+		goto exit_kfree;
+	}
+
+#ifdef USE_EARLY_SUSPEND
+	obj->early_drv.level    = EARLY_SUSPEND_LEVEL_STOP_DRAWING - 2,
+	obj->early_drv.suspend  = bma222_early_suspend,
+	obj->early_drv.resume   = bma222_late_resume,    
+	register_early_suspend(&obj->early_drv);
+#endif 
+
+	GSE_LOG("%s: OK\n", __func__);    
+	return 0;
+
+	exit_create_attr_failed:
+	misc_deregister(&bma222_device);
+	exit_misc_device_register_failed:
+	exit_init_failed:
+	//i2c_detach_client(new_client);
+	exit_kfree:
+	kfree(obj);
+	exit:
+	GSE_ERR("%s: err = %d\n", __func__, err);        
+	return err;
+}
+
+/*----------------------------------------------------------------------------*/
+static int bma222_i2c_remove(struct i2c_client *client)
+{
+	int err = 0;	
+	
+	if((err = bma222_delete_attr(&bma222_gsensor_driver.driver)))
+	{
+		GSE_ERR("bma150_delete_attr fail: %d\n", err);
+	}
+	
+	if((err = misc_deregister(&bma222_device)))
+	{
+		GSE_ERR("misc_deregister fail: %d\n", err);
+	}
+
+	if((err = hwmsen_detach(ID_ACCELEROMETER)))
+	    
+
+	bma222_i2c_client = NULL;
+	i2c_unregister_device(client);
+	kfree(i2c_get_clientdata(client));
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int bma222_probe(struct platform_device *pdev) 
+{
+	struct acc_hw *hw = get_cust_acc_hw();
+	GSE_FUN();
+
+	BMA222_power(hw, 1);
+	//bma222_force[0] = hw->i2c_num;
+	if(i2c_add_driver(&bma222_i2c_driver))
+	{
+		GSE_ERR("add driver error\n");
+		return -1;
+	}
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int bma222_remove(struct platform_device *pdev)
+{
+    struct acc_hw *hw = get_cust_acc_hw();
+
+    GSE_FUN();    
+    BMA222_power(hw, 0);    
+    i2c_del_driver(&bma222_i2c_driver);
+    return 0;
+}
+/*----------------------------------------------------------------------------*/
+#if 1
+#ifdef CONFIG_OF
+static const struct of_device_id gsensor_of_match[] = {
+	{ .compatible = "mediatek,gsensor", },
+	{},
+};
+#endif
+
+static struct platform_driver bma222_gsensor_driver = {
+	.probe      = bma222_probe,
+	.remove     = bma222_remove,    
+	.driver     = 
+	{
+		.name  = "gsensor",
+		.owner  = THIS_MODULE,
+        #ifdef CONFIG_OF
+		.of_match_table = gsensor_of_match,
+		#endif
+	}
+};
+#else
+
+static struct platform_driver bma222_gsensor_driver = {
+	.probe      = bma222_probe,
+	.remove     = bma222_remove,    
+	.driver     = {
+		.name  = "gsensor",
+		.owner = THIS_MODULE,
+	}
+};
+#endif
+/*----------------------------------------------------------------------------*/
+static int __init bma222_init(void)
+{
+	//GSE_FUN();
+	struct acc_hw *hw = get_cust_acc_hw();
+	GSE_LOG("%s: i2c_number=%d\n", __func__,hw->i2c_num);
+	i2c_register_board_info(hw->i2c_num, &i2c_BMA222, 1);
+	if(platform_driver_register(&bma222_gsensor_driver))
+	{
+		GSE_ERR("failed to register driver");
+		return -ENODEV;
+	}
+	return 0;    
+}
+/*----------------------------------------------------------------------------*/
+static void __exit bma222_exit(void)
+{
+	GSE_FUN();
+	platform_driver_unregister(&bma222_gsensor_driver);
+}
+/*----------------------------------------------------------------------------*/
+module_init(bma222_init);
+module_exit(bma222_exit);
+/*----------------------------------------------------------------------------*/
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("BMA222 I2C driver");
+MODULE_AUTHOR("Xiaoli.li@mediatek.com");
diff --git a/drivers/misc/mediatek/accelerometer/bma222E/bma222E.h b/drivers/misc/mediatek/accelerometer/bma222E/bma222E.h
new file mode 100644
index 000000000..4146844df
--- /dev/null
+++ b/drivers/misc/mediatek/accelerometer/bma222E/bma222E.h
@@ -0,0 +1,71 @@
+/* linux/drivers/hwmon/adxl345.c
+ *
+ * (C) Copyright 2008 
+ * MediaTek <www.mediatek.com>
+ *
+ * BMA150 driver for MT6516
+ *
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA  BMA150
+ */
+#ifndef BMA150_H
+#define BMA150_H
+	 
+#include <linux/ioctl.h>
+	 
+	#define BMA222_I2C_SLAVE_WRITE_ADDR		0x10
+	 
+	 /* BMA222 Register Map  (Please refer to BMA150 Specifications) */
+	#define BMA222_REG_DEVID				0x00
+	#define BMA222_FIXED_DEVID			0x03
+	#define BMA222_REG_OFSX				0x16
+	#define BMA222_REG_OFSX_HIGH			0x1A
+	#define BMA222_REG_BW_RATE			0x10
+	#define BMA222_BW_MASK				0x1f
+	#define BMA222_BW_200HZ				0x0d
+	#define BMA222_BW_100HZ				0x0c
+	#define BMA222_BW_50HZ				0x0b
+	#define BMA222_BW_25HZ				0x0a
+	#define BMA222_REG_POWER_CTL		0x11		
+	#define BMA222_REG_DATA_FORMAT		0x0f
+	#define BMA222_RANGE_MASK			0x0f
+	#define BMA222_RANGE_2G				0x03
+	#define BMA222_RANGE_4G				0x05
+	#define BMA222_RANGE_8G				0x08
+	#define BMA222_REG_DATAXLOW			0x03
+	#define BMA222_REG_DATA_RESOLUTION	0x14
+	#define BMA222_MEASURE_MODE			0x80	
+	#define BMA222_SELF_TEST           			0x32
+	#define BMA222_SELF_TEST_AXIS_X		0x01
+	#define BMA222_SELF_TEST_AXIS_Y		0x02
+	#define BMA222_SELF_TEST_AXIS_Z		0x03
+	#define BMA222_SELF_TEST_POSITIVE	0x00
+	#define BMA222_SELF_TEST_NEGATIVE	0x04
+	#define BMA222_INT_REG_1           			0x16
+	#define BMA222_INT_REG_2          		 	0x17
+
+	
+#define BMA222_SUCCESS						0
+#define BMA222_ERR_I2C						-1
+#define BMA222_ERR_STATUS					-3
+#define BMA222_ERR_SETUP_FAILURE			-4
+#define BMA222_ERR_GETGSENSORDATA			-5
+#define BMA222_ERR_IDENTIFICATION			-6
+	 
+	 
+	 
+#define BMA222_BUFSIZE				256
+	 
+#endif
+
diff --git a/drivers/misc/mediatek/accelerometer/bma250/Makefile b/drivers/misc/mediatek/accelerometer/bma250/Makefile
new file mode 100644
index 000000000..e6ebdf822
--- /dev/null
+++ b/drivers/misc/mediatek/accelerometer/bma250/Makefile
@@ -0,0 +1,4 @@
+include $(srctree)/drivers/misc/mediatek/Makefile.custom
+
+obj-y	:=  bma250.o
+
diff --git a/drivers/misc/mediatek/accelerometer/bma250/bma250.c b/drivers/misc/mediatek/accelerometer/bma250/bma250.c
new file mode 100644
index 000000000..4391ae50c
--- /dev/null
+++ b/drivers/misc/mediatek/accelerometer/bma250/bma250.c
@@ -0,0 +1,1797 @@
+/* BMA150 motion sensor driver
+ *
+ *
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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.
+ *
+ */
+
+#include <linux/interrupt.h>
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include <linux/irq.h>
+#include <linux/miscdevice.h>
+#include <asm/uaccess.h>
+#include <linux/delay.h>
+#include <linux/input.h>
+#include <linux/workqueue.h>
+#include <linux/kobject.h>
+#include <linux/earlysuspend.h>
+#include <linux/platform_device.h>
+#include <asm/atomic.h>
+
+#include <mach/mt_typedefs.h>
+#include <mach/mt_gpio.h>
+#include <mach/mt_pm_ldo.h>
+
+
+#define POWER_NONE_MACRO MT65XX_POWER_NONE
+
+
+
+#include <cust_acc.h>
+#include <linux/hwmsensor.h>
+#include <linux/hwmsen_dev.h>
+#include <linux/sensors_io.h>
+#include "bma250.h"
+#include <linux/hwmsen_helper.h>
+/*----------------------------------------------------------------------------*/
+#define I2C_DRIVERID_BMA250 250
+/*----------------------------------------------------------------------------*/
+#define DEBUG 1
+/*----------------------------------------------------------------------------*/
+//#define CONFIG_BMA150_LOWPASS   /*apply low pass filter on output*/       
+#define SW_CALIBRATION
+
+/*----------------------------------------------------------------------------*/
+#define BMA250_AXIS_X          0
+#define BMA250_AXIS_Y          1
+#define BMA250_AXIS_Z          2
+#define BMA250_AXES_NUM        3
+#define BMA250_DATA_LEN        6
+#define BMA250_DEV_NAME        "BMA250"
+/*----------------------------------------------------------------------------*/
+
+/*----------------------------------------------------------------------------*/
+static const struct i2c_device_id bma250_i2c_id[] = {{BMA250_DEV_NAME,0},{}};
+static struct i2c_board_info __initdata i2c_bma250={ I2C_BOARD_INFO("BMA250", (0x30>>1))};
+
+
+/*the adapter id will be available in customization*/
+//static unsigned short bma250_force[] = {0x00, BMA250_I2C_SLAVE_WRITE_ADDR, I2C_CLIENT_END, I2C_CLIENT_END};
+//static const unsigned short *const bma250_forces[] = { bma250_force, NULL };
+//static struct i2c_client_address_data bma250_addr_data = { .forces = bma250_forces,};
+
+/*----------------------------------------------------------------------------*/
+static int bma250_i2c_probe(struct i2c_client *client, const struct i2c_device_id *id); 
+static int bma250_i2c_remove(struct i2c_client *client);
+//static int bma250_i2c_detect(struct i2c_client *client, int kind, struct i2c_board_info *info);
+
+/*----------------------------------------------------------------------------*/
+typedef enum {
+    ADX_TRC_FILTER  = 0x01,
+    ADX_TRC_RAWDATA = 0x02,
+    ADX_TRC_IOCTL   = 0x04,
+    ADX_TRC_CALI	= 0X08,
+    ADX_TRC_INFO	= 0X10,
+} ADX_TRC;
+/*----------------------------------------------------------------------------*/
+struct scale_factor{
+    u8  whole;
+    u8  fraction;
+};
+/*----------------------------------------------------------------------------*/
+struct data_resolution {
+    struct scale_factor scalefactor;
+    int                 sensitivity;
+};
+/*----------------------------------------------------------------------------*/
+#define C_MAX_FIR_LENGTH (32)
+/*----------------------------------------------------------------------------*/
+struct data_filter {
+    s16 raw[C_MAX_FIR_LENGTH][BMA250_AXES_NUM];
+    int sum[BMA250_AXES_NUM];
+    int num;
+    int idx;
+};
+/*----------------------------------------------------------------------------*/
+struct bma250_i2c_data {
+    struct i2c_client *client;
+    struct acc_hw *hw;
+    struct hwmsen_convert   cvt;
+    
+    /*misc*/
+    struct data_resolution *reso;
+    atomic_t                trace;
+    atomic_t                suspend;
+    atomic_t                selftest;
+	atomic_t				filter;
+    s16                     cali_sw[BMA250_AXES_NUM+1];
+
+    /*data*/
+    s8                      offset[BMA250_AXES_NUM+1];  /*+1: for 4-byte alignment*/
+    s16                     data[BMA250_AXES_NUM+1];
+
+#if defined(CONFIG_BMA250_LOWPASS)
+    atomic_t                firlen;
+    atomic_t                fir_en;
+    struct data_filter      fir;
+#endif 
+    /*early suspend*/
+#if defined(CONFIG_HAS_EARLYSUSPEND)
+    struct early_suspend    early_drv;
+#endif     
+};
+/*----------------------------------------------------------------------------*/
+static struct i2c_driver bma250_i2c_driver = {
+    .driver = {
+ //       .owner          = THIS_MODULE,
+        .name           = BMA250_DEV_NAME,
+    },
+	.probe      		= bma250_i2c_probe,
+	.remove    			= bma250_i2c_remove,
+//	.detect				= bma250_i2c_detect,
+#if !defined(CONFIG_HAS_EARLYSUSPEND)    
+    .suspend            = bma250_suspend,
+    .resume             = bma250_resume,
+#endif
+	.id_table = bma250_i2c_id,
+//	.address_data = &bma250_addr_data,
+};
+
+/*----------------------------------------------------------------------------*/
+static struct i2c_client *bma250_i2c_client = NULL;
+static struct platform_driver bma250_gsensor_driver;
+static struct bma250_i2c_data *obj_i2c_data = NULL;
+static bool sensor_power = true;
+static GSENSOR_VECTOR3D gsensor_gain;
+//static char selftestRes[8]= {0}; 
+
+/*----------------------------------------------------------------------------*/
+#define GSE_TAG                  "[Gsensor] "
+#define GSE_FUN(f)               printk(KERN_INFO GSE_TAG"%s\n", __FUNCTION__)
+#define GSE_ERR(fmt, args...)    printk(KERN_ERR GSE_TAG"%s %d : "fmt, __FUNCTION__, __LINE__, ##args)
+#define GSE_LOG(fmt, args...)    printk(KERN_INFO GSE_TAG fmt, ##args)
+/*----------------------------------------------------------------------------*/
+static struct data_resolution bma250_data_resolution[1] = {
+ /* combination by {FULL_RES,RANGE}*/
+    {{ 3, 9}, 256},   // dataformat +/-2g  in 8-bit resolution;  { 15, 6} = 15.6= (2*2*1000)/(2^8);  64 = (2^8)/(2*2)          
+};
+/*----------------------------------------------------------------------------*/
+static struct data_resolution bma250_offset_resolution = {{3, 9}, 256};
+
+/*--------------------BMA250 power control function----------------------------------*/
+static void BMA250_power(struct acc_hw *hw, unsigned int on) 
+{
+	static unsigned int power_on = 0;
+
+	if(hw->power_id != POWER_NONE_MACRO)		// have externel LDO
+	{        
+		GSE_LOG("power %s\n", on ? "on" : "off");
+		if(power_on == on)	// power status not change
+		{
+			GSE_LOG("ignore power control: %d\n", on);
+		}
+		else if(on)	// power on
+		{
+			if(!hwPowerOn(hw->power_id, hw->power_vol, "BMA250"))
+			{
+				GSE_ERR("power on fails!!\n");
+			}
+		}
+		else	// power off
+		{
+			if (!hwPowerDown(hw->power_id, "BMA250"))
+			{
+				GSE_ERR("power off fail!!\n");
+			}			  
+		}
+	}
+	power_on = on;    
+}
+/*----------------------------------------------------------------------------*/
+
+/*----------------------------------------------------------------------------*/
+static int BMA250_SetDataResolution(struct bma250_i2c_data *obj)
+{
+
+/*set g sensor dataresolution here*/
+
+/*BMA250 only can set to 10-bit dataresolution, so do nothing in bma250 driver here*/
+
+/*end of set dataresolution*/
+
+
+ 
+ /*we set measure range from -2g to +2g in BMA150_SetDataFormat(client, BMA150_RANGE_2G), 
+                                                    and set 10-bit dataresolution BMA150_SetDataResolution()*/
+                                                    
+ /*so bma250_data_resolution[0] set value as {{ 3, 9}, 256} when declaration, and assign the value to obj->reso here*/  
+
+ 	obj->reso = &bma250_data_resolution[0];
+	return 0;
+	
+/*if you changed the measure range, for example call: BMA250_SetDataFormat(client, BMA150_RANGE_4G), 
+you must set the right value to bma250_data_resolution*/
+
+}
+/*----------------------------------------------------------------------------*/
+static int BMA250_ReadData(struct i2c_client *client, s16 data[BMA250_AXES_NUM])
+{
+	struct bma250_i2c_data *priv = i2c_get_clientdata(client);        
+	u8 addr = BMA250_REG_DATAXLOW;
+	u8 buf[BMA250_DATA_LEN] = {0};
+	int err = 0;
+	int i;
+
+	if(NULL == client)
+	{
+		err = -EINVAL;
+	}
+	else if((err = hwmsen_read_block(client, addr, buf, 0x06)))
+	{
+		GSE_ERR("error: %d\n", err);
+	}
+	else
+	{
+		data[BMA250_AXIS_X] = (s16)((buf[BMA250_AXIS_X*2] >> 6) |
+		         (buf[BMA250_AXIS_X*2+1] << 2));
+		data[BMA250_AXIS_Y] = (s16)((buf[BMA250_AXIS_Y*2] >> 6) |
+		         (buf[BMA250_AXIS_Y*2+1] << 2));
+		data[BMA250_AXIS_Z] = (s16)((buf[BMA250_AXIS_Z*2] >> 6) |
+		         (buf[BMA250_AXIS_Z*2+1] << 2));
+
+		for(i=0;i<3;i++)				
+		{								//because the data is store in binary complement number formation in computer system
+			if ( data[i] == 0x0200 )	//so we want to calculate actual number here
+				data[i]= -512;			//10bit resolution, 512= 2^(10-1)
+			else if ( data[i] & 0x0200 )//transfor format
+			{							//printk("data 0 step %x \n",data[i]);
+				data[i] -= 0x1;			//printk("data 1 step %x \n",data[i]);
+				data[i] = ~data[i];		//printk("data 2 step %x \n",data[i]);
+				data[i] &= 0x01ff;		//printk("data 3 step %x \n\n",data[i]);
+				data[i] = -data[i];		
+			}
+		}	
+
+		if(atomic_read(&priv->trace) & ADX_TRC_RAWDATA)
+		{
+			GSE_LOG("[%08X %08X %08X] => [%5d %5d %5d] after\n", data[BMA250_AXIS_X], data[BMA250_AXIS_Y], data[BMA250_AXIS_Z],
+		                               data[BMA250_AXIS_X], data[BMA250_AXIS_Y], data[BMA250_AXIS_Z]);
+		}
+#ifdef CONFIG_BMA250_LOWPASS
+		if(atomic_read(&priv->filter))
+		{
+			if(atomic_read(&priv->fir_en) && !atomic_read(&priv->suspend))
+			{
+				int idx, firlen = atomic_read(&priv->firlen);   
+				if(priv->fir.num < firlen)
+				{                
+					priv->fir.raw[priv->fir.num][BMA250_AXIS_X] = data[BMA250_AXIS_X];
+					priv->fir.raw[priv->fir.num][BMA250_AXIS_Y] = data[BMA250_AXIS_Y];
+					priv->fir.raw[priv->fir.num][BMA250_AXIS_Z] = data[BMA250_AXIS_Z];
+					priv->fir.sum[BMA250_AXIS_X] += data[BMA250_AXIS_X];
+					priv->fir.sum[BMA250_AXIS_Y] += data[BMA250_AXIS_Y];
+					priv->fir.sum[BMA250_AXIS_Z] += data[BMA250_AXIS_Z];
+					if(atomic_read(&priv->trace) & ADX_TRC_FILTER)
+					{
+						GSE_LOG("add [%2d] [%5d %5d %5d] => [%5d %5d %5d]\n", priv->fir.num,
+							priv->fir.raw[priv->fir.num][BMA250_AXIS_X], priv->fir.raw[priv->fir.num][BMA250_AXIS_Y], priv->fir.raw[priv->fir.num][BMA250_AXIS_Z],
+							priv->fir.sum[BMA250_AXIS_X], priv->fir.sum[BMA250_AXIS_Y], priv->fir.sum[BMA250_AXIS_Z]);
+					}
+					priv->fir.num++;
+					priv->fir.idx++;
+				}
+				else
+				{
+					idx = priv->fir.idx % firlen;
+					priv->fir.sum[BMA250_AXIS_X] -= priv->fir.raw[idx][BMA250_AXIS_X];
+					priv->fir.sum[BMA250_AXIS_Y] -= priv->fir.raw[idx][BMA250_AXIS_Y];
+					priv->fir.sum[BMA250_AXIS_Z] -= priv->fir.raw[idx][BMA250_AXIS_Z];
+					priv->fir.raw[idx][BMA250_AXIS_X] = data[BMA250_AXIS_X];
+					priv->fir.raw[idx][BMA250_AXIS_Y] = data[BMA250_AXIS_Y];
+					priv->fir.raw[idx][BMA250_AXIS_Z] = data[BMA250_AXIS_Z];
+					priv->fir.sum[BMA250_AXIS_X] += data[BMA250_AXIS_X];
+					priv->fir.sum[BMA250_AXIS_Y] += data[BMA250_AXIS_Y];
+					priv->fir.sum[BMA250_AXIS_Z] += data[BMA250_AXIS_Z];
+					priv->fir.idx++;
+					data[BMA250_AXIS_X] = priv->fir.sum[BMA250_AXIS_X]/firlen;
+					data[BMA250_AXIS_Y] = priv->fir.sum[BMA250_AXIS_Y]/firlen;
+					data[BMA250_AXIS_Z] = priv->fir.sum[BMA250_AXIS_Z]/firlen;
+					if(atomic_read(&priv->trace) & ADX_TRC_FILTER)
+					{
+						GSE_LOG("add [%2d] [%5d %5d %5d] => [%5d %5d %5d] : [%5d %5d %5d]\n", idx,
+						priv->fir.raw[idx][BMA250_AXIS_X], priv->fir.raw[idx][BMA250_AXIS_Y], priv->fir.raw[idx][BMA250_AXIS_Z],
+						priv->fir.sum[BMA250_AXIS_X], priv->fir.sum[BMA250_AXIS_Y], priv->fir.sum[BMA250_AXIS_Z],
+						data[BMA250_AXIS_X], data[BMA250_AXIS_Y], data[BMA250_AXIS_Z]);
+					}
+				}
+			}
+		}	
+#endif         
+	}
+	return err;
+}
+/*----------------------------------------------------------------------------*/
+static int BMA250_ReadOffset(struct i2c_client *client, s8 ofs[BMA250_AXES_NUM])
+{    
+	int err;
+#ifdef SW_CALIBRATION
+	err = 0;
+	ofs[0]=ofs[1]=ofs[2]=0x0;
+#else
+	if(err = hwmsen_read_block(client, BMA250_REG_OFSX, ofs, BMA250_AXES_NUM))
+	{
+		GSE_ERR("error: %d\n", err);
+	}
+#endif
+	//printk("offesx=%x, y=%x, z=%x",ofs[0],ofs[1],ofs[2]);
+	
+	return err;    
+}
+/*----------------------------------------------------------------------------*/
+static int BMA250_ResetCalibration(struct i2c_client *client)
+{
+	struct bma250_i2c_data *obj = i2c_get_clientdata(client);
+	#ifndef SW_CALIBRATION 
+	int err;
+	u8 ofs[4]={0,0,0,0};
+	#endif 
+	
+	#ifdef SW_CALIBRATION
+		
+	#else
+		if(err = hwmsen_write_block(client, BMA250_REG_OFSX, ofs, 4))
+		{
+			GSE_ERR("error: %d\n", err);
+			return err;
+		}
+	#endif
+
+	memset(obj->cali_sw, 0x00, sizeof(obj->cali_sw));
+	memset(obj->offset, 0x00, sizeof(obj->offset));
+	return 0;    
+}
+/*----------------------------------------------------------------------------*/
+static int BMA250_ReadCalibration(struct i2c_client *client, int dat[BMA250_AXES_NUM])
+{
+    struct bma250_i2c_data *obj = i2c_get_clientdata(client);
+    #ifndef SW_CALIBRATION	
+    int err;
+    #endif
+    int mul;
+
+	#ifdef SW_CALIBRATION
+		mul = 0;//only SW Calibration, disable HW Calibration
+	#else
+	    if ((err = BMA250_ReadOffset(client, obj->offset))) {
+        GSE_ERR("read offset fail, %d\n", err);
+        return err;
+    	}    
+    	mul = obj->reso->sensitivity/bma250_offset_resolution.sensitivity;
+	#endif
+
+    dat[obj->cvt.map[BMA250_AXIS_X]] = obj->cvt.sign[BMA250_AXIS_X]*(obj->offset[BMA250_AXIS_X]*mul * GRAVITY_EARTH_1000 / obj->reso->sensitivity + obj->cali_sw[BMA250_AXIS_X]);
+    dat[obj->cvt.map[BMA250_AXIS_Y]] = obj->cvt.sign[BMA250_AXIS_Y]*(obj->offset[BMA250_AXIS_Y]*mul * GRAVITY_EARTH_1000 / obj->reso->sensitivity + obj->cali_sw[BMA250_AXIS_Y]);
+    dat[obj->cvt.map[BMA250_AXIS_Z]] = obj->cvt.sign[BMA250_AXIS_Z]*(obj->offset[BMA250_AXIS_Z]*mul * GRAVITY_EARTH_1000 / obj->reso->sensitivity + obj->cali_sw[BMA250_AXIS_Z]);                        
+                                       
+    return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int BMA250_ReadCalibrationEx(struct i2c_client *client, int act[BMA250_AXES_NUM], int raw[BMA250_AXES_NUM])
+{  
+	/*raw: the raw calibration data; act: the actual calibration data*/
+	struct bma250_i2c_data *obj = i2c_get_clientdata(client);
+	#ifndef SW_CALIBRATION
+	int err;
+	#endif 
+	int mul;
+
+ 
+
+	#ifdef SW_CALIBRATION
+		mul = 0;//only SW Calibration, disable HW Calibration
+	#else
+		if((err = BMA250_ReadOffset(client, obj->offset)))
+		{
+			GSE_ERR("read offset fail, %d\n", err);
+			return err;
+		}   
+		mul = obj->reso->sensitivity/bma250_offset_resolution.sensitivity;
+	#endif
+	
+	raw[BMA250_AXIS_X] = obj->offset[BMA250_AXIS_X]*mul * GRAVITY_EARTH_1000 / obj->reso->sensitivity + obj->cali_sw[BMA250_AXIS_X];
+	raw[BMA250_AXIS_Y] = obj->offset[BMA250_AXIS_Y]*mul * GRAVITY_EARTH_1000 / obj->reso->sensitivity + obj->cali_sw[BMA250_AXIS_Y];
+	raw[BMA250_AXIS_Z] = obj->offset[BMA250_AXIS_Z]*mul * GRAVITY_EARTH_1000 / obj->reso->sensitivity + obj->cali_sw[BMA250_AXIS_Z];
+
+	act[obj->cvt.map[BMA250_AXIS_X]] = obj->cvt.sign[BMA250_AXIS_X]*raw[BMA250_AXIS_X];
+	act[obj->cvt.map[BMA250_AXIS_Y]] = obj->cvt.sign[BMA250_AXIS_Y]*raw[BMA250_AXIS_Y];
+	act[obj->cvt.map[BMA250_AXIS_Z]] = obj->cvt.sign[BMA250_AXIS_Z]*raw[BMA250_AXIS_Z];                        
+	                       
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int BMA250_WriteCalibration(struct i2c_client *client, int dat[BMA250_AXES_NUM])
+{
+	struct bma250_i2c_data *obj = i2c_get_clientdata(client);
+	int err;
+	int cali[BMA250_AXES_NUM], raw[BMA250_AXES_NUM];
+#ifndef SW_CALIBRATION	
+	int lsb = bma250_offset_resolution.sensitivity;
+	int divisor = obj->reso->sensitivity/lsb;
+#endif 
+	if((err = BMA250_ReadCalibrationEx(client, cali, raw)))	/*offset will be updated in obj->offset*/
+	{ 
+		GSE_ERR("read offset fail, %d\n", err);
+		return err;
+	}
+
+	GSE_LOG("OLDOFF: (%+3d %+3d %+3d): (%+3d %+3d %+3d) / (%+3d %+3d %+3d)\n", 
+		raw[BMA250_AXIS_X], raw[BMA250_AXIS_Y], raw[BMA250_AXIS_Z],
+		obj->offset[BMA250_AXIS_X], obj->offset[BMA250_AXIS_Y], obj->offset[BMA250_AXIS_Z],
+		obj->cali_sw[BMA250_AXIS_X], obj->cali_sw[BMA250_AXIS_Y], obj->cali_sw[BMA250_AXIS_Z]);
+
+	/*calculate the real offset expected by caller*/
+	cali[BMA250_AXIS_X] += dat[BMA250_AXIS_X];
+	cali[BMA250_AXIS_Y] += dat[BMA250_AXIS_Y];
+	cali[BMA250_AXIS_Z] += dat[BMA250_AXIS_Z];
+
+	GSE_LOG("UPDATE: (%+3d %+3d %+3d)\n", 
+		dat[BMA250_AXIS_X], dat[BMA250_AXIS_Y], dat[BMA250_AXIS_Z]);
+
+#ifdef SW_CALIBRATION
+	obj->cali_sw[BMA250_AXIS_X] = obj->cvt.sign[BMA250_AXIS_X]*(cali[obj->cvt.map[BMA250_AXIS_X]]);
+	obj->cali_sw[BMA250_AXIS_Y] = obj->cvt.sign[BMA250_AXIS_Y]*(cali[obj->cvt.map[BMA250_AXIS_Y]]);
+	obj->cali_sw[BMA250_AXIS_Z] = obj->cvt.sign[BMA250_AXIS_Z]*(cali[obj->cvt.map[BMA250_AXIS_Z]]);	
+#else
+	obj->offset[BMA250_AXIS_X] = (s8)(obj->cvt.sign[BMA250_AXIS_X]*(cali[obj->cvt.map[BMA250_AXIS_X]]) * obj->reso->sensitivity / GRAVITY_EARTH_1000/(divisor));
+	obj->offset[BMA250_AXIS_Y] = (s8)(obj->cvt.sign[BMA250_AXIS_Y]*(cali[obj->cvt.map[BMA250_AXIS_Y]]) * obj->reso->sensitivity / GRAVITY_EARTH_1000/(divisor));
+	obj->offset[BMA250_AXIS_Z] = (s8)(obj->cvt.sign[BMA250_AXIS_Z]*(cali[obj->cvt.map[BMA250_AXIS_Z]]) * obj->reso->sensitivity / GRAVITY_EARTH_1000/(divisor));
+
+	/*convert software calibration using standard calibration*/
+	obj->cali_sw[BMA250_AXIS_X] = obj->cvt.sign[BMA250_AXIS_X]*(cali[obj->cvt.map[BMA250_AXIS_X]])%(divisor);
+	obj->cali_sw[BMA250_AXIS_Y] = obj->cvt.sign[BMA250_AXIS_Y]*(cali[obj->cvt.map[BMA250_AXIS_Y]])%(divisor);
+	obj->cali_sw[BMA250_AXIS_Z] = obj->cvt.sign[BMA250_AXIS_Z]*(cali[obj->cvt.map[BMA250_AXIS_Z]])%(divisor);
+
+	GSE_LOG("NEWOFF: (%+3d %+3d %+3d): (%+3d %+3d %+3d) / (%+3d %+3d %+3d)\n", 
+		obj->offset[BMA250_AXIS_X]*divisor + obj->cali_sw[BMA250_AXIS_X], 
+		obj->offset[BMA250_AXIS_Y]*divisor + obj->cali_sw[BMA250_AXIS_Y], 
+		obj->offset[BMA250_AXIS_Z]*divisor + obj->cali_sw[BMA250_AXIS_Z], 
+		obj->offset[BMA250_AXIS_X], obj->offset[BMA250_AXIS_Y], obj->offset[BMA250_AXIS_Z],
+		obj->cali_sw[BMA250_AXIS_X], obj->cali_sw[BMA250_AXIS_Y], obj->cali_sw[BMA250_AXIS_Z]);
+
+	if(err = hwmsen_write_block(obj->client, BMA250_REG_OFSX, obj->offset, BMA250_AXES_NUM))
+	{
+		GSE_ERR("write offset fail: %d\n", err);
+		return err;
+	}
+#endif
+
+	return err;
+}
+/*----------------------------------------------------------------------------*/
+static int BMA250_CheckDeviceID(struct i2c_client *client)
+{
+	u8 databuf[2];    
+	int res = 0;
+
+	memset(databuf, 0, sizeof(u8)*2);    
+	databuf[0] = BMA250_REG_DEVID;    
+
+	res = i2c_master_send(client, databuf, 0x1);
+	if(res <= 0)
+	{
+		goto exit_BMA250_CheckDeviceID;
+	}
+	
+	udelay(500);
+
+	databuf[0] = 0x0;        
+	res = i2c_master_recv(client, databuf, 0x01);
+	if(res <= 0)
+	{
+		goto exit_BMA250_CheckDeviceID;
+	}
+	
+
+	if(databuf[0]!=BMA250_FIXED_DEVID)
+	{
+		printk("BMA250_CheckDeviceID %d failt!\n ", databuf[0]);
+		return BMA250_ERR_IDENTIFICATION;
+	}
+	else
+	{
+		printk("BMA250_CheckDeviceID %d pass!\n ", databuf[0]);
+	}
+
+	exit_BMA250_CheckDeviceID:
+	if (res <= 0)
+	{
+		return BMA250_ERR_I2C;
+	}
+	
+	return BMA250_SUCCESS;
+}
+/*----------------------------------------------------------------------------*/
+static int BMA250_SetPowerMode(struct i2c_client *client, bool enable)
+{
+	u8 databuf[2];    
+	int res = 0;
+	u8 addr = BMA250_REG_POWER_CTL;
+	struct bma250_i2c_data *obj = i2c_get_clientdata(client);
+	
+	
+	if(enable == sensor_power )
+	{
+		GSE_LOG("Sensor power status is newest!\n");
+		return BMA250_SUCCESS;
+	}
+
+	if(hwmsen_read_block(client, addr, databuf, 0x01))
+	{
+		GSE_ERR("read power ctl register err!\n");
+		return BMA250_ERR_I2C;
+	}
+
+	
+	if(enable == TRUE)
+	{
+		databuf[0] &= ~BMA250_MEASURE_MODE;
+	}
+	else
+	{
+		databuf[0] |= BMA250_MEASURE_MODE;
+	}
+	databuf[1] = databuf[0];
+	databuf[0] = BMA250_REG_POWER_CTL;
+	
+
+	res = i2c_master_send(client, databuf, 0x2);
+
+	if(res <= 0)
+	{
+		GSE_LOG("set power mode failed!\n");
+		return BMA250_ERR_I2C;
+	}
+	else if(atomic_read(&obj->trace) & ADX_TRC_INFO)
+	{
+		GSE_LOG("set power mode ok %d!\n", databuf[1]);
+	}
+
+	//GSE_LOG("BMA250_SetPowerMode ok!\n");
+
+
+	sensor_power = enable;
+
+	mdelay(20);
+	
+	return BMA250_SUCCESS;    
+}
+/*----------------------------------------------------------------------------*/
+static int BMA250_SetDataFormat(struct i2c_client *client, u8 dataformat)
+{
+	struct bma250_i2c_data *obj = i2c_get_clientdata(client);
+	u8 databuf[10];    
+	int res = 0;
+
+	memset(databuf, 0, sizeof(u8)*10);    
+
+	if(hwmsen_read_block(client, BMA250_REG_DATA_FORMAT, databuf, 0x01))
+	{
+		printk("bma250 read Dataformat failt \n");
+		return BMA250_ERR_I2C;
+	}
+
+	databuf[0] &= ~BMA250_RANGE_MASK;
+	databuf[0] |= dataformat;
+	databuf[1] = databuf[0];
+	databuf[0] = BMA250_REG_DATA_FORMAT;
+
+
+	res = i2c_master_send(client, databuf, 0x2);
+
+	if(res <= 0)
+	{
+		return BMA250_ERR_I2C;
+	}
+	
+	//printk("BMA250_SetDataFormat OK! \n");
+	
+
+	return BMA250_SetDataResolution(obj);    
+}
+/*----------------------------------------------------------------------------*/
+static int BMA250_SetBWRate(struct i2c_client *client, u8 bwrate)
+{
+	u8 databuf[10];    
+	int res = 0;
+
+	memset(databuf, 0, sizeof(u8)*10);    
+
+	if(hwmsen_read_block(client, BMA250_REG_BW_RATE, databuf, 0x01))
+	{
+		printk("bma250 read rate failt \n");
+		return BMA250_ERR_I2C;
+	}
+
+	databuf[0] &= ~BMA250_BW_MASK;
+	databuf[0] |= bwrate;
+	databuf[1] = databuf[0];
+	databuf[0] = BMA250_REG_BW_RATE;
+
+
+	res = i2c_master_send(client, databuf, 0x2);
+
+	if(res <= 0)
+	{
+		return BMA250_ERR_I2C;
+	}
+	
+	//printk("BMA250_SetBWRate OK! \n");
+	
+	return BMA250_SUCCESS;    
+}
+/*----------------------------------------------------------------------------*/
+static int BMA250_SetIntEnable(struct i2c_client *client, u8 intenable)
+{
+	//		u8 databuf[10];    
+			int res = 0;
+		
+			res = hwmsen_write_byte(client, BMA250_INT_REG_1, 0x00);
+			if(res != BMA250_SUCCESS) 
+			{
+				return res;
+			}
+			res = hwmsen_write_byte(client, BMA250_INT_REG_2, 0x00);
+			if(res != BMA250_SUCCESS) 
+			{
+				return res;
+			}
+			printk("BMA250 disable interrupt ...\n");
+		
+			/*for disable interrupt function*/
+			
+			return BMA250_SUCCESS;	  
+}
+
+/*----------------------------------------------------------------------------*/
+static int bma250_init_client(struct i2c_client *client, int reset_cali)
+{
+	struct bma250_i2c_data *obj = i2c_get_clientdata(client);
+	int res = 0;
+	printk("bma250_init_client \n");
+
+	res = BMA250_CheckDeviceID(client); 
+	if(res != BMA250_SUCCESS)
+	{
+		return res;
+	}	
+	printk("BMA250_CheckDeviceID ok \n");
+	
+	res = BMA250_SetBWRate(client, BMA250_BW_100HZ);
+	if(res != BMA250_SUCCESS ) 
+	{
+		return res;
+	}
+	printk("BMA250_SetBWRate OK!\n");
+	
+	res = BMA250_SetDataFormat(client, BMA250_RANGE_2G);
+	if(res != BMA250_SUCCESS) 
+	{
+		return res;
+	}
+	printk("BMA250_SetDataFormat OK!\n");
+
+	gsensor_gain.x = gsensor_gain.y = gsensor_gain.z = obj->reso->sensitivity;
+
+
+	res = BMA250_SetIntEnable(client, 0x00);        
+	if(res != BMA250_SUCCESS)
+	{
+		return res;
+	}
+	printk("BMA250 disable interrupt function!\n");
+
+	res = BMA250_SetPowerMode(client, false);
+		if(res != BMA250_SUCCESS)
+		{
+			return res;
+		}
+		printk("BMA250_SetPowerMode OK!\n");
+
+
+	if(0 != reset_cali)
+	{ 
+		/*reset calibration only in power on*/
+		res = BMA250_ResetCalibration(client);
+		if(res != BMA250_SUCCESS)
+		{
+			return res;
+		}
+	}
+	printk("bma250_init_client OK!\n");
+#ifdef CONFIG_BMA250_LOWPASS
+	memset(&obj->fir, 0x00, sizeof(obj->fir));  
+#endif
+
+	mdelay(20);
+
+	return BMA250_SUCCESS;
+}
+/*----------------------------------------------------------------------------*/
+static int BMA250_ReadChipInfo(struct i2c_client *client, char *buf, int bufsize)
+{
+	u8 databuf[10];    
+
+	memset(databuf, 0, sizeof(u8)*10);
+
+	if((NULL == buf)||(bufsize<=30))
+	{
+		return -1;
+	}
+	
+	if(NULL == client)
+	{
+		*buf = 0;
+		return -2;
+	}
+
+	sprintf(buf, "BMA250 Chip");
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int BMA250_ReadSensorData(struct i2c_client *client, char *buf, int bufsize)
+{
+	struct bma250_i2c_data *obj = (struct bma250_i2c_data*)i2c_get_clientdata(client);
+	u8 databuf[20];
+	int acc[BMA250_AXES_NUM];
+	int res = 0;
+	memset(databuf, 0, sizeof(u8)*10);
+
+	if(NULL == buf)
+	{
+		return -1;
+	}
+	if(NULL == client)
+	{
+		*buf = 0;
+		return -2;
+	}
+
+	if(sensor_power == FALSE)
+	{
+		res = BMA250_SetPowerMode(client, true);
+		if(res)
+		{
+			GSE_ERR("Power on bma250 error %d!\n", res);
+		}
+	}
+
+	if((res = BMA250_ReadData(client, obj->data)))
+	{        
+		GSE_ERR("I2C error: ret value=%d", res);
+		return -3;
+	}
+	else
+	{
+	#if 1
+		obj->data[BMA250_AXIS_X] = obj->data[BMA250_AXIS_X] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+		obj->data[BMA250_AXIS_Y] = obj->data[BMA250_AXIS_Y] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+		obj->data[BMA250_AXIS_Z] = obj->data[BMA250_AXIS_Z] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;		
+	#endif	
+		//printk("raw data x=%d, y=%d, z=%d \n",obj->data[BMA150_AXIS_X],obj->data[BMA150_AXIS_Y],obj->data[BMA150_AXIS_Z]);
+		obj->data[BMA250_AXIS_X] += obj->cali_sw[BMA250_AXIS_X];
+		obj->data[BMA250_AXIS_Y] += obj->cali_sw[BMA250_AXIS_Y];
+		obj->data[BMA250_AXIS_Z] += obj->cali_sw[BMA250_AXIS_Z];
+		
+		//printk("cali_sw x=%d, y=%d, z=%d \n",obj->cali_sw[BMA150_AXIS_X],obj->cali_sw[BMA150_AXIS_Y],obj->cali_sw[BMA150_AXIS_Z]);
+		
+		/*remap coordinate*/
+		acc[obj->cvt.map[BMA250_AXIS_X]] = obj->cvt.sign[BMA250_AXIS_X]*obj->data[BMA250_AXIS_X];
+		acc[obj->cvt.map[BMA250_AXIS_Y]] = obj->cvt.sign[BMA250_AXIS_Y]*obj->data[BMA250_AXIS_Y];
+		acc[obj->cvt.map[BMA250_AXIS_Z]] = obj->cvt.sign[BMA250_AXIS_Z]*obj->data[BMA250_AXIS_Z];
+		//printk("cvt x=%d, y=%d, z=%d \n",obj->cvt.sign[BMA150_AXIS_X],obj->cvt.sign[BMA150_AXIS_Y],obj->cvt.sign[BMA150_AXIS_Z]);
+
+
+		//GSE_LOG("Mapped gsensor data: %d, %d, %d!\n", acc[BMA150_AXIS_X], acc[BMA150_AXIS_Y], acc[BMA150_AXIS_Z]);
+
+		//Out put the mg
+		//printk("mg acc=%d, GRAVITY=%d, sensityvity=%d \n",acc[BMA150_AXIS_X],GRAVITY_EARTH_1000,obj->reso->sensitivity);
+#if 0
+		acc[BMA250_AXIS_X] = acc[BMA250_AXIS_X] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+		acc[BMA250_AXIS_Y] = acc[BMA250_AXIS_Y] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+		acc[BMA250_AXIS_Z] = acc[BMA250_AXIS_Z] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;		
+	#endif	
+	
+
+		sprintf(buf, "%04x %04x %04x", acc[BMA250_AXIS_X], acc[BMA250_AXIS_Y], acc[BMA250_AXIS_Z]);
+		if(atomic_read(&obj->trace) & ADX_TRC_IOCTL)
+		{
+			GSE_LOG("gsensor data: %s!\n", buf);
+		}
+	}
+	
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int BMA250_ReadRawData(struct i2c_client *client, char *buf)
+{
+	struct bma250_i2c_data *obj = (struct bma250_i2c_data*)i2c_get_clientdata(client);
+	int res = 0;
+
+	if (!buf || !client)
+	{
+		return EINVAL;
+	}
+	
+	if((res = BMA250_ReadData(client, obj->data)))
+	{        
+		GSE_ERR("I2C error: ret value=%d", res);
+		return EIO;
+	}
+	else
+	{
+		sprintf(buf, "BMA250_ReadRawData %04x %04x %04x", obj->data[BMA250_AXIS_X], 
+			obj->data[BMA250_AXIS_Y], obj->data[BMA250_AXIS_Z]);
+	
+	}
+	
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t show_chipinfo_value(struct device_driver *ddri, char *buf)
+{
+	struct i2c_client *client = bma250_i2c_client;
+	char strbuf[BMA250_BUFSIZE];
+	if(NULL == client)
+	{
+		GSE_ERR("i2c client is null!!\n");
+		return 0;
+	}
+	
+	BMA250_ReadChipInfo(client, strbuf, BMA250_BUFSIZE);
+	return snprintf(buf, PAGE_SIZE, "%s\n", strbuf);        
+}
+
+/*
+static ssize_t gsensor_init(struct device_driver *ddri, char *buf, size_t count)
+	{
+		struct i2c_client *client = bma250_i2c_client;
+		char strbuf[BMA250_BUFSIZE];
+		
+		if(NULL == client)
+		{
+			GSE_ERR("i2c client is null!!\n");
+			return 0;
+		}
+		bma250_init_client(client, 1);
+		return snprintf(buf, PAGE_SIZE, "%s\n", strbuf);			
+	}
+
+*/
+
+/*----------------------------------------------------------------------------*/
+static ssize_t show_sensordata_value(struct device_driver *ddri, char *buf)
+{
+	struct i2c_client *client = bma250_i2c_client;
+	char strbuf[BMA250_BUFSIZE];
+	
+	if(NULL == client)
+	{
+		GSE_ERR("i2c client is null!!\n");
+		return 0;
+	}
+	BMA250_ReadSensorData(client, strbuf, BMA250_BUFSIZE);
+	//BMA150_ReadRawData(client, strbuf);
+	return snprintf(buf, PAGE_SIZE, "%s\n", strbuf);            
+}
+
+/*
+static ssize_t show_sensorrawdata_value(struct device_driver *ddri, char *buf, size_t count)
+	{
+		struct i2c_client *client = bma250_i2c_client;
+		char strbuf[BMA250_BUFSIZE];
+		
+		if(NULL == client)
+		{
+			GSE_ERR("i2c client is null!!\n");
+			return 0;
+		}
+		//BMA150_ReadSensorData(client, strbuf, BMA150_BUFSIZE);
+		BMA250_ReadRawData(client, strbuf);
+		return snprintf(buf, PAGE_SIZE, "%s\n", strbuf);			
+	}
+*/
+/*----------------------------------------------------------------------------*/
+static ssize_t show_cali_value(struct device_driver *ddri, char *buf)
+{
+	struct i2c_client *client = bma250_i2c_client;
+	struct bma250_i2c_data *obj;
+	int err, len = 0, mul;
+	int tmp[BMA250_AXES_NUM];
+
+	if(NULL == client)
+	{
+		GSE_ERR("i2c client is null!!\n");
+		return 0;
+	}
+
+	obj = i2c_get_clientdata(client);
+
+
+
+	if((err = BMA250_ReadOffset(client, obj->offset)))
+	{
+		return -EINVAL;
+	}
+	else if((err = BMA250_ReadCalibration(client, tmp)))
+	{
+		return -EINVAL;
+	}
+	else
+	{    
+		mul = obj->reso->sensitivity/bma250_offset_resolution.sensitivity;
+		len += snprintf(buf+len, PAGE_SIZE-len, "[HW ][%d] (%+3d, %+3d, %+3d) : (0x%02X, 0x%02X, 0x%02X)\n", mul,                        
+			obj->offset[BMA250_AXIS_X], obj->offset[BMA250_AXIS_Y], obj->offset[BMA250_AXIS_Z],
+			obj->offset[BMA250_AXIS_X], obj->offset[BMA250_AXIS_Y], obj->offset[BMA250_AXIS_Z]);
+		len += snprintf(buf+len, PAGE_SIZE-len, "[SW ][%d] (%+3d, %+3d, %+3d)\n", 1, 
+			obj->cali_sw[BMA250_AXIS_X], obj->cali_sw[BMA250_AXIS_Y], obj->cali_sw[BMA250_AXIS_Z]);
+
+		len += snprintf(buf+len, PAGE_SIZE-len, "[ALL]    (%+3d, %+3d, %+3d) : (%+3d, %+3d, %+3d)\n", 
+			obj->offset[BMA250_AXIS_X]*mul + obj->cali_sw[BMA250_AXIS_X],
+			obj->offset[BMA250_AXIS_Y]*mul + obj->cali_sw[BMA250_AXIS_Y],
+			obj->offset[BMA250_AXIS_Z]*mul + obj->cali_sw[BMA250_AXIS_Z],
+			tmp[BMA250_AXIS_X], tmp[BMA250_AXIS_Y], tmp[BMA250_AXIS_Z]);
+		
+		return len;
+    }
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t store_cali_value(struct device_driver *ddri, const char *buf, size_t count)
+{
+	struct i2c_client *client = bma250_i2c_client;  
+	int err, x, y, z;
+	int dat[BMA250_AXES_NUM];
+
+	if(!strncmp(buf, "rst", 3))
+	{
+		if((err = BMA250_ResetCalibration(client)))
+		{
+			GSE_ERR("reset offset err = %d\n", err);
+		}	
+	}
+	else if(3 == sscanf(buf, "0x%02X 0x%02X 0x%02X", &x, &y, &z))
+	{
+		dat[BMA250_AXIS_X] = x;
+		dat[BMA250_AXIS_Y] = y;
+		dat[BMA250_AXIS_Z] = z;
+		if((err = BMA250_WriteCalibration(client, dat)))
+		{
+			GSE_ERR("write calibration err = %d\n", err);
+		}		
+	}
+	else
+	{
+		GSE_ERR("invalid format\n");
+	}
+	
+	return count;
+}
+
+
+/*----------------------------------------------------------------------------*/
+static ssize_t show_firlen_value(struct device_driver *ddri, char *buf)
+{
+#ifdef CONFIG_BMA250_LOWPASS
+	struct i2c_client *client = bma250_i2c_client;
+	struct bma250_i2c_data *obj = i2c_get_clientdata(client);
+	if(atomic_read(&obj->firlen))
+	{
+		int idx, len = atomic_read(&obj->firlen);
+		GSE_LOG("len = %2d, idx = %2d\n", obj->fir.num, obj->fir.idx);
+
+		for(idx = 0; idx < len; idx++)
+		{
+			GSE_LOG("[%5d %5d %5d]\n", obj->fir.raw[idx][BMA250_AXIS_X], obj->fir.raw[idx][BMA250_AXIS_Y], obj->fir.raw[idx][BMA250_AXIS_Z]);
+		}
+		
+		GSE_LOG("sum = [%5d %5d %5d]\n", obj->fir.sum[BMA250_AXIS_X], obj->fir.sum[BMA250_AXIS_Y], obj->fir.sum[BMA250_AXIS_Z]);
+		GSE_LOG("avg = [%5d %5d %5d]\n", obj->fir.sum[BMA250_AXIS_X]/len, obj->fir.sum[BMA250_AXIS_Y]/len, obj->fir.sum[BMA250_AXIS_Z]/len);
+	}
+	return snprintf(buf, PAGE_SIZE, "%d\n", atomic_read(&obj->firlen));
+#else
+	return snprintf(buf, PAGE_SIZE, "not support\n");
+#endif
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t store_firlen_value(struct device_driver *ddri, const char *buf, size_t count)
+{
+#ifdef CONFIG_BMA250_LOWPASS
+	struct i2c_client *client = bma250_i2c_client;  
+	struct bma250_i2c_data *obj = i2c_get_clientdata(client);
+	int firlen;
+
+	if(1 != sscanf(buf, "%d", &firlen))
+	{
+		GSE_ERR("invallid format\n");
+	}
+	else if(firlen > C_MAX_FIR_LENGTH)
+	{
+		GSE_ERR("exceeds maximum filter length\n");
+	}
+	else
+	{ 
+		atomic_set(&obj->firlen, firlen);
+		if(NULL == firlen)
+		{
+			atomic_set(&obj->fir_en, 0);
+		}
+		else
+		{
+			memset(&obj->fir, 0x00, sizeof(obj->fir));
+			atomic_set(&obj->fir_en, 1);
+		}
+	}
+#endif    
+	return count;
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t show_trace_value(struct device_driver *ddri, char *buf)
+{
+	ssize_t res;
+	struct bma250_i2c_data *obj = obj_i2c_data;
+	if (obj == NULL)
+	{
+		GSE_ERR("i2c_data obj is null!!\n");
+		return 0;
+	}
+	
+	res = snprintf(buf, PAGE_SIZE, "0x%04X\n", atomic_read(&obj->trace));     
+	return res;    
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t store_trace_value(struct device_driver *ddri, const char *buf, size_t count)
+{
+	struct bma250_i2c_data *obj = obj_i2c_data;
+	int trace;
+	if (obj == NULL)
+	{
+		GSE_ERR("i2c_data obj is null!!\n");
+		return 0;
+	}
+	
+	if(1 == sscanf(buf, "0x%x", &trace))
+	{
+		atomic_set(&obj->trace, trace);
+	}	
+	else
+	{
+		GSE_ERR("invalid content: '%s', length = %d\n", buf, count);
+	}
+	
+	return count;    
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t show_status_value(struct device_driver *ddri, char *buf)
+{
+	ssize_t len = 0;    
+	struct bma250_i2c_data *obj = obj_i2c_data;
+	if (obj == NULL)
+	{
+		GSE_ERR("i2c_data obj is null!!\n");
+		return 0;
+	}	
+	
+	if(obj->hw)
+	{
+		len += snprintf(buf+len, PAGE_SIZE-len, "CUST: %d %d (%d %d)\n", 
+	            obj->hw->i2c_num, obj->hw->direction, obj->hw->power_id, obj->hw->power_vol);   
+	}
+	else
+	{
+		len += snprintf(buf+len, PAGE_SIZE-len, "CUST: NULL\n");
+	}
+	return len;    
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t show_power_status_value(struct device_driver *ddri, char *buf)
+{
+	if(sensor_power)
+		printk("G sensor is in work mode, sensor_power = %d\n", sensor_power);
+	else
+		printk("G sensor is in standby mode, sensor_power = %d\n", sensor_power);
+
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static DRIVER_ATTR(chipinfo,   S_IWUSR | S_IRUGO, show_chipinfo_value,      NULL);
+static DRIVER_ATTR(sensordata, S_IWUSR | S_IRUGO, show_sensordata_value,    NULL);
+static DRIVER_ATTR(cali,       S_IWUSR | S_IRUGO, show_cali_value,          store_cali_value);
+static DRIVER_ATTR(firlen,     S_IWUSR | S_IRUGO, show_firlen_value,        store_firlen_value);
+static DRIVER_ATTR(trace,      S_IWUSR | S_IRUGO, show_trace_value,         store_trace_value);
+static DRIVER_ATTR(status,               S_IRUGO, show_status_value,        NULL);
+static DRIVER_ATTR(powerstatus,               S_IRUGO, show_power_status_value,        NULL);
+
+/*----------------------------------------------------------------------------*/
+static struct driver_attribute *bma250_attr_list[] = {
+	&driver_attr_chipinfo,     /*chip information*/
+	&driver_attr_sensordata,   /*dump sensor data*/
+	&driver_attr_cali,         /*show calibration data*/
+	&driver_attr_firlen,       /*filter length: 0: disable, others: enable*/
+	&driver_attr_trace,        /*trace log*/
+	&driver_attr_status,
+	&driver_attr_powerstatus,
+};
+/*----------------------------------------------------------------------------*/
+static int bma250_create_attr(struct device_driver *driver) 
+{
+	int idx, err = 0;
+	int num = (int)(sizeof(bma250_attr_list)/sizeof(bma250_attr_list[0]));
+	if (driver == NULL)
+	{
+		return -EINVAL;
+	}
+
+	for(idx = 0; idx < num; idx++)
+	{
+		if((err = driver_create_file(driver, bma250_attr_list[idx])))
+		{            
+			GSE_ERR("driver_create_file (%s) = %d\n", bma250_attr_list[idx]->attr.name, err);
+			break;
+		}
+	}    
+	return err;
+}
+/*----------------------------------------------------------------------------*/
+static int bma250_delete_attr(struct device_driver *driver)
+{
+	int idx ,err = 0;
+	int num = (int)(sizeof(bma250_attr_list)/sizeof(bma250_attr_list[0]));
+
+	if(driver == NULL)
+	{
+		return -EINVAL;
+	}
+	
+
+	for(idx = 0; idx < num; idx++)
+	{
+		driver_remove_file(driver, bma250_attr_list[idx]);
+	}
+	
+
+	return err;
+}
+
+/*----------------------------------------------------------------------------*/
+int gsensor_operate(void* self, uint32_t command, void* buff_in, int size_in,
+		void* buff_out, int size_out, int* actualout)
+{
+	int err = 0;
+	int value, sample_delay;	
+	struct bma250_i2c_data *priv = (struct bma250_i2c_data*)self;
+	hwm_sensor_data* gsensor_data;
+	char buff[BMA250_BUFSIZE];
+	
+	//GSE_FUN(f);
+	switch (command)
+	{
+		case SENSOR_DELAY:
+			if((buff_in == NULL) || (size_in < sizeof(int)))
+			{
+				GSE_ERR("Set delay parameter error!\n");
+				err = -EINVAL;
+			}
+			else
+			{
+				value = *(int *)buff_in;
+				if(value <= 5)
+				{
+					sample_delay = BMA250_BW_200HZ;
+				}
+				else if(value <= 10)
+				{
+					sample_delay = BMA250_BW_100HZ;
+				}
+				else
+				{
+					sample_delay = BMA250_BW_50HZ;
+				}
+				
+				err = BMA250_SetBWRate(priv->client, sample_delay);
+				if(err != BMA250_SUCCESS ) //0x2C->BW=100Hz
+				{
+					GSE_ERR("Set delay parameter error!\n");
+				}
+
+				if(value >= 50)
+				{
+					atomic_set(&priv->filter, 0);
+				}
+				else
+				{	
+				#if defined(CONFIG_BMA250_LOWPASS)
+					priv->fir.num = 0;
+					priv->fir.idx = 0;
+					priv->fir.sum[BMA250_AXIS_X] = 0;
+					priv->fir.sum[BMA250_AXIS_Y] = 0;
+					priv->fir.sum[BMA250_AXIS_Z] = 0;
+					atomic_set(&priv->filter, 1);
+				#endif
+				}
+			}
+			break;
+
+		case SENSOR_ENABLE:
+			if((buff_in == NULL) || (size_in < sizeof(int)))
+			{
+				GSE_ERR("Enable sensor parameter error!\n");
+				err = -EINVAL;
+			}
+			else
+			{
+				value = *(int *)buff_in;
+				if(((value == 0) && (sensor_power == false)) ||((value == 1) && (sensor_power == true)))
+				{
+					GSE_LOG("Gsensor device have updated!\n");
+				}
+				else
+				{
+					err = BMA250_SetPowerMode( priv->client, !sensor_power);
+				}
+			}
+			break;
+
+		case SENSOR_GET_DATA:
+			if((buff_out == NULL) || (size_out< sizeof(hwm_sensor_data)))
+			{
+				GSE_ERR("get sensor data parameter error!\n");
+				err = -EINVAL;
+			}
+			else
+			{
+				gsensor_data = (hwm_sensor_data *)buff_out;
+				BMA250_ReadSensorData(priv->client, buff, BMA250_BUFSIZE);
+				sscanf(buff, "%x %x %x", &gsensor_data->values[0], 
+					&gsensor_data->values[1], &gsensor_data->values[2]);				
+				gsensor_data->status = SENSOR_STATUS_ACCURACY_MEDIUM;				
+				gsensor_data->value_divide = 1000;
+			}
+			break;
+		default:
+			GSE_ERR("gsensor operate function no this parameter %d!\n", command);
+			err = -1;
+			break;
+	}
+	
+	return err;
+}
+
+/****************************************************************************** 
+ * Function Configuration
+******************************************************************************/
+static int bma250_open(struct inode *inode, struct file *file)
+{
+	file->private_data = bma250_i2c_client;
+
+	if(file->private_data == NULL)
+	{
+		GSE_ERR("null pointer!!\n");
+		return -EINVAL;
+	}
+	return nonseekable_open(inode, file);
+}
+/*----------------------------------------------------------------------------*/
+static int bma250_release(struct inode *inode, struct file *file)
+{
+	file->private_data = NULL;
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+//static int bma250_ioctl(struct inode *inode, struct file *file, unsigned int cmd,
+//       unsigned long arg)
+static long bma250_unlocked_ioctl(struct file *file, unsigned int cmd,unsigned long arg)       
+{
+	struct i2c_client *client = (struct i2c_client*)file->private_data;
+	struct bma250_i2c_data *obj = (struct bma250_i2c_data*)i2c_get_clientdata(client);	
+	char strbuf[BMA250_BUFSIZE];
+	void __user *data;
+	SENSOR_DATA sensor_data;
+	long err = 0;
+	int cali[3];
+
+	//GSE_FUN(f);
+	if(_IOC_DIR(cmd) & _IOC_READ)
+	{
+		err = !access_ok(VERIFY_WRITE, (void __user *)arg, _IOC_SIZE(cmd));
+	}
+	else if(_IOC_DIR(cmd) & _IOC_WRITE)
+	{
+		err = !access_ok(VERIFY_READ, (void __user *)arg, _IOC_SIZE(cmd));
+	}
+
+	if(err)
+	{
+		GSE_ERR("access error: %08X, (%2d, %2d)\n", cmd, _IOC_DIR(cmd), _IOC_SIZE(cmd));
+		return -EFAULT;
+	}
+
+	switch(cmd)
+	{
+		case GSENSOR_IOCTL_INIT:
+			bma250_init_client(client, 0);			
+			break;
+
+		case GSENSOR_IOCTL_READ_CHIPINFO:
+			data = (void __user *) arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}
+			
+			BMA250_ReadChipInfo(client, strbuf, BMA250_BUFSIZE);
+			if(copy_to_user(data, strbuf, strlen(strbuf)+1))
+			{
+				err = -EFAULT;
+				break;
+			}				 
+			break;	  
+
+		case GSENSOR_IOCTL_READ_SENSORDATA:
+			data = (void __user *) arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}
+			
+			BMA250_ReadSensorData(client, strbuf, BMA250_BUFSIZE);
+			if(copy_to_user(data, strbuf, strlen(strbuf)+1))
+			{
+				err = -EFAULT;
+				break;	  
+			}				 
+			break;
+
+		case GSENSOR_IOCTL_READ_GAIN:
+			data = (void __user *) arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}			
+			
+			if(copy_to_user(data, &gsensor_gain, sizeof(GSENSOR_VECTOR3D)))
+			{
+				err = -EFAULT;
+				break;
+			}				 
+			break;
+
+		case GSENSOR_IOCTL_READ_RAW_DATA:
+			data = (void __user *) arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}
+			BMA250_ReadRawData(client, strbuf);
+			if(copy_to_user(data, &strbuf, strlen(strbuf)+1))
+			{
+				err = -EFAULT;
+				break;	  
+			}
+			break;	  
+
+		case GSENSOR_IOCTL_SET_CALI:
+			data = (void __user*)arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}
+			if(copy_from_user(&sensor_data, data, sizeof(sensor_data)))
+			{
+				err = -EFAULT;
+				break;	  
+			}
+			if(atomic_read(&obj->suspend))
+			{
+				GSE_ERR("Perform calibration in suspend state!!\n");
+				err = -EINVAL;
+			}
+			else
+			{
+			#if 0
+				cali[BMA250_AXIS_X] = sensor_data.x * obj->reso->sensitivity / GRAVITY_EARTH_1000;
+				cali[BMA250_AXIS_Y] = sensor_data.y * obj->reso->sensitivity / GRAVITY_EARTH_1000;
+				cali[BMA250_AXIS_Z] = sensor_data.z * obj->reso->sensitivity / GRAVITY_EARTH_1000;			  
+			#else
+			cali[BMA250_AXIS_X] = sensor_data.x;
+			cali[BMA250_AXIS_Y] = sensor_data.y;
+			cali[BMA250_AXIS_Z] = sensor_data.z;			  
+						
+
+			#endif
+				err = BMA250_WriteCalibration(client, cali);			 
+			}
+			break;
+
+		case GSENSOR_IOCTL_CLR_CALI:
+			err = BMA250_ResetCalibration(client);
+			break;
+
+		case GSENSOR_IOCTL_GET_CALI:
+			data = (void __user*)arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}
+			if((err = BMA250_ReadCalibration(client, cali)))
+			{
+				break;
+			}
+			#if 0
+			sensor_data.x = cali[BMA250_AXIS_X] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+			sensor_data.y = cali[BMA250_AXIS_Y] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+			sensor_data.z = cali[BMA250_AXIS_Z] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+			#else
+			sensor_data.x = cali[BMA250_AXIS_X];
+			sensor_data.y = cali[BMA250_AXIS_Y];
+			sensor_data.z = cali[BMA250_AXIS_Z];
+						
+
+			#endif
+			if(copy_to_user(data, &sensor_data, sizeof(sensor_data)))
+			{
+				err = -EFAULT;
+				break;
+			}		
+			break;
+		
+
+		default:
+			GSE_ERR("unknown IOCTL: 0x%08x\n", cmd);
+			err = -ENOIOCTLCMD;
+			break;
+			
+	}
+
+	return err;
+}
+
+
+/*----------------------------------------------------------------------------*/
+static struct file_operations bma250_fops = {
+	//.owner = THIS_MODULE,
+	.open = bma250_open,
+	.release = bma250_release,
+	.unlocked_ioctl = bma250_unlocked_ioctl,
+};
+/*----------------------------------------------------------------------------*/
+static struct miscdevice bma250_device = {
+	.minor = MISC_DYNAMIC_MINOR,
+	.name = "gsensor",
+	.fops = &bma250_fops,
+};
+/*----------------------------------------------------------------------------*/
+#ifndef CONFIG_HAS_EARLYSUSPEND
+/*----------------------------------------------------------------------------*/
+static int bma250_suspend(struct i2c_client *client, pm_message_t msg) 
+{
+	struct bma250_i2c_data *obj = i2c_get_clientdata(client);    
+	int err = 0;
+	GSE_FUN();    
+
+	if(msg.event == PM_EVENT_SUSPEND)
+	{   
+		if(obj == NULL)
+		{
+			GSE_ERR("null pointer!!\n");
+			return -EINVAL;
+		}
+		atomic_set(&obj->suspend, 1);
+		if(err = BMA250_SetPowerMode(obj->client, false))
+		{
+			GSE_ERR("write power control fail!!\n");
+			return;
+		}       
+		BMA250_power(obj->hw, 0);
+	}
+	return err;
+}
+/*----------------------------------------------------------------------------*/
+static int bma250_resume(struct i2c_client *client)
+{
+	struct bma250_i2c_data *obj = i2c_get_clientdata(client);        
+	int err;
+	GSE_FUN();
+
+	if(obj == NULL)
+	{
+		GSE_ERR("null pointer!!\n");
+		return -EINVAL;
+	}
+
+	BMA250_power(obj->hw, 1);
+	if(err = bma250_init_client(client, 0))
+	{
+		GSE_ERR("initialize client fail!!\n");
+		return err;        
+	}
+	atomic_set(&obj->suspend, 0);
+
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+#else /*CONFIG_HAS_EARLY_SUSPEND is defined*/
+/*----------------------------------------------------------------------------*/
+static void bma250_early_suspend(struct early_suspend *h) 
+{
+	struct bma250_i2c_data *obj = container_of(h, struct bma250_i2c_data, early_drv);   
+	int err;
+	GSE_FUN();    
+
+	if(obj == NULL)
+	{
+		GSE_ERR("null pointer!!\n");
+		return;
+	}
+	atomic_set(&obj->suspend, 1); 
+	if((err = BMA250_SetPowerMode(obj->client, false)))
+	{
+		GSE_ERR("write power control fail!!\n");
+		return;
+	}
+
+	sensor_power = false;
+	
+	BMA250_power(obj->hw, 0);
+}
+/*----------------------------------------------------------------------------*/
+static void bma250_late_resume(struct early_suspend *h)
+{
+	struct bma250_i2c_data *obj = container_of(h, struct bma250_i2c_data, early_drv);         
+	int err;
+	GSE_FUN();
+
+	if(obj == NULL)
+	{
+		GSE_ERR("null pointer!!\n");
+		return;
+	}
+
+	BMA250_power(obj->hw, 1);
+	if((err = bma250_init_client(obj->client, 0)))
+	{
+		GSE_ERR("initialize client fail!!\n");
+		return;        
+	}
+	atomic_set(&obj->suspend, 0);    
+}
+/*----------------------------------------------------------------------------*/
+#endif /*CONFIG_HAS_EARLYSUSPEND*/
+/*----------------------------------------------------------------------------*/
+//static int bma250_i2c_detect(struct i2c_client *client, int kind, struct i2c_board_info *info) 
+//{    
+//	strcpy(info->type, BMA250_DEV_NAME);
+//	return 0;
+//}
+
+/*----------------------------------------------------------------------------*/
+static int bma250_i2c_probe(struct i2c_client *client, const struct i2c_device_id *id)
+{
+	struct i2c_client *new_client;
+	struct bma250_i2c_data *obj;
+	struct hwmsen_object sobj;
+	int err = 0;
+	GSE_FUN();
+
+	if(!(obj = kzalloc(sizeof(*obj), GFP_KERNEL)))
+	{
+		err = -ENOMEM;
+		goto exit;
+	}
+	
+	memset(obj, 0, sizeof(struct bma250_i2c_data));
+
+	obj->hw = get_cust_acc_hw();
+	
+	if((err = hwmsen_get_convert(obj->hw->direction, &obj->cvt)))
+	{
+		GSE_ERR("invalid direction: %d\n", obj->hw->direction);
+		goto exit;
+	}
+
+	obj_i2c_data = obj;
+	obj->client = client;
+	new_client = obj->client;
+	i2c_set_clientdata(new_client,obj);
+	
+	atomic_set(&obj->trace, 0);
+	atomic_set(&obj->suspend, 0);
+	
+#ifdef CONFIG_BMA250_LOWPASS
+	if(obj->hw->firlen > C_MAX_FIR_LENGTH)
+	{
+		atomic_set(&obj->firlen, C_MAX_FIR_LENGTH);
+	}	
+	else
+	{
+		atomic_set(&obj->firlen, obj->hw->firlen);
+	}
+	
+	if(atomic_read(&obj->firlen) > 0)
+	{
+		atomic_set(&obj->fir_en, 1);
+	}
+	
+#endif
+
+	bma250_i2c_client = new_client;	
+
+	if((err = bma250_init_client(new_client, 1)))
+	{
+		goto exit_init_failed;
+	}
+	
+
+	if((err = misc_register(&bma250_device)))
+	{
+		GSE_ERR("bma250_device register failed\n");
+		goto exit_misc_device_register_failed;
+	}
+
+	if((err = bma250_create_attr(&bma250_gsensor_driver.driver)))
+	{
+		GSE_ERR("create attribute err = %d\n", err);
+		goto exit_create_attr_failed;
+	}
+
+	sobj.self = obj;
+    sobj.polling = 1;
+    sobj.sensor_operate = gsensor_operate;
+	if((err = hwmsen_attach(ID_ACCELEROMETER, &sobj)))
+	{
+		GSE_ERR("attach fail = %d\n", err);
+		goto exit_kfree;
+	}
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+	obj->early_drv.level    = EARLY_SUSPEND_LEVEL_DISABLE_FB - 1,
+	obj->early_drv.suspend  = bma250_early_suspend,
+	obj->early_drv.resume   = bma250_late_resume,    
+	register_early_suspend(&obj->early_drv);
+#endif 
+
+	GSE_LOG("%s: OK\n", __func__);    
+	return 0;
+
+	exit_create_attr_failed:
+	misc_deregister(&bma250_device);
+	exit_misc_device_register_failed:
+	exit_init_failed:
+	//i2c_detach_client(new_client);
+	exit_kfree:
+	kfree(obj);
+	exit:
+	GSE_ERR("%s: err = %d\n", __func__, err);        
+	return err;
+}
+
+/*----------------------------------------------------------------------------*/
+static int bma250_i2c_remove(struct i2c_client *client)
+{
+	int err = 0;	
+	
+	if((err = bma250_delete_attr(&bma250_gsensor_driver.driver)))
+	{
+		GSE_ERR("bma150_delete_attr fail: %d\n", err);
+	}
+	
+	if((err = misc_deregister(&bma250_device)))
+	{
+		GSE_ERR("misc_deregister fail: %d\n", err);
+	}
+
+	if((err = hwmsen_detach(ID_ACCELEROMETER)))
+	    
+
+	bma250_i2c_client = NULL;
+	i2c_unregister_device(client);
+	kfree(i2c_get_clientdata(client));
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int bma250_probe(struct platform_device *pdev) 
+{
+	struct acc_hw *hw = get_cust_acc_hw();
+	GSE_FUN();
+
+	BMA250_power(hw, 1);
+	//bma250_force[0] = hw->i2c_num;
+	if(i2c_add_driver(&bma250_i2c_driver))
+	{
+		GSE_ERR("add driver error\n");
+		return -1;
+	}
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int bma250_remove(struct platform_device *pdev)
+{
+    struct acc_hw *hw = get_cust_acc_hw();
+
+    GSE_FUN();    
+    BMA250_power(hw, 0);    
+    i2c_del_driver(&bma250_i2c_driver);
+    return 0;
+}
+/*----------------------------------------------------------------------------*/
+#if 1
+#ifdef CONFIG_OF
+static const struct of_device_id gsensor_of_match[] = {
+	{ .compatible = "mediatek,gsensor", },
+	{},
+};
+#endif
+
+static struct platform_driver bma250_gsensor_driver = {
+	.probe      = bma250_probe,
+	.remove     = bma250_remove,    
+	.driver     = 
+	{
+		.name  = "gsensor",
+	//	.owner  = THIS_MODULE,
+        #ifdef CONFIG_OF
+		.of_match_table = gsensor_of_match,
+		#endif
+	}
+};
+#else
+
+static struct platform_driver bma250_gsensor_driver = {
+	.probe      = bma250_probe,
+	.remove     = bma250_remove,    
+	.driver     = {
+		.name  = "gsensor",
+		//.owner = THIS_MODULE,
+	}
+};
+#endif
+/*----------------------------------------------------------------------------*/
+static int __init bma250_init(void)
+{
+	struct acc_hw *hw = get_cust_acc_hw();
+	GSE_FUN();
+	GSE_LOG("%s: i2c_number=%d\n", __func__,hw->i2c_num); 
+	i2c_register_board_info(hw->i2c_num, &i2c_bma250, 1);
+	if(platform_driver_register(&bma250_gsensor_driver))
+	{
+		GSE_ERR("failed to register driver");
+		return -ENODEV;
+	}
+	return 0;    
+}
+/*----------------------------------------------------------------------------*/
+static void __exit bma250_exit(void)
+{
+	GSE_FUN();
+	platform_driver_unregister(&bma250_gsensor_driver);
+}
+/*----------------------------------------------------------------------------*/
+module_init(bma250_init);
+module_exit(bma250_exit);
+/*----------------------------------------------------------------------------*/
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("BMA250 I2C driver");
+MODULE_AUTHOR("Xiaoli.li@mediatek.com");
diff --git a/drivers/misc/mediatek/accelerometer/bma250/bma250.h b/drivers/misc/mediatek/accelerometer/bma250/bma250.h
new file mode 100644
index 000000000..88a5cca89
--- /dev/null
+++ b/drivers/misc/mediatek/accelerometer/bma250/bma250.h
@@ -0,0 +1,71 @@
+/* linux/drivers/hwmon/adxl345.c
+ *
+ * (C) Copyright 2008 
+ * MediaTek <www.mediatek.com>
+ *
+ * BMA150 driver for MT6516
+ *
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA  BMA150
+ */
+#ifndef BMA250_H
+#define BMA250_H
+	 
+#include <linux/ioctl.h>
+	 
+	#define BMA250_I2C_SLAVE_WRITE_ADDR		0x30
+	 
+	 /* BMA222 Register Map  (Please refer to BMA150 Specifications) */
+	#define BMA250_REG_DEVID				0x00
+	#define BMA250_FIXED_DEVID			0x03
+	#define BMA250_REG_OFSX				0x16
+	#define BMA250_REG_OFSX_HIGH			0x1A
+	#define BMA250_REG_BW_RATE			0x10
+	#define BMA250_BW_MASK				0x1f
+	#define BMA250_BW_200HZ				0x0d
+	#define BMA250_BW_100HZ				0x0c
+	#define BMA250_BW_50HZ				0x0b
+	#define BMA250_BW_25HZ				0x0a
+	#define BMA250_REG_POWER_CTL		0x11		
+	#define BMA250_REG_DATA_FORMAT		0x0f
+	#define BMA250_RANGE_MASK			0x0f
+	#define BMA250_RANGE_2G				0x03
+	#define BMA250_RANGE_4G				0x05
+	#define BMA250_RANGE_8G				0x08
+	#define BMA250_REG_DATAXLOW			0x02
+	#define BMA250_REG_DATA_RESOLUTION	0x14
+	#define BMA250_MEASURE_MODE			0x80	
+	#define BMA250_SELF_TEST           			0x32
+	#define BMA250_SELF_TEST_AXIS_X		0x01
+	#define BMA250_SELF_TEST_AXIS_Y		0x02
+	#define BMA250_SELF_TEST_AXIS_Z		0x03
+	#define BMA250_SELF_TEST_POSITIVE	0x00
+	#define BMA250_SELF_TEST_NEGATIVE	0x04
+	#define BMA250_INT_REG_1           			0x16
+	#define BMA250_INT_REG_2          		 	0x17
+
+	
+#define BMA250_SUCCESS						0
+#define BMA250_ERR_I2C						-1
+#define BMA250_ERR_STATUS					-3
+#define BMA250_ERR_SETUP_FAILURE			-4
+#define BMA250_ERR_GETGSENSORDATA			-5
+#define BMA250_ERR_IDENTIFICATION			-6
+	 
+	 
+	 
+#define BMA250_BUFSIZE				256
+	 
+#endif
+
diff --git a/drivers/misc/mediatek/accelerometer/bma250e/Makefile b/drivers/misc/mediatek/accelerometer/bma250e/Makefile
new file mode 100755
index 000000000..e6ebdf822
--- /dev/null
+++ b/drivers/misc/mediatek/accelerometer/bma250e/Makefile
@@ -0,0 +1,4 @@
+include $(srctree)/drivers/misc/mediatek/Makefile.custom
+
+obj-y	:=  bma250.o
+
diff --git a/drivers/misc/mediatek/accelerometer/bma250e/bma250.c b/drivers/misc/mediatek/accelerometer/bma250e/bma250.c
new file mode 100644
index 000000000..ae9dbde9f
--- /dev/null
+++ b/drivers/misc/mediatek/accelerometer/bma250e/bma250.c
@@ -0,0 +1,2186 @@
+/* BMA150 motion sensor driver
+ *
+ *
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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.
+ *
+ */
+
+#include <linux/interrupt.h>
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include <linux/irq.h>
+#include <linux/miscdevice.h>
+#include <asm/uaccess.h>
+#include <linux/delay.h>
+#include <linux/input.h>
+#include <linux/workqueue.h>
+#include <linux/kobject.h>
+#include <linux/earlysuspend.h>
+#include <linux/platform_device.h>
+#include <asm/atomic.h>
+
+#include <mach/mt_typedefs.h>
+#include <mach/mt_gpio.h>
+#include <mach/mt_pm_ldo.h>
+
+
+#define POWER_NONE_MACRO MT65XX_POWER_NONE
+
+
+
+#include <cust_acc.h>
+#include <linux/hwmsensor.h>
+#include <linux/hwmsen_dev.h>
+#include <linux/sensors_io.h>
+#include "bma250.h"
+#include <linux/hwmsen_helper.h>
+
+#include <accel.h>
+#include <linux/batch.h>
+#ifdef CUSTOM_KERNEL_SENSORHUB
+#include <SCP_sensorHub.h>
+#endif//#ifdef CUSTOM_KERNEL_SENSORHUB
+
+/*----------------------------------------------------------------------------*/
+#define I2C_DRIVERID_BMA250 250
+/*----------------------------------------------------------------------------*/
+#define DEBUG 1
+/*----------------------------------------------------------------------------*/
+//#define CONFIG_BMA150_LOWPASS   /*apply low pass filter on output*/       
+#define SW_CALIBRATION
+
+/*----------------------------------------------------------------------------*/
+
+/*----------------------------------------------------------------------------*/
+static const struct i2c_device_id bma250_i2c_id[] = {{BMA250_DEV_NAME,0},{}};
+static struct i2c_board_info __initdata i2c_bma250={ I2C_BOARD_INFO("BMA250", (0x30>>1))};
+
+
+/*the adapter id will be available in customization*/
+//static unsigned short bma250_force[] = {0x00, BMA250_I2C_SLAVE_WRITE_ADDR, I2C_CLIENT_END, I2C_CLIENT_END};
+//static const unsigned short *const bma250_forces[] = { bma250_force, NULL };
+//static struct i2c_client_address_data bma250_addr_data = { .forces = bma250_forces,};
+
+/*----------------------------------------------------------------------------*/
+static int bma250_i2c_probe(struct i2c_client *client, const struct i2c_device_id *id); 
+static int bma250_i2c_remove(struct i2c_client *client);
+#ifdef CUSTOM_KERNEL_SENSORHUB
+static int gsensor_setup_irq(void);
+#endif//#ifdef CUSTOM_KERNEL_SENSORHUB
+
+static int gsensor_local_init(void);
+static int gsensor_remove(void);
+static int gsensor_set_delay(u64 ns);
+
+static DEFINE_MUTEX(gsensor_mutex);
+static DEFINE_MUTEX(gsensor_scp_en_mutex);
+
+
+static bool enable_status = false;
+
+static int gsensor_init_flag =-1; // 0<==>OK -1 <==> fail
+
+
+//static int bma250_i2c_detect(struct i2c_client *client, int kind, struct i2c_board_info *info);
+
+static struct acc_init_info bma250_init_info = {
+    .name = "bma250",
+    .init = gsensor_local_init,
+    .uninit = gsensor_remove,
+};
+
+/*----------------------------------------------------------------------------*/
+typedef enum {
+    ADX_TRC_FILTER  = 0x01,
+    ADX_TRC_RAWDATA = 0x02,
+    ADX_TRC_IOCTL   = 0x04,
+    ADX_TRC_CALI	= 0X08,
+    ADX_TRC_INFO	= 0X10,
+} ADX_TRC;
+/*----------------------------------------------------------------------------*/
+struct scale_factor{
+    u8  whole;
+    u8  fraction;
+};
+/*----------------------------------------------------------------------------*/
+struct data_resolution {
+    struct scale_factor scalefactor;
+    int                 sensitivity;
+};
+/*----------------------------------------------------------------------------*/
+#define C_MAX_FIR_LENGTH (32)
+/*----------------------------------------------------------------------------*/
+struct data_filter {
+    s16 raw[C_MAX_FIR_LENGTH][BMA250_AXES_NUM];
+    int sum[BMA250_AXES_NUM];
+    int num;
+    int idx;
+};
+/*----------------------------------------------------------------------------*/
+struct bma250_i2c_data {
+    struct i2c_client *client;
+    struct acc_hw *hw;
+    struct hwmsen_convert   cvt;
+
+#ifdef CUSTOM_KERNEL_SENSORHUB
+    struct work_struct	irq_work;
+#endif//#ifdef CUSTOM_KERNEL_SENSORHUB
+	
+    /*misc*/
+    struct data_resolution *reso;
+    atomic_t                trace;
+    atomic_t                suspend;
+    atomic_t                selftest;
+	atomic_t				filter;
+    s16                     cali_sw[BMA250_AXES_NUM+1];
+
+    /*data*/
+    s8                      offset[BMA250_AXES_NUM+1];  /*+1: for 4-byte alignment*/
+    s16                     data[BMA250_AXES_NUM+1];
+
+#ifdef CUSTOM_KERNEL_SENSORHUB
+    int                     SCP_init_done;
+#endif//#ifdef CUSTOM_KERNEL_SENSORHUB
+
+#if defined(CONFIG_BMA250_LOWPASS)
+    atomic_t                firlen;
+    atomic_t                fir_en;
+    struct data_filter      fir;
+#endif 
+    /*early suspend*/
+#if defined(CONFIG_HAS_EARLYSUSPEND)
+    struct early_suspend    early_drv;
+#endif     
+};
+enum bma250_rev {
+	BMA250,
+	BMA250E,
+};
+
+static int bma250_revesion = BMA250;
+/*----------------------------------------------------------------------------*/
+static struct i2c_driver bma250_i2c_driver = {
+    .driver = {
+ //       .owner          = THIS_MODULE,
+        .name           = BMA250_DEV_NAME,
+    },
+	.probe      		= bma250_i2c_probe,
+	.remove    			= bma250_i2c_remove,
+//	.detect				= bma250_i2c_detect,
+#if !defined(CONFIG_HAS_EARLYSUSPEND)    
+    .suspend            = bma250_suspend,
+    .resume             = bma250_resume,
+#endif
+	.id_table = bma250_i2c_id,
+//	.address_data = &bma250_addr_data,
+};
+
+/*----------------------------------------------------------------------------*/
+static struct i2c_client *bma250_i2c_client = NULL;
+static struct bma250_i2c_data *obj_i2c_data = NULL;
+static bool sensor_power = true;
+static GSENSOR_VECTOR3D gsensor_gain;
+/* static char selftestRes[8]= {0}; */
+
+/*----------------------------------------------------------------------------*/
+#define GSE_TAG                  "[Gsensor] "
+#define GSE_FUN(f)               printk(KERN_INFO GSE_TAG"%s\n", __FUNCTION__)
+#define GSE_ERR(fmt, args...)    printk(KERN_ERR GSE_TAG"%s %d : "fmt, __FUNCTION__, __LINE__, ##args)
+#define GSE_LOG(fmt, args...)    printk(KERN_INFO GSE_TAG fmt, ##args)
+/*----------------------------------------------------------------------------*/
+static struct data_resolution bma250_data_resolution[1] = {
+ /* combination by {FULL_RES,RANGE}*/
+    {{ 3, 9}, 256},   // dataformat +/-2g  in 8-bit resolution;  { 15, 6} = 15.6= (2*2*1000)/(2^8);  64 = (2^8)/(2*2)          
+};
+/*----------------------------------------------------------------------------*/
+static struct data_resolution bma250_offset_resolution = {{3, 9}, 256};
+
+/*--------------------BMA250 power control function----------------------------------*/
+static void BMA250_power(struct acc_hw *hw, unsigned int on) 
+{
+	static unsigned int power_on = 0;
+
+	if(hw->power_id != POWER_NONE_MACRO)		// have externel LDO
+	{        
+		GSE_LOG("power %s\n", on ? "on" : "off");
+		if(power_on == on)	// power status not change
+		{
+			GSE_LOG("ignore power control: %d\n", on);
+		}
+		else if(on)	// power on
+		{
+			if(!hwPowerOn(hw->power_id, hw->power_vol, "BMA250"))
+			{
+				GSE_ERR("power on fails!!\n");
+			}
+		}
+		else	// power off
+		{
+			if (!hwPowerDown(hw->power_id, "BMA250"))
+			{
+				GSE_ERR("power off fail!!\n");
+			}			  
+		}
+	}
+	power_on = on;    
+}
+/*----------------------------------------------------------------------------*/
+
+#ifdef CUSTOM_KERNEL_SENSORHUB
+int BMA250_SCP_SetPowerMode(bool enable, int sensorType)
+{
+    static bool gsensor_scp_en_status = false;
+    static unsigned int gsensor_scp_en_map = 0;
+    SCP_SENSOR_HUB_DATA req;
+    int len;
+    int err = 0;
+
+    mutex_lock(&gsensor_scp_en_mutex);
+
+    if (sensorType >= 32)
+    {
+        GSE_ERR("Out of index!\n");
+        return -1;
+    }
+
+    if (true == enable)
+    {
+        gsensor_scp_en_map |= (1<<sensorType);
+    }
+    else
+    {
+        gsensor_scp_en_map &= ~(1<<sensorType);
+    }
+
+    if (0 == gsensor_scp_en_map)
+        enable = false;
+    else
+        enable = true;
+
+    if (gsensor_scp_en_status != enable)
+    {
+        gsensor_scp_en_status = enable;
+        
+        req.activate_req.sensorType = ID_ACCELEROMETER;
+        req.activate_req.action = SENSOR_HUB_ACTIVATE;
+        req.activate_req.enable = enable;
+        len = sizeof(req.activate_req);
+        err = SCP_sensorHub_req_send(&req, &len, 1);
+        if (err)
+        {
+            GSE_ERR("SCP_sensorHub_req_send fail!\n");
+        }
+    }
+
+    mutex_unlock(&gsensor_scp_en_mutex);
+
+    return err;
+}
+EXPORT_SYMBOL(BMA250_SCP_SetPowerMode);
+#endif //#ifdef CUSTOM_KERNEL_SENSORHUB
+
+/*----------------------------------------------------------------------------*/
+static int BMA250_SetDataResolution(struct bma250_i2c_data *obj)
+{
+
+/*set g sensor dataresolution here*/
+
+/*BMA250 only can set to 10-bit dataresolution, so do nothing in bma250 driver here*/
+
+/*end of set dataresolution*/
+
+
+ 
+ /*we set measure range from -2g to +2g in BMA150_SetDataFormat(client, BMA150_RANGE_2G), 
+                                                    and set 10-bit dataresolution BMA150_SetDataResolution()*/
+                                                    
+ /*so bma250_data_resolution[0] set value as {{ 3, 9}, 256} when declaration, and assign the value to obj->reso here*/  
+
+ 	obj->reso = &bma250_data_resolution[0];
+	return 0;
+	
+/*if you changed the measure range, for example call: BMA250_SetDataFormat(client, BMA150_RANGE_4G), 
+you must set the right value to bma250_data_resolution*/
+
+}
+/*----------------------------------------------------------------------------*/
+static int BMA250_ReadData(struct i2c_client *client, s16 data[BMA250_AXES_NUM])
+{
+	struct bma250_i2c_data *priv = i2c_get_clientdata(client);        
+	u8 addr = BMA250_REG_DATAXLOW;
+	u8 buf[BMA250_DATA_LEN] = {0};
+	int err = 0;
+	int i;
+
+	if(NULL == client)
+	{
+		err = -EINVAL;
+		return err;
+	}
+	
+	err = hwmsen_read_block(client, addr, buf, 0x06);
+	if(err)
+	{
+		GSE_ERR("error: %d\n", err);
+	}
+	else
+	{
+		data[BMA250_AXIS_X] = (s16)((buf[BMA250_AXIS_X*2] >> 6) |
+		         (buf[BMA250_AXIS_X*2+1] << 2));
+		data[BMA250_AXIS_Y] = (s16)((buf[BMA250_AXIS_Y*2] >> 6) |
+		         (buf[BMA250_AXIS_Y*2+1] << 2));
+		data[BMA250_AXIS_Z] = (s16)((buf[BMA250_AXIS_Z*2] >> 6) |
+		         (buf[BMA250_AXIS_Z*2+1] << 2));
+
+		for(i=0;i<3;i++)				
+		{								//because the data is store in binary complement number formation in computer system
+			if ( data[i] == 0x0200 )	//so we want to calculate actual number here
+				data[i]= -512;			//10bit resolution, 512= 2^(10-1)
+			else if ( data[i] & 0x0200 )//transfor format
+			{							//printk("data 0 step %x \n",data[i]);
+				data[i] -= 0x1;			//printk("data 1 step %x \n",data[i]);
+				data[i] = ~data[i];		//printk("data 2 step %x \n",data[i]);
+				data[i] &= 0x01ff;		//printk("data 3 step %x \n\n",data[i]);
+				data[i] = -data[i];		
+			}
+		}	
+
+		if(atomic_read(&priv->trace) & ADX_TRC_RAWDATA)
+		{
+			GSE_LOG("[%08X %08X %08X] => [%5d %5d %5d] after\n", data[BMA250_AXIS_X], data[BMA250_AXIS_Y], data[BMA250_AXIS_Z],
+		                               data[BMA250_AXIS_X], data[BMA250_AXIS_Y], data[BMA250_AXIS_Z]);
+		}
+#ifdef CONFIG_BMA250_LOWPASS
+		if(atomic_read(&priv->filter))
+		{
+			if(atomic_read(&priv->fir_en) && !atomic_read(&priv->suspend))
+			{
+				int idx, firlen = atomic_read(&priv->firlen);   
+				if(priv->fir.num < firlen)
+				{                
+					priv->fir.raw[priv->fir.num][BMA250_AXIS_X] = data[BMA250_AXIS_X];
+					priv->fir.raw[priv->fir.num][BMA250_AXIS_Y] = data[BMA250_AXIS_Y];
+					priv->fir.raw[priv->fir.num][BMA250_AXIS_Z] = data[BMA250_AXIS_Z];
+					priv->fir.sum[BMA250_AXIS_X] += data[BMA250_AXIS_X];
+					priv->fir.sum[BMA250_AXIS_Y] += data[BMA250_AXIS_Y];
+					priv->fir.sum[BMA250_AXIS_Z] += data[BMA250_AXIS_Z];
+					if(atomic_read(&priv->trace) & ADX_TRC_FILTER)
+					{
+						GSE_LOG("add [%2d] [%5d %5d %5d] => [%5d %5d %5d]\n", priv->fir.num,
+							priv->fir.raw[priv->fir.num][BMA250_AXIS_X], priv->fir.raw[priv->fir.num][BMA250_AXIS_Y], priv->fir.raw[priv->fir.num][BMA250_AXIS_Z],
+							priv->fir.sum[BMA250_AXIS_X], priv->fir.sum[BMA250_AXIS_Y], priv->fir.sum[BMA250_AXIS_Z]);
+					}
+					priv->fir.num++;
+					priv->fir.idx++;
+				}
+				else
+				{
+					idx = priv->fir.idx % firlen;
+					priv->fir.sum[BMA250_AXIS_X] -= priv->fir.raw[idx][BMA250_AXIS_X];
+					priv->fir.sum[BMA250_AXIS_Y] -= priv->fir.raw[idx][BMA250_AXIS_Y];
+					priv->fir.sum[BMA250_AXIS_Z] -= priv->fir.raw[idx][BMA250_AXIS_Z];
+					priv->fir.raw[idx][BMA250_AXIS_X] = data[BMA250_AXIS_X];
+					priv->fir.raw[idx][BMA250_AXIS_Y] = data[BMA250_AXIS_Y];
+					priv->fir.raw[idx][BMA250_AXIS_Z] = data[BMA250_AXIS_Z];
+					priv->fir.sum[BMA250_AXIS_X] += data[BMA250_AXIS_X];
+					priv->fir.sum[BMA250_AXIS_Y] += data[BMA250_AXIS_Y];
+					priv->fir.sum[BMA250_AXIS_Z] += data[BMA250_AXIS_Z];
+					priv->fir.idx++;
+					data[BMA250_AXIS_X] = priv->fir.sum[BMA250_AXIS_X]/firlen;
+					data[BMA250_AXIS_Y] = priv->fir.sum[BMA250_AXIS_Y]/firlen;
+					data[BMA250_AXIS_Z] = priv->fir.sum[BMA250_AXIS_Z]/firlen;
+					if(atomic_read(&priv->trace) & ADX_TRC_FILTER)
+					{
+						GSE_LOG("add [%2d] [%5d %5d %5d] => [%5d %5d %5d] : [%5d %5d %5d]\n", idx,
+						priv->fir.raw[idx][BMA250_AXIS_X], priv->fir.raw[idx][BMA250_AXIS_Y], priv->fir.raw[idx][BMA250_AXIS_Z],
+						priv->fir.sum[BMA250_AXIS_X], priv->fir.sum[BMA250_AXIS_Y], priv->fir.sum[BMA250_AXIS_Z],
+						data[BMA250_AXIS_X], data[BMA250_AXIS_Y], data[BMA250_AXIS_Z]);
+					}
+				}
+			}
+		}	
+#endif         
+	}
+	return err;
+}
+/*----------------------------------------------------------------------------*/
+static int BMA250_ReadOffset(struct i2c_client *client, s8 ofs[BMA250_AXES_NUM])
+{    
+	int err = 0;
+#ifdef SW_CALIBRATION
+	ofs[0]=ofs[1]=ofs[2]=0x0;
+#else
+	if(err = hwmsen_read_block(client, BMA250_REG_OFSX, ofs, BMA250_AXES_NUM))
+	{
+		GSE_ERR("error: %d\n", err);
+	}
+#endif
+	//printk("offesx=%x, y=%x, z=%x",ofs[0],ofs[1],ofs[2]);
+	
+	return err;    
+}
+/*----------------------------------------------------------------------------*/
+static int BMA250_ResetCalibration(struct i2c_client *client)
+{
+	struct bma250_i2c_data *obj = i2c_get_clientdata(client);
+	#ifndef SW_CALIBRATION
+	u8 ofs[4]={0,0,0,0};
+	#endif
+	int err = 0;
+
+#ifdef CUSTOM_KERNEL_SENSORHUB
+    SCP_SENSOR_HUB_DATA data;
+    BMA250_CUST_DATA *pCustData;
+    unsigned int len;
+	
+	if (0 != obj->SCP_init_done)
+	{
+		pCustData = (BMA250_CUST_DATA *)&data.set_cust_req.custData;
+		data.set_cust_req.sensorType = ID_ACCELEROMETER;
+		data.set_cust_req.action = SENSOR_HUB_SET_CUST;
+		pCustData->resetCali.action = BMA250_CUST_ACTION_RESET_CALI;
+		len = offsetof(SCP_SENSOR_HUB_SET_CUST_REQ, custData) + sizeof(pCustData->resetCali);
+		SCP_sensorHub_req_send(&data, &len, 1);
+	}
+#endif
+
+	#ifdef SW_CALIBRATION
+		
+	#else
+		if(err = hwmsen_write_block(client, BMA250_REG_OFSX, ofs, 4))
+		{
+			GSE_ERR("error: %d\n", err);
+		}
+	#endif
+
+	memset(obj->cali_sw, 0x00, sizeof(obj->cali_sw));
+	memset(obj->offset, 0x00, sizeof(obj->offset));
+	return err;    
+}
+/*----------------------------------------------------------------------------*/
+static int BMA250_ReadCalibration(struct i2c_client *client, int dat[BMA250_AXES_NUM])
+{
+    struct bma250_i2c_data *obj = i2c_get_clientdata(client);
+    #ifndef SW_CALIBRATION
+    int err;
+    #endif
+    int mul;
+
+	#ifdef SW_CALIBRATION
+		mul = 0;//only SW Calibration, disable HW Calibration
+	#else
+	    if ((err = BMA250_ReadOffset(client, obj->offset))) {
+        GSE_ERR("read offset fail, %d\n", err);
+        return err;
+    	}    
+    	mul = obj->reso->sensitivity/bma250_offset_resolution.sensitivity;
+	#endif
+
+    dat[obj->cvt.map[BMA250_AXIS_X]] = obj->cvt.sign[BMA250_AXIS_X]*(obj->offset[BMA250_AXIS_X]*mul * GRAVITY_EARTH_1000 / obj->reso->sensitivity + obj->cali_sw[BMA250_AXIS_X]);
+    dat[obj->cvt.map[BMA250_AXIS_Y]] = obj->cvt.sign[BMA250_AXIS_Y]*(obj->offset[BMA250_AXIS_Y]*mul * GRAVITY_EARTH_1000 / obj->reso->sensitivity + obj->cali_sw[BMA250_AXIS_Y]);
+    dat[obj->cvt.map[BMA250_AXIS_Z]] = obj->cvt.sign[BMA250_AXIS_Z]*(obj->offset[BMA250_AXIS_Z]*mul * GRAVITY_EARTH_1000 / obj->reso->sensitivity + obj->cali_sw[BMA250_AXIS_Z]);                        
+                                       
+    return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int BMA250_ReadCalibrationEx(struct i2c_client *client, int act[BMA250_AXES_NUM], int raw[BMA250_AXES_NUM])
+{  
+	/*raw: the raw calibration data; act: the actual calibration data*/
+	struct bma250_i2c_data *obj = i2c_get_clientdata(client);
+	#ifndef SW_CALIBRATION
+	int err;
+	#endif
+	int mul;
+
+ 
+
+	#ifdef SW_CALIBRATION
+		mul = 0;//only SW Calibration, disable HW Calibration
+	#else
+		if(err = BMA250_ReadOffset(client, obj->offset))
+		{
+			GSE_ERR("read offset fail, %d\n", err);
+			return err;
+		}   
+		mul = obj->reso->sensitivity/bma250_offset_resolution.sensitivity;
+	#endif
+	
+	raw[BMA250_AXIS_X] = obj->offset[BMA250_AXIS_X]*mul * GRAVITY_EARTH_1000 / obj->reso->sensitivity + obj->cali_sw[BMA250_AXIS_X];
+	raw[BMA250_AXIS_Y] = obj->offset[BMA250_AXIS_Y]*mul * GRAVITY_EARTH_1000 / obj->reso->sensitivity + obj->cali_sw[BMA250_AXIS_Y];
+	raw[BMA250_AXIS_Z] = obj->offset[BMA250_AXIS_Z]*mul * GRAVITY_EARTH_1000 / obj->reso->sensitivity + obj->cali_sw[BMA250_AXIS_Z];
+
+	act[obj->cvt.map[BMA250_AXIS_X]] = obj->cvt.sign[BMA250_AXIS_X]*raw[BMA250_AXIS_X];
+	act[obj->cvt.map[BMA250_AXIS_Y]] = obj->cvt.sign[BMA250_AXIS_Y]*raw[BMA250_AXIS_Y];
+	act[obj->cvt.map[BMA250_AXIS_Z]] = obj->cvt.sign[BMA250_AXIS_Z]*raw[BMA250_AXIS_Z];                        
+	                       
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int BMA250_WriteCalibration(struct i2c_client *client, int dat[BMA250_AXES_NUM])
+{
+	struct bma250_i2c_data *obj = i2c_get_clientdata(client);
+	int err;
+	int cali[BMA250_AXES_NUM], raw[BMA250_AXES_NUM];
+	#ifndef SW_CALIBRATION
+	int lsb = bma250_offset_resolution.sensitivity;
+	int divisor = obj->reso->sensitivity/lsb;
+	#endif
+
+#ifdef CUSTOM_KERNEL_SENSORHUB
+	SCP_SENSOR_HUB_DATA data;
+	BMA250_CUST_DATA *pCustData;
+	unsigned int len;
+#endif
+
+	err = BMA250_ReadCalibrationEx(client, cali, raw);
+	if(err)	/*offset will be updated in obj->offset*/
+	{ 
+		GSE_ERR("read offset fail, %d\n", err);
+		return err;
+	}
+
+	GSE_LOG("OLDOFF: (%+3d %+3d %+3d): (%+3d %+3d %+3d) / (%+3d %+3d %+3d)\n", 
+		raw[BMA250_AXIS_X], raw[BMA250_AXIS_Y], raw[BMA250_AXIS_Z],
+		obj->offset[BMA250_AXIS_X], obj->offset[BMA250_AXIS_Y], obj->offset[BMA250_AXIS_Z],
+		obj->cali_sw[BMA250_AXIS_X], obj->cali_sw[BMA250_AXIS_Y], obj->cali_sw[BMA250_AXIS_Z]);
+
+#ifdef CUSTOM_KERNEL_SENSORHUB
+	pCustData = (BMA250_CUST_DATA *)data.set_cust_req.custData;
+	data.set_cust_req.sensorType = ID_ACCELEROMETER;
+	data.set_cust_req.action = SENSOR_HUB_SET_CUST;
+	pCustData->setCali.action = BMA250_CUST_ACTION_SET_CALI;
+	pCustData->setCali.data[BMA250_AXIS_X] = dat[BMA250_AXIS_X];
+	pCustData->setCali.data[BMA250_AXIS_Y] = dat[BMA250_AXIS_Y];
+	pCustData->setCali.data[BMA250_AXIS_Z] = dat[BMA250_AXIS_Z];
+	len = offsetof(SCP_SENSOR_HUB_SET_CUST_REQ, custData) + sizeof(pCustData->setCali);
+	SCP_sensorHub_req_send(&data, &len, 1);
+#endif
+
+	/*calculate the real offset expected by caller*/
+	cali[BMA250_AXIS_X] += dat[BMA250_AXIS_X];
+	cali[BMA250_AXIS_Y] += dat[BMA250_AXIS_Y];
+	cali[BMA250_AXIS_Z] += dat[BMA250_AXIS_Z];
+
+	GSE_LOG("UPDATE: (%+3d %+3d %+3d)\n", 
+		dat[BMA250_AXIS_X], dat[BMA250_AXIS_Y], dat[BMA250_AXIS_Z]);
+
+#ifdef SW_CALIBRATION
+	obj->cali_sw[BMA250_AXIS_X] = obj->cvt.sign[BMA250_AXIS_X]*(cali[obj->cvt.map[BMA250_AXIS_X]]);
+	obj->cali_sw[BMA250_AXIS_Y] = obj->cvt.sign[BMA250_AXIS_Y]*(cali[obj->cvt.map[BMA250_AXIS_Y]]);
+	obj->cali_sw[BMA250_AXIS_Z] = obj->cvt.sign[BMA250_AXIS_Z]*(cali[obj->cvt.map[BMA250_AXIS_Z]]);	
+#else
+	obj->offset[BMA250_AXIS_X] = (s8)(obj->cvt.sign[BMA250_AXIS_X]*(cali[obj->cvt.map[BMA250_AXIS_X]]) * obj->reso->sensitivity / GRAVITY_EARTH_1000/(divisor));
+	obj->offset[BMA250_AXIS_Y] = (s8)(obj->cvt.sign[BMA250_AXIS_Y]*(cali[obj->cvt.map[BMA250_AXIS_Y]]) * obj->reso->sensitivity / GRAVITY_EARTH_1000/(divisor));
+	obj->offset[BMA250_AXIS_Z] = (s8)(obj->cvt.sign[BMA250_AXIS_Z]*(cali[obj->cvt.map[BMA250_AXIS_Z]]) * obj->reso->sensitivity / GRAVITY_EARTH_1000/(divisor));
+
+	/*convert software calibration using standard calibration*/
+	obj->cali_sw[BMA250_AXIS_X] = obj->cvt.sign[BMA250_AXIS_X]*(cali[obj->cvt.map[BMA250_AXIS_X]])%(divisor);
+	obj->cali_sw[BMA250_AXIS_Y] = obj->cvt.sign[BMA250_AXIS_Y]*(cali[obj->cvt.map[BMA250_AXIS_Y]])%(divisor);
+	obj->cali_sw[BMA250_AXIS_Z] = obj->cvt.sign[BMA250_AXIS_Z]*(cali[obj->cvt.map[BMA250_AXIS_Z]])%(divisor);
+
+	GSE_LOG("NEWOFF: (%+3d %+3d %+3d): (%+3d %+3d %+3d) / (%+3d %+3d %+3d)\n", 
+		obj->offset[BMA250_AXIS_X]*divisor + obj->cali_sw[BMA250_AXIS_X], 
+		obj->offset[BMA250_AXIS_Y]*divisor + obj->cali_sw[BMA250_AXIS_Y], 
+		obj->offset[BMA250_AXIS_Z]*divisor + obj->cali_sw[BMA250_AXIS_Z], 
+		obj->offset[BMA250_AXIS_X], obj->offset[BMA250_AXIS_Y], obj->offset[BMA250_AXIS_Z],
+		obj->cali_sw[BMA250_AXIS_X], obj->cali_sw[BMA250_AXIS_Y], obj->cali_sw[BMA250_AXIS_Z]);
+
+	if(err = hwmsen_write_block(obj->client, BMA250_REG_OFSX, obj->offset, BMA250_AXES_NUM))
+	{
+		GSE_ERR("write offset fail: %d\n", err);
+		return err;
+	}
+#endif
+
+	return err;
+}
+/*----------------------------------------------------------------------------*/
+static int BMA250_CheckDeviceID(struct i2c_client *client)
+{
+	u8 databuf[2];    
+	int res = 0;
+
+	memset(databuf, 0, sizeof(u8)*2);    
+	databuf[0] = BMA250_REG_DEVID;    
+	i2c_master_send(client, databuf, 0x1);
+	mdelay(40);
+
+	databuf[0] = 0x0;        
+	res = i2c_master_recv(client, databuf, 0x01);
+	if(res <= 0)
+	{
+		i2c_master_send(client, databuf, 0x1);
+
+		mdelay(40);
+
+		databuf[0] = 0x0;        
+		res = i2c_master_recv(client, databuf, 0x01);
+		if(res <= 0)
+		{
+			goto exit_BMA250_CheckDeviceID;
+		}
+	}
+
+
+	if(databuf[0]==BMA250_FIXED_DEVID)
+	{
+		printk("BMA250_CheckDeviceID %4xh pass!\n ", databuf[0]);
+		bma250_revesion = BMA250;
+	}
+	else if (databuf[0]==BMA250E_FIXED_DEVID)
+	{
+		printk("BMA250E_CheckDeviceID %4xh pass!\n ", databuf[0]);
+		bma250_revesion = BMA250E;
+	}
+	else
+	{
+		printk("BMA250_CheckDeviceID %d fail!\n ", databuf[0]);
+	}
+
+	exit_BMA250_CheckDeviceID:
+	if (res <= 0)
+	{
+		return BMA250_ERR_I2C;
+	}
+	
+	return BMA250_SUCCESS;
+}
+/*----------------------------------------------------------------------------*/
+static int BMA250_SetPowerMode(struct i2c_client *client, bool enable)
+{
+	u8 databuf[2];    
+	int res = 0;
+	u8 addr = BMA250_REG_POWER_CTL;
+	struct bma250_i2c_data *obj = i2c_get_clientdata(client);
+	
+	
+	if(enable == sensor_power )
+	{
+		GSE_LOG("Sensor power status is newest!\n");
+		return BMA250_SUCCESS;
+	}
+
+	if(hwmsen_read_block(client, addr, databuf, 0x01))
+	{
+		GSE_ERR("read power ctl register err!\n");
+		return BMA250_ERR_I2C;
+	}
+
+	
+	if(enable == TRUE)
+	{
+		databuf[0] &= ~BMA250_MEASURE_MODE;
+	}
+	else
+	{
+		databuf[0] |= BMA250_MEASURE_MODE;
+	}
+	databuf[1] = databuf[0];
+	databuf[0] = BMA250_REG_POWER_CTL;
+	
+
+	res = i2c_master_send(client, databuf, 0x2);
+
+	if(res <= 0)
+	{
+		GSE_LOG("set power mode failed!\n");
+		return BMA250_ERR_I2C;
+	}
+	else if(atomic_read(&obj->trace) & ADX_TRC_INFO)
+	{
+		GSE_LOG("set power mode ok %d!\n", databuf[1]);
+	}
+
+	//GSE_LOG("BMA250_SetPowerMode ok!\n");
+
+
+	sensor_power = enable;
+
+	mdelay(20);
+	
+	return BMA250_SUCCESS;    
+}
+/*----------------------------------------------------------------------------*/
+static int BMA250_SetDataFormat(struct i2c_client *client, u8 dataformat)
+{
+	struct bma250_i2c_data *obj = i2c_get_clientdata(client);
+	u8 databuf[10];    
+	int res = 0;
+
+	memset(databuf, 0, sizeof(u8)*10);    
+
+	if(hwmsen_read_block(client, BMA250_REG_DATA_FORMAT, databuf, 0x01))
+	{
+		printk("bma250 read Dataformat failt \n");
+		return BMA250_ERR_I2C;
+	}
+
+	databuf[0] &= ~BMA250_RANGE_MASK;
+	databuf[0] |= dataformat;
+	databuf[1] = databuf[0];
+	databuf[0] = BMA250_REG_DATA_FORMAT;
+
+
+	res = i2c_master_send(client, databuf, 0x2);
+
+	if(res <= 0)
+	{
+		return BMA250_ERR_I2C;
+	}
+	
+	//printk("BMA250_SetDataFormat OK! \n");
+	
+
+	return BMA250_SetDataResolution(obj);    
+}
+/*----------------------------------------------------------------------------*/
+static int BMA250_SetBWRate(struct i2c_client *client, u8 bwrate)
+{
+	u8 databuf[10];    
+	int res = 0;
+
+	memset(databuf, 0, sizeof(u8)*10);    
+
+	if(hwmsen_read_block(client, BMA250_REG_BW_RATE, databuf, 0x01))
+	{
+		printk("bma250 read rate failt \n");
+		return BMA250_ERR_I2C;
+	}
+
+	databuf[0] &= ~BMA250_BW_MASK;
+	databuf[0] |= bwrate;
+	databuf[1] = databuf[0];
+	databuf[0] = BMA250_REG_BW_RATE;
+
+
+	res = i2c_master_send(client, databuf, 0x2);
+
+	if(res <= 0)
+	{
+		return BMA250_ERR_I2C;
+	}
+	
+	//printk("BMA250_SetBWRate OK! \n");
+	
+	return BMA250_SUCCESS;    
+}
+/*----------------------------------------------------------------------------*/
+static int BMA250_SetIntEnable(struct i2c_client *client, u8 intenable)
+{
+			int res = 0;
+		
+			res = hwmsen_write_byte(client, BMA250_INT_REG_1, 0x00);
+			if(res != BMA250_SUCCESS) 
+			{
+				return res;
+			}
+			res = hwmsen_write_byte(client, BMA250_INT_REG_2, 0x00);
+			if(res != BMA250_SUCCESS) 
+			{
+				return res;
+			}
+			printk("BMA250 disable interrupt ...\n");
+		
+			/*for disable interrupt function*/
+			
+			return BMA250_SUCCESS;	  
+}
+
+
+/*----------------------------------------------------------------------------*/
+static int bma250_init_client(struct i2c_client *client, int reset_cali)
+{
+	struct bma250_i2c_data *obj = i2c_get_clientdata(client);
+	int res = 0;
+	printk("bma250_init_client \n");
+
+	res = BMA250_CheckDeviceID(client); 
+	if(res != BMA250_SUCCESS)
+	{
+		return res;
+	}	
+	printk("BMA250_CheckDeviceID ok \n");
+	
+	res = BMA250_SetBWRate(client, BMA250_BW_100HZ);
+	if(res != BMA250_SUCCESS ) 
+	{
+		return res;
+	}
+	printk("BMA250_SetBWRate OK!\n");
+	
+	res = BMA250_SetDataFormat(client, BMA250_RANGE_2G);
+	if(res != BMA250_SUCCESS) 
+	{
+		return res;
+	}
+	printk("BMA250_SetDataFormat OK!\n");
+
+	gsensor_gain.x = gsensor_gain.y = gsensor_gain.z = obj->reso->sensitivity;
+
+
+#ifdef CUSTOM_KERNEL_SENSORHUB
+	res = gsensor_setup_irq();
+	if(res != BMA250_SUCCESS)
+	{
+		return res;
+	}
+#endif//#ifdef CUSTOM_KERNEL_SENSORHUB
+
+	res = BMA250_SetIntEnable(client, 0x00);        
+	if(res != BMA250_SUCCESS)
+	{
+		return res;
+	}
+	printk("BMA250 disable interrupt function!\n");
+
+	res = BMA250_SetPowerMode(client, false);
+	if(res != BMA250_SUCCESS)
+	{
+		return res;
+	}
+	printk("BMA250_SetPowerMode OK!\n");
+
+
+	if(0 != reset_cali)
+	{ 
+		/*reset calibration only in power on*/
+		res = BMA250_ResetCalibration(client);
+		if(res != BMA250_SUCCESS)
+		{
+			return res;
+		}
+	}
+	printk("bma250_init_client OK!\n");
+#ifdef CONFIG_BMA250_LOWPASS
+	memset(&obj->fir, 0x00, sizeof(obj->fir));  
+#endif
+
+	mdelay(20);
+
+	return BMA250_SUCCESS;
+}
+/*----------------------------------------------------------------------------*/
+static int BMA250_ReadChipInfo(struct i2c_client *client, char *buf, int bufsize)
+{
+	u8 databuf[10];    
+
+	memset(databuf, 0, sizeof(u8)*10);
+
+	if((NULL == buf)||(bufsize<=30))
+	{
+		return -1;
+	}
+	
+	if(NULL == client)
+	{
+		*buf = 0;
+		return -2;
+	}
+
+	sprintf(buf, "BMA250 Chip");
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int BMA250_ReadSensorData(struct i2c_client *client, char *buf, int bufsize)
+{
+	struct bma250_i2c_data *obj = (struct bma250_i2c_data*)i2c_get_clientdata(client);
+	u8 databuf[20];
+	int acc[BMA250_AXES_NUM];
+	int res = 0;
+	memset(databuf, 0, sizeof(u8)*10);
+
+	if(NULL == buf)
+	{
+		return -1;
+	}
+	if(NULL == client)
+	{
+		*buf = 0;
+		return -2;
+	}
+
+	if(sensor_power == FALSE)
+	{
+		res = BMA250_SetPowerMode(client, true);
+		if(res)
+		{
+			GSE_ERR("Power on bma250 error %d!\n", res);
+		}
+	}
+
+	res = BMA250_ReadData(client, obj->data);
+	if(res)
+	{        
+		GSE_ERR("I2C error: ret value=%d", res);
+		return -3;
+	}
+	else
+	{
+	#if 1
+		obj->data[BMA250_AXIS_X] = obj->data[BMA250_AXIS_X] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+		obj->data[BMA250_AXIS_Y] = obj->data[BMA250_AXIS_Y] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+		obj->data[BMA250_AXIS_Z] = obj->data[BMA250_AXIS_Z] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;		
+	#endif	
+		//printk("raw data x=%d, y=%d, z=%d \n",obj->data[BMA150_AXIS_X],obj->data[BMA150_AXIS_Y],obj->data[BMA150_AXIS_Z]);
+		obj->data[BMA250_AXIS_X] += obj->cali_sw[BMA250_AXIS_X];
+		obj->data[BMA250_AXIS_Y] += obj->cali_sw[BMA250_AXIS_Y];
+		obj->data[BMA250_AXIS_Z] += obj->cali_sw[BMA250_AXIS_Z];
+		
+		//printk("cali_sw x=%d, y=%d, z=%d \n",obj->cali_sw[BMA150_AXIS_X],obj->cali_sw[BMA150_AXIS_Y],obj->cali_sw[BMA150_AXIS_Z]);
+		
+		/*remap coordinate*/
+		acc[obj->cvt.map[BMA250_AXIS_X]] = obj->cvt.sign[BMA250_AXIS_X]*obj->data[BMA250_AXIS_X];
+		acc[obj->cvt.map[BMA250_AXIS_Y]] = obj->cvt.sign[BMA250_AXIS_Y]*obj->data[BMA250_AXIS_Y];
+		acc[obj->cvt.map[BMA250_AXIS_Z]] = obj->cvt.sign[BMA250_AXIS_Z]*obj->data[BMA250_AXIS_Z];
+		//printk("cvt x=%d, y=%d, z=%d \n",obj->cvt.sign[BMA150_AXIS_X],obj->cvt.sign[BMA150_AXIS_Y],obj->cvt.sign[BMA150_AXIS_Z]);
+
+
+		//GSE_LOG("Mapped gsensor data: %d, %d, %d!\n", acc[BMA150_AXIS_X], acc[BMA150_AXIS_Y], acc[BMA150_AXIS_Z]);
+
+		//Out put the mg
+		//printk("mg acc=%d, GRAVITY=%d, sensityvity=%d \n",acc[BMA150_AXIS_X],GRAVITY_EARTH_1000,obj->reso->sensitivity);
+#if 0
+		acc[BMA250_AXIS_X] = acc[BMA250_AXIS_X] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+		acc[BMA250_AXIS_Y] = acc[BMA250_AXIS_Y] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+		acc[BMA250_AXIS_Z] = acc[BMA250_AXIS_Z] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;		
+	#endif	
+	
+
+		sprintf(buf, "%04x %04x %04x", acc[BMA250_AXIS_X], acc[BMA250_AXIS_Y], acc[BMA250_AXIS_Z]);
+		if(atomic_read(&obj->trace) & ADX_TRC_IOCTL)
+		{
+			GSE_LOG("gsensor data: %s!\n", buf);
+		}
+	}
+	
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int BMA250_ReadRawData(struct i2c_client *client, char *buf)
+{
+	struct bma250_i2c_data *obj = (struct bma250_i2c_data*)i2c_get_clientdata(client);
+	int res = 0;
+
+	if (!buf || !client)
+	{
+		return EINVAL;
+	}
+	
+	res = BMA250_ReadData(client, obj->data);
+	if(res)
+	{        
+		GSE_ERR("I2C error: ret value=%d", res);
+		return EIO;
+	}
+	else
+	{
+		sprintf(buf, "BMA250_ReadRawData %04x %04x %04x", obj->data[BMA250_AXIS_X], 
+			obj->data[BMA250_AXIS_Y], obj->data[BMA250_AXIS_Z]);
+	
+	}
+	
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t show_chipinfo_value(struct device_driver *ddri, char *buf)
+{
+	struct i2c_client *client = bma250_i2c_client;
+	char strbuf[BMA250_BUFSIZE];
+	if(NULL == client)
+	{
+		GSE_ERR("i2c client is null!!\n");
+		return 0;
+	}
+	
+	BMA250_ReadChipInfo(client, strbuf, BMA250_BUFSIZE);
+	return snprintf(buf, PAGE_SIZE, "%s\n", strbuf);        
+}
+
+
+/*----------------------------------------------------------------------------*/
+static ssize_t show_sensordata_value(struct device_driver *ddri, char *buf)
+{
+	struct i2c_client *client = bma250_i2c_client;
+	char strbuf[BMA250_BUFSIZE];
+	
+	if(NULL == client)
+	{
+		GSE_ERR("i2c client is null!!\n");
+		return 0;
+	}
+	BMA250_ReadSensorData(client, strbuf, BMA250_BUFSIZE);
+	//BMA150_ReadRawData(client, strbuf);
+	return snprintf(buf, PAGE_SIZE, "%s\n", strbuf);            
+}
+
+
+/*----------------------------------------------------------------------------*/
+static ssize_t show_cali_value(struct device_driver *ddri, char *buf)
+{
+	struct i2c_client *client = bma250_i2c_client;
+	struct bma250_i2c_data *obj;
+	int err, len = 0, mul;
+	int tmp[BMA250_AXES_NUM];
+
+	if(NULL == client)
+	{
+		GSE_ERR("i2c client is null!!\n");
+		return 0;
+	}
+
+	obj = i2c_get_clientdata(client);
+
+	err = BMA250_ReadOffset(client, obj->offset);
+	if(err)
+	{
+		return -EINVAL;
+	}
+	err = BMA250_ReadCalibration(client, tmp);
+	if(err)
+	{
+		return -EINVAL;
+	}
+	else
+	{    
+		mul = obj->reso->sensitivity/bma250_offset_resolution.sensitivity;
+		len += snprintf(buf+len, PAGE_SIZE-len, "[HW ][%d] (%+3d, %+3d, %+3d) : (0x%02X, 0x%02X, 0x%02X)\n", mul,                        
+			obj->offset[BMA250_AXIS_X], obj->offset[BMA250_AXIS_Y], obj->offset[BMA250_AXIS_Z],
+			obj->offset[BMA250_AXIS_X], obj->offset[BMA250_AXIS_Y], obj->offset[BMA250_AXIS_Z]);
+		len += snprintf(buf+len, PAGE_SIZE-len, "[SW ][%d] (%+3d, %+3d, %+3d)\n", 1, 
+			obj->cali_sw[BMA250_AXIS_X], obj->cali_sw[BMA250_AXIS_Y], obj->cali_sw[BMA250_AXIS_Z]);
+
+		len += snprintf(buf+len, PAGE_SIZE-len, "[ALL]    (%+3d, %+3d, %+3d) : (%+3d, %+3d, %+3d)\n", 
+			obj->offset[BMA250_AXIS_X]*mul + obj->cali_sw[BMA250_AXIS_X],
+			obj->offset[BMA250_AXIS_Y]*mul + obj->cali_sw[BMA250_AXIS_Y],
+			obj->offset[BMA250_AXIS_Z]*mul + obj->cali_sw[BMA250_AXIS_Z],
+			tmp[BMA250_AXIS_X], tmp[BMA250_AXIS_Y], tmp[BMA250_AXIS_Z]);
+		
+		return len;
+    }
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t store_cali_value(struct device_driver *ddri, const char *buf, size_t count)
+{
+	struct i2c_client *client = bma250_i2c_client;  
+	int err, x, y, z;
+	int dat[BMA250_AXES_NUM];
+
+	if(!strncmp(buf, "rst", 3))
+	{
+		err = BMA250_ResetCalibration(client);
+		if(err)
+		{
+			GSE_ERR("reset offset err = %d\n", err);
+		}	
+	}
+	else if(3 == sscanf(buf, "0x%02X 0x%02X 0x%02X", &x, &y, &z))
+	{
+		dat[BMA250_AXIS_X] = x;
+		dat[BMA250_AXIS_Y] = y;
+		dat[BMA250_AXIS_Z] = z;
+		err = BMA250_WriteCalibration(client, dat);
+		if(err)
+		{
+			GSE_ERR("write calibration err = %d\n", err);
+		}		
+	}
+	else
+	{
+		GSE_ERR("invalid format\n");
+	}
+	
+	return count;
+}
+
+
+/*----------------------------------------------------------------------------*/
+static ssize_t show_firlen_value(struct device_driver *ddri, char *buf)
+{
+#ifdef CONFIG_BMA250_LOWPASS
+	struct i2c_client *client = bma250_i2c_client;
+	struct bma250_i2c_data *obj = i2c_get_clientdata(client);
+	if(atomic_read(&obj->firlen))
+	{
+		int idx, len = atomic_read(&obj->firlen);
+		GSE_LOG("len = %2d, idx = %2d\n", obj->fir.num, obj->fir.idx);
+
+		for(idx = 0; idx < len; idx++)
+		{
+			GSE_LOG("[%5d %5d %5d]\n", obj->fir.raw[idx][BMA250_AXIS_X], obj->fir.raw[idx][BMA250_AXIS_Y], obj->fir.raw[idx][BMA250_AXIS_Z]);
+		}
+		
+		GSE_LOG("sum = [%5d %5d %5d]\n", obj->fir.sum[BMA250_AXIS_X], obj->fir.sum[BMA250_AXIS_Y], obj->fir.sum[BMA250_AXIS_Z]);
+		GSE_LOG("avg = [%5d %5d %5d]\n", obj->fir.sum[BMA250_AXIS_X]/len, obj->fir.sum[BMA250_AXIS_Y]/len, obj->fir.sum[BMA250_AXIS_Z]/len);
+	}
+	return snprintf(buf, PAGE_SIZE, "%d\n", atomic_read(&obj->firlen));
+#else
+	return snprintf(buf, PAGE_SIZE, "not support\n");
+#endif
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t store_firlen_value(struct device_driver *ddri, const char *buf, size_t count)
+{
+#ifdef CONFIG_BMA250_LOWPASS
+	struct i2c_client *client = bma250_i2c_client;  
+	struct bma250_i2c_data *obj = i2c_get_clientdata(client);
+	int firlen;
+
+	if(1 != sscanf(buf, "%d", &firlen))
+	{
+		GSE_ERR("invallid format\n");
+	}
+	else if(firlen > C_MAX_FIR_LENGTH)
+	{
+		GSE_ERR("exceeds maximum filter length\n");
+	}
+	else
+	{ 
+		atomic_set(&obj->firlen, firlen);
+		if(NULL == firlen)
+		{
+			atomic_set(&obj->fir_en, 0);
+		}
+		else
+		{
+			memset(&obj->fir, 0x00, sizeof(obj->fir));
+			atomic_set(&obj->fir_en, 1);
+		}
+	}
+#endif    
+	return count;
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t show_trace_value(struct device_driver *ddri, char *buf)
+{
+	ssize_t res;
+	struct bma250_i2c_data *obj = obj_i2c_data;
+	if (obj == NULL)
+	{
+		GSE_ERR("i2c_data obj is null!!\n");
+		return 0;
+	}
+	
+	res = snprintf(buf, PAGE_SIZE, "0x%04X\n", atomic_read(&obj->trace));     
+	return res;    
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t store_trace_value(struct device_driver *ddri, const char *buf, size_t count)
+{
+	struct bma250_i2c_data *obj = obj_i2c_data;
+	int trace;
+	if (obj == NULL)
+	{
+		GSE_ERR("i2c_data obj is null!!\n");
+		return 0;
+	}
+	
+	if(1 == sscanf(buf, "0x%x", &trace))
+	{
+		atomic_set(&obj->trace, trace);
+	}	
+	else
+	{
+		GSE_ERR("invalid content: '%s', length = %d\n", buf, (int)count);
+	}
+	
+	return count;    
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t show_status_value(struct device_driver *ddri, char *buf)
+{
+	ssize_t len = 0;    
+	struct bma250_i2c_data *obj = obj_i2c_data;
+	if (obj == NULL)
+	{
+		GSE_ERR("i2c_data obj is null!!\n");
+		return 0;
+	}	
+	
+	if(obj->hw)
+	{
+		len += snprintf(buf+len, PAGE_SIZE-len, "CUST: %d %d (%d %d)\n", 
+	            obj->hw->i2c_num, obj->hw->direction, obj->hw->power_id, obj->hw->power_vol);   
+	}
+	else
+	{
+		len += snprintf(buf+len, PAGE_SIZE-len, "CUST: NULL\n");
+	}
+	return len;    
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t show_power_status_value(struct device_driver *ddri, char *buf)
+{
+	if(sensor_power)
+		printk("G sensor is in work mode, sensor_power = %d\n", sensor_power);
+	else
+		printk("G sensor is in standby mode, sensor_power = %d\n", sensor_power);
+
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static DRIVER_ATTR(chipinfo,   S_IWUSR | S_IRUGO, show_chipinfo_value,      NULL);
+static DRIVER_ATTR(sensordata, S_IWUSR | S_IRUGO, show_sensordata_value,    NULL);
+static DRIVER_ATTR(cali, S_IWUSR | S_IRUGO, show_cali_value, store_cali_value);
+static DRIVER_ATTR(firlen,     S_IWUSR | S_IRUGO, show_firlen_value, store_firlen_value);
+static DRIVER_ATTR(trace,      S_IWUSR | S_IRUGO, show_trace_value, store_trace_value);
+static DRIVER_ATTR(status,               S_IRUGO, show_status_value,        NULL);
+static DRIVER_ATTR(powerstatus,               S_IRUGO, show_power_status_value,        NULL);
+
+/*----------------------------------------------------------------------------*/
+static struct driver_attribute *bma250_attr_list[] = {
+	&driver_attr_chipinfo,     /*chip information*/
+	&driver_attr_sensordata,   /*dump sensor data*/
+	&driver_attr_cali,         /*show calibration data*/
+	&driver_attr_firlen,       /*filter length: 0: disable, others: enable*/
+	&driver_attr_trace,        /*trace log*/
+	&driver_attr_status,
+	&driver_attr_powerstatus,
+};
+/*----------------------------------------------------------------------------*/
+static int bma250_create_attr(struct device_driver *driver) 
+{
+	int idx, err = 0;
+	int num = (int)(sizeof(bma250_attr_list)/sizeof(bma250_attr_list[0]));
+	if (driver == NULL)
+	{
+		return -EINVAL;
+	}
+
+	for(idx = 0; idx < num; idx++)
+	{
+		err = driver_create_file(driver, bma250_attr_list[idx]);
+		if(err)
+		{            
+			GSE_ERR("driver_create_file (%s) = %d\n", bma250_attr_list[idx]->attr.name, err);
+			break;
+		}
+	}    
+	return err;
+}
+/*----------------------------------------------------------------------------*/
+static int bma250_delete_attr(struct device_driver *driver)
+{
+	int idx ,err = 0;
+	int num = (int)(sizeof(bma250_attr_list)/sizeof(bma250_attr_list[0]));
+
+	if(driver == NULL)
+	{
+		return -EINVAL;
+	}
+	
+
+	for(idx = 0; idx < num; idx++)
+	{
+		driver_remove_file(driver, bma250_attr_list[idx]);
+	}
+	
+
+	return err;
+}
+
+/*----------------------------------------------------------------------------*/
+#ifdef CUSTOM_KERNEL_SENSORHUB
+static void gsensor_irq_work(struct work_struct *work)
+{
+    struct bma250_i2c_data *obj = obj_i2c_data;
+    struct scp_acc_hw scp_hw;
+    BMA250_CUST_DATA *p_cust_data;
+    SCP_SENSOR_HUB_DATA data;
+    int max_cust_data_size_per_packet;
+    int i;
+    uint sizeOfCustData;
+    uint len;
+    char *p = (char *)&scp_hw;
+
+    GSE_FUN();
+
+    scp_hw.i2c_num = obj->hw->i2c_num;
+    scp_hw.direction = obj->hw->direction;
+    scp_hw.power_id = obj->hw->power_id;
+    scp_hw.power_vol = obj->hw->power_vol;
+    scp_hw.firlen = obj->hw->firlen;
+    memcpy(scp_hw.i2c_addr, obj->hw->i2c_addr, sizeof(obj->hw->i2c_addr));
+    scp_hw.power_vio_id = obj->hw->power_vio_id;
+    scp_hw.power_vio_vol = obj->hw->power_vio_vol;
+    scp_hw.is_batch_supported = obj->hw->is_batch_supported;
+
+    p_cust_data = (BMA250_CUST_DATA *)data.set_cust_req.custData;
+    sizeOfCustData = sizeof(scp_hw);
+    max_cust_data_size_per_packet = sizeof(data.set_cust_req.custData) - offsetof(BMA250_SET_CUST, data);
+    
+    for (i=0;sizeOfCustData>0;i++)
+    {
+        data.set_cust_req.sensorType = ID_ACCELEROMETER;
+        data.set_cust_req.action = SENSOR_HUB_SET_CUST;
+        p_cust_data->setCust.action = BMA250_CUST_ACTION_SET_CUST;
+        p_cust_data->setCust.part = i;
+        if (sizeOfCustData > max_cust_data_size_per_packet)
+        {
+            len = max_cust_data_size_per_packet;
+        }
+        else
+        {
+            len = sizeOfCustData;
+        }
+
+        memcpy(p_cust_data->setCust.data, p, len);
+        sizeOfCustData -= len;
+        p += len;
+        
+        len += offsetof(SCP_SENSOR_HUB_SET_CUST_REQ, custData) + offsetof(BMA250_SET_CUST, data);
+        SCP_sensorHub_req_send(&data, &len, 1);
+    }
+
+    p_cust_data = (BMA250_CUST_DATA *)&data.set_cust_req.custData;
+
+    data.set_cust_req.sensorType = ID_ACCELEROMETER;
+    data.set_cust_req.action = SENSOR_HUB_SET_CUST;
+    p_cust_data->resetCali.action = BMA250_CUST_ACTION_RESET_CALI;
+    len = offsetof(SCP_SENSOR_HUB_SET_CUST_REQ, custData) + sizeof(p_cust_data->resetCali);
+    SCP_sensorHub_req_send(&data, &len, 1);
+
+    obj->SCP_init_done = 1;
+}
+/*----------------------------------------------------------------------------*/
+static int gsensor_irq_handler(void* data, uint len)
+{
+	struct bma250_i2c_data *obj = obj_i2c_data;
+    SCP_SENSOR_HUB_DATA_P rsp = (SCP_SENSOR_HUB_DATA_P)data;
+
+    GSE_FUN();
+    GSE_ERR("len = %d, type = %d, action = %d, errCode = %d\n", len, rsp->rsp.sensorType, rsp->rsp.action, rsp->rsp.errCode);
+    
+	if(!obj)
+	{
+		return -1;
+	}
+
+    switch(rsp->rsp.action)
+    {
+        case SENSOR_HUB_NOTIFY:
+            switch(rsp->notify_rsp.event)
+            {
+                case SCP_INIT_DONE:
+                    schedule_work(&obj->irq_work);
+                    //schedule_delayed_work(&obj->irq_work, HZ);
+                    break;
+                default:
+                    GSE_ERR("Error sensor hub notify");
+                    break;
+            }
+            break;
+        default:
+            GSE_ERR("Error sensor hub action");
+            break;
+    }
+
+    return 0;
+}
+
+static int gsensor_setup_irq()
+{
+    int err = 0;
+    
+#ifdef GSENSOR_UT
+    GSE_FUN();
+#endif
+
+    err = SCP_sensorHub_rsp_registration(ID_ACCELEROMETER, gsensor_irq_handler);
+    
+	return err;
+}
+#endif//#ifdef CUSTOM_KERNEL_SENSORHUB
+
+
+/****************************************************************************** 
+ * Function Configuration
+******************************************************************************/
+static int bma250_open(struct inode *inode, struct file *file)
+{
+	file->private_data = bma250_i2c_client;
+
+	if(file->private_data == NULL)
+	{
+		GSE_ERR("null pointer!!\n");
+		return -EINVAL;
+	}
+	return nonseekable_open(inode, file);
+}
+/*----------------------------------------------------------------------------*/
+static int bma250_release(struct inode *inode, struct file *file)
+{
+	file->private_data = NULL;
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+//static int bma250_ioctl(struct inode *inode, struct file *file, unsigned int cmd,
+//       unsigned long arg)
+static long bma250_unlocked_ioctl(struct file *file, unsigned int cmd,unsigned long arg)       
+{
+	struct i2c_client *client = (struct i2c_client*)file->private_data;
+	struct bma250_i2c_data *obj = (struct bma250_i2c_data*)i2c_get_clientdata(client);	
+	char strbuf[BMA250_BUFSIZE];
+	void __user *data;
+	SENSOR_DATA sensor_data;
+	long err = 0;
+	int cali[3];
+
+	//GSE_FUN(f);
+	if(_IOC_DIR(cmd) & _IOC_READ)
+	{
+		err = !access_ok(VERIFY_WRITE, (void __user *)arg, _IOC_SIZE(cmd));
+	}
+	else if(_IOC_DIR(cmd) & _IOC_WRITE)
+	{
+		err = !access_ok(VERIFY_READ, (void __user *)arg, _IOC_SIZE(cmd));
+	}
+
+	if(err)
+	{
+		GSE_ERR("access error: %08X, (%2d, %2d)\n", cmd, _IOC_DIR(cmd), _IOC_SIZE(cmd));
+		return -EFAULT;
+	}
+
+	switch(cmd)
+	{
+		case GSENSOR_IOCTL_INIT:
+			bma250_init_client(client, 0);			
+			break;
+
+		case GSENSOR_IOCTL_READ_CHIPINFO:
+			data = (void __user *) arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}
+			
+			BMA250_ReadChipInfo(client, strbuf, BMA250_BUFSIZE);
+			if(copy_to_user(data, strbuf, strlen(strbuf)+1))
+			{
+				err = -EFAULT;
+				break;
+			}				 
+			break;	  
+
+		case GSENSOR_IOCTL_READ_SENSORDATA:
+			data = (void __user *) arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}
+			
+			BMA250_ReadSensorData(client, strbuf, BMA250_BUFSIZE);
+			if(copy_to_user(data, strbuf, strlen(strbuf)+1))
+			{
+				err = -EFAULT;
+				break;	  
+			}				 
+			break;
+
+		case GSENSOR_IOCTL_READ_GAIN:
+			data = (void __user *) arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}			
+			
+			if(copy_to_user(data, &gsensor_gain, sizeof(GSENSOR_VECTOR3D)))
+			{
+				err = -EFAULT;
+				break;
+			}				 
+			break;
+
+		case GSENSOR_IOCTL_READ_RAW_DATA:
+			data = (void __user *) arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}
+			BMA250_ReadRawData(client, strbuf);
+			if(copy_to_user(data, &strbuf, strlen(strbuf)+1))
+			{
+				err = -EFAULT;
+				break;	  
+			}
+			break;	  
+
+		case GSENSOR_IOCTL_SET_CALI:
+			data = (void __user*)arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}
+			if(copy_from_user(&sensor_data, data, sizeof(sensor_data)))
+			{
+				err = -EFAULT;
+				break;	  
+			}
+			if(atomic_read(&obj->suspend))
+			{
+				GSE_ERR("Perform calibration in suspend state!!\n");
+				err = -EINVAL;
+			}
+			else
+			{
+			#if 0
+				cali[BMA250_AXIS_X] = sensor_data.x * obj->reso->sensitivity / GRAVITY_EARTH_1000;
+				cali[BMA250_AXIS_Y] = sensor_data.y * obj->reso->sensitivity / GRAVITY_EARTH_1000;
+				cali[BMA250_AXIS_Z] = sensor_data.z * obj->reso->sensitivity / GRAVITY_EARTH_1000;			  
+			#else
+			cali[BMA250_AXIS_X] = sensor_data.x;
+			cali[BMA250_AXIS_Y] = sensor_data.y;
+			cali[BMA250_AXIS_Z] = sensor_data.z;			  
+						
+
+			#endif
+				err = BMA250_WriteCalibration(client, cali);			 
+			}
+			break;
+
+		case GSENSOR_IOCTL_CLR_CALI:
+			err = BMA250_ResetCalibration(client);
+			break;
+
+		case GSENSOR_IOCTL_GET_CALI:
+			data = (void __user*)arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}
+			err = BMA250_ReadCalibration(client, cali);
+			if(err)
+			{
+				break;
+			}
+			#if 0
+			sensor_data.x = cali[BMA250_AXIS_X] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+			sensor_data.y = cali[BMA250_AXIS_Y] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+			sensor_data.z = cali[BMA250_AXIS_Z] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+			#else
+			sensor_data.x = cali[BMA250_AXIS_X];
+			sensor_data.y = cali[BMA250_AXIS_Y];
+			sensor_data.z = cali[BMA250_AXIS_Z];
+						
+
+			#endif
+			if(copy_to_user(data, &sensor_data, sizeof(sensor_data)))
+			{
+				err = -EFAULT;
+				break;
+			}		
+			break;
+
+		default:
+			GSE_ERR("unknown IOCTL: 0x%08x\n", cmd);
+			err = -ENOIOCTLCMD;
+			break;
+			
+	}
+
+	return err;
+}
+
+
+/*----------------------------------------------------------------------------*/
+static struct file_operations bma250_fops = {
+	//.owner = THIS_MODULE,
+	.open = bma250_open,
+	.release = bma250_release,
+	.unlocked_ioctl = bma250_unlocked_ioctl,
+};
+/*----------------------------------------------------------------------------*/
+static struct miscdevice bma250_device = {
+	.minor = MISC_DYNAMIC_MINOR,
+	.name = "gsensor",
+	.fops = &bma250_fops,
+};
+/*----------------------------------------------------------------------------*/
+#ifndef CONFIG_HAS_EARLYSUSPEND
+/*----------------------------------------------------------------------------*/
+static int bma250_suspend(struct i2c_client *client, pm_message_t msg) 
+{
+	struct bma250_i2c_data *obj = i2c_get_clientdata(client);    
+	int err = 0;
+	GSE_FUN();    
+
+	if(msg.event == PM_EVENT_SUSPEND)
+	{   
+		if(obj == NULL)
+		{
+			GSE_ERR("null pointer!!\n");
+			return -EINVAL;
+		}
+		atomic_set(&obj->suspend, 1);
+		
+#ifndef CUSTOM_KERNEL_SENSORHUB
+		if(err = BMA250_SetPowerMode(obj->client, false))
+#else //#ifndef CUSTOM_KERNEL_SENSORHUB
+		if ((err = BMA250_SCP_SetPowerMode(false, ID_ACCELEROMETER)))
+#endif //#ifndef CUSTOM_KERNEL_SENSORHUB
+		{
+			GSE_ERR("write power control fail!!\n");
+			return;
+		}
+#ifndef CUSTOM_KERNEL_SENSORHUB		
+		BMA250_power(obj->hw, 0);
+#endif
+	}
+	return err;
+}
+/*----------------------------------------------------------------------------*/
+static int bma250_resume(struct i2c_client *client)
+{
+	struct bma250_i2c_data *obj = i2c_get_clientdata(client);        
+	int err;
+	GSE_FUN();
+
+	if(obj == NULL)
+	{
+		GSE_ERR("null pointer!!\n");
+		return -EINVAL;
+	}
+#ifndef CUSTOM_KERNEL_SENSORHUB
+	BMA250_power(obj->hw, 1);
+#endif
+
+#ifndef CUSTOM_KERNEL_SENSORHUB
+	if(err = bma250_init_client(client, 0))
+#else
+	if ((err = BMA250_SCP_SetPowerMode(enable_status, ID_ACCELEROMETER)))
+#endif
+	{
+		GSE_ERR("initialize client fail!!\n");
+		return err;        
+	}
+
+	atomic_set(&obj->suspend, 0);
+
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+#else /*CONFIG_HAS_EARLY_SUSPEND is defined*/
+/*----------------------------------------------------------------------------*/
+static void bma250_early_suspend(struct early_suspend *h) 
+{
+	struct bma250_i2c_data *obj = container_of(h, struct bma250_i2c_data, early_drv);   
+	int err;
+	GSE_FUN();    
+
+	if(obj == NULL)
+	{
+		GSE_ERR("null pointer!!\n");
+		return;
+	}
+	atomic_set(&obj->suspend, 1);
+#ifndef CUSTOM_KERNEL_SENSORHUB
+	err = BMA250_SetPowerMode(obj->client, false); 
+#else
+	if ((err = BMA250_SCP_SetPowerMode(false, ID_ACCELEROMETER)))
+#endif
+	if(err)
+	{
+		GSE_ERR("write power control fail!!\n");
+		return;
+	}
+
+	sensor_power = false;
+#ifndef CUSTOM_KERNEL_SENSORHUB	
+	BMA250_power(obj->hw, 0);
+#endif
+}
+/*----------------------------------------------------------------------------*/
+static void bma250_late_resume(struct early_suspend *h)
+{
+	struct bma250_i2c_data *obj = container_of(h, struct bma250_i2c_data, early_drv);         
+	int err;
+	GSE_FUN();
+
+	if(obj == NULL)
+	{
+		GSE_ERR("null pointer!!\n");
+		return;
+	}
+
+#ifndef CUSTOM_KERNEL_SENSORHUB
+	BMA250_power(obj->hw, 1);
+#endif
+
+#ifndef CUSTOM_KERNEL_SENSORHUB
+	if (bma250_revesion == BMA250E)
+	{
+		u8 buf = 0xE6;
+		u8 addr = 0x14;
+		err = hwmsen_write_block(obj->client, addr, &buf, 1);
+		if(err)
+		{
+			GSE_ERR("error: %d\n", err);
+		}
+		mdelay(1);
+	}
+	err = BMA250_SetPowerMode(obj->client, true);
+#else
+	if ((err = BMA250_SCP_SetPowerMode(enable_status, ID_ACCELEROMETER)))
+#endif
+	if(err)
+	{
+		GSE_ERR("write power control fail!!\n");
+		return;
+	}
+	atomic_set(&obj->suspend, 0);    
+}
+/*----------------------------------------------------------------------------*/
+#endif /*CONFIG_HAS_EARLYSUSPEND*/
+
+
+/*----------------------------------------------------------------------------*/
+// if use  this typ of enable , Gsensor should report inputEvent(x, y, z ,stats, div) to HAL
+static int gsensor_open_report_data(int open)
+{
+	//should queuq work to report event if  is_report_input_direct=true
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+// if use  this typ of enable , Gsensor only enabled but not report inputEvent to HAL
+static int gsensor_enable_nodata(int en)
+{
+    int err = 0;
+
+#ifdef GSENSOR_UT
+    GSE_FUN();
+#endif
+
+    mutex_lock(&gsensor_mutex);
+	if(((en == 0) && (sensor_power == false)) ||((en == 1) && (sensor_power == true)))
+	{
+		enable_status = sensor_power;
+		GSE_LOG("Gsensor device have updated!\n");
+	}
+	else
+	{
+		enable_status = !sensor_power;
+		if (atomic_read(&obj_i2c_data->suspend) == 0)
+		{
+#ifdef CUSTOM_KERNEL_SENSORHUB
+            err = BMA250_SCP_SetPowerMode(enable_status, ID_ACCELEROMETER);
+            if (0 == err)
+            {
+                sensor_power = enable_status;
+            }
+#else//#ifdef CUSTOM_KERNEL_SENSORHUB
+			err = BMA250_SetPowerMode(obj_i2c_data->client, enable_status);
+#endif
+			GSE_LOG("Gsensor not in suspend gsensor_SetPowerMode!, enable_status = %d\n",enable_status);
+		}
+		else
+		{
+			GSE_LOG("Gsensor in suspend and can not enable or disable!enable_status = %d\n",enable_status);
+		}
+	}
+	mutex_unlock(&gsensor_mutex);
+
+    if(err != BMA250_SUCCESS)
+	{
+		printk("gsensor_enable_nodata fail!\n");
+		return -1;
+	}
+
+    printk("gsensor_enable_nodata OK!!!\n");
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int gsensor_set_delay(u64 ns)
+{
+    int err = 0;
+    int value;
+#ifdef CUSTOM_KERNEL_SENSORHUB
+    SCP_SENSOR_HUB_DATA req;
+    int len;
+#else//#ifdef CUSTOM_KERNEL_SENSORHUB
+	int sample_delay;
+#endif//#ifdef CUSTOM_KERNEL_SENSORHUB
+
+#ifdef GSENSOR_UT
+    GSE_FUN();
+#endif
+
+    value = (int)ns/1000/1000;
+
+#ifdef CUSTOM_KERNEL_SENSORHUB
+    req.set_delay_req.sensorType = ID_ACCELEROMETER;
+    req.set_delay_req.action = SENSOR_HUB_SET_DELAY;
+    req.set_delay_req.delay = value;
+    len = sizeof(req.activate_req);
+    err = SCP_sensorHub_req_send(&req, &len, 1);
+    if (err)
+    {
+        GSE_ERR("SCP_sensorHub_req_send!\n");
+        return err;
+    }
+#else//#ifdef CUSTOM_KERNEL_SENSORHUB    
+	if(value <= 5)
+	{
+		sample_delay = BMA250_BW_200HZ;
+	}
+	else if(value <= 10)
+	{
+		sample_delay = BMA250_BW_100HZ;
+	}
+	else
+	{
+		sample_delay = BMA250_BW_50HZ;
+	}
+
+	mutex_lock(&gsensor_mutex);
+	err = BMA250_SetBWRate(obj_i2c_data->client, sample_delay);
+	mutex_unlock(&gsensor_mutex);
+	if(err != BMA250_SUCCESS ) //0x2C->BW=100Hz
+	{
+		GSE_ERR("Set delay parameter error!\n");
+        return -1;
+	}
+
+	if(value >= 50)
+	{
+		atomic_set(&obj_i2c_data->filter, 0);
+	}
+	else
+	{	
+	#if defined(CONFIG_BMA250_LOWPASS)
+		obj_i2c_data->fir.num = 0;
+		obj_i2c_data->fir.idx = 0;
+		obj_i2c_data->fir.sum[BMA250_AXIS_X] = 0;
+		obj_i2c_data->fir.sum[BMA250_AXIS_Y] = 0;
+		obj_i2c_data->fir.sum[BMA250_AXIS_Z] = 0;
+		atomic_set(&obj_i2c_data->filter, 1);
+	#endif
+	}
+#endif//#ifdef CUSTOM_KERNEL_SENSORHUB
+    
+    GSE_LOG("gsensor_set_delay (%d)\n",value);
+
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int gsensor_get_data(int* x ,int* y,int* z, int* status)
+{
+#ifdef CUSTOM_KERNEL_SENSORHUB
+    SCP_SENSOR_HUB_DATA req;
+    int len;
+    int err = 0;
+#else
+    char buff[BMA250_BUFSIZE];
+#endif //#ifdef CUSTOM_KERNEL_SENSORHUB
+
+    //GSE_FUN();
+
+#ifdef CUSTOM_KERNEL_SENSORHUB
+    req.get_data_req.sensorType = ID_ACCELEROMETER;
+    req.get_data_req.action = SENSOR_HUB_GET_DATA;
+    len = sizeof(req.get_data_req);
+    err = SCP_sensorHub_req_send(&req, &len, 1);
+    if (err)
+    {
+        GSE_ERR("SCP_sensorHub_req_send!\n");
+        return err;
+    }
+
+    if (ID_ACCELEROMETER != req.get_data_rsp.sensorType ||
+        SENSOR_HUB_GET_DATA != req.get_data_rsp.action ||
+        0 != req.get_data_rsp.errCode)
+    {
+        GSE_ERR("error : %d\n", req.get_data_rsp.errCode);
+        return req.get_data_rsp.errCode;
+    }
+
+    //sscanf(buff, "%x %x %x", req.get_data_rsp.int16_Data[0], req.get_data_rsp.int16_Data[1], req.get_data_rsp.int16_Data[2]);
+    *x = req.get_data_rsp.int16_Data[0];
+    *y = req.get_data_rsp.int16_Data[1];
+    *z = req.get_data_rsp.int16_Data[2];
+    //GSE_ERR("x = %d, y = %d, z = %d\n", *x, *y, *z);
+    *status = SENSOR_STATUS_ACCURACY_MEDIUM;
+
+#else//#ifdef CUSTOM_KERNEL_SENSORHUB
+    mutex_lock(&gsensor_mutex);
+	BMA250_ReadSensorData(obj_i2c_data->client, buff, BMA250_BUFSIZE);
+	mutex_unlock(&gsensor_mutex);
+	sscanf(buff, "%x %x %x", x, y, z);
+	*status = SENSOR_STATUS_ACCURACY_MEDIUM;
+#endif
+
+	return 0;
+}
+
+/*----------------------------------------------------------------------------*/
+//static int bma250_i2c_detect(struct i2c_client *client, int kind, struct i2c_board_info *info) 
+//{    
+//	strcpy(info->type, BMA250_DEV_NAME);
+//	return 0;
+//}
+
+/*----------------------------------------------------------------------------*/
+static int bma250_i2c_probe(struct i2c_client *client, const struct i2c_device_id *id)
+{
+	struct i2c_client *new_client;
+	struct bma250_i2c_data *obj;
+	struct hwmsen_object sobj;
+    struct acc_control_path ctl={0};
+    struct acc_data_path data={0};
+	int err = 0;
+	GSE_FUN();
+
+	if(!(obj = kzalloc(sizeof(*obj), GFP_KERNEL)))
+	{
+		err = -ENOMEM;
+		goto exit;
+	}
+	
+	memset(obj, 0, sizeof(struct bma250_i2c_data));
+
+	obj->hw = get_cust_acc_hw();
+	err = hwmsen_get_convert(obj->hw->direction, &obj->cvt);
+	if(err)
+	{
+		GSE_ERR("invalid direction: %d\n", obj->hw->direction);
+		goto exit;
+	}
+
+#ifdef CUSTOM_KERNEL_SENSORHUB
+	INIT_WORK(&obj->irq_work, gsensor_irq_work);
+#endif//#ifdef CUSTOM_KERNEL_SENSORHUB
+
+	obj_i2c_data = obj;
+	obj->client = client;
+	new_client = obj->client;
+	i2c_set_clientdata(new_client,obj);
+	
+	atomic_set(&obj->trace, 0);
+	atomic_set(&obj->suspend, 0);
+
+#ifdef CUSTOM_KERNEL_SENSORHUB
+    obj->SCP_init_done = 0;
+#endif//#ifdef CUSTOM_KERNEL_SENSORHUB
+
+#ifdef CONFIG_BMA250_LOWPASS
+	if(obj->hw->firlen > C_MAX_FIR_LENGTH)
+	{
+		atomic_set(&obj->firlen, C_MAX_FIR_LENGTH);
+	}	
+	else
+	{
+		atomic_set(&obj->firlen, obj->hw->firlen);
+	}
+	
+	if(atomic_read(&obj->firlen) > 0)
+	{
+		atomic_set(&obj->fir_en, 1);
+	}
+	
+#endif
+
+	bma250_i2c_client = new_client;	
+
+	err = bma250_init_client(new_client, 1);
+	if(err)
+	{
+		goto exit_init_failed;
+	}
+	
+	err = misc_register(&bma250_device);
+	if(err)
+	{
+		GSE_ERR("bma250_device register failed\n");
+		goto exit_misc_device_register_failed;
+	}
+
+	err = bma250_create_attr(&bma250_init_info.platform_diver_addr->driver);
+	if(err)
+	{
+		GSE_ERR("create attribute err = %d\n", err);
+		goto exit_create_attr_failed;
+	}
+
+	ctl.open_report_data= gsensor_open_report_data;
+	ctl.enable_nodata = gsensor_enable_nodata;
+	ctl.set_delay  = gsensor_set_delay;
+	ctl.is_report_input_direct = false;
+	
+#ifdef CUSTOM_KERNEL_SENSORHUB
+	ctl.is_support_batch = obj->hw->is_batch_supported;
+#else
+	ctl.is_support_batch = false;
+#endif
+	
+	err = acc_register_control_path(&ctl);
+	if(err)
+	{
+		GSE_ERR("register acc control path err\n");
+		printk("register acc control path err\n");
+		goto exit_create_attr_failed;
+	}
+
+	printk("acc_register_control_path sucess\n");
+
+	data.get_data = gsensor_get_data;
+	data.vender_div = 1000;
+	err = acc_register_data_path(&data);
+	if(err)
+	{
+		GSE_ERR("register acc data path err\n");
+		printk("register acc data path err\n");
+		goto exit_create_attr_failed;
+	}
+
+	printk("acc_register_data_path sucess\n");
+	
+	err = batch_register_support_info(ID_ACCELEROMETER,ctl.is_support_batch, 1000, 0);
+	if(err)
+	{
+		GSE_ERR("register gsensor batch support err = %d\n", err);
+		printk("register gsensor batch support err\n");
+		goto exit_create_attr_failed;
+	}
+
+	printk("batch_register_support_info sucess\n");
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+	obj->early_drv.level    = EARLY_SUSPEND_LEVEL_DISABLE_FB - 1,
+	obj->early_drv.suspend  = bma250_early_suspend,
+	obj->early_drv.resume   = bma250_late_resume,    
+	register_early_suspend(&obj->early_drv);
+#endif 
+
+	gsensor_init_flag = 0;
+	GSE_LOG("%s: OK\n", __func__); 
+	printk("bma250_i2c_probe sucess\n");
+
+	return 0;
+
+	exit_create_attr_failed:
+	misc_deregister(&bma250_device);
+	exit_misc_device_register_failed:
+	exit_init_failed:
+	//i2c_detach_client(new_client);
+	exit_kfree:
+	kfree(obj);
+	exit:
+	GSE_ERR("%s: err = %d\n", __func__, err);
+	gsensor_init_flag =-1;
+	return err;
+}
+
+/*----------------------------------------------------------------------------*/
+static int bma250_i2c_remove(struct i2c_client *client)
+{
+	int err = 0;	
+	
+	err = bma250_delete_attr(&bma250_init_info.platform_diver_addr->driver);
+	if(err)
+	{
+		GSE_ERR("bma150_delete_attr fail: %d\n", err);
+	}
+	
+	err = misc_deregister(&bma250_device);
+	if(err)
+	{
+		GSE_ERR("misc_deregister fail: %d\n", err);
+	}
+
+	bma250_i2c_client = NULL;
+	i2c_unregister_device(client);
+	kfree(i2c_get_clientdata(client));
+	
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int bma250_probe(struct platform_device *pdev) 
+{
+	struct acc_hw *hw = get_cust_acc_hw();
+	GSE_FUN();
+
+	BMA250_power(hw, 1);
+	//bma250_force[0] = hw->i2c_num;
+	if(i2c_add_driver(&bma250_i2c_driver))
+	{
+		GSE_ERR("add driver error\n");
+		return -1;
+	}
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int bma250_remove(struct platform_device *pdev)
+{
+    struct acc_hw *hw = get_cust_acc_hw();
+
+    GSE_FUN();    
+    BMA250_power(hw, 0);    
+    i2c_del_driver(&bma250_i2c_driver);
+    return 0;
+}
+
+/*----------------------------------------------------------------------------*/
+static int gsensor_local_init(void)
+{
+    struct acc_hw *hw = get_cust_acc_hw();
+
+    GSE_FUN();
+
+	BMA250_power(hw, 1);
+	if(i2c_add_driver(&bma250_i2c_driver))
+	{
+		GSE_ERR("add driver error\n");
+		return -1;
+	}
+	if(-1 == gsensor_init_flag)
+	{
+	   return -1;
+	}
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int gsensor_remove()
+{
+    struct acc_hw *hw = get_cust_acc_hw();
+
+    GSE_FUN();
+    BMA250_power(hw, 0);    
+    i2c_del_driver(&bma250_i2c_driver);
+    return 0;
+}
+
+/*----------------------------------------------------------------------------*/
+static int __init bma250_init(void)
+{
+	struct acc_hw *hw = get_cust_acc_hw();
+	GSE_FUN();
+	GSE_LOG("%s: i2c_number=%d\n", __func__,hw->i2c_num); 
+	i2c_register_board_info(hw->i2c_num, &i2c_bma250, 1);
+	acc_driver_add(&bma250_init_info);
+	return 0;    
+}
+/*----------------------------------------------------------------------------*/
+static void __exit bma250_exit(void)
+{
+	GSE_FUN();
+}
+/*----------------------------------------------------------------------------*/
+module_init(bma250_init);
+module_exit(bma250_exit);
+/*----------------------------------------------------------------------------*/
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("BMA250 I2C driver");
+MODULE_AUTHOR("Xiaoli.li@mediatek.com");
diff --git a/drivers/misc/mediatek/accelerometer/bma250e/bma250.h b/drivers/misc/mediatek/accelerometer/bma250e/bma250.h
new file mode 100644
index 000000000..9527355e5
--- /dev/null
+++ b/drivers/misc/mediatek/accelerometer/bma250e/bma250.h
@@ -0,0 +1,120 @@
+/* linux/drivers/hwmon/adxl345.c
+ *
+ * (C) Copyright 2008 
+ * MediaTek <www.mediatek.com>
+ *
+ * BMA150 driver for MT6516
+ *
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA  BMA150
+ */
+#ifndef BMA250_H
+#define BMA250_H
+	 
+#include <linux/ioctl.h>
+	 
+	#define BMA250_I2C_SLAVE_WRITE_ADDR		0x30
+	 
+	 /* BMA222 Register Map  (Please refer to BMA150 Specifications) */
+	#define BMA250_REG_DEVID				0x00
+	#define BMA250_FIXED_DEVID			0x03
+	#define BMA250E_FIXED_DEVID			0xF9
+	#define BMA250_REG_OFSX				0x16
+	#define BMA250_REG_OFSX_HIGH			0x1A
+	#define BMA250_REG_BW_RATE			0x10
+	#define BMA250_BW_MASK				0x1f
+	#define BMA250_BW_200HZ				0x0d
+	#define BMA250_BW_100HZ				0x0c
+	#define BMA250_BW_50HZ				0x0b
+	#define BMA250_BW_25HZ				0x0a
+	#define BMA250_REG_POWER_CTL		0x11		
+	#define BMA250_REG_DATA_FORMAT		0x0f
+	#define BMA250_RANGE_MASK			0x0f
+	#define BMA250_RANGE_2G				0x03
+	#define BMA250_RANGE_4G				0x05
+	#define BMA250_RANGE_8G				0x08
+	#define BMA250_REG_DATAXLOW			0x02
+	#define BMA250_REG_DATA_RESOLUTION	0x14
+	#define BMA250_MEASURE_MODE			0x80	
+	#define BMA250_SELF_TEST           			0x32
+	#define BMA250_SELF_TEST_AXIS_X		0x01
+	#define BMA250_SELF_TEST_AXIS_Y		0x02
+	#define BMA250_SELF_TEST_AXIS_Z		0x03
+	#define BMA250_SELF_TEST_POSITIVE	0x00
+	#define BMA250_SELF_TEST_NEGATIVE	0x04
+	#define BMA250_INT_REG_1           			0x16
+	#define BMA250_INT_REG_2          		 	0x17
+	#define BMA250_EEPROM_CTL_REG       0x33
+	#define BMA250_RESERVED_REG_1       0x3E
+	#define BMA250E_RESERVED_REG_1      0x3B
+	
+#define BMA250_SUCCESS						0
+#define BMA250_ERR_I2C						-1
+#define BMA250_ERR_STATUS					-3
+#define BMA250_ERR_SETUP_FAILURE			-4
+#define BMA250_ERR_GETGSENSORDATA			-5
+#define BMA250_ERR_IDENTIFICATION			-6
+	 
+	 
+	 
+#define BMA250_BUFSIZE				256
+
+/*----------------------------------------------------------------------------*/
+#define BMA250_AXIS_X          0
+#define BMA250_AXIS_Y          1
+#define BMA250_AXIS_Z          2
+#define BMA250_AXES_NUM        3
+#define BMA250_DATA_LEN        6
+#define BMA250_DEV_NAME        "BMA250"
+
+/*----------------------------------------------------------------------------*/
+typedef enum{
+    BMA250_CUST_ACTION_SET_CUST = 1,
+    BMA250_CUST_ACTION_SET_CALI,
+    BMA250_CUST_ACTION_RESET_CALI
+}CUST_ACTION;
+/*----------------------------------------------------------------------------*/
+typedef struct
+{
+    uint16_t    action;
+}BMA250_CUST;
+/*----------------------------------------------------------------------------*/
+typedef struct
+{
+    uint16_t    action;
+    uint16_t    part;
+    int32_t     data[0];
+}BMA250_SET_CUST;
+/*----------------------------------------------------------------------------*/
+typedef struct
+{
+    uint16_t    action;
+    int32_t     data[BMA250_AXES_NUM];
+}BMA250_SET_CALI;
+/*----------------------------------------------------------------------------*/
+typedef BMA250_CUST BMA250_RESET_CALI;
+/*----------------------------------------------------------------------------*/
+typedef union
+{
+    uint32_t                data[10];
+    BMA250_CUST         cust;
+    BMA250_SET_CUST     setCust;
+    BMA250_SET_CALI     setCali;
+    BMA250_RESET_CALI   resetCali;
+}BMA250_CUST_DATA;
+/*----------------------------------------------------------------------------*/
+
+	 
+#endif
+
diff --git a/drivers/misc/mediatek/accelerometer/bma255-sdo0/Makefile b/drivers/misc/mediatek/accelerometer/bma255-sdo0/Makefile
new file mode 100755
index 000000000..425d91fe0
--- /dev/null
+++ b/drivers/misc/mediatek/accelerometer/bma255-sdo0/Makefile
@@ -0,0 +1,4 @@
+include $(srctree)/drivers/misc/mediatek/Makefile.custom
+
+obj-y	:=  bma255.o
+
diff --git a/drivers/misc/mediatek/accelerometer/bma255-sdo0/bma255.c b/drivers/misc/mediatek/accelerometer/bma255-sdo0/bma255.c
new file mode 100644
index 000000000..2991cd614
--- /dev/null
+++ b/drivers/misc/mediatek/accelerometer/bma255-sdo0/bma255.c
@@ -0,0 +1,3291 @@
+/* BMA255 motion sensor driver
+ *
+ *
+ * This software program is licensed subject to the GNU General Public License
+ * (GPL).Version 2,June 1991, available at http://www.fsf.org/copyleft/gpl.html
+
+ * (C) Copyright 2011 Bosch Sensortec GmbH
+ * All Rights Reserved
+ */
+
+#include <linux/interrupt.h>
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include <linux/irq.h>
+#include <linux/miscdevice.h>
+#include <asm/uaccess.h>
+#include <linux/delay.h>
+#include <linux/input.h>
+#include <linux/workqueue.h>
+#include <linux/kobject.h>
+#include <linux/earlysuspend.h>
+#include <linux/platform_device.h>
+#include <asm/atomic.h>
+#include <linux/module.h>
+
+#include <mach/devs.h>
+#include <mach/mt_typedefs.h>
+#include <mach/mt_gpio.h>
+#include <mach/mt_pm_ldo.h>
+
+#define POWER_NONE_MACRO MT65XX_POWER_NONE
+
+#include <cust_acc.h>
+#include <linux/hwmsensor.h>
+#include <linux/hwmsen_dev.h>
+#include <linux/sensors_io.h>
+#include "bma255.h"
+#include <linux/hwmsen_helper.h>
+
+#include <accel.h>
+
+#if defined(TARGET_C90)
+#define CONFIG_OF_DT
+#endif
+
+/* Use CALIBRATION_TO_FILE define for saving cal data to file      */
+/* Without CALIBRATION_TO_FILE define, Cal data is saved to MISC2 */
+#define CALIBRATION_TO_FILE 1
+
+#if defined(CALIBRATION_TO_FILE)
+#include <linux/syscalls.h>
+#include <linux/fs.h>
+#endif
+
+#ifdef CONFIG_OF
+static const struct of_device_id gsensor_of_match[] = {
+	{ .compatible = "mediatek,gsensor", },
+	{},
+};
+#endif
+
+/* range for selftest */
+enum BMA_RANGE_ENUM {
+	BMA_RANGE_2G = 0x0, /* +/- 2G */
+	BMA_RANGE_4G,		/* +/- 4G */
+	BMA_RANGE_8G,		/* +/- 8G */
+
+	BMA_UNDEFINED_RANGE = 0xff
+};
+
+
+#if defined(TARGET_C90)
+//#include "../../tc1_interface/gpt/lg_partition.h" //for MT6732
+#else
+//#include "../../tc1_interface/pmt/lg_partition.h" //for MT6582
+#endif
+
+/*----------------------------------------------------------------------------*/
+#define I2C_DRIVERID_BMA255 255
+/*----------------------------------------------------------------------------*/
+#define DEBUG 1
+/*----------------------------------------------------------------------------*/
+//#define CONFIG_BMA255_LOWPASS   /*apply low pass filter on output*/
+#define SW_CALIBRATION
+
+/*----------------------------------------------------------------------------*/
+#define BMA255_AXIS_X          0
+#define BMA255_AXIS_Y          1
+#define BMA255_AXIS_Z          2
+#define BMA255_AXES_NUM        3
+#define BMA255_DATA_LEN        6
+#define BMA255_DEV_NAME        "BMA255"
+
+#define BMA255_MODE_NORMAL      0
+#define BMA255_MODE_LOWPOWER    1
+#define BMA255_MODE_SUSPEND     2
+
+#define BMA255_ACC_X_LSB__POS           4
+#define BMA255_ACC_X_LSB__LEN           4
+#define BMA255_ACC_X_LSB__MSK           0xF0
+#define BMA255_ACC_X_LSB__REG           BMA255_X_AXIS_LSB_REG
+
+#define BMA255_ACC_X_MSB__POS           0
+#define BMA255_ACC_X_MSB__LEN           8
+#define BMA255_ACC_X_MSB__MSK           0xFF
+#define BMA255_ACC_X_MSB__REG           BMA255_X_AXIS_MSB_REG
+
+#define BMA255_ACC_Y_LSB__POS           4
+#define BMA255_ACC_Y_LSB__LEN           4
+#define BMA255_ACC_Y_LSB__MSK           0xF0
+#define BMA255_ACC_Y_LSB__REG           BMA255_Y_AXIS_LSB_REG
+
+#define BMA255_ACC_Y_MSB__POS           0
+#define BMA255_ACC_Y_MSB__LEN           8
+#define BMA255_ACC_Y_MSB__MSK           0xFF
+#define BMA255_ACC_Y_MSB__REG           BMA255_Y_AXIS_MSB_REG
+
+#define BMA255_ACC_Z_LSB__POS           4
+#define BMA255_ACC_Z_LSB__LEN           4
+#define BMA255_ACC_Z_LSB__MSK           0xF0
+#define BMA255_ACC_Z_LSB__REG           BMA255_Z_AXIS_LSB_REG
+
+#define BMA255_ACC_Z_MSB__POS           0
+#define BMA255_ACC_Z_MSB__LEN           8
+#define BMA255_ACC_Z_MSB__MSK           0xFF
+#define BMA255_ACC_Z_MSB__REG           BMA255_Z_AXIS_MSB_REG
+
+#define BMA255_EN_LOW_POWER__POS          6
+#define BMA255_EN_LOW_POWER__LEN          1
+#define BMA255_EN_LOW_POWER__MSK          0x40
+#define BMA255_EN_LOW_POWER__REG          BMA255_REG_POWER_CTL
+
+#define BMA255_EN_SUSPEND__POS            7
+#define BMA255_EN_SUSPEND__LEN            1
+#define BMA255_EN_SUSPEND__MSK            0x80
+#define BMA255_EN_SUSPEND__REG            BMA255_REG_POWER_CTL
+
+#define BMA255_RANGE_SEL__POS             0
+#define BMA255_RANGE_SEL__LEN             4
+#define BMA255_RANGE_SEL__MSK             0x0F
+#define BMA255_RANGE_SEL__REG             BMA255_REG_DATA_FORMAT
+
+#define BMA255_BANDWIDTH__POS             0
+#define BMA255_BANDWIDTH__LEN             5
+#define BMA255_BANDWIDTH__MSK             0x1F
+#define BMA255_BANDWIDTH__REG             BMA255_REG_BW_RATE
+
+
+
+//#ifdef CONFIG_MACH_LGE
+#define BMA255_ACCEL_CALIBRATION 1
+
+#ifdef BMA255_ACCEL_CALIBRATION
+#define BMA255_SHAKING_DETECT_THRESHOLD	(300)	 //clubsh cal2 50 -> 200
+/* Calibration zero-g offset check */
+#define BMA255_AXIS_X             0
+#define BMA255_AXIS_Y             1
+#define BMA255_AXIS_Z             2
+#define CALIBRATION_DATA_AMOUNT         10
+#if 0 /* vendor recommand */
+#define TESTLIMIT_XY                     (175)
+#define TESTLIMIT_Z_USL_LSB             (1270)
+#define TESTLIMIT_Z_LSL_LSB             (778)
+#endif
+#if defined(TARGET_Y70)
+#if 1
+#define TESTLIMIT_XY                     (240)
+#else
+int TESTLIMIT_XY = 240;
+#endif
+#define TESTLIMIT_Z_USL_LSB             (1226)
+#define TESTLIMIT_Z_LSL_LSB             (822)
+#elif defined(TARGET_Y90)
+#define TESTLIMIT_XY                     (232)
+#define TESTLIMIT_Z_USL_LSB             (1226)
+#define TESTLIMIT_Z_LSL_LSB             (822)
+#elif defined(TARGET_C90)
+#define TESTLIMIT_XY                     (237)
+#define TESTLIMIT_Z_USL_LSB             (1226)
+#define TESTLIMIT_Z_LSL_LSB             (822)
+#else//Y50
+#define TESTLIMIT_XY                     (233)
+#define TESTLIMIT_Z_USL_LSB             (1226)
+#define TESTLIMIT_Z_LSL_LSB             (822)
+#endif
+
+/* Calibration zero-g offset check */
+#endif
+
+struct bma255acc{
+	s16	x,
+		y,
+		z;
+} ;
+//LGE_CHANGE_E : 2012-12-07 taebum81.kim@lge.com AT%SURV : (1)
+
+
+#define BMA255_GET_BITSLICE(regvar, bitname)\
+	((regvar & bitname##__MSK) >> bitname##__POS)
+
+#define BMA255_SET_BITSLICE(regvar, bitname, val)\
+	((regvar & ~bitname##__MSK) | ((val<<bitname##__POS)&bitname##__MSK))
+
+/*----------------------------------------------------------------------------*/
+
+/*----------------------------------------------------------------------------*/
+static const struct i2c_device_id bma255_i2c_id[] = {{BMA255_DEV_NAME,0},{}};
+static struct i2c_board_info __initdata bma255_i2c_info ={ I2C_BOARD_INFO(BMA255_DEV_NAME, BMA255_I2C_ADDR)};
+
+/*----------------------------------------------------------------------------*/
+static int bma255_i2c_probe(struct i2c_client *client, const struct i2c_device_id *id);
+static int bma255_i2c_remove(struct i2c_client *client);
+static int bma255_do_calibration(void);
+
+
+/*----------------------------------------------------------------------------*/
+typedef enum {
+    BMA_TRC_FILTER  = 0x01,
+    BMA_TRC_RAWDATA = 0x02,
+    BMA_TRC_IOCTL   = 0x04,
+    BMA_TRC_CALI	= 0X08,
+    BMA_TRC_INFO	= 0X10,
+} BMA_TRC;
+/*----------------------------------------------------------------------------*/
+struct scale_factor{
+    u8  whole;
+    u8  fraction;
+};
+/*----------------------------------------------------------------------------*/
+struct data_resolution {
+    struct scale_factor scalefactor;
+    int                 sensitivity;
+};
+/*----------------------------------------------------------------------------*/
+#define C_MAX_FIR_LENGTH (32)
+/*----------------------------------------------------------------------------*/
+struct data_filter {
+    s16 raw[C_MAX_FIR_LENGTH][BMA255_AXES_NUM];
+    int sum[BMA255_AXES_NUM];
+    int num;
+    int idx;
+};
+/*----------------------------------------------------------------------------*/
+struct bma255_i2c_data {
+    struct i2c_client *client;
+    struct acc_hw *hw;
+    struct hwmsen_convert   cvt;
+
+	/* for selftest */
+	enum BMA_RANGE_ENUM     range;
+	u8 bandwidth;
+
+    /*misc*/
+    struct data_resolution *reso;
+    atomic_t                trace;
+    atomic_t                suspend;
+    atomic_t                selftest;
+	atomic_t				filter;
+    s16                     cali_sw[BMA255_AXES_NUM+1];
+
+    /*data*/
+    s8                      offset[BMA255_AXES_NUM+1];  /*+1: for 4-byte alignment*/
+    s16                     data[BMA255_AXES_NUM+1];
+
+#if defined(CONFIG_BMA255_LOWPASS)
+    atomic_t                firlen;
+    atomic_t                fir_en;
+    struct data_filter      fir;
+#endif
+    /*early suspend*/
+#if defined(CONFIG_HAS_EARLYSUSPEND)
+    struct early_suspend    early_drv;
+#endif
+//LGE_CHANGE_S : 2012-12-07 taebum81.kim@lge.com sensor SURV shake detection : (2)
+	atomic_t fast_calib_x_rslt;
+	atomic_t fast_calib_y_rslt;
+	atomic_t fast_calib_z_rslt;
+	atomic_t fast_calib_rslt;
+//LG_CHANGE_E : 2012-12-07 taebum81.kim@lge.com sensor SURV shake detection : (2)
+};
+/*----------------------------------------------------------------------------*/
+static struct i2c_driver bma255_i2c_driver = {
+    .driver = {
+        .name           = BMA255_DEV_NAME,
+    },
+	.probe      		= bma255_i2c_probe,
+	.remove    			= bma255_i2c_remove,
+#if !defined(CONFIG_HAS_EARLYSUSPEND)
+    .suspend            = bma255_suspend,
+    .resume             = bma255_resume,
+#endif
+	.id_table = bma255_i2c_id,
+};
+
+/*----------------------------------------------------------------------------*/
+static struct i2c_client *bma255_i2c_client = NULL;
+static struct platform_driver bma255_gsensor_driver;
+static struct bma255_i2c_data *obj_i2c_data = NULL;
+static bool sensor_power = true;
+static int test_status = 0;
+static GSENSOR_VECTOR3D gsensor_gain;
+static char selftestRes[8]= {0};
+
+static int data_count = 0;
+
+/*----------------------------------------------------------------------------*/
+#define GSE_TAG                  "[Gsensor] "
+#define GSE_FUN(f)               printk(KERN_ERR GSE_TAG"%s\n", __FUNCTION__)
+#define GSE_ERR(fmt, args...)    printk(KERN_ERR GSE_TAG"%s %d : "fmt, __FUNCTION__, __LINE__, ##args)
+#define GSE_LOG(fmt, args...)    printk(KERN_ERR GSE_TAG fmt, ##args)
+/*----------------------------------------------------------------------------*/
+static struct data_resolution bma255_data_resolution[1] = {
+ /* combination by {FULL_RES,RANGE}*/
+    {{ 4, 0}, 512},   // dataformat +/-2g  in 12-bit resolution;  { 1, 0} = 1.0= (2*2*1000)/(2^12);  1024 = (2^12)/(2*2)
+};
+/*----------------------------------------------------------------------------*/
+static struct data_resolution bma255_offset_resolution = {{1, 0}, 1024};
+
+/*--------------------BMA255 power control function----------------------------------*/
+static void BMA255_power(struct acc_hw *hw, unsigned int on)
+{
+	static unsigned int power_on = 0;
+
+	if(hw->power_id != POWER_NONE_MACRO)		// have externel LDO
+	{
+		GSE_LOG("power %s\n", on ? "on" : "off");
+		if(power_on == on)	// power status not change
+		{
+			GSE_LOG("ignore power control: %d\n", on);
+		}
+		else if(on)	// power on
+		{
+			if(!hwPowerOn(hw->power_id, hw->power_vol, "BMA255"))
+			{
+				GSE_ERR("power on fails!!\n");
+			}
+		}
+		else	// power off
+		{
+			if (!hwPowerDown(hw->power_id, "BMA255"))
+			{
+				GSE_ERR("power off fail!!\n");
+			}
+		}
+	}
+	power_on = on;
+}
+/*----------------------------------------------------------------------------*/
+static int bma255_smbus_read_byte(struct i2c_client *client, unsigned char reg_addr, unsigned char *data)
+{
+	s32 dummy;
+	dummy = i2c_smbus_read_byte_data(client, reg_addr);
+	if (dummy < 0)
+		return -1;
+	*data = dummy & 0x000000ff;
+
+	return 0;
+}
+
+static int bma255_smbus_write_byte(struct i2c_client *client, unsigned char reg_addr, unsigned char *data)
+{
+	s32 dummy;
+	dummy = i2c_smbus_write_byte_data(client, reg_addr, *data);
+	if (dummy < 0)
+		return -1;
+	return 0;
+}
+
+static int bma255_smbus_read_byte_block(struct i2c_client *client, unsigned char reg_addr, unsigned char *data, unsigned char len)
+{
+	s32 dummy;
+	dummy = i2c_smbus_read_i2c_block_data(client, reg_addr, len, data);
+	if (dummy < 0)
+		return -1;
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int BMA255_SetDataResolution(struct bma255_i2c_data *obj)
+{
+
+/* set g sensor data resolution here */
+
+/* BMA255 only can set to 10-bit dataresolution, so do nothing in bma255 driver here */
+
+/* end of set dataresolution */
+
+/* we set measure range from -2g to +2g in BMA255_SetDataFormat(client, BMA255_RANGE_2G),
+                                                    and set 10-bit dataresolution BMA255_SetDataResolution() */
+
+/* so bma255_data_resolution[0] set value as {{ 3, 9}, 256} when declaration, and assign the value to obj->reso here */
+
+    obj->reso = &bma255_data_resolution[0];
+	return 0;
+
+/* if you changed the measure range, for example call: BMA255_SetDataFormat(client, BMA255_RANGE_4G),
+    you must set the right value to bma255_data_resolution */
+
+}
+/*----------------------------------------------------------------------------*/
+static int BMA255_ReadData(struct i2c_client *client, s16 data[BMA255_AXES_NUM])
+{
+	struct bma255_i2c_data *priv = i2c_get_clientdata(client);
+	u8 addr = BMA255_REG_DATAXLOW;
+	u8 buf[BMA255_DATA_LEN] = {0};
+	int err = 0;
+
+	if(NULL == client)
+	{
+		err = -EINVAL;
+	}
+	else if(err = hwmsen_read_block(client, addr, buf, BMA255_DATA_LEN))
+	{
+		GSE_ERR("error: %d\n", err);
+	}
+	else
+	{
+		/* Convert sensor raw data to 16-bit integer */
+		data[BMA255_AXIS_X] = BMA255_GET_BITSLICE(buf[0], BMA255_ACC_X_LSB)
+                                |(BMA255_GET_BITSLICE(buf[1], BMA255_ACC_X_MSB)<<BMA255_ACC_X_LSB__LEN);
+		data[BMA255_AXIS_X] = data[BMA255_AXIS_X] << (sizeof(short)*8-(BMA255_ACC_X_LSB__LEN
+                                + BMA255_ACC_X_MSB__LEN));
+		data[BMA255_AXIS_X] = data[BMA255_AXIS_X] >> (sizeof(short)*8-(BMA255_ACC_X_LSB__LEN
+                                + BMA255_ACC_X_MSB__LEN));
+		data[BMA255_AXIS_Y] = BMA255_GET_BITSLICE(buf[2], BMA255_ACC_Y_LSB)
+                                | (BMA255_GET_BITSLICE(buf[3], BMA255_ACC_Y_MSB)<<BMA255_ACC_Y_LSB__LEN);
+		data[BMA255_AXIS_Y] = data[BMA255_AXIS_Y] << (sizeof(short)*8-(BMA255_ACC_Y_LSB__LEN
+                                + BMA255_ACC_Y_MSB__LEN));
+		data[BMA255_AXIS_Y] = data[BMA255_AXIS_Y] >> (sizeof(short)*8-(BMA255_ACC_Y_LSB__LEN
+                                + BMA255_ACC_Y_MSB__LEN));
+		data[BMA255_AXIS_Z] = BMA255_GET_BITSLICE(buf[4], BMA255_ACC_Z_LSB)
+                                | (BMA255_GET_BITSLICE(buf[5], BMA255_ACC_Z_MSB)<<BMA255_ACC_Z_LSB__LEN);
+		data[BMA255_AXIS_Z] = data[BMA255_AXIS_Z] << (sizeof(short)*8-(BMA255_ACC_Z_LSB__LEN
+                                + BMA255_ACC_Z_MSB__LEN));
+		data[BMA255_AXIS_Z] = data[BMA255_AXIS_Z] >> (sizeof(short)*8-(BMA255_ACC_Z_LSB__LEN
+                                + BMA255_ACC_Z_MSB__LEN));
+
+#ifdef CONFIG_BMA255_LOWPASS
+		if(atomic_read(&priv->filter))
+		{
+			if(atomic_read(&priv->fir_en) && !atomic_read(&priv->suspend))
+			{
+				int idx, firlen = atomic_read(&priv->firlen);
+				if(priv->fir.num < firlen)
+				{
+					priv->fir.raw[priv->fir.num][BMA255_AXIS_X] = data[BMA255_AXIS_X];
+					priv->fir.raw[priv->fir.num][BMA255_AXIS_Y] = data[BMA255_AXIS_Y];
+					priv->fir.raw[priv->fir.num][BMA255_AXIS_Z] = data[BMA255_AXIS_Z];
+					priv->fir.sum[BMA255_AXIS_X] += data[BMA255_AXIS_X];
+					priv->fir.sum[BMA255_AXIS_Y] += data[BMA255_AXIS_Y];
+					priv->fir.sum[BMA255_AXIS_Z] += data[BMA255_AXIS_Z];
+					if(atomic_read(&priv->trace) & BMA_TRC_FILTER)
+					{
+						GSE_LOG("add [%2d] [%5d %5d %5d] => [%5d %5d %5d]\n", priv->fir.num,
+							priv->fir.raw[priv->fir.num][BMA255_AXIS_X], priv->fir.raw[priv->fir.num][BMA255_AXIS_Y], priv->fir.raw[priv->fir.num][BMA255_AXIS_Z],
+							priv->fir.sum[BMA255_AXIS_X], priv->fir.sum[BMA255_AXIS_Y], priv->fir.sum[BMA255_AXIS_Z]);
+					}
+					priv->fir.num++;
+					priv->fir.idx++;
+				}
+				else
+				{
+					idx = priv->fir.idx % firlen;
+					priv->fir.sum[BMA255_AXIS_X] -= priv->fir.raw[idx][BMA255_AXIS_X];
+					priv->fir.sum[BMA255_AXIS_Y] -= priv->fir.raw[idx][BMA255_AXIS_Y];
+					priv->fir.sum[BMA255_AXIS_Z] -= priv->fir.raw[idx][BMA255_AXIS_Z];
+					priv->fir.raw[idx][BMA255_AXIS_X] = data[BMA255_AXIS_X];
+					priv->fir.raw[idx][BMA255_AXIS_Y] = data[BMA255_AXIS_Y];
+					priv->fir.raw[idx][BMA255_AXIS_Z] = data[BMA255_AXIS_Z];
+					priv->fir.sum[BMA255_AXIS_X] += data[BMA255_AXIS_X];
+					priv->fir.sum[BMA255_AXIS_Y] += data[BMA255_AXIS_Y];
+					priv->fir.sum[BMA255_AXIS_Z] += data[BMA255_AXIS_Z];
+					priv->fir.idx++;
+					data[BMA255_AXIS_X] = priv->fir.sum[BMA255_AXIS_X]/firlen;
+					data[BMA255_AXIS_Y] = priv->fir.sum[BMA255_AXIS_Y]/firlen;
+					data[BMA255_AXIS_Z] = priv->fir.sum[BMA255_AXIS_Z]/firlen;
+					if(atomic_read(&priv->trace) & BMA_TRC_FILTER)
+					{
+						GSE_LOG("add [%2d] [%5d %5d %5d] => [%5d %5d %5d] : [%5d %5d %5d]\n", idx,
+						priv->fir.raw[idx][BMA255_AXIS_X], priv->fir.raw[idx][BMA255_AXIS_Y], priv->fir.raw[idx][BMA255_AXIS_Z],
+						priv->fir.sum[BMA255_AXIS_X], priv->fir.sum[BMA255_AXIS_Y], priv->fir.sum[BMA255_AXIS_Z],
+						data[BMA255_AXIS_X], data[BMA255_AXIS_Y], data[BMA255_AXIS_Z]);
+					}
+				}
+			}
+		}
+#endif
+	}
+	return err;
+}
+/*----------------------------------------------------------------------------*/
+static int BMA255_ResetCalibration(struct i2c_client *client)
+{
+	struct bma255_i2c_data *obj = i2c_get_clientdata(client);
+	u8 ofs[4]={0,0,0,0};
+	int err;
+
+	#ifdef SW_CALIBRATION
+
+	#else
+		if(err = hwmsen_write_block(client, BMA255_REG_OFSX, ofs, 4))
+		{
+			GSE_ERR("error: %d\n", err);
+		}
+	#endif
+
+	memset(obj->cali_sw, 0x00, sizeof(obj->cali_sw));
+	memset(obj->offset, 0x00, sizeof(obj->offset));
+	return err;
+}
+/*----------------------------------------------------------------------------*/
+static int BMA255_CheckDeviceID(struct i2c_client *client)
+{
+	u8 databuf[2];
+	int res = 0;
+
+	memset(databuf, 0, sizeof(u8)*2);
+	databuf[0] = BMA255_REG_DEVID;
+
+	res = i2c_master_send(client, databuf, 0x1);
+	if(res <= 0)
+	{
+		res = i2c_master_send(client, databuf, 0x1);
+		if(res <= 0)
+		{
+			goto exit_BMA255_CheckDeviceID;
+		}
+	}
+
+	udelay(500);
+
+	databuf[0] = 0x0;
+	res = i2c_master_recv(client, databuf, 0x01);
+	if(res <= 0)
+	{
+		goto exit_BMA255_CheckDeviceID;
+	}
+
+	if(databuf[0]!=BMA255_FIXED_DEVID)
+	{
+		GSE_ERR("BMA255_CheckDeviceID %d failt!\n ", databuf[0]);
+		return BMA255_ERR_IDENTIFICATION;
+	}
+	else
+	{
+		GSE_LOG("BMA255_CheckDeviceID %d pass!\n ", databuf[0]);
+	}
+
+	exit_BMA255_CheckDeviceID:
+	if (res <= 0)
+	{
+		return BMA255_ERR_I2C;
+	}
+
+	return BMA255_SUCCESS;
+}
+/*----------------------------------------------------------------------------*/
+static int BMA255_SetPowerMode(struct i2c_client *client, bool enable)
+{
+	GSE_FUN();
+	u8 databuf[2] = {0};
+	int res = 0;
+	struct bma255_i2c_data *obj = i2c_get_clientdata(client);
+
+	if(enable == sensor_power )
+	{
+		GSE_LOG("Sensor power status is newest!\n");
+		return BMA255_SUCCESS;
+	}
+
+	if(enable == TRUE)
+	{
+		databuf[1] = 0;
+	}
+	else
+	{
+		databuf[1] = BMA255_MEASURE_MODE;
+	}
+
+	databuf[0] = BMA255_REG_POWER_CTL;
+
+	res = i2c_master_send(client, databuf, 0x2);
+	if(res <= 0)
+	{
+		GSE_LOG("set power mode failed!\n");
+		return BMA255_ERR_I2C;
+	}
+	else
+	{
+		GSE_LOG("set power mode ok: 0x%02x\n", databuf[1]);
+	}
+	//GSE_LOG("BMA255_SetPowerMode ok!\n");
+
+	sensor_power = enable;
+	test_status = sensor_power;
+
+	mdelay(10);
+
+	return BMA255_SUCCESS;
+}
+/*----------------------------------------------------------------------------*/
+static int BMA255_SetDataFormat(struct i2c_client *client, u8 dataformat)
+{
+	struct bma255_i2c_data *obj = i2c_get_clientdata(client);
+	u8 databuf[2] = {0};
+	int res = 0;
+
+	databuf[1] = dataformat;
+	databuf[0] = BMA255_REG_DATA_FORMAT;
+
+	res = i2c_master_send(client, databuf, 0x2);
+	if(res <= 0)
+	{
+		return BMA255_ERR_I2C;
+	}
+	else
+	{
+		GSE_LOG("Set DataFormat: 0x%02x\n", dataformat);
+	}
+	//printk("BMA255_SetDataFormat OK! \n");
+
+	return BMA255_SetDataResolution(obj);
+}
+/*----------------------------------------------------------------------------*/
+static int BMA255_SetBWRate(struct i2c_client *client, u8 bwrate)
+{
+	u8 databuf[2] = {0};
+	int res = 0;
+
+	databuf[1] = bwrate;
+	databuf[0] = BMA255_REG_BW_RATE;
+
+	res = i2c_master_send(client, databuf, 0x2);
+	if(res <= 0)
+	{
+		return BMA255_ERR_I2C;
+	}
+	else
+	{
+		GSE_LOG("Set BWrate: 0x%02x\n", bwrate);
+	}
+
+	return BMA255_SUCCESS;
+}
+/*----------------------------------------------------------------------------*/
+static int BMA255_SetIntEnable(struct i2c_client *client, u8 intenable)
+{
+	int res = 0;
+
+	res = hwmsen_write_byte(client, BMA255_INT_REG_1, intenable);
+	if(res != BMA255_SUCCESS)
+	{
+		return res;
+	}
+	res = hwmsen_write_byte(client, BMA255_INT_REG_2, intenable);
+	if(res != BMA255_SUCCESS)
+	{
+		return res;
+	}
+	GSE_LOG("BMA255 interrupt was disabled\n");
+
+	/*for disable interrupt function*/
+
+	return BMA255_SUCCESS;
+}
+
+/*----------------------------------------------------------------------------*/
+static int bma255_init_client(struct i2c_client *client, int reset_cali)
+{
+	struct bma255_i2c_data *obj = i2c_get_clientdata(client);
+	int res = 0;
+	GSE_FUN();
+
+	res = BMA255_CheckDeviceID(client);
+	if(res != BMA255_SUCCESS)
+	{
+		return res;
+	}
+	printk("BMA255_CheckDeviceID ok \n");
+
+	res = BMA255_SetBWRate(client, BMA255_BW_100HZ);
+	if(res != BMA255_SUCCESS )
+	{
+		return res;
+	}
+	printk("BMA255_SetBWRate OK!\n");
+
+	res = BMA255_SetDataFormat(client, BMA255_RANGE_4G);
+	if(res != BMA255_SUCCESS)
+	{
+		return res;
+	}
+	printk("BMA255_SetDataFormat OK!\n");
+
+	gsensor_gain.x = gsensor_gain.y = gsensor_gain.z = obj->reso->sensitivity;
+
+	res = BMA255_SetIntEnable(client, 0x00);
+	if(res != BMA255_SUCCESS)
+	{
+		return res;
+	}
+	printk("BMA255 disable interrupt function!\n");
+
+	res = BMA255_SetPowerMode(client, false);
+	if(res != BMA255_SUCCESS)
+	{
+		return res;
+	}
+	printk("BMA255_SetPowerMode OK!\n");
+
+	if(0 != reset_cali)
+	{
+		/*reset calibration only in power on*/
+		res = BMA255_ResetCalibration(client);
+		if(res != BMA255_SUCCESS)
+		{
+			return res;
+		}
+	}
+	printk("bma255_init_client OK!\n");
+#ifdef CONFIG_BMA255_LOWPASS
+	memset(&obj->fir, 0x00, sizeof(obj->fir));
+#endif
+
+	mdelay(20);
+
+	return BMA255_SUCCESS;
+}
+/*----------------------------------------------------------------------------*/
+static int BMA255_ReadChipInfo(struct i2c_client *client, char *buf, int bufsize)
+{
+	u8 databuf[10];
+
+	memset(databuf, 0, sizeof(u8)*10);
+
+	if((NULL == buf)||(bufsize<=30))
+	{
+		return -1;
+	}
+
+	if(NULL == client)
+	{
+		*buf = 0;
+		return -2;
+	}
+
+	sprintf(buf, "BMA255 Chip");
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int BMA255_CompassReadData(struct i2c_client *client, char *buf, int bufsize)
+{
+	struct bma255_i2c_data *obj = (struct bma255_i2c_data*)i2c_get_clientdata(client);
+	u8 databuf[20];
+	int acc[BMA255_AXES_NUM];
+	int res = 0;
+	memset(databuf, 0, sizeof(u8)*10);
+
+	if(NULL == buf)
+	{
+		return -1;
+	}
+	if(NULL == client)
+	{
+		*buf = 0;
+		return -2;
+	}
+
+	if(sensor_power == FALSE)
+	{
+		res = BMA255_SetPowerMode(client, true);
+		if(res)
+		{
+			GSE_ERR("Power on bma255 error %d!\n", res);
+		}
+	}
+
+	if(res = BMA255_ReadData(client, obj->data))
+	{
+		GSE_ERR("I2C error: ret value=%d", res);
+		return -3;
+	}
+	else
+	{
+		/*remap coordinate*/
+		acc[obj->cvt.map[BMA255_AXIS_X]] = obj->cvt.sign[BMA255_AXIS_X]*obj->data[BMA255_AXIS_X];
+		acc[obj->cvt.map[BMA255_AXIS_Y]] = obj->cvt.sign[BMA255_AXIS_Y]*obj->data[BMA255_AXIS_Y];
+		acc[obj->cvt.map[BMA255_AXIS_Z]] = obj->cvt.sign[BMA255_AXIS_Z]*obj->data[BMA255_AXIS_Z];
+
+		sprintf(buf, "%d %d %d", (s16)acc[BMA255_AXIS_X], (s16)acc[BMA255_AXIS_Y], (s16)acc[BMA255_AXIS_Z]);
+		if(atomic_read(&obj->trace) & BMA_TRC_IOCTL)
+		{
+			GSE_LOG("gsensor data for compass: %s!\n", buf);
+		}
+	}
+
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int BMA255_ReadSensorData(struct i2c_client *client, char *buf, int bufsize)
+{
+	struct bma255_i2c_data *obj = (struct bma255_i2c_data*)i2c_get_clientdata(client);
+	u8 databuf[20];
+	int acc[BMA255_AXES_NUM];
+	int res = 0;
+	memset(databuf, 0, sizeof(u8)*10);
+
+	if(NULL == buf)
+	{
+		return -1;
+	}
+	if(NULL == client)
+	{
+		*buf = 0;
+		return -2;
+	}
+
+	if(sensor_power == FALSE)
+	{
+		res = BMA255_SetPowerMode(client, true);
+		if(res)
+		{
+			GSE_ERR("Power on bma255 error %d!\n", res);
+		}
+	}
+
+	if(res = BMA255_ReadData(client, obj->data))
+	{
+		GSE_ERR("I2C error: ret value=%d", res);
+		return -3;
+	}
+	else
+	{
+		obj->data[BMA255_AXIS_X] += obj->cali_sw[BMA255_AXIS_X];
+		obj->data[BMA255_AXIS_Y] += obj->cali_sw[BMA255_AXIS_Y];
+		obj->data[BMA255_AXIS_Z] += obj->cali_sw[BMA255_AXIS_Z];
+
+		/*remap coordinate*/
+		acc[obj->cvt.map[BMA255_AXIS_X]] = obj->cvt.sign[BMA255_AXIS_X]*obj->data[BMA255_AXIS_X];
+		acc[obj->cvt.map[BMA255_AXIS_Y]] = obj->cvt.sign[BMA255_AXIS_Y]*obj->data[BMA255_AXIS_Y];
+		acc[obj->cvt.map[BMA255_AXIS_Z]] = obj->cvt.sign[BMA255_AXIS_Z]*obj->data[BMA255_AXIS_Z];
+
+		//Out put the mg
+		acc[BMA255_AXIS_X] = acc[BMA255_AXIS_X] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+		acc[BMA255_AXIS_Y] = acc[BMA255_AXIS_Y] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+		acc[BMA255_AXIS_Z] = acc[BMA255_AXIS_Z] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+		data_count++;
+		if(data_count > 100000)
+		{
+			data_count = 0;
+		}
+
+		sprintf(buf, "%04x %04x %04x %04x", acc[BMA255_AXIS_X], acc[BMA255_AXIS_Y], acc[BMA255_AXIS_Z], data_count);
+
+        /* Add for accel sensor data error debugging : start */
+        if((data_count%500)==0)
+            GSE_LOG("gsensor data: debug count = %d, x=%d, y=%d, z=%d!\n",data_count,acc[BMA255_AXIS_X], acc[BMA255_AXIS_Y], acc[BMA255_AXIS_Z]);
+        /* Add for accel sensor data error debugging : end */
+
+		if(atomic_read(&obj->trace) & BMA_TRC_IOCTL)
+		{
+			GSE_LOG("gsensor data: %s!\n", buf);
+		}
+	}
+
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int BMA255_ReadRawData(struct i2c_client *client, char *buf)
+{
+	struct bma255_i2c_data *obj = (struct bma255_i2c_data*)i2c_get_clientdata(client);
+	int res = 0;
+
+	if (!buf || !client)
+	{
+		return EINVAL;
+	}
+
+	if(res = BMA255_ReadData(client, obj->data))
+	{
+		GSE_ERR("I2C error: ret value=%d", res);
+		return EIO;
+	}
+	else
+	{
+		sprintf(buf, "BMA255_ReadRawData %04x %04x %04x", obj->data[BMA255_AXIS_X],
+			obj->data[BMA255_AXIS_Y], obj->data[BMA255_AXIS_Z]);
+	}
+
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int bma255_set_mode(struct i2c_client *client, unsigned char mode)
+{
+	int comres = 0;
+	unsigned char data[2] = {BMA255_EN_LOW_POWER__REG};
+
+	if ((client == NULL) || (mode >= 3))
+	{
+		return -1;
+	}
+
+	comres = hwmsen_read_block(client, BMA255_EN_LOW_POWER__REG, data+1, 1);
+	switch (mode) {
+	case BMA255_MODE_NORMAL:
+		data[1]  = BMA255_SET_BITSLICE(data[1], BMA255_EN_LOW_POWER, 0);
+		data[1]  = BMA255_SET_BITSLICE(data[1],	BMA255_EN_SUSPEND, 0);
+		break;
+	case BMA255_MODE_LOWPOWER:
+		data[1]  = BMA255_SET_BITSLICE(data[1],	BMA255_EN_LOW_POWER, 1);
+		data[1]  = BMA255_SET_BITSLICE(data[1],	BMA255_EN_SUSPEND, 0);
+		break;
+	case BMA255_MODE_SUSPEND:
+		data[1]  = BMA255_SET_BITSLICE(data[1],	BMA255_EN_LOW_POWER, 0);
+		data[1]  = BMA255_SET_BITSLICE(data[1],	BMA255_EN_SUSPEND, 1);
+		break;
+	default:
+		break;
+	}
+
+	comres = i2c_master_send(client, data, 2);
+
+	if(comres <= 0)
+	{
+		return BMA255_ERR_I2C;
+	}
+	else
+	{
+		return comres;
+	}
+}
+/*----------------------------------------------------------------------------*/
+static int bma255_get_mode(struct i2c_client *client, unsigned char *mode)
+{
+	int comres = 0;
+
+	if (client == NULL)
+	{
+		return -1;
+	}
+	comres = hwmsen_read_block(client,
+			BMA255_EN_LOW_POWER__REG, mode, 1);
+	*mode  = (*mode) >> 6;
+
+	return comres;
+}
+
+/*----------------------------------------------------------------------------*/
+static int bma255_set_range(struct i2c_client *client, unsigned char range)
+{
+	struct bma255_i2c_data *obj = (struct bma255_i2c_data*)i2c_get_clientdata(client);
+	int comres = 0;
+	unsigned char data[2] = {BMA255_RANGE_SEL__REG};
+
+	if (client == NULL)
+	{
+		return -1;
+	}
+	if (range == obj->range)
+	{
+		return 0;
+	}
+
+	comres = hwmsen_read_block(client,
+			BMA255_RANGE_SEL__REG, data+1, 1);
+
+	data[1]  = BMA255_SET_BITSLICE(data[1],
+			BMA255_RANGE_SEL, range);
+
+	comres = i2c_master_send(client, data, 2);
+	mdelay(1);
+
+	if(comres <= 0)
+	{
+		return BMA255_ERR_I2C;
+	}
+	else
+	{
+		obj->range = range;
+		return comres;
+	}
+}
+/*----------------------------------------------------------------------------*/
+static int bma255_get_range(struct i2c_client *client, unsigned char *range)
+{
+	int comres = 0;
+	unsigned char data;
+
+	if (client == NULL)
+	{
+		return -1;
+	}
+
+	comres = hwmsen_read_block(client, BMA255_RANGE_SEL__REG,	&data, 1);
+	*range = BMA255_GET_BITSLICE(data, BMA255_RANGE_SEL);
+
+	return comres;
+}
+/*----------------------------------------------------------------------------*/
+static int bma255_set_bandwidth(struct i2c_client *client, unsigned char bandwidth)
+{
+	int comres = 0;
+	unsigned char data[2] = {BMA255_BANDWIDTH__REG};
+
+	if (client == NULL)
+	{
+		return -1;
+	}
+
+	comres = hwmsen_read_block(client,
+			BMA255_BANDWIDTH__REG, data+1, 1);
+
+	data[1]  = BMA255_SET_BITSLICE(data[1],
+			BMA255_BANDWIDTH, bandwidth);
+
+	comres = i2c_master_send(client, data, 2);
+	mdelay(1);
+
+	if(comres <= 0)
+	{
+		return BMA255_ERR_I2C;
+	}
+	else
+	{
+		return comres;
+	}
+}
+/*----------------------------------------------------------------------------*/
+static int bma255_get_bandwidth(struct i2c_client *client, unsigned char *bandwidth)
+{
+	int comres = 0;
+	unsigned char data;
+
+	if (client == NULL)
+	{
+		return -1;
+	}
+
+	comres = hwmsen_read_block(client, BMA255_BANDWIDTH__REG, &data, 1);
+	data = BMA255_GET_BITSLICE(data, BMA255_BANDWIDTH);
+
+	if (data < 0x08)        //7.81Hz
+	{
+		*bandwidth = BMA255_BW_7_81HZ;
+	}
+	else if (data > 0x0f)   // 1000Hz
+	{
+		*bandwidth = BMA255_BW_1000HZ;
+	}
+	else
+	{
+		*bandwidth = data;
+	}
+	return comres;
+}
+/*----------------------------------------------------------------------------*/
+#ifdef BMA255_ACCEL_CALIBRATION
+static int bma255_set_offset_target(struct i2c_client *client, unsigned char channel, unsigned char offset)
+{
+	unsigned char data;
+	int comres = 0;
+
+	switch (channel)
+	{
+		case BMA255_CUT_OFF:
+			comres = bma255_smbus_read_byte(client, BMA255_COMP_CUTOFF__REG, &data);
+			data = BMA255_SET_BITSLICE(data, BMA255_COMP_CUTOFF, offset);
+			comres = bma255_smbus_write_byte(client, BMA255_COMP_CUTOFF__REG, &data);
+			break;
+
+		case BMA255_OFFSET_TRIGGER_X:
+			comres = bma255_smbus_read_byte(client, BMA255_COMP_TARGET_OFFSET_X__REG, &data);
+			data = BMA255_SET_BITSLICE(data, BMA255_COMP_TARGET_OFFSET_X, offset);
+			comres = bma255_smbus_write_byte(client, BMA255_COMP_TARGET_OFFSET_X__REG, &data);
+			break;
+
+		case BMA255_OFFSET_TRIGGER_Y:
+			comres = bma255_smbus_read_byte(client, BMA255_COMP_TARGET_OFFSET_Y__REG, &data);
+			data = BMA255_SET_BITSLICE(data, BMA255_COMP_TARGET_OFFSET_Y, offset);
+			comres = bma255_smbus_write_byte(client, BMA255_COMP_TARGET_OFFSET_Y__REG, &data);
+			break;
+
+		case BMA255_OFFSET_TRIGGER_Z:
+			comres = bma255_smbus_read_byte(client, BMA255_COMP_TARGET_OFFSET_Z__REG, &data);
+			data = BMA255_SET_BITSLICE(data, BMA255_COMP_TARGET_OFFSET_Z, offset);
+			comres = bma255_smbus_write_byte(client, BMA255_COMP_TARGET_OFFSET_Z__REG, &data);
+			break;
+
+		default:
+			comres = -1;
+			break;
+	}
+
+	return comres;
+}
+
+static int bma255_get_cal_ready(struct i2c_client *client, unsigned char *calrdy)
+{
+	int comres = 0 ;
+	unsigned char data;
+
+	comres = bma255_smbus_read_byte(client, BMA255_FAST_CAL_RDY_S__REG, &data);
+	data = BMA255_GET_BITSLICE(data, BMA255_FAST_CAL_RDY_S);
+	*calrdy = data;
+
+	return comres;
+}
+
+static int bma255_set_cal_trigger(struct i2c_client *client, unsigned char caltrigger)
+{
+	int comres = 0;
+	unsigned char data;
+	struct bma255_i2c_data *bma255 = i2c_get_clientdata(client);
+
+	if(atomic_read(&bma255->fast_calib_rslt) != 0)
+	{
+		atomic_set(&bma255->fast_calib_rslt, 0);
+		GSE_LOG(KERN_INFO "[set] bma2X2->fast_calib_rslt:%d\n",atomic_read(&bma255->fast_calib_rslt));
+	}
+
+	comres = bma255_smbus_read_byte(client, BMA255_CAL_TRIGGER__REG, &data);
+	data = BMA255_SET_BITSLICE(data, BMA255_CAL_TRIGGER, caltrigger);
+	comres = bma255_smbus_write_byte(client, BMA255_CAL_TRIGGER__REG, &data);
+
+	return comres;
+}
+
+static int bma255_set_offset_x(struct i2c_client *client, unsigned char offsetfilt)
+{
+	int comres = 0;
+	unsigned char data;
+
+	data =  offsetfilt;
+	comres = bma255_smbus_write_byte(client, BMA255_OFFSET_X_AXIS_REG, &data);
+
+	return comres;
+}
+
+static int bma255_get_offset_x(struct i2c_client *client, unsigned char *offsetfilt)
+{
+	int comres = 0;
+	unsigned char data;
+
+	comres = bma255_smbus_read_byte(client, BMA255_OFFSET_X_AXIS_REG, &data);
+	*offsetfilt = data;
+
+	return comres;
+}
+
+static int bma255_set_offset_y(struct i2c_client *client, unsigned char offsetfilt)
+{
+	int comres = 0;
+	unsigned char data;
+
+	data =  offsetfilt;
+	comres = bma255_smbus_write_byte(client, BMA255_OFFSET_Y_AXIS_REG, &data);
+
+	return comres;
+}
+
+static int bma255_get_offset_y(struct i2c_client *client, unsigned char *offsetfilt)
+{
+	int comres = 0;
+	unsigned char data;
+
+	comres = bma255_smbus_read_byte(client, BMA255_OFFSET_Y_AXIS_REG, &data);
+	*offsetfilt = data;
+
+	return comres;
+}
+
+static int bma255_set_offset_z(struct i2c_client *client, unsigned char offsetfilt)
+{
+	int comres = 0;
+	unsigned char data;
+
+	data =  offsetfilt;
+	comres = bma255_smbus_write_byte(client, BMA255_OFFSET_Z_AXIS_REG, &data);
+
+	return comres;
+}
+
+static int bma255_get_offset_z(struct i2c_client *client, unsigned char *offsetfilt)
+{
+	int comres = 0;
+	unsigned char data;
+
+	comres = bma255_smbus_read_byte(client, BMA255_OFFSET_Z_AXIS_REG, &data);
+	*offsetfilt = data;
+
+	return comres;
+}
+
+static int bma255_read_accel_xyz(struct i2c_client *client, struct bma255acc *acc)
+{
+	int comres = 0;
+	unsigned char data[6];
+
+	comres = bma255_smbus_read_byte_block(client, BMA255_ACC_X_LSB__REG, data, 6);
+
+	acc->x = BMA255_GET_BITSLICE(data[0], BMA255_ACC_X_LSB) | (BMA255_GET_BITSLICE(data[1], BMA255_ACC_X_MSB)<<(BMA255_ACC_X_LSB__LEN));
+	acc->x = acc->x << (sizeof(short)*8-(BMA255_ACC_X_LSB__LEN + BMA255_ACC_X_MSB__LEN));
+	acc->x = acc->x >> (sizeof(short)*8-(BMA255_ACC_X_LSB__LEN + BMA255_ACC_X_MSB__LEN));
+
+	acc->y = BMA255_GET_BITSLICE(data[2], BMA255_ACC_Y_LSB) | (BMA255_GET_BITSLICE(data[3], BMA255_ACC_Y_MSB)<<(BMA255_ACC_Y_LSB__LEN));
+	acc->y = acc->y << (sizeof(short)*8-(BMA255_ACC_Y_LSB__LEN + BMA255_ACC_Y_MSB__LEN));
+	acc->y = acc->y >> (sizeof(short)*8-(BMA255_ACC_Y_LSB__LEN + BMA255_ACC_Y_MSB__LEN));
+
+	acc->z = BMA255_GET_BITSLICE(data[4], BMA255_ACC_Z_LSB) | (BMA255_GET_BITSLICE(data[5], BMA255_ACC_Z_MSB)<<(BMA255_ACC_Z_LSB__LEN));
+	acc->z = acc->z << (sizeof(short)*8-(BMA255_ACC_Z_LSB__LEN + BMA255_ACC_Z_MSB__LEN));
+	acc->z = acc->z >> (sizeof(short)*8-(BMA255_ACC_Z_LSB__LEN + BMA255_ACC_Z_MSB__LEN));
+
+	return comres;
+}
+
+static ssize_t show_cali_value(struct device_driver *ddri, char *buf)
+{
+	struct i2c_client *client = bma255_i2c_client;
+    struct bma255_i2c_data *bma255 = i2c_get_clientdata(client);
+	unsigned char offset_x,offset_y,offset_z;
+
+	if(bma255_get_offset_x(bma255->client, &offset_x) < 0)
+		return -EINVAL;
+	if(bma255_get_offset_y(bma255->client, &offset_y) < 0)
+		return -EINVAL;
+	if(bma255_get_offset_z(bma255->client, &offset_z) < 0)
+		return -EINVAL;
+
+    GSE_LOG("offset_x: %d, offset_y: %d, offset_z: %d\n",offset_x,offset_y,offset_z);
+
+	return snprintf(buf, PAGE_SIZE, "%d %d %d \n", (unsigned int)offset_x, (unsigned int)offset_y, (unsigned int)offset_z);
+}
+
+#if defined(CALIBRATION_TO_FILE)
+static int bma255_calibration_save(int *cal)
+{
+   int fd;
+   int i;
+   int res;
+   char *fname = "/persist-lg/sensor/sensor_cal_data.txt";
+   mm_segment_t old_fs = get_fs();
+   char temp_str[5];
+
+   set_fs(KERNEL_DS);
+
+   fd = sys_open(fname,O_WRONLY|O_CREAT|S_IROTH, 0666);
+	if(fd< 0){
+           GSE_LOG("[%s] File Open Error !!!(%d)\n", __func__,fd);
+   		   sys_close(fd);
+	       return -EINVAL;
+		}
+   for(i=0;i<6;i++)
+   {
+   		memset(temp_str,0x00,sizeof(temp_str));
+   		sprintf(temp_str, "%d", cal[i]);
+	   	res = sys_write(fd,temp_str, sizeof(temp_str));
+	   
+   		if(res<0) {
+   			GSE_LOG("[%s] Write Error !!!\n", __func__);
+   			sys_close(fd);
+	 		return -EINVAL;
+   		}
+   	}
+   sys_fsync(fd);
+   sys_close(fd);
+
+   sys_chmod(fname, 0664);
+   set_fs(old_fs);
+   GSE_LOG("bma255_calibration_save Done.\n");
+   
+   return 0;
+}
+
+static int bma255_calibration_read(int *cal_read)
+{
+   int fd;
+   int i;  
+   int res;
+   char *fname = "/persist-lg/sensor/sensor_cal_data.txt";
+   mm_segment_t old_fs = get_fs();
+   char temp_str[5];
+   
+   set_fs(KERNEL_DS);
+
+   fd = sys_open(fname, O_RDONLY, 0);
+	if(fd< 0){
+         GSE_LOG("[%s] File Open Error !!!\n", __func__);
+	     sys_close(fd);
+     	 return -EINVAL;
+		}
+   for(i=0;i<6;i++)
+   {
+   		memset(temp_str,0x00,sizeof(temp_str));   
+        res = sys_read(fd, temp_str, sizeof(temp_str));
+         if(res<0){
+   			 GSE_LOG("[%s] Read Error !!!\n", __func__);
+              sys_close(fd);
+              return -EINVAL;
+         	}
+        sscanf(temp_str,"%d",&cal_read[i]);
+        GSE_LOG("bma255_calibration_read : cal_read[%d]=%d\n",i,cal_read[i]);
+   	}
+   sys_close(fd);
+   set_fs(old_fs);
+   GSE_LOG("bma255_calibration_read Done.\n");
+   
+   return 0;
+}
+
+/* Make gsensor cal file to save calibration data */
+/* 1. If file exist, do nothing                  */
+/* 2. If file does not exist, read cal data from misc2 (for L-OS upgrade model) */
+static int make_cal_data_file(void)
+{
+   int fd;
+   int i;
+   int res;
+   char *fname = "/persist-lg/sensor/sensor_cal_data.txt";
+   mm_segment_t old_fs = get_fs();
+   char temp_str[5];
+   int cal_misc[6]={0,};
+
+   set_fs(KERNEL_DS);
+   
+   fd = sys_open(fname, O_RDONLY, 0); /* Cal file exist check */
+   
+   if(fd==-2)
+   GSE_LOG("[%s] Open Cal File Error. Need to make file !!!(%d)\n", __func__,fd);
+   
+ 	if(fd< 0){
+	       fd = sys_open(fname,O_WRONLY|O_CREAT|S_IROTH, 0666);
+	     if(fd< 0){
+	       GSE_LOG("[%s] Open or Make Cal File Error !!!(%d)\n", __func__,fd);
+  		   sys_close(fd);
+	       return -EINVAL;
+		   }
+
+#if 0
+	     if(LGE_FacReadAccelerometerCalibration((unsigned int*)cal_misc) == TRUE)
+	     	{
+		     GSE_LOG("Read Cal from misc Old x: %d, y: %d, z: %d\n",cal_misc[3],cal_misc[4],cal_misc[5]);
+		     GSE_LOG("Read Cal from misc Now x: %d, y: %d, z: %d\n",cal_misc[0],cal_misc[1],cal_misc[2]);
+	     	}
+         else{
+		     GSE_LOG("Read Cal from misc Error !!!\n");
+         	}
+#endif
+
+		     /* set default cal */   
+			  if((cal_misc[0]==0)&&(cal_misc[1]==0)&&(cal_misc[2]==0))
+			  	{
+			  	 #if defined(TARGET_Y90)
+                   cal_misc[0]=3;
+                   cal_misc[1]=6;
+                   cal_misc[2]=5;
+                 #endif 
+		           GSE_LOG("Default Cal is set : %d, %d, %d\n",cal_misc[0],cal_misc[1],cal_misc[2]);					  	
+			  	}
+    	      /* set default cal */   
+
+		   for(i=0;i<6;i++)
+		   {
+		   		memset(temp_str,0x00,sizeof(temp_str));
+		   		sprintf(temp_str, "%d", cal_misc[i]);
+			   	res = sys_write(fd,temp_str, sizeof(temp_str));
+			   
+		   		if(res<0) {
+		   			GSE_LOG("[%s] Write Cal Error !!!\n", __func__);
+		   			sys_close(fd);
+			 		return -EINVAL;
+		   		}
+		   	}
+         GSE_LOG("make_cal_data_file Done.\n"); 
+		}
+	else
+         GSE_LOG("Sensor Cal File exist.\n"); 
+   
+   sys_fsync(fd);
+   sys_close(fd);
+
+   sys_chmod(fname, 0664);
+   set_fs(old_fs);
+   return 0;
+}
+#endif
+
+#if 1 //motion sensor cal backup
+static ssize_t show_cali_backup_value(struct device_driver *ddri, char *buf)
+{
+	struct i2c_client *client = bma255_i2c_client;
+    struct bma255_i2c_data *bma255 = i2c_get_clientdata(client);
+	unsigned char offset_x,offset_y,offset_z;
+	int cal_backup[6]={0,};
+	int i;
+
+#if defined(CALIBRATION_TO_FILE)
+   if(bma255_calibration_read(cal_backup) == 0)
+		GSE_LOG("previous offset_x: %d,  offset_y: %d,  offset_z: %d\n",cal_backup[3],cal_backup[4],cal_backup[5]);
+   else
+        GSE_LOG("Fail to read previous gsensor cal value from Cal File\n");
+#else
+	if(LGE_FacReadAccelerometerCalibration((unsigned int*)cal_backup) == TRUE)
+		GSE_LOG("Read from LGPServer gsensor previous offset_x: %d,  offset_y: %d,  offset_z: %d\n",cal_backup[3],cal_backup[4],cal_backup[5]);
+	else
+        GSE_LOG("Fail to read previous gsensor cal value from LGPserver\n");
+#endif
+
+	if(bma255_get_offset_x(bma255->client, &offset_x) < 0)
+		return -EINVAL;
+	if(bma255_get_offset_y(bma255->client, &offset_y) < 0)
+		return -EINVAL;
+	if(bma255_get_offset_z(bma255->client, &offset_z) < 0)
+		return -EINVAL;
+
+    GSE_LOG("Current offset_x: %d, offset_y: %d, offset_z: %d\n",offset_x,offset_y,offset_z);
+
+	return snprintf(buf, PAGE_SIZE, "Old %d %d %d   Now %d %d %d \n", (unsigned int)cal_backup[3], (unsigned int)cal_backup[4], (unsigned int)cal_backup[5], (unsigned int)offset_x, (unsigned int)offset_y, (unsigned int)offset_z);
+}
+#endif
+
+static ssize_t store_cali_value(struct device_driver *ddri, char *buf, size_t count)
+{
+	struct i2c_client *client = bma255_i2c_client;
+	struct bma255_i2c_data *bma255 = i2c_get_clientdata(client);
+	int err;
+	int offset_x,offset_y,offset_z;
+	unsigned char offsets[3];
+	int dat[BMA255_AXES_NUM];
+
+	if(!strncmp(buf, "rst", 3))
+	{
+		if(err = BMA255_ResetCalibration(client))
+		{
+			GSE_ERR("reset offset err = %d\n", err);
+		}
+	}
+	else if(3 == sscanf(buf, "%d %d %d", &offset_x, &offset_y, &offset_z))
+	{
+		GSE_LOG("store_cali_value: x=%d, y=%d, z=%d\n", offset_x, offset_y, offset_z);
+		offsets[0] = (unsigned char)offset_x;
+		offsets[1] = (unsigned char)offset_y;
+		offsets[2] = (unsigned char)offset_z;
+		if(bma255_set_offset_x(bma255->client, (unsigned char)offsets[0]) < 0)
+			return -EINVAL;
+		if(bma255_set_offset_y(bma255->client, (unsigned char)offsets[1]) < 0)
+			return -EINVAL;
+		if(bma255_set_offset_z(bma255->client, (unsigned char)offsets[2]) < 0)
+			return -EINVAL;
+		GSE_LOG("store_cali_value success\n");
+	}
+	else
+	{
+		GSE_ERR("invalid format\n");
+	}
+
+	return count;
+}
+
+static ssize_t bma255_fast_calibration_x_show(struct device_driver *ddri, char *buf)
+{
+	struct i2c_client *client = bma255_i2c_client;
+	struct bma255_i2c_data *bma255 = i2c_get_clientdata(client);
+
+	return sprintf(buf, "%d\n", atomic_read(&bma255->fast_calib_x_rslt));
+}
+
+static ssize_t bma255_fast_calibration_x_store(struct device_driver *ddri, char *buf, size_t count)
+{
+	unsigned long data;
+	signed char tmp;
+	unsigned char timeout = 0;
+	int error;
+	struct i2c_client *client =bma255_i2c_client;
+	struct bma255_i2c_data *bma255 = i2c_get_clientdata(client);
+	test_status = 4;	// calibration status
+
+	if (error = strict_strtoul(buf, 10, &data))
+		return error;
+	atomic_set(&bma255->fast_calib_x_rslt, 0);
+
+    if(bma255_do_calibration() != BMA255_SUCCESS)
+    {
+    	atomic_set(&bma255->fast_calib_x_rslt, 0);
+    	return -EINVAL;
+    }
+
+	if (bma255_set_offset_target(bma255->client, 1, (unsigned char)data) < 0)
+		return -EINVAL;
+
+	if (bma255_set_cal_trigger(bma255->client, 1) < 0)
+		return -EINVAL;
+
+	do{
+		mdelay(2);
+		bma255_get_cal_ready(bma255->client, &tmp);
+
+		GSE_LOG(KERN_INFO "x wait 2ms cal ready flag is %d\n", tmp);
+
+		timeout++;
+		if (timeout == 50)
+		{
+			GSE_LOG(KERN_INFO "get fast calibration ready error\n");
+			return -EINVAL;
+		}
+	} while (tmp == 0);
+
+	atomic_set(&bma255->fast_calib_x_rslt, 1);
+	GSE_LOG(KERN_INFO "x axis fast calibration finished\n");
+
+	return count;
+}
+
+static ssize_t bma255_fast_calibration_y_show(struct device_driver *ddri, char *buf)
+{
+
+	struct i2c_client *client = bma255_i2c_client;
+	struct bma255_i2c_data *bma255 = i2c_get_clientdata(client);
+
+	return sprintf(buf, "%d\n", atomic_read(&bma255->fast_calib_y_rslt));
+}
+
+static ssize_t bma255_fast_calibration_y_store(struct device_driver *ddri, char *buf, size_t count)
+{
+	unsigned long data;
+	signed char tmp;
+	unsigned char timeout = 0;
+	int error;
+	struct i2c_client *client = bma255_i2c_client;
+	struct bma255_i2c_data *bma255 = i2c_get_clientdata(client);
+
+	if (error = strict_strtoul(buf, 10, &data))
+		return error;
+	atomic_set(&bma255->fast_calib_y_rslt, 0);
+
+	if (bma255_set_offset_target(bma255->client, 2, (unsigned char)data) < 0)
+		return -EINVAL;
+
+	if (bma255_set_cal_trigger(bma255->client, 2) < 0)
+		return -EINVAL;
+
+	do {
+		mdelay(2);
+		bma255_get_cal_ready(bma255->client, &tmp);
+
+		GSE_LOG(KERN_INFO "y wait 2ms cal ready flag is %d\n", tmp);
+
+		timeout++;
+		if (timeout == 50)
+		{
+			GSE_LOG(KERN_INFO "get fast calibration ready error\n");
+			return -EINVAL;
+		}
+	} while (tmp == 0);
+
+	atomic_set(&bma255->fast_calib_y_rslt, 1);
+	GSE_LOG(KERN_INFO "y axis fast calibration finished\n");
+
+	return count;
+}
+
+static ssize_t bma255_fast_calibration_z_show(struct device_driver *ddri, char *buf)
+{
+	struct i2c_client *client = bma255_i2c_client;
+	struct bma255_i2c_data *bma255 = i2c_get_clientdata(client);
+
+	return sprintf(buf, "%d\n", atomic_read(&bma255->fast_calib_z_rslt));
+}
+
+static ssize_t bma255_fast_calibration_z_store(struct device_driver *ddri, char *buf, size_t count)
+{
+	unsigned long data;
+	signed char tmp;
+	unsigned char timeout = 0;
+	int error;
+	struct i2c_client *client = bma255_i2c_client;
+	struct bma255_i2c_data *bma255 = i2c_get_clientdata(client);
+
+	if (error = strict_strtoul(buf, 10, &data))
+		return error;
+	atomic_set(&bma255->fast_calib_z_rslt, 0);
+
+	if (bma255_set_offset_target(bma255->client, 3, (unsigned char)data) < 0)
+		return -EINVAL;
+
+	if (bma255_set_cal_trigger(bma255->client, 3) < 0)
+		return -EINVAL;
+
+	do {
+		mdelay(2);
+		bma255_get_cal_ready(bma255->client, &tmp);
+
+		GSE_LOG(KERN_INFO " z wait 2ms cal ready flag is %d\n", tmp);
+
+		timeout++;
+		if (timeout == 50)
+		{
+			GSE_LOG(KERN_INFO "get fast calibration ready error\n");
+			return -EINVAL;
+		}
+	} while (tmp == 0);
+
+	atomic_set(&bma255->fast_calib_z_rslt, 1);
+	GSE_LOG(KERN_INFO "z axis fast calibration finished\n");
+
+	test_status = sensor_power;
+
+	return count;
+}
+
+/* LGE_BSP_COMMON LGE_CHANGE_S 140228 : Calibration for user Apps */
+static int bma255_runCalibration(void)
+{
+	signed char tmp;
+	unsigned char timeout = 0;
+	struct i2c_client *client = bma255_i2c_client;
+	struct bma255_i2c_data *bma255 = i2c_get_clientdata(client);
+	unsigned char backup_offset_x, backup_offset_y, backup_offset_z;
+	int res = 0;
+	int res2 = 0;
+	#if 0 //motion sensor cal backup
+	int cali[3];
+	#else
+	int cali[6];  
+	#endif
+
+	GSE_FUN();
+	if(bma255_get_offset_x(bma255->client, &backup_offset_x) < 0)
+		return FALSE;
+	if(bma255_get_offset_y(bma255->client, &backup_offset_y) < 0)
+		return FALSE;
+	if(bma255_get_offset_z(bma255->client, &backup_offset_z) < 0)
+		return FALSE;
+
+	GSE_LOG("backup_offset_x: %d, backup_offset_y: %d, backup_offset_z: %d\n",backup_offset_x,backup_offset_y,backup_offset_z);
+
+	if(sensor_power == FALSE)
+	{
+		res = BMA255_SetPowerMode(client, true);
+		if(res)
+		{
+			GSE_ERR("Power on bma255 error %d!\n", res);
+			return FALSE;
+		}
+	}
+
+	res2= BMA255_SetDataFormat(client, BMA255_RANGE_2G);
+	if (res2 < 0)//change range before calibration
+	{
+		GSE_ERR("SetDataFormat 2G error");
+	}
+	res = bma255_do_calibration();
+
+	res2 = BMA255_SetDataFormat(client, BMA255_RANGE_4G);
+	if (res2 < 0)//change range before calibration
+	{
+		GSE_ERR("SetDataFormat 4G error");
+	}
+
+	if(res != BMA255_SUCCESS)
+	{
+		GSE_LOG("Fail flatDetection, backup offset\n");
+		if(bma255_set_offset_x(bma255->client, (unsigned char)backup_offset_x) < 0)
+			return -EINVAL;
+		if(bma255_set_offset_y(bma255->client, (unsigned char)backup_offset_y) < 0)
+			return -EINVAL;
+		if(bma255_set_offset_z(bma255->client, (unsigned char)backup_offset_z) < 0)
+			return -EINVAL;
+		GSE_LOG("Recovery backup cal value: x=%d, y=%d, z=%d\n", backup_offset_x, backup_offset_y, backup_offset_z);
+
+		if(res==BMA255_ERR_SETUP_FAILURE)
+			return BMA255_ERR_SETUP_FAILURE;
+		else
+			return BMA255_ERR_STATUS;
+    }
+	else
+	{
+       #if 1 //motion sensor cal backup
+		cali[3] = (int)backup_offset_x;
+		cali[4] = (int)backup_offset_y;
+		cali[5] = (int)backup_offset_z;
+      #endif
+	
+        bma255_get_offset_x(bma255->client, &backup_offset_x);
+        bma255_get_offset_y(bma255->client, &backup_offset_y);
+        bma255_get_offset_z(bma255->client, &backup_offset_z);
+        GSE_LOG("new_offset_x: %d, new_offset_y: %d, new_offset_z: %d\n",(int)backup_offset_x,(int)backup_offset_y,(int)backup_offset_z);
+		cali[0] = (int)backup_offset_x;
+		cali[1] = (int)backup_offset_y;
+		cali[2] = (int)backup_offset_z;
+        #if defined(CALIBRATION_TO_FILE)
+         bma255_calibration_save(cali);
+        #else
+		 if(LGE_FacWriteAccelerometerCalibration((unsigned int*)cali) == TRUE)
+		 {
+		 	atomic_set(&bma255->fast_calib_rslt, 1);
+		 	GSE_LOG("Calibration factory write END\n");
+		 }
+		#endif
+	}
+
+	return BMA255_SUCCESS;
+}
+/* LGE_BSP_COMMON LGE_CHANGE_E 140228 : Calibration for user Apps */
+
+static int bma255_do_calibration(void)
+{
+	signed char tmp;
+	unsigned char timeout = 0;
+	unsigned int timeout_shaking = 0;
+	int sum[3] = {0, };
+	int err = 0;
+	struct i2c_client *client =bma255_i2c_client;
+	struct bma255_i2c_data *bma255 = i2c_get_clientdata(client);
+	struct bma255acc acc_cal;
+	struct bma255acc acc_cal_pre;
+
+	GSE_FUN();
+	test_status = 4;	// calibration status
+   /* set axis off set to zero */
+	if(bma255_set_offset_x(bma255->client, (unsigned char)0) < 0)
+		return -EINVAL;
+    if(bma255_set_offset_y(bma255->client, (unsigned char)0) < 0)
+ 		return -EINVAL;
+   	if(bma255_set_offset_z(bma255->client, (unsigned char)0) < 0)
+		return -EINVAL;
+   /* set axis off set to zero */
+
+	mdelay(20);
+
+	bma255_read_accel_xyz(bma255->client, &acc_cal_pre);
+	do{
+		mdelay(20);
+		bma255_read_accel_xyz(bma255->client, &acc_cal);
+
+		GSE_LOG(KERN_INFO "===============moved x=============== timeout = %d\n",timeout_shaking);
+		GSE_LOG(KERN_INFO "(%d, %d, %d) (%d, %d, %d)\n", acc_cal_pre.x,	acc_cal_pre.y,	acc_cal_pre.z, acc_cal.x,acc_cal.y,acc_cal.z );
+
+		if((abs(acc_cal.x - acc_cal_pre.x) > BMA255_SHAKING_DETECT_THRESHOLD)
+			|| (abs((acc_cal.y - acc_cal_pre.y)) > BMA255_SHAKING_DETECT_THRESHOLD)
+			|| (abs((acc_cal.z - acc_cal_pre.z)) > BMA255_SHAKING_DETECT_THRESHOLD))
+		{
+			atomic_set(&bma255->fast_calib_rslt, 0);
+			GSE_LOG(KERN_INFO "===============shaking x===============\n");
+			return -EINVAL;
+		}
+		else
+		{
+         /* Calibration zero-g offset check */
+            sum[BMA255_AXIS_X] += acc_cal.x;
+            sum[BMA255_AXIS_Y] += acc_cal.y;
+            sum[BMA255_AXIS_Z] += acc_cal.z;
+         /* Calibration zero-g offset check */
+
+			acc_cal_pre.x = acc_cal.x;
+			acc_cal_pre.y = acc_cal.y;
+			acc_cal_pre.z = acc_cal.z;
+		}
+		timeout_shaking++;
+		GSE_LOG(KERN_INFO "===============timeout_shaking: %d=============== \n",timeout_shaking);
+	} while(timeout_shaking < 10);
+
+	GSE_LOG(KERN_INFO "===============complete shaking check===============\n");
+#if 1  /* Calibration zero-g offset check */
+        // check zero-g offset
+        if((abs(sum[BMA255_AXIS_X]/CALIBRATION_DATA_AMOUNT) >TESTLIMIT_XY) ||
+            (abs(sum[BMA255_AXIS_Y]/CALIBRATION_DATA_AMOUNT) >TESTLIMIT_XY) ||
+            ((abs(sum[BMA255_AXIS_Z]/CALIBRATION_DATA_AMOUNT) > TESTLIMIT_Z_USL_LSB) || (abs(sum[BMA255_AXIS_Z]/CALIBRATION_DATA_AMOUNT) < TESTLIMIT_Z_LSL_LSB)))
+        {
+            GSE_LOG("Calibration zero-g offset check failed (%d, %d, %d)\n", sum[BMA255_AXIS_X]/CALIBRATION_DATA_AMOUNT,
+                                                                    sum[BMA255_AXIS_Y]/CALIBRATION_DATA_AMOUNT, sum[BMA255_AXIS_Z]/CALIBRATION_DATA_AMOUNT);
+			atomic_set(&bma255->fast_calib_rslt, 0);
+			return BMA255_ERR_SETUP_FAILURE;
+        }
+#endif  /* Calibration zero-g offset check */
+
+	GSE_LOG(KERN_INFO "===============complete zero-g check===============\n");
+
+	atomic_set(&bma255->fast_calib_x_rslt, 0);
+	if (bma255_set_offset_target(bma255->client, 1, (unsigned char)0) < 0)
+		return -EINVAL;
+	if (bma255_set_cal_trigger(bma255->client, 1) < 0)
+		return -EINVAL;
+	do{
+		mdelay(2);
+		bma255_get_cal_ready(bma255->client, &tmp);
+
+		GSE_LOG(KERN_INFO "x wait 2ms cal ready flag is %d\n", tmp);
+
+		timeout++;
+		if (timeout == 50)
+		{
+			GSE_LOG(KERN_INFO "get fast calibration ready error\n");
+			return -EINVAL;
+		}
+	} while (tmp == 0);
+	atomic_set(&bma255->fast_calib_x_rslt, 1);
+	GSE_LOG(KERN_INFO "===============x axis fast calibration finished\n");
+
+	atomic_set(&bma255->fast_calib_y_rslt, 0);
+	if (bma255_set_offset_target(bma255->client, 2, (unsigned char)0) < 0)
+		return -EINVAL;
+	if (bma255_set_cal_trigger(bma255->client, 2) < 0)
+		return -EINVAL;
+	do {
+		mdelay(2);
+		bma255_get_cal_ready(bma255->client, &tmp);
+
+		GSE_LOG(KERN_INFO "y wait 2ms cal ready flag is %d\n", tmp);
+
+		timeout++;
+		if (timeout == 50)
+		{
+			GSE_LOG(KERN_INFO "get fast calibration ready error\n");
+			return -EINVAL;
+		}
+	} while (tmp == 0);
+	atomic_set(&bma255->fast_calib_y_rslt, 1);
+	GSE_LOG(KERN_INFO "===============y axis fast calibration finished\n");
+
+	atomic_set(&bma255->fast_calib_z_rslt, 0);
+	if (bma255_set_offset_target(bma255->client, 3, (unsigned char)2) < 0)
+		return -EINVAL;
+	if (bma255_set_cal_trigger(bma255->client, 3) < 0)
+		return -EINVAL;
+	do {
+		mdelay(2);
+		bma255_get_cal_ready(bma255->client, &tmp);
+
+		GSE_LOG(KERN_INFO " z wait 2ms cal ready flag is %d\n", tmp);
+
+		timeout++;
+		if (timeout == 50)
+		{
+			GSE_LOG(KERN_INFO "get fast calibration ready error\n");
+			return -EINVAL;
+		}
+	} while (tmp == 0);
+	atomic_set(&bma255->fast_calib_z_rslt, 1);
+	GSE_LOG(KERN_INFO "===============z axis fast calibration finished\n");
+
+
+	test_status = sensor_power;
+
+	return err;
+}
+
+#endif
+
+static int bma255_get_selftest(struct i2c_client *client)
+{
+	int value = 0;
+    struct bma255_i2c_data *obj = i2c_get_clientdata(client);
+
+	return value;
+}
+
+static ssize_t show_chipinfo_value(struct device_driver *ddri, char *buf)
+{
+	struct i2c_client *client = bma255_i2c_client;
+	char strbuf[BMA255_BUFSIZE]={0,};
+	if(NULL == client)
+	{
+		GSE_ERR("i2c client is null!!\n");
+		return 0;
+	}
+
+	BMA255_ReadChipInfo(client, strbuf, BMA255_BUFSIZE);
+	return snprintf(buf, PAGE_SIZE, "%s\n", strbuf);
+}
+
+static ssize_t gsensor_init(struct device_driver *ddri, char *buf, size_t count)
+{
+	struct i2c_client *client = bma255_i2c_client;
+	char strbuf[BMA255_BUFSIZE]={0,};
+
+	if(NULL == client)
+	{
+		GSE_ERR("i2c client is null!!\n");
+		return 0;
+	}
+	bma255_init_client(client, 1);
+	return snprintf(buf, PAGE_SIZE, "%s\n", strbuf);
+}
+
+/*----------------------------------------------------------------------------*/
+/* g sensor opmode for compass tilt compensation
+ */
+static ssize_t show_cpsopmode_value(struct device_driver *ddri, char *buf)
+{
+	unsigned char data;
+
+	if (bma255_get_mode(bma255_i2c_client, &data) < 0)
+	{
+		return sprintf(buf, "Read error\n");
+	}
+	else
+	{
+		return sprintf(buf, "%d\n", data);
+	}
+}
+
+/*----------------------------------------------------------------------------*/
+/* g sensor opmode for compass tilt compensation
+ */
+static ssize_t store_cpsopmode_value(struct device_driver *ddri, char *buf, size_t count)
+{
+	unsigned long data;
+	int error;
+
+	if (error = strict_strtoul(buf, 10, &data))
+	{
+		return error;
+	}
+	if (data == BMA255_MODE_NORMAL)
+	{
+		BMA255_SetPowerMode(bma255_i2c_client, true);
+	}
+	else if (data == BMA255_MODE_SUSPEND)
+	{
+		BMA255_SetPowerMode(bma255_i2c_client, false);
+	}
+	else if (bma255_set_mode(bma255_i2c_client, (unsigned char) data) < 0)
+	{
+		GSE_ERR("invalid content: '%s', length = %d\n", buf, count);
+	}
+
+	return count;
+}
+
+/*----------------------------------------------------------------------------*/
+/* g sensor range for compass tilt compensation
+ */
+static ssize_t show_cpsrange_value(struct device_driver *ddri, char *buf)
+{
+	unsigned char data;
+
+	if (bma255_get_range(bma255_i2c_client, &data) < 0)
+	{
+		return sprintf(buf, "Read error\n");
+	}
+	else
+	{
+		return sprintf(buf, "%d\n", data);
+	}
+}
+
+/*----------------------------------------------------------------------------*/
+/* g sensor range for compass tilt compensation
+ */
+static ssize_t store_cpsrange_value(struct device_driver *ddri, char *buf, size_t count)
+{
+	unsigned long data;
+	int error;
+
+	if (error = strict_strtoul(buf, 10, &data))
+	{
+		return error;
+	}
+	if (bma255_set_range(bma255_i2c_client, (unsigned char) data) < 0)
+	{
+		GSE_ERR("invalid content: '%s', length = %d\n", buf, count);
+	}
+
+	return count;
+}
+/*----------------------------------------------------------------------------*/
+/* g sensor bandwidth for compass tilt compensation
+ */
+static ssize_t show_cpsbandwidth_value(struct device_driver *ddri, char *buf)
+{
+	unsigned char data;
+
+	if (bma255_get_bandwidth(bma255_i2c_client, &data) < 0)
+	{
+		return sprintf(buf, "Read error\n");
+	}
+	else
+	{
+		return sprintf(buf, "%d\n", data);
+	}
+}
+
+/*----------------------------------------------------------------------------*/
+/* g sensor bandwidth for compass tilt compensation
+ */
+static ssize_t store_cpsbandwidth_value(struct device_driver *ddri, char *buf, size_t count)
+{
+	unsigned long data;
+	int error;
+
+	if (error = strict_strtoul(buf, 10, &data))
+	{
+		return error;
+	}
+	if (bma255_set_bandwidth(bma255_i2c_client, (unsigned char) data) < 0)
+	{
+		GSE_ERR("invalid content: '%s', length = %d\n", buf, count);
+	}
+
+	return count;
+}
+//jwseo_test
+#if 0
+static ssize_t show_limit_value(struct device_driver *ddri, char *buf)
+{
+	return sprintf ( buf, "%d\n", (int)TESTLIMIT_XY);
+}
+
+/*----------------------------------------------------------------------------*/
+/* g sensor range for compass tilt compensation
+ */
+static ssize_t store_limit_value(struct device_driver *ddri, char *buf, size_t count)
+{
+	unsigned long data;
+	int error;
+
+	if (error = strict_strtoul(buf, 10, &data))
+		return error;
+	
+	TESTLIMIT_XY = (int)data;
+
+	return count;
+}
+#endif
+
+/*----------------------------------------------------------------------------*/
+/* g sensor data for compass tilt compensation
+ */
+static ssize_t show_cpsdata_value(struct device_driver *ddri, char *buf)
+{
+	struct i2c_client *client = bma255_i2c_client;
+	char strbuf[BMA255_BUFSIZE]={0,};
+
+	if(NULL == client)
+	{
+		GSE_ERR("i2c client is null!!\n");
+		return 0;
+	}
+	BMA255_CompassReadData(client, strbuf, BMA255_BUFSIZE);
+	return snprintf(buf, PAGE_SIZE, "%s\n", strbuf);
+}
+
+/*----------------------------------------------------------------------------*/
+static ssize_t show_sensordata_value(struct device_driver *ddri, char *buf)
+{
+	struct i2c_client *client = bma255_i2c_client;
+	char strbuf[BMA255_BUFSIZE]={0,};
+
+	if(NULL == client)
+	{
+		GSE_ERR("i2c client is null!!\n");
+		return 0;
+	}
+	BMA255_ReadSensorData(client, strbuf, BMA255_BUFSIZE);
+	return snprintf(buf, PAGE_SIZE, "%s\n", strbuf);
+}
+
+static ssize_t show_sensorrawdata_value(struct device_driver *ddri, char *buf, size_t count)
+{
+	struct i2c_client *client = bma255_i2c_client;
+	char strbuf[BMA255_BUFSIZE]={0,};
+
+	if(NULL == client)
+	{
+		GSE_ERR("i2c client is null!!\n");
+		return 0;
+	}
+	BMA255_ReadRawData(client, strbuf);
+	return snprintf(buf, PAGE_SIZE, "%s\n", strbuf);
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t show_firlen_value(struct device_driver *ddri, char *buf)
+{
+#ifdef CONFIG_BMA255_LOWPASS
+	struct i2c_client *client = bma255_i2c_client;
+	struct bma255_i2c_data *obj = i2c_get_clientdata(client);
+	if(atomic_read(&obj->firlen))
+	{
+		int idx, len = atomic_read(&obj->firlen);
+		GSE_LOG("len = %2d, idx = %2d\n", obj->fir.num, obj->fir.idx);
+
+		for(idx = 0; idx < len; idx++)
+		{
+			GSE_LOG("[%5d %5d %5d]\n", obj->fir.raw[idx][BMA255_AXIS_X], obj->fir.raw[idx][BMA255_AXIS_Y], obj->fir.raw[idx][BMA255_AXIS_Z]);
+		}
+
+		GSE_LOG("sum = [%5d %5d %5d]\n", obj->fir.sum[BMA255_AXIS_X], obj->fir.sum[BMA255_AXIS_Y], obj->fir.sum[BMA255_AXIS_Z]);
+		GSE_LOG("avg = [%5d %5d %5d]\n", obj->fir.sum[BMA255_AXIS_X]/len, obj->fir.sum[BMA255_AXIS_Y]/len, obj->fir.sum[BMA255_AXIS_Z]/len);
+	}
+	return snprintf(buf, PAGE_SIZE, "%d\n", atomic_read(&obj->firlen));
+#else
+	return snprintf(buf, PAGE_SIZE, "not support\n");
+#endif
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t store_firlen_value(struct device_driver *ddri, char *buf, size_t count)
+{
+#ifdef CONFIG_BMA255_LOWPASS
+	struct i2c_client *client = bma255_i2c_client;
+	struct bma255_i2c_data *obj = i2c_get_clientdata(client);
+	int firlen;
+
+	if(1 != sscanf(buf, "%d", &firlen))
+	{
+		GSE_ERR("invallid format\n");
+	}
+	else if(firlen > C_MAX_FIR_LENGTH)
+	{
+		GSE_ERR("exceeds maximum filter length\n");
+	}
+	else
+	{
+		atomic_set(&obj->firlen, firlen);
+		if(NULL == firlen)
+		{
+			atomic_set(&obj->fir_en, 0);
+		}
+		else
+		{
+			memset(&obj->fir, 0x00, sizeof(obj->fir));
+			atomic_set(&obj->fir_en, 1);
+		}
+	}
+#endif
+	return count;
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t show_trace_value(struct device_driver *ddri, char *buf)
+{
+	ssize_t res;
+	struct bma255_i2c_data *obj = obj_i2c_data;
+	if (obj == NULL)
+	{
+		GSE_ERR("i2c_data obj is null!!\n");
+		return 0;
+	}
+
+	res = snprintf(buf, PAGE_SIZE, "0x%04X\n", atomic_read(&obj->trace));
+	return res;
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t store_trace_value(struct device_driver *ddri, char *buf, size_t count)
+{
+	struct bma255_i2c_data *obj = obj_i2c_data;
+	int trace;
+	if (obj == NULL)
+	{
+		GSE_ERR("i2c_data obj is null!!\n");
+		return 0;
+	}
+
+	if(1 == sscanf(buf, "0x%x", &trace))
+	{
+		atomic_set(&obj->trace, trace);
+	}
+	else
+	{
+		GSE_ERR("invalid content: '%s', length = %d\n", buf, count);
+	}
+
+	return count;
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t show_status_value(struct device_driver *ddri, char *buf)
+{
+	ssize_t len = 0;
+	struct bma255_i2c_data *obj = obj_i2c_data;
+	if (obj == NULL)
+	{
+		GSE_ERR("i2c_data obj is null!!\n");
+		return 0;
+	}
+
+	if(obj->hw)
+	{
+		len += snprintf(buf+len, PAGE_SIZE-len, "CUST: %d %d (%d %d)\n",
+	            obj->hw->i2c_num, obj->hw->direction, obj->hw->power_id, obj->hw->power_vol);
+	}
+	else
+	{
+		len += snprintf(buf+len, PAGE_SIZE-len, "CUST: NULL\n");
+	}
+	return len;
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t show_power_status_value(struct device_driver *ddri, char *buf)
+{
+	if(sensor_power)
+		GSE_LOG("G sensor is in work mode, sensor_power = %d\n", sensor_power);
+	else
+		GSE_LOG("G sensor is in standby mode, sensor_power = %d\n", sensor_power);
+
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t show_teststatus_value(struct device_driver *ddri, char *buf)
+{
+	struct i2c_client *client = bma255_i2c_client;
+
+	if(client == NULL)
+	{
+        GSE_ERR("i2c client is null!!\n");
+		return 0;
+	}
+
+	return snprintf(buf, PAGE_SIZE, "%d\n", test_status);
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t bma255_bandwidth_show(struct device_driver *ddri, char *buf)
+{
+	unsigned char data;
+	struct i2c_client *client = bma255_i2c_client;
+	struct bma255_i2c_data *bma255 = i2c_get_clientdata(client);
+
+	if (bma255_get_bandwidth(bma255->client, &data) < 0)
+		return sprintf(buf, "Read error\n");
+
+	return sprintf(buf, "%d\n", data);
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t bma255_bandwidth_store(struct device_driver *ddri, char *buf, size_t count)
+{
+	unsigned long data;
+	int error;
+	struct i2c_client *client = bma255_i2c_client;
+	struct bma255_i2c_data *bma255 = i2c_get_clientdata(client);
+
+	if (error = strict_strtoul(buf, 10, &data))
+		return error;
+
+	if (bma255_set_bandwidth(bma255->client, (unsigned char) data) < 0)
+		return -EINVAL;
+
+	return count;
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t bma255_eeprom_writing_show(struct device_driver *ddri, char *buf)
+{
+	struct i2c_client *client = bma255_i2c_client;
+	struct bma255_i2c_data *bma255 = i2c_get_clientdata(client);
+
+	return sprintf(buf, "%d\n", atomic_read(&bma255->fast_calib_rslt));
+}
+
+static ssize_t bma255_eeprom_writing_store(struct device_driver *ddri, char *buf, size_t count)
+{
+	unsigned char offset_x, offset_y, offset_z;
+	struct i2c_client *client = bma255_i2c_client;
+	struct bma255_i2c_data *bma255 = i2c_get_clientdata(client);
+
+	if(bma255_get_offset_x(bma255->client, &offset_x) < 0)
+		return -EINVAL;
+	if(bma255_get_offset_y(bma255->client, &offset_y) < 0)
+		return -EINVAL;
+	if(bma255_get_offset_z(bma255->client, &offset_z) < 0)
+		return -EINVAL;
+
+	atomic_set(&bma255->fast_calib_rslt, 1);
+
+	return count;
+}
+
+static int bma255_soft_reset(struct i2c_client *client)
+{
+	int comres = 0;
+	unsigned char data = BMA255_EN_SOFT_RESET_VALUE;
+
+	comres = bma255_smbus_write_byte(client, BMA255_EN_SOFT_RESET__REG, &data);
+
+	return comres;
+}
+static ssize_t bma255_softreset_store(struct device *dev, struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct i2c_client *client = bma255_i2c_client;
+	struct bma255_i2c_data *bma255 = i2c_get_clientdata(client);
+
+	if (bma255_soft_reset(bma255->client) < 0)
+		return -EINVAL;
+
+	return count;
+}
+/*----------------------------------------------------------------------------*/
+/* LGE_BSP_COMMON LGE_CHANGE_S 140228 : Calibration for user Apps */
+static int selfCalibration = 1;
+static ssize_t bma255_runCalibration_store(struct device *dev, struct device_attribute *attr, const char *buf, size_t count)
+{
+	int res = 0;
+	res = bma255_runCalibration();
+	if(res == BMA255_SUCCESS)
+	{
+		selfCalibration = BMA255_SUCCESS;
+	}
+	else if(res == BMA255_ERR_SETUP_FAILURE)
+	{
+		selfCalibration = 2;  // cal fail(flat)
+	}
+	else
+	{
+		selfCalibration = 1;  // cal fail
+	}
+
+	return count;
+}
+
+static ssize_t bma255_runCalibration_show(struct device_driver *ddri, char *buf)
+{
+	return sprintf(buf, "%d\n", selfCalibration);
+}
+/* LGE_BSP_COMMON LGE_CHANGE_E 140228 : Calibration for user Apps */
+
+/*----------------------------------------------------------------------------*/
+/* for self test */
+static int bma255_set_selftest_st(struct i2c_client *client, unsigned char
+		selftest)
+{
+	int comres = 0;
+	unsigned char data;
+
+	comres = bma255_smbus_read_byte_block(client, BMA255_EN_SELF_TEST__REG,
+			&data, 1);
+	data = BMA255_SET_BITSLICE(data, BMA255_EN_SELF_TEST, selftest);
+	comres = bma255_smbus_write_byte(client, BMA255_EN_SELF_TEST__REG,
+			&data);
+	//comres = i2c_master_send(client, &data, 1);
+	GSE_LOG("selftest_st comres : %d\n",comres);
+	return comres;
+}
+
+static int bma255_set_selftest_stn(struct i2c_client *client, unsigned char stn)
+{
+	int comres = 0;
+	unsigned char data;
+
+	comres = bma255_smbus_read_byte_block(client, BMA255_NEG_SELF_TEST__REG,
+			&data, 1);
+	data = BMA255_SET_BITSLICE(data, BMA255_NEG_SELF_TEST, stn);
+	comres = bma255_smbus_write_byte(client, BMA255_NEG_SELF_TEST__REG,
+			&data);
+	//comres = i2c_master_send(client, &data, 1);
+	GSE_LOG("selftest_stN comres : %d\n",comres);
+	return comres;
+}
+
+/*
+Read:
+0 ------ success
+1 ------ x  axis failed
+2 ------ y  axis failed
+3 ------ x and y axis failed
+4 ------ z  axis failed
+5 ------ x and z axis failed
+6 ------ y and z axis failed
+7 ------ x, y and z axis failed
+*/
+static ssize_t bma255_SelfTest_store(struct device_driver *ddri, char *buf, size_t count)
+{
+	unsigned long data = 0;
+    int error = -1;
+	struct i2c_client *client = bma255_i2c_client;
+    struct bma255_i2c_data *obj = i2c_get_clientdata(client);
+	enum BMA_RANGE_ENUM range;
+	bool power_mode;
+	u8 bandwidth = 0;
+	u8 value = 0;
+	u8 amp = 0;
+	s16 value1[BMA255_AXES_NUM];
+	s16 value2[BMA255_AXES_NUM];
+	s16 diff = 0;
+	u8 result = 0;
+
+    if (error = strict_strtoul(buf, 10, &data))
+    {
+        return error;
+    }
+    GSE_LOG("Self test CMD value : %d\n", (int)data);
+
+    if(data == 1)// self test start command
+    {
+		/*backup settings*/
+		range = obj->range;
+		power_mode = sensor_power;
+		bandwidth = obj->bandwidth;
+		/*Step 1:Soft Reset*/
+		bma255_soft_reset(obj->client);
+		/*Step 2:Clear Selftest Register*/
+		{
+			value = 0;
+			bma255_smbus_write_byte(obj->client, BMA255_SELF_TEST_REG,&value);
+		}
+
+		/*Step 3:Set to +/-4G Range*/
+		BMA255_SetDataFormat(obj->client, BMA255_RANGE_4G);
+		//bma255_set_range(obj->client, BMA_RANGE_4G);
+		/*Step 4:Set Amplitude of Deflection*/
+		{
+			value = 0;
+			amp = 1;
+			bma255_smbus_read_byte(obj->client, BMA255_SELF_TEST_AMP__REG,&value);
+			value = BMA255_SET_BITSLICE(value, BMA255_SELF_TEST_AMP, amp);
+			bma255_smbus_write_byte(obj->client, BMA255_SELF_TEST_AMP__REG,&value);
+		}
+
+		/*Step 5:X-axis Selftest*/
+		bma255_set_selftest_st(obj->client, 1);/*1 for x-axis*/
+		bma255_set_selftest_stn(obj->client, 0);/*positive direction*/
+		mdelay(10);
+		if(error = BMA255_ReadData(client, obj->data))
+		{
+			GSE_ERR("I2C error: ret value=%d", error);
+			return EIO;
+		}
+		else
+		{
+			value1[BMA255_AXIS_X] = obj->data[BMA255_AXIS_X];
+			value1[BMA255_AXIS_Y] = obj->data[BMA255_AXIS_Y];
+			value1[BMA255_AXIS_Z] = obj->data[BMA255_AXIS_Z];
+		}
+		bma255_set_selftest_stn(obj->client, 1);/*negative direction*/
+		mdelay(10);
+		if(error = BMA255_ReadData(client, obj->data))
+		{
+			GSE_ERR("I2C error: ret value=%d", error);
+			return EIO;
+		}
+		else
+		{
+			value2[BMA255_AXIS_X] = obj->data[BMA255_AXIS_X];
+			value2[BMA255_AXIS_Y] = obj->data[BMA255_AXIS_Y];
+			value2[BMA255_AXIS_Z] = obj->data[BMA255_AXIS_Z];
+		}
+		diff = value1[BMA255_AXIS_X]-value2[BMA255_AXIS_X];
+
+		GSE_LOG("diff x is %d,value1 is %d, value2 is %d\n", diff,
+				value1[BMA255_AXIS_X], value2[BMA255_AXIS_X]);
+
+		if (abs(diff) < 204)
+			result |= 1;
+
+		/*Step 6:Y-axis Selftest*/
+		bma255_set_selftest_st(obj->client, 2);/*2 for y-axis*/
+		bma255_set_selftest_stn(obj->client, 0);/*positive direction*/
+		mdelay(10);
+		if(error = BMA255_ReadData(client, obj->data))
+		{
+			GSE_ERR("I2C error: ret value=%d", error);
+			return EIO;
+		}
+		else
+		{
+			value1[BMA255_AXIS_X] = obj->data[BMA255_AXIS_X];
+			value1[BMA255_AXIS_Y] = obj->data[BMA255_AXIS_Y];
+			value1[BMA255_AXIS_Z] = obj->data[BMA255_AXIS_Z];
+		}
+		bma255_set_selftest_stn(obj->client, 1);/*negative direction*/
+		mdelay(10);
+		if(error = BMA255_ReadData(client, obj->data))
+		{
+			GSE_ERR("I2C error: ret value=%d", error);
+			return EIO;
+		}
+		else
+		{
+			value2[BMA255_AXIS_X] = obj->data[BMA255_AXIS_X];
+			value2[BMA255_AXIS_Y] = obj->data[BMA255_AXIS_Y];
+			value2[BMA255_AXIS_Z] = obj->data[BMA255_AXIS_Z];
+		}
+		diff = value1[BMA255_AXIS_Y]-value2[BMA255_AXIS_Y];
+		GSE_LOG("diff y is %d,value1 is %d, value2 is %d\n", diff,
+				value1[BMA255_AXIS_Y], value2[BMA255_AXIS_Y]);
+
+		if (abs(diff) < 204)
+			result |= 2;
+
+		/*Step 7:Z-axis Selftest*/
+		bma255_set_selftest_st(obj->client, 3);/*3 for z-axis*/
+		bma255_set_selftest_stn(obj->client, 0);/*positive direction*/
+		mdelay(10);
+		if(error = BMA255_ReadData(client, obj->data))
+		{
+			GSE_ERR("I2C error: ret value=%d", error);
+			return EIO;
+		}
+		else
+		{
+			value1[BMA255_AXIS_X] = obj->data[BMA255_AXIS_X];
+			value1[BMA255_AXIS_Y] = obj->data[BMA255_AXIS_Y];
+			value1[BMA255_AXIS_Z] = obj->data[BMA255_AXIS_Z];
+		}
+		bma255_set_selftest_stn(obj->client, 1);/*negative direction*/
+		mdelay(10);
+		if(error = BMA255_ReadData(client, obj->data))
+		{
+			GSE_ERR("I2C error: ret value=%d", error);
+			return EIO;
+		}
+		else
+		{
+			value2[BMA255_AXIS_X] = obj->data[BMA255_AXIS_X];
+			value2[BMA255_AXIS_Y] = obj->data[BMA255_AXIS_Y];
+			value2[BMA255_AXIS_Z] = obj->data[BMA255_AXIS_Z];
+		}
+		diff = value1[BMA255_AXIS_Z]-value2[BMA255_AXIS_Z];
+
+		GSE_LOG("diff z is %d,value1 is %d, value2 is %d\n", diff,
+				value1[BMA255_AXIS_Z], value2[BMA255_AXIS_Z]);
+
+		if (abs(diff) < 204)
+			result |= 4;
+
+		/*Step 8:Soft Reset*/
+		bma255_soft_reset(obj->client);
+		/*Sync sw settings to hw settings*/
+		obj->range = range;//fix for 4G
+		obj->bandwidth= bandwidth;
+
+		atomic_set(&obj->selftest, result);;
+
+		/*restore settings*/
+		BMA255_SetDataFormat(obj->client, range);
+		//bma255_set_range(obj->client, range);
+		bma255_set_bandwidth(obj->client, bandwidth);
+		BMA255_SetPowerMode(obj->client, power_mode);
+		GSE_LOG("self test finished. result : %d\n",result);
+    }
+	else // wrong input
+	{
+    	GSE_LOG("SelfTest command FAIL\n");
+    	return -EINVAL;
+	}
+	return count;
+}
+
+static ssize_t bma255_SelfTest_show(struct device_driver *ddri, char *buf)
+{
+    int selftest_rslt = 1;  // fail
+
+    struct i2c_client *client = bma255_i2c_client;
+    struct bma255_i2c_data *obj = i2c_get_clientdata(client);
+	if (obj == NULL) {
+		GSE_ERR("bma i2c data pointer is null\n");
+		return 0;
+	}
+
+//    if(atomic_read(&obj->selftest) == 1)  // selftest success
+	if(atomic_read(&obj->selftest) == 0)  // selftest success
+    {
+        selftest_rslt = 0;  // success
+    }
+    else
+    {
+    	GSE_LOG("Self Test Fail : %d\n",atomic_read(&obj->selftest));
+        selftest_rslt = 1;  // fail
+    }
+	return sprintf(buf, "%d\n", selftest_rslt);
+}
+/*----------------------------------------------------------------------------*/
+static DRIVER_ATTR(chipinfo,   S_IWUSR | S_IRUGO, show_chipinfo_value,   NULL);
+static DRIVER_ATTR(cpsdata,    S_IWUSR | S_IRUGO, show_cpsdata_value,    NULL);
+static DRIVER_ATTR(cpsopmode, S_IWUSR|S_IRUGO|S_IWGRP, show_cpsopmode_value, store_cpsopmode_value);
+static DRIVER_ATTR(cpsrange, S_IWUSR|S_IRUGO|S_IWGRP, show_cpsrange_value, store_cpsrange_value);
+static DRIVER_ATTR(cpsbandwidth, S_IWUSR|S_IRUGO|S_IWGRP, show_cpsbandwidth_value, store_cpsbandwidth_value);
+static DRIVER_ATTR(sensordata, S_IWUSR | S_IRUGO, show_sensordata_value,    NULL);
+static DRIVER_ATTR(cali, S_IWUSR|S_IRUGO|S_IWGRP, show_cali_value, store_cali_value);
+static DRIVER_ATTR(firlen, S_IWUSR|S_IRUGO|S_IWGRP, show_firlen_value, store_firlen_value);
+static DRIVER_ATTR(trace, S_IWUSR|S_IRUGO|S_IWGRP, show_trace_value, store_trace_value);
+static DRIVER_ATTR(status,     S_IRUGO, show_status_value,        NULL);
+static DRIVER_ATTR(powerstatus, S_IRUGO, show_power_status_value, NULL);
+//LGE_CHANGE_S : 2012-12-07 taebum81.kim@lge.com AT%SURV : (4)
+static DRIVER_ATTR(softreset, S_IWUSR|S_IWGRP, NULL, bma255_softreset_store);
+static DRIVER_ATTR(teststatus, S_IWUSR|S_IRUGO, show_teststatus_value, NULL);
+static DRIVER_ATTR(fast_calibration_x, S_IRUGO|S_IWUSR|S_IWGRP, bma255_fast_calibration_x_show, bma255_fast_calibration_x_store);
+static DRIVER_ATTR(fast_calibration_y, S_IRUGO|S_IWUSR|S_IWGRP, bma255_fast_calibration_y_show, bma255_fast_calibration_y_store);
+static DRIVER_ATTR(fast_calibration_z, S_IRUGO|S_IWUSR|S_IWGRP, bma255_fast_calibration_z_show, bma255_fast_calibration_z_store);
+static DRIVER_ATTR(run_fast_calibration, S_IRUGO|S_IWUSR|S_IWGRP, bma255_runCalibration_show, bma255_runCalibration_store);
+static DRIVER_ATTR(eeprom_writing, S_IRUGO|S_IWUSR|S_IWGRP, bma255_eeprom_writing_show, bma255_eeprom_writing_store);
+static DRIVER_ATTR(bandwidth, S_IRUGO|S_IWUSR|S_IWGRP, bma255_bandwidth_show, bma255_bandwidth_store);
+static DRIVER_ATTR(selftest, S_IRUGO|S_IWUSR|S_IWGRP, bma255_SelfTest_show, bma255_SelfTest_store);
+#if 1 //motion sensor cal backup
+static DRIVER_ATTR(cali_backup, S_IWUSR|S_IRUGO|S_IWGRP, show_cali_backup_value, NULL);
+#endif
+#if 0
+static DRIVER_ATTR(limit, S_IWUSR|S_IRUGO|S_IWGRP, show_limit_value, store_limit_value); //jwseo_test
+#endif
+//LGE_CHANGE_E : 2012-12-07 taebum81.kim@lge.com AT%SURV : (4)
+/*----------------------------------------------------------------------------*/
+static struct driver_attribute *bma255_attr_list[] = {
+	&driver_attr_chipinfo,     /*chip information*/
+	&driver_attr_sensordata,   /*dump sensor data*/
+	&driver_attr_cali,         /*show calibration data*/
+	&driver_attr_firlen,       /*filter length: 0: disable, others: enable*/
+	&driver_attr_trace,        /*trace log*/
+	&driver_attr_status,
+	&driver_attr_powerstatus,
+	&driver_attr_cpsdata,	/*g sensor data for compass tilt compensation*/
+	&driver_attr_cpsopmode,	/*g sensor opmode for compass tilt compensation*/
+	&driver_attr_cpsrange,	/*g sensor range for compass tilt compensation*/
+	&driver_attr_cpsbandwidth,	/*g sensor bandwidth for compass tilt compensation*/
+	&driver_attr_bandwidth,
+	&driver_attr_softreset,
+	&driver_attr_teststatus,
+	&driver_attr_fast_calibration_x,
+	&driver_attr_fast_calibration_y,
+	&driver_attr_fast_calibration_z,
+	&driver_attr_run_fast_calibration,
+	&driver_attr_eeprom_writing,
+    &driver_attr_selftest,
+#if 1 //motion sensor cal backup
+	&driver_attr_cali_backup,         /*show calibration backup data*/
+#endif
+#if 0
+    &driver_attr_limit,  //jwseo_test
+#endif
+};
+/*----------------------------------------------------------------------------*/
+static int bma255_create_attr(struct device_driver *driver)
+{
+	int idx, err = 0;
+	int num = (int)(sizeof(bma255_attr_list)/sizeof(bma255_attr_list[0]));
+	if (driver == NULL)
+	{
+		return -EINVAL;
+	}
+
+	for(idx = 0; idx < num; idx++)
+	{
+		if(err = driver_create_file(driver, bma255_attr_list[idx]))
+		{
+			GSE_ERR("driver_create_file (%s) = %d\n", bma255_attr_list[idx]->attr.name, err);
+			break;
+		}
+	}
+	return err;
+}
+/*----------------------------------------------------------------------------*/
+static int bma255_delete_attr(struct device_driver *driver)
+{
+	int idx ,err = 0;
+	int num = (int)(sizeof(bma255_attr_list)/sizeof(bma255_attr_list[0]));
+
+	if(driver == NULL)
+	{
+		return -EINVAL;
+	}
+
+	for(idx = 0; idx < num; idx++)
+	{
+		driver_remove_file(driver, bma255_attr_list[idx]);
+	}
+
+	return err;
+}
+
+/*----------------------------------------------------------------------------*/
+int gsensor_operate(void* self, uint32_t command, void* buff_in, int size_in,
+		void* buff_out, int size_out, int* actualout)
+{
+	int err = 0;
+	int value, sample_delay;
+	struct bma255_i2c_data *priv = (struct bma255_i2c_data*)self;
+	hwm_sensor_data* gsensor_data;
+	char buff[BMA255_BUFSIZE];
+
+	switch (command)
+	{
+		case SENSOR_DELAY:
+			if((buff_in == NULL) || (size_in < sizeof(int)))
+			{
+				GSE_ERR("Set delay parameter error!\n");
+				err = -EINVAL;
+			}
+			else
+			{
+				value = *(int *)buff_in;
+				if(value <= 5)
+				{
+					sample_delay = BMA255_BW_200HZ;
+				}
+				else if(value <= 10)
+				{
+					sample_delay = BMA255_BW_100HZ;
+				}
+				else
+				{
+					sample_delay = BMA255_BW_50HZ;
+				}
+
+				err = BMA255_SetBWRate(priv->client, sample_delay);
+				if(err != BMA255_SUCCESS ) //0x2C->BW=100Hz
+				{
+					GSE_ERR("Set delay parameter error!\n");
+				}
+
+				if(value >= 50)
+				{
+					atomic_set(&priv->filter, 0);
+				}
+				else
+				{
+				#if defined(CONFIG_BMA255_LOWPASS)
+					priv->fir.num = 0;
+					priv->fir.idx = 0;
+					priv->fir.sum[BMA255_AXIS_X] = 0;
+					priv->fir.sum[BMA255_AXIS_Y] = 0;
+					priv->fir.sum[BMA255_AXIS_Z] = 0;
+					atomic_set(&priv->filter, 1);
+				#endif
+				}
+			}
+			break;
+
+		case SENSOR_ENABLE:
+			if((buff_in == NULL) || (size_in < sizeof(int)))
+			{
+				GSE_ERR("Enable sensor parameter error!\n");
+				err = -EINVAL;
+			}
+			else
+			{
+				value = *(int *)buff_in;
+				if(((value == 0) && (sensor_power == false)) ||((value == 1) && (sensor_power == true)))
+				{
+					GSE_LOG("Gsensor device have updated!, power: %d\n", sensor_power);
+				}
+				else
+				{
+					err = BMA255_SetPowerMode( priv->client, !sensor_power);
+				}
+			}
+			break;
+
+		case SENSOR_GET_DATA:
+			if((buff_out == NULL) || (size_out< sizeof(hwm_sensor_data)))
+			{
+				GSE_ERR("get sensor data parameter error!\n");
+				err = -EINVAL;
+			}
+			else
+			{
+				gsensor_data = (hwm_sensor_data *)buff_out;
+				BMA255_ReadSensorData(priv->client, buff, BMA255_BUFSIZE);
+				sscanf(buff, "%x %x %x", &gsensor_data->values[0],
+					&gsensor_data->values[1], &gsensor_data->values[2]);
+				gsensor_data->status = SENSOR_STATUS_ACCURACY_MEDIUM;
+				gsensor_data->value_divide = 1000;
+			}
+			break;
+		default:
+			GSE_ERR("gsensor operate function no this parameter %d!\n", command);
+			err = -1;
+			break;
+	}
+
+	return err;
+}
+
+/******************************************************************************
+ * Function Configuration
+******************************************************************************/
+static int bma255_open(struct inode *inode, struct file *file)
+{
+	file->private_data = bma255_i2c_client;
+
+	if(file->private_data == NULL)
+	{
+		GSE_ERR("null pointer!!\n");
+		return -EINVAL;
+	}
+	return nonseekable_open(inode, file);
+}
+/*----------------------------------------------------------------------------*/
+static int bma255_release(struct inode *inode, struct file *file)
+{
+	file->private_data = NULL;
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static long bma255_unlocked_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
+{
+	struct i2c_client *client = (struct i2c_client*)file->private_data;
+	struct bma255_i2c_data *obj = (struct bma255_i2c_data*)i2c_get_clientdata(client);
+	char strbuf[BMA255_BUFSIZE]={0,};
+	void __user *data;
+	SENSOR_DATA sensor_data;
+	long err = 0;
+    #if defined(CALIBRATION_TO_FILE)
+	int cali[6];
+    #endif
+	uint32_t enable = 0;
+
+	GSE_FUN(f);
+	if(_IOC_DIR(cmd) & _IOC_READ)
+	{
+		err = !access_ok(VERIFY_WRITE, (void __user *)arg, _IOC_SIZE(cmd));
+	}
+	else if(_IOC_DIR(cmd) & _IOC_WRITE)
+	{
+		err = !access_ok(VERIFY_READ, (void __user *)arg, _IOC_SIZE(cmd));
+	}
+
+	if(err)
+	{
+		GSE_ERR("access error: %08X, (%2d, %2d)\n", cmd, _IOC_DIR(cmd), _IOC_SIZE(cmd));
+		return -EFAULT;
+	}
+
+	switch(cmd)
+	{
+#if 0
+		case GSENSOR_IOCTL_SET_ENABLE:
+			data = (void __user *) arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;
+			}
+
+			if(copy_from_user(&enable, data, sizeof(enable)))
+			{
+				return -EFAULT;
+			}
+			else
+			{
+				if(enable == 1)
+				{
+					BMA255_SetPowerMode(obj_i2c_data->client, 1);
+				}
+				else if(enable == 0)
+				{
+					BMA255_SetPowerMode(obj_i2c_data->client, 0);
+				}
+			}
+			break;
+#endif
+
+		case GSENSOR_IOCTL_INIT:
+			bma255_init_client(client, 0);
+			break;
+
+		case GSENSOR_IOCTL_READ_CHIPINFO:
+			data = (void __user *) arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;
+			}
+
+			BMA255_ReadChipInfo(client, strbuf, BMA255_BUFSIZE);
+			if(copy_to_user(data, strbuf, strlen(strbuf)+1))
+			{
+				err = -EFAULT;
+				break;
+			}
+			break;
+
+		case GSENSOR_IOCTL_READ_SENSORDATA:
+			data = (void __user *) arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;
+			}
+
+			BMA255_ReadSensorData(client, strbuf, BMA255_BUFSIZE);
+			if(copy_to_user(data, strbuf, strlen(strbuf)+1))
+			{
+				err = -EFAULT;
+				break;
+			}
+			break;
+
+		case GSENSOR_IOCTL_READ_GAIN:
+			data = (void __user *) arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;
+			}
+
+			if(copy_to_user(data, &gsensor_gain, sizeof(GSENSOR_VECTOR3D)))
+			{
+				err = -EFAULT;
+				break;
+			}
+			break;
+
+		case GSENSOR_IOCTL_READ_RAW_DATA:
+			data = (void __user *) arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;
+			}
+			BMA255_ReadRawData(client, strbuf);
+			if(copy_to_user(data, &strbuf, strlen(strbuf)+1))
+			{
+				err = -EFAULT;
+				break;
+			}
+			break;
+
+		case GSENSOR_IOCTL_SET_CALI:
+			data = (void __user*)arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;
+			}
+			if(copy_from_user(&sensor_data, data, sizeof(sensor_data)))
+			{
+				err = -EFAULT;
+				break;
+			}
+
+			//LGE_CHANGE_S : 2012-12-07 taebum81.kim@lge.com AT%SURV : (6)
+
+           #if defined(CALIBRATION_TO_FILE)
+				make_cal_data_file(); 
+				
+	            err = bma255_calibration_read(cali);
+				if(err!=0){
+				    GSE_LOG("Read Cal Fail from file !!!\n");
+					break;
+					}
+				else
+					{
+	                  sensor_data.x = cali[0];
+	                  sensor_data.y = cali[1];
+	                  sensor_data.z = cali[2];
+					}
+			#endif
+			
+            if((sensor_data.x==0)&&(sensor_data.y==0)&&(sensor_data.z==0))
+             {
+			    GSE_LOG("Read Cal Data x : %d / y : %d / z : %d\n",sensor_data.x,sensor_data.y,sensor_data.z);
+			    GSE_LOG("Read Cal Data all Zero, Do not set register\n");
+				err = -EINVAL;
+				break;
+             }
+						
+			if(bma255_set_offset_x(obj->client, (unsigned char)sensor_data.x) < 0){
+				err = -EINVAL;
+				break;
+				}
+			if(bma255_set_offset_y(obj->client, (unsigned char)sensor_data.y) < 0){
+				err = -EINVAL;
+				break;
+				}
+			if(bma255_set_offset_z(obj->client, (unsigned char)sensor_data.z) < 0){
+				err = -EINVAL;
+				break;
+				}
+
+			GSE_LOG("-------------- set sensor cal --------------\n");
+			GSE_LOG("x : %d / y : %d / z : %d\n",sensor_data.x,sensor_data.y,sensor_data.z);
+			//LGE_CHANGE_S : 2012-12-07 taebum81.kim@lge.com AT%SURV : (6)
+			break;
+
+		case GSENSOR_IOCTL_CLR_CALI:
+			err = BMA255_ResetCalibration(client);
+			break;
+
+		case GSENSOR_IOCTL_GET_CALI:
+			GSE_LOG("GSENSOR_IOCTL_GET_CALI\n");
+			break;
+
+		default:
+			GSE_ERR("unknown IOCTL: 0x%08x\n", cmd);
+			err = -ENOIOCTLCMD;
+			break;
+
+	}
+
+	return err;
+}
+
+
+/*----------------------------------------------------------------------------*/
+static struct file_operations bma255_fops = {
+	//.owner = THIS_MODULE,
+	.open = bma255_open,
+	.release = bma255_release,
+	.unlocked_ioctl = bma255_unlocked_ioctl,
+};
+/*----------------------------------------------------------------------------*/
+static struct miscdevice bma255_device = {
+	.minor = MISC_DYNAMIC_MINOR,
+	.name = "gsensor",
+	.fops = &bma255_fops,
+};
+/*----------------------------------------------------------------------------*/
+#ifndef CONFIG_HAS_EARLYSUSPEND
+/*----------------------------------------------------------------------------*/
+static int bma255_suspend(struct i2c_client *client, pm_message_t msg)
+{
+	struct bma255_i2c_data *obj = i2c_get_clientdata(client);
+	int err = 0;
+
+	GSE_FUN();
+
+	if(msg.event == PM_EVENT_SUSPEND)
+	{
+		if(obj == NULL)
+		{
+			GSE_ERR("null pointer!!\n");
+			return -EINVAL;
+		}
+		atomic_set(&obj->suspend, 1);
+		#if 0
+		if(err = BMA255_SetPowerMode(obj->client, false))
+		{
+			GSE_ERR("write power control fail!!\n");
+			return;
+		}
+		#endif
+		BMA255_power(obj->hw, 0);
+	}
+	return err;
+}
+/*----------------------------------------------------------------------------*/
+static int bma255_resume(struct i2c_client *client)
+{
+	struct bma255_i2c_data *obj = i2c_get_clientdata(client);
+	int err;
+
+    GSE_FUN();
+
+	if(obj == NULL)
+	{
+		GSE_ERR("null pointer!!\n");
+		return -EINVAL;
+	}
+
+	BMA255_power(obj->hw, 1);
+	#if 0//already call PowerManagerSer
+	if(err = bma255_init_client(client, 0))
+	{
+		GSE_ERR("initialize client fail!!\n");
+		return err;
+	}
+	#endif
+	atomic_set(&obj->suspend, 0);
+
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+#else /*CONFIG_HAS_EARLY_SUSPEND is defined*/
+/*----------------------------------------------------------------------------*/
+static void bma255_early_suspend(struct early_suspend *h)
+{
+	struct bma255_i2c_data *obj = container_of(h, struct bma255_i2c_data, early_drv);
+	int err;
+
+	GSE_FUN();
+
+	if(obj == NULL)
+	{
+		GSE_ERR("null pointer!!\n");
+		return;
+	}
+	atomic_set(&obj->suspend, 1);
+	#if 0
+	if(err = BMA255_SetPowerMode(obj->client, false))
+	{
+		GSE_ERR("write power control fail!!\n");
+		return;
+	}
+
+	sensor_power = false;
+	#endif
+	BMA255_power(obj->hw, 0);
+}
+/*----------------------------------------------------------------------------*/
+static void bma255_late_resume(struct early_suspend *h)
+{
+	struct bma255_i2c_data *obj = container_of(h, struct bma255_i2c_data, early_drv);
+	int err;
+
+	GSE_FUN();
+
+	if(obj == NULL)
+	{
+		GSE_ERR("null pointer!!\n");
+		return;
+	}
+
+	BMA255_power(obj->hw, 1);
+	#if 0//already call PowerManagerSer
+	if(err = bma255_init_client(obj->client, 0))
+	{
+		GSE_ERR("initialize client fail!!\n");
+		return;
+	}
+	#endif
+	atomic_set(&obj->suspend, 0);
+}
+/*----------------------------------------------------------------------------*/
+#endif /*CONFIG_HAS_EARLYSUSPEND*/
+/*----------------------------------------------------------------------------*/
+static int bma255_i2c_probe(struct i2c_client *client, const struct i2c_device_id *id)
+{
+	struct i2c_client *new_client;
+	struct bma255_i2c_data *obj;
+	struct hwmsen_object sobj;
+	int err = 0;
+
+	GSE_FUN();
+
+	if(!(obj = kzalloc(sizeof(*obj), GFP_KERNEL)))
+	{
+		err = -ENOMEM;
+		goto exit;
+	}
+
+	memset(obj, 0, sizeof(struct bma255_i2c_data));
+
+	obj->hw = get_cust_acc_hw();
+
+	if(err = hwmsen_get_convert(obj->hw->direction, &obj->cvt))
+	{
+		GSE_ERR("invalid direction: %d\n", obj->hw->direction);
+		goto exit;
+	}
+
+	obj_i2c_data = obj;
+	obj->client = client;
+	new_client = obj->client;
+	i2c_set_clientdata(new_client,obj);
+
+	atomic_set(&obj->trace, 0);
+	atomic_set(&obj->suspend, 0);
+
+#ifdef CONFIG_BMA255_LOWPASS
+	if(obj->hw->firlen > C_MAX_FIR_LENGTH)
+	{
+		atomic_set(&obj->firlen, C_MAX_FIR_LENGTH);
+	}
+	else
+	{
+		atomic_set(&obj->firlen, obj->hw->firlen);
+	}
+
+	if(atomic_read(&obj->firlen) > 0)
+	{
+		atomic_set(&obj->fir_en, 1);
+	}
+
+#endif
+
+	bma255_i2c_client = new_client;
+
+	if(err = bma255_init_client(new_client, 1))
+	{
+		GSE_ERR ( "failed to init BMA255 ( err = %d )\n", err );
+		goto exit_init_failed;
+	}
+
+
+	if(err = misc_register(&bma255_device))
+	{
+		GSE_ERR("bma255_device register failed\n");
+		goto exit_misc_device_register_failed;
+	}
+
+	if(err = bma255_create_attr(&bma255_gsensor_driver.driver))
+	{
+		GSE_ERR("create attribute err = %d\n", err);
+		goto exit_create_attr_failed;
+	}
+
+	sobj.self = obj;
+    sobj.polling = 1;
+    sobj.sensor_operate = gsensor_operate;
+	if(err = hwmsen_attach(ID_ACCELEROMETER, &sobj))
+	{
+		GSE_ERR("attach fail = %d\n", err);
+		goto exit_kfree;
+	}
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+	obj->early_drv.level    = EARLY_SUSPEND_LEVEL_DISABLE_FB - 1,
+	obj->early_drv.suspend  = bma255_early_suspend,
+	obj->early_drv.resume   = bma255_late_resume,
+	register_early_suspend(&obj->early_drv);
+#endif
+
+	GSE_LOG("%s: OK\n", __func__);
+	return 0;
+
+exit_create_attr_failed:
+	misc_deregister(&bma255_device);
+exit_misc_device_register_failed:
+exit_init_failed:
+	//i2c_detach_client(new_client);
+exit_kfree:
+	kfree(obj);
+exit:
+	GSE_ERR("%s: err = %d\n", __func__, err);
+	return err;
+}
+
+/*----------------------------------------------------------------------------*/
+static int bma255_i2c_remove(struct i2c_client *client)
+{
+	int err = 0;
+
+	if(err = bma255_delete_attr(&bma255_gsensor_driver.driver))
+	{
+		GSE_ERR("bma150_delete_attr fail: %d\n", err);
+	}
+
+	if(err = misc_deregister(&bma255_device))
+	{
+		GSE_ERR("misc_deregister fail: %d\n", err);
+	}
+
+	if(err = hwmsen_detach(ID_ACCELEROMETER))
+    {
+		GSE_ERR("hwmsen_detach fail: %d\n", err);
+    }
+
+    bma255_i2c_client = NULL;
+	i2c_unregister_device(client);
+	kfree(i2c_get_clientdata(client));
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int bma255_probe(struct platform_device *pdev)
+{
+	struct acc_hw *hw = get_cust_acc_hw();
+
+	GSE_FUN();
+
+	BMA255_power(hw, 1);
+	if(i2c_add_driver(&bma255_i2c_driver))
+	{
+		GSE_ERR("add driver error\n");
+		return -1;
+	}
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int bma255_remove(struct platform_device *pdev)
+{
+    struct acc_hw *hw = get_cust_acc_hw();
+
+    GSE_FUN();
+    BMA255_power(hw, 0);
+    i2c_del_driver(&bma255_i2c_driver);
+    return 0;
+}
+/*----------------------------------------------------------------------------*/
+static struct platform_driver bma255_gsensor_driver = {
+	.probe      = bma255_probe,
+	.remove     = bma255_remove,
+	.driver     = {
+		.name  = "gsensor",
+#ifdef CONFIG_OF_DT
+		.of_match_table = gsensor_of_match,
+#endif
+	}
+};
+
+/*----------------------------------------------------------------------------*/
+static int __init bma255_init(void)
+{
+	struct acc_hw *hw = get_cust_acc_hw();
+	unsigned short bma255_i2c_addr = 0x0;
+
+	GSE_FUN();
+	bma255_i2c_addr = 0x10;
+	bma255_i2c_info.addr = bma255_i2c_addr;
+	i2c_register_board_info(hw->i2c_num, &bma255_i2c_info, 1);
+	if(platform_driver_register(&bma255_gsensor_driver))
+	{
+		GSE_ERR("failed to register driver");
+		return -ENODEV;
+	}
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static void __exit bma255_exit(void)
+{
+	GSE_FUN();
+	platform_driver_unregister(&bma255_gsensor_driver);
+}
+/*----------------------------------------------------------------------------*/
+module_init(bma255_init);
+module_exit(bma255_exit);
+/*----------------------------------------------------------------------------*/
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("BMA255 I2C driver");
+MODULE_AUTHOR("hongji.zhou@bosch-sensortec.com");
diff --git a/drivers/misc/mediatek/accelerometer/bma255-sdo0/bma255.h b/drivers/misc/mediatek/accelerometer/bma255-sdo0/bma255.h
new file mode 100644
index 000000000..95adbe5c4
--- /dev/null
+++ b/drivers/misc/mediatek/accelerometer/bma255-sdo0/bma255.h
@@ -0,0 +1,158 @@
+/* BMA255 motion sensor driver
+ *
+ *
+ * This software program is licensed subject to the GNU General Public License
+ * (GPL).Version 2,June 1991, available at http://www.fsf.org/copyleft/gpl.html
+
+ * (C) Copyright 2011 Bosch Sensortec GmbH
+ * All Rights Reserved
+ */
+
+#ifndef BMA255_H
+#define BMA255_H
+
+#include <linux/ioctl.h>
+
+/* 7-bit addr: 0x10 (SDO connected to GND); 0x11 (SDO connected to VDDIO) */
+#define BMA255_I2C_ADDR				0x11
+/* chip ID */
+#define BMA255_FIXED_DEVID			0xFA
+
+ /* BMA255 Register Map  (Please refer to BMA255 Specifications) */
+#define BMA255_REG_DEVID			0x00
+#define BMA255_REG_OFSX				0x16
+#define BMA255_REG_OFSX_HIGH		0x1A
+#define BMA255_REG_BW_RATE			0x10
+#define BMA255_BW_MASK				0x1f
+
+#define BMA255_BW_250HZ				0x0d
+#define BMA255_BW_125HZ				0x0c
+#define BMA255_BW_62_5HZ			0x0b
+#define BMA255_BW_31_25HZ			0x0a
+#define BMA255_BW_15_63HZ			0x09
+#define BMA255_BW_7_81HZ			0x08
+#define BMA255_BW_200HZ				0x0d
+#define BMA255_BW_100HZ				0x0c
+#define BMA255_BW_50HZ				0x0b
+#define BMA255_BW_25HZ				0x0a
+
+#define BMA255_REG_POWER_CTL		0x11
+#define BMA255_REG_DATA_FORMAT		0x0f
+#define BMA255_RANGE_MASK			0x0f
+#define BMA255_RANGE_2G				0x03
+#define BMA255_RANGE_4G				0x05
+#define BMA255_RANGE_8G				0x08
+#define BMA255_REG_DATAXLOW			0x02
+#define BMA255_REG_DATA_RESOLUTION	0x14
+#define BMA255_MEASURE_MODE			0x80
+#define BMA255_SELF_TEST           	0x32
+#define BMA255_SELF_TEST_AXIS_X		0x01
+#define BMA255_SELF_TEST_AXIS_Y		0x02
+#define BMA255_SELF_TEST_AXIS_Z		0x03
+#define BMA255_SELF_TEST_POSITIVE	0x00
+#define BMA255_SELF_TEST_NEGATIVE	0x04
+#define BMA255_INT_REG_1           	0x16
+#define BMA255_INT_REG_2          	0x17
+
+#define BMA255_X_AXIS_LSB_REG		0x02
+#define BMA255_X_AXIS_MSB_REG		0x03
+#define BMA255_Y_AXIS_LSB_REG		0x04
+#define BMA255_Y_AXIS_MSB_REG		0x05
+#define BMA255_Z_AXIS_LSB_REG		0x06
+#define BMA255_Z_AXIS_MSB_REG		0x07
+
+#define BMA255_EN_SOFT_RESET__POS	0
+#define BMA255_EN_SOFT_RESET__LEN	8
+#define BMA255_EN_SOFT_RESET__MSK	0xFF
+#define BMA255_EN_SOFT_RESET__REG	BMA255_RESET_REG
+
+#define BMA255_EEPROM_CTRL_REG		0x33
+#define BMA255_OFFSET_CTRL_REG		0x36
+#define BMA255_OFFSET_PARAMS_REG	0x37
+#define BMA255_OFFSET_X_AXIS_REG	0x38
+#define BMA255_OFFSET_Y_AXIS_REG	0x39
+#define BMA255_OFFSET_Z_AXIS_REG	0x3A
+
+#define BMA255_CUT_OFF				0
+#define BMA255_OFFSET_TRIGGER_X		1
+#define BMA255_OFFSET_TRIGGER_Y		2
+#define BMA255_OFFSET_TRIGGER_Z		3
+
+#define BMA255_FAST_CAL_RDY_S__POS	4
+#define BMA255_FAST_CAL_RDY_S__LEN	1
+#define BMA255_FAST_CAL_RDY_S__MSK	0x10
+#define BMA255_FAST_CAL_RDY_S__REG	BMA255_OFFSET_CTRL_REG
+
+#define BMA255_CAL_TRIGGER__POS		5
+#define BMA255_CAL_TRIGGER__LEN		2
+#define BMA255_CAL_TRIGGER__MSK		0x60
+#define BMA255_CAL_TRIGGER__REG		BMA255_OFFSET_CTRL_REG
+
+#define BMA255_COMP_CUTOFF__POS		0
+#define BMA255_COMP_CUTOFF__LEN		1
+#define BMA255_COMP_CUTOFF__MSK		0x01
+#define BMA255_COMP_CUTOFF__REG		BMA255_OFFSET_PARAMS_REG
+
+#define BMA255_COMP_TARGET_OFFSET_X__POS		1
+#define BMA255_COMP_TARGET_OFFSET_X__LEN		2
+#define BMA255_COMP_TARGET_OFFSET_X__MSK		0x06
+#define BMA255_COMP_TARGET_OFFSET_X__REG		BMA255_OFFSET_PARAMS_REG
+
+#define BMA255_COMP_TARGET_OFFSET_Y__POS		3
+#define BMA255_COMP_TARGET_OFFSET_Y__LEN		2
+#define BMA255_COMP_TARGET_OFFSET_Y__MSK		0x18
+#define BMA255_COMP_TARGET_OFFSET_Y__REG		BMA255_OFFSET_PARAMS_REG
+
+#define BMA255_COMP_TARGET_OFFSET_Z__POS		5
+#define BMA255_COMP_TARGET_OFFSET_Z__LEN		2
+#define BMA255_COMP_TARGET_OFFSET_Z__MSK		0x60
+#define BMA255_COMP_TARGET_OFFSET_Z__REG		BMA255_OFFSET_PARAMS_REG
+
+#define BMA255_SUCCESS					0
+#define BMA255_ERR_I2C					-1
+#define BMA255_ERR_STATUS				-3
+#define BMA255_ERR_SETUP_FAILURE		-4
+#define BMA255_ERR_GETGSENSORDATA		-5
+#define BMA255_ERR_IDENTIFICATION		-6
+
+#define BMA255_BUFSIZE					256
+
+/* soft reset */
+#define BMA255_RESET_REG                        0x14
+#define BMA255_EN_SOFT_RESET_VALUE        	0xB6
+
+#define BMA255_EN_SOFT_RESET__POS         0
+#define BMA255_EN_SOFT_RESET__LEN         8
+#define BMA255_EN_SOFT_RESET__MSK         0xFF
+#define BMA255_EN_SOFT_RESET__REG         BMA255_RESET_REG
+
+/* self test */
+#define BMA255_SELF_TEST_REG                        0x32
+
+#define BMA255_SELF_TEST_AMP__POS               4
+#define BMA255_SELF_TEST_AMP__LEN               1
+#define BMA255_SELF_TEST_AMP__MSK               0x10
+#define BMA255_SELF_TEST_AMP__REG               BMA255_SELF_TEST_REG
+
+#define BMA255_EN_SELF_TEST__POS                0
+#define BMA255_EN_SELF_TEST__LEN                2
+#define BMA255_EN_SELF_TEST__MSK                0x03
+#define BMA255_EN_SELF_TEST__REG                BMA255_SELF_TEST_REG
+
+#define BMA255_NEG_SELF_TEST__POS               2
+#define BMA255_NEG_SELF_TEST__LEN               1
+#define BMA255_NEG_SELF_TEST__MSK               0x04
+#define BMA255_NEG_SELF_TEST__REG               BMA255_SELF_TEST_REG
+
+/* bandwidth */
+#define BMA255_BW_7_81HZ			0x08
+#define BMA255_BW_15_63HZ			0x09
+#define BMA255_BW_31_25HZ			0x0A
+#define BMA255_BW_62_50HZ			0x0B
+#define BMA255_BW_125HZ				0x0C
+#define BMA255_BW_250HZ				0x0D
+#define BMA255_BW_500HZ				0x0E
+#define BMA255_BW_1000HZ			0x0F
+
+#endif
+
diff --git a/drivers/misc/mediatek/accelerometer/bma255-sdo1/Makefile b/drivers/misc/mediatek/accelerometer/bma255-sdo1/Makefile
new file mode 100755
index 000000000..425d91fe0
--- /dev/null
+++ b/drivers/misc/mediatek/accelerometer/bma255-sdo1/Makefile
@@ -0,0 +1,4 @@
+include $(srctree)/drivers/misc/mediatek/Makefile.custom
+
+obj-y	:=  bma255.o
+
diff --git a/drivers/misc/mediatek/accelerometer/bma255-sdo1/bma255.c b/drivers/misc/mediatek/accelerometer/bma255-sdo1/bma255.c
new file mode 100644
index 000000000..fbcb7e93a
--- /dev/null
+++ b/drivers/misc/mediatek/accelerometer/bma255-sdo1/bma255.c
@@ -0,0 +1,3324 @@
+/* BMA255 motion sensor driver
+ *
+ *
+ * This software program is licensed subject to the GNU General Public License
+ * (GPL).Version 2,June 1991, available at http://www.fsf.org/copyleft/gpl.html
+
+ * (C) Copyright 2011 Bosch Sensortec GmbH
+ * All Rights Reserved
+ */
+
+#include <linux/interrupt.h>
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include <linux/irq.h>
+#include <linux/miscdevice.h>
+#include <asm/uaccess.h>
+#include <linux/delay.h>
+#include <linux/input.h>
+#include <linux/workqueue.h>
+#include <linux/kobject.h>
+#include <linux/earlysuspend.h>
+#include <linux/platform_device.h>
+#include <asm/atomic.h>
+#include <linux/module.h>
+
+#include <mach/devs.h>
+#include <mach/mt_typedefs.h>
+#include <mach/mt_gpio.h>
+#include <mach/mt_pm_ldo.h>
+
+#define POWER_NONE_MACRO MT65XX_POWER_NONE
+
+#include <cust_acc.h>
+#include <linux/hwmsensor.h>
+#include <linux/hwmsen_dev.h>
+#include <linux/sensors_io.h>
+#include "bma255.h"
+#include <linux/hwmsen_helper.h>
+
+#include <accel.h>
+
+#if defined(TARGET_C90)
+#define CONFIG_OF_DT
+#endif
+
+/* Use CALIBRATION_TO_FILE define for saving cal data to file      */
+/* Without CALIBRATION_TO_FILE define, Cal data is saved to MISC2 */
+#define CALIBRATION_TO_FILE 1
+
+#if defined(CALIBRATION_TO_FILE)
+#include <linux/syscalls.h>
+#include <linux/fs.h>
+#endif
+
+#ifdef CONFIG_OF
+static const struct of_device_id gsensor_of_match[] = {
+	{ .compatible = "mediatek,gsensor", },
+	{},
+};
+#endif
+
+/* range for selftest */
+enum BMA_RANGE_ENUM {
+	BMA_RANGE_2G = 0x0, /* +/- 2G */
+	BMA_RANGE_4G,		/* +/- 4G */
+	BMA_RANGE_8G,		/* +/- 8G */
+
+	BMA_UNDEFINED_RANGE = 0xff
+};
+
+
+#if defined(TARGET_C90)
+//#include "../../tc1_interface/gpt/lg_partition.h" //for MT6732
+#else
+//#include "../../tc1_interface/pmt/lg_partition.h" //for MT6582
+#endif
+
+/*----------------------------------------------------------------------------*/
+#define I2C_DRIVERID_BMA255 255
+/*----------------------------------------------------------------------------*/
+#define DEBUG 1
+/*----------------------------------------------------------------------------*/
+//#define CONFIG_BMA255_LOWPASS   /*apply low pass filter on output*/
+#define SW_CALIBRATION
+
+/*----------------------------------------------------------------------------*/
+#define BMA255_AXIS_X          0
+#define BMA255_AXIS_Y          1
+#define BMA255_AXIS_Z          2
+#define BMA255_AXES_NUM        3
+#define BMA255_DATA_LEN        6
+#define BMA255_DEV_NAME        "BMA255"
+
+#define BMA255_MODE_NORMAL      0
+#define BMA255_MODE_LOWPOWER    1
+#define BMA255_MODE_SUSPEND     2
+
+#define BMA255_ACC_X_LSB__POS           4
+#define BMA255_ACC_X_LSB__LEN           4
+#define BMA255_ACC_X_LSB__MSK           0xF0
+#define BMA255_ACC_X_LSB__REG           BMA255_X_AXIS_LSB_REG
+
+#define BMA255_ACC_X_MSB__POS           0
+#define BMA255_ACC_X_MSB__LEN           8
+#define BMA255_ACC_X_MSB__MSK           0xFF
+#define BMA255_ACC_X_MSB__REG           BMA255_X_AXIS_MSB_REG
+
+#define BMA255_ACC_Y_LSB__POS           4
+#define BMA255_ACC_Y_LSB__LEN           4
+#define BMA255_ACC_Y_LSB__MSK           0xF0
+#define BMA255_ACC_Y_LSB__REG           BMA255_Y_AXIS_LSB_REG
+
+#define BMA255_ACC_Y_MSB__POS           0
+#define BMA255_ACC_Y_MSB__LEN           8
+#define BMA255_ACC_Y_MSB__MSK           0xFF
+#define BMA255_ACC_Y_MSB__REG           BMA255_Y_AXIS_MSB_REG
+
+#define BMA255_ACC_Z_LSB__POS           4
+#define BMA255_ACC_Z_LSB__LEN           4
+#define BMA255_ACC_Z_LSB__MSK           0xF0
+#define BMA255_ACC_Z_LSB__REG           BMA255_Z_AXIS_LSB_REG
+
+#define BMA255_ACC_Z_MSB__POS           0
+#define BMA255_ACC_Z_MSB__LEN           8
+#define BMA255_ACC_Z_MSB__MSK           0xFF
+#define BMA255_ACC_Z_MSB__REG           BMA255_Z_AXIS_MSB_REG
+
+#define BMA255_EN_LOW_POWER__POS          6
+#define BMA255_EN_LOW_POWER__LEN          1
+#define BMA255_EN_LOW_POWER__MSK          0x40
+#define BMA255_EN_LOW_POWER__REG          BMA255_REG_POWER_CTL
+
+#define BMA255_EN_SUSPEND__POS            7
+#define BMA255_EN_SUSPEND__LEN            1
+#define BMA255_EN_SUSPEND__MSK            0x80
+#define BMA255_EN_SUSPEND__REG            BMA255_REG_POWER_CTL
+
+#define BMA255_RANGE_SEL__POS             0
+#define BMA255_RANGE_SEL__LEN             4
+#define BMA255_RANGE_SEL__MSK             0x0F
+#define BMA255_RANGE_SEL__REG             BMA255_REG_DATA_FORMAT
+
+#define BMA255_BANDWIDTH__POS             0
+#define BMA255_BANDWIDTH__LEN             5
+#define BMA255_BANDWIDTH__MSK             0x1F
+#define BMA255_BANDWIDTH__REG             BMA255_REG_BW_RATE
+
+
+
+//#ifdef CONFIG_MACH_LGE
+#define BMA255_ACCEL_CALIBRATION 1
+
+#ifdef BMA255_ACCEL_CALIBRATION
+#define BMA255_SHAKING_DETECT_THRESHOLD	(300)	 //clubsh cal2 50 -> 200
+/* Calibration zero-g offset check */
+#define BMA255_AXIS_X             0
+#define BMA255_AXIS_Y             1
+#define BMA255_AXIS_Z             2
+#define CALIBRATION_DATA_AMOUNT         10
+#if 0 /* vendor recommand */
+#define TESTLIMIT_XY                     (175)
+#define TESTLIMIT_Z_USL_LSB             (1270)
+#define TESTLIMIT_Z_LSL_LSB             (778)
+#endif
+#if defined(TARGET_Y70)
+#if 1
+#define TESTLIMIT_XY                     (240)
+#else
+int TESTLIMIT_XY = 240;
+#endif
+#define TESTLIMIT_Z_USL_LSB             (1226)
+#define TESTLIMIT_Z_LSL_LSB             (822)
+#elif defined(TARGET_Y90)
+#define TESTLIMIT_XY                     (232)
+#define TESTLIMIT_Z_USL_LSB             (1226)
+#define TESTLIMIT_Z_LSL_LSB             (822)
+#elif defined(TARGET_C90)
+#define TESTLIMIT_XY                     (237)
+#define TESTLIMIT_Z_USL_LSB             (1226)
+#define TESTLIMIT_Z_LSL_LSB             (822)
+#else//Y50
+#define TESTLIMIT_XY                     (233)
+#define TESTLIMIT_Z_USL_LSB             (1226)
+#define TESTLIMIT_Z_LSL_LSB             (822)
+#endif
+
+/* Calibration zero-g offset check */
+#endif
+
+struct bma255acc{
+	s16	x,
+		y,
+		z;
+} ;
+//LGE_CHANGE_E : 2012-12-07 taebum81.kim@lge.com AT%SURV : (1)
+
+
+#define BMA255_GET_BITSLICE(regvar, bitname)\
+	((regvar & bitname##__MSK) >> bitname##__POS)
+
+#define BMA255_SET_BITSLICE(regvar, bitname, val)\
+	((regvar & ~bitname##__MSK) | ((val<<bitname##__POS)&bitname##__MSK))
+
+/*----------------------------------------------------------------------------*/
+
+/*----------------------------------------------------------------------------*/
+static const struct i2c_device_id bma255_i2c_id[] = {{BMA255_DEV_NAME,0},{}};
+static struct i2c_board_info __initdata bma255_i2c_info ={ I2C_BOARD_INFO(BMA255_DEV_NAME, BMA255_I2C_ADDR)};
+
+/*----------------------------------------------------------------------------*/
+static int bma255_i2c_probe(struct i2c_client *client, const struct i2c_device_id *id);
+static int bma255_i2c_remove(struct i2c_client *client);
+static int bma255_do_calibration(void);
+
+
+/*----------------------------------------------------------------------------*/
+typedef enum {
+    BMA_TRC_FILTER  = 0x01,
+    BMA_TRC_RAWDATA = 0x02,
+    BMA_TRC_IOCTL   = 0x04,
+    BMA_TRC_CALI	= 0X08,
+    BMA_TRC_INFO	= 0X10,
+} BMA_TRC;
+/*----------------------------------------------------------------------------*/
+struct scale_factor{
+    u8  whole;
+    u8  fraction;
+};
+/*----------------------------------------------------------------------------*/
+struct data_resolution {
+    struct scale_factor scalefactor;
+    int                 sensitivity;
+};
+/*----------------------------------------------------------------------------*/
+#define C_MAX_FIR_LENGTH (32)
+/*----------------------------------------------------------------------------*/
+struct data_filter {
+    s16 raw[C_MAX_FIR_LENGTH][BMA255_AXES_NUM];
+    int sum[BMA255_AXES_NUM];
+    int num;
+    int idx;
+};
+/*----------------------------------------------------------------------------*/
+struct bma255_i2c_data {
+    struct i2c_client *client;
+    struct acc_hw *hw;
+    struct hwmsen_convert   cvt;
+
+	/* for selftest */
+	enum BMA_RANGE_ENUM     range;
+	u8 bandwidth;
+
+    /*misc*/
+    struct data_resolution *reso;
+    atomic_t                trace;
+    atomic_t                suspend;
+    atomic_t                selftest;
+	atomic_t				filter;
+    s16                     cali_sw[BMA255_AXES_NUM+1];
+
+    /*data*/
+    s8                      offset[BMA255_AXES_NUM+1];  /*+1: for 4-byte alignment*/
+    s16                     data[BMA255_AXES_NUM+1];
+
+#if defined(CONFIG_BMA255_LOWPASS)
+    atomic_t                firlen;
+    atomic_t                fir_en;
+    struct data_filter      fir;
+#endif
+    /*early suspend*/
+#if defined(CONFIG_HAS_EARLYSUSPEND)
+    struct early_suspend    early_drv;
+#endif
+//LGE_CHANGE_S : 2012-12-07 taebum81.kim@lge.com sensor SURV shake detection : (2)
+	atomic_t fast_calib_x_rslt;
+	atomic_t fast_calib_y_rslt;
+	atomic_t fast_calib_z_rslt;
+	atomic_t fast_calib_rslt;
+//LG_CHANGE_E : 2012-12-07 taebum81.kim@lge.com sensor SURV shake detection : (2)
+};
+/*----------------------------------------------------------------------------*/
+static struct i2c_driver bma255_i2c_driver = {
+    .driver = {
+        .name           = BMA255_DEV_NAME,
+    },
+	.probe      		= bma255_i2c_probe,
+	.remove    			= bma255_i2c_remove,
+#if !defined(CONFIG_HAS_EARLYSUSPEND)
+    .suspend            = bma255_suspend,
+    .resume             = bma255_resume,
+#endif
+	.id_table = bma255_i2c_id,
+};
+
+/*----------------------------------------------------------------------------*/
+static struct i2c_client *bma255_i2c_client = NULL;
+static struct platform_driver bma255_gsensor_driver;
+static struct bma255_i2c_data *obj_i2c_data = NULL;
+static bool sensor_power = true;
+static int test_status = 0;
+static GSENSOR_VECTOR3D gsensor_gain;
+//static char selftestRes[8]= {0};
+
+static int data_count = 0;
+
+/*----------------------------------------------------------------------------*/
+#define GSE_TAG                  "[Gsensor] "
+#define GSE_FUN(f)               printk(KERN_ERR GSE_TAG"%s\n", __FUNCTION__)
+#define GSE_ERR(fmt, args...)    printk(KERN_ERR GSE_TAG"%s %d : "fmt, __FUNCTION__, __LINE__, ##args)
+#define GSE_LOG(fmt, args...)    printk(KERN_ERR GSE_TAG fmt, ##args)
+/*----------------------------------------------------------------------------*/
+static struct data_resolution bma255_data_resolution[1] = {
+ /* combination by {FULL_RES,RANGE}*/
+    {{ 4, 0}, 512},   // dataformat +/-2g  in 12-bit resolution;  { 1, 0} = 1.0= (2*2*1000)/(2^12);  1024 = (2^12)/(2*2)
+};
+/*----------------------------------------------------------------------------*/
+//static struct data_resolution bma255_offset_resolution = {{1, 0}, 1024};
+
+/*--------------------BMA255 power control function----------------------------------*/
+static void BMA255_power(struct acc_hw *hw, unsigned int on)
+{
+	static unsigned int power_on = 0;
+
+	if(hw->power_id != POWER_NONE_MACRO)		// have externel LDO
+	{
+		GSE_LOG("power %s\n", on ? "on" : "off");
+		if(power_on == on)	// power status not change
+		{
+			GSE_LOG("ignore power control: %d\n", on);
+		}
+		else if(on)	// power on
+		{
+			if(!hwPowerOn(hw->power_id, hw->power_vol, "BMA255"))
+			{
+				GSE_ERR("power on fails!!\n");
+			}
+		}
+		else	// power off
+		{
+			if (!hwPowerDown(hw->power_id, "BMA255"))
+			{
+				GSE_ERR("power off fail!!\n");
+			}
+		}
+	}
+	power_on = on;
+}
+/*----------------------------------------------------------------------------*/
+static int bma255_smbus_read_byte(struct i2c_client *client, unsigned char reg_addr, unsigned char *data)
+{
+	s32 dummy;
+	dummy = i2c_smbus_read_byte_data(client, reg_addr);
+	if (dummy < 0)
+		return -1;
+	*data = dummy & 0x000000ff;
+
+	return 0;
+}
+
+static int bma255_smbus_write_byte(struct i2c_client *client, unsigned char reg_addr, unsigned char *data)
+{
+	s32 dummy;
+	dummy = i2c_smbus_write_byte_data(client, reg_addr, *data);
+	if (dummy < 0)
+		return -1;
+	return 0;
+}
+
+static int bma255_smbus_read_byte_block(struct i2c_client *client, unsigned char reg_addr, unsigned char *data, unsigned char len)
+{
+	s32 dummy;
+	dummy = i2c_smbus_read_i2c_block_data(client, reg_addr, len, data);
+	if (dummy < 0)
+		return -1;
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int BMA255_SetDataResolution(struct bma255_i2c_data *obj)
+{
+
+/* set g sensor data resolution here */
+
+/* BMA255 only can set to 10-bit dataresolution, so do nothing in bma255 driver here */
+
+/* end of set dataresolution */
+
+/* we set measure range from -2g to +2g in BMA255_SetDataFormat(client, BMA255_RANGE_2G),
+                                                    and set 10-bit dataresolution BMA255_SetDataResolution() */
+
+/* so bma255_data_resolution[0] set value as {{ 3, 9}, 256} when declaration, and assign the value to obj->reso here */
+
+    obj->reso = &bma255_data_resolution[0];
+	return 0;
+
+/* if you changed the measure range, for example call: BMA255_SetDataFormat(client, BMA255_RANGE_4G),
+    you must set the right value to bma255_data_resolution */
+
+}
+/*----------------------------------------------------------------------------*/
+static int BMA255_ReadData(struct i2c_client *client, s16 data[BMA255_AXES_NUM])
+{
+#ifdef CONFIG_BMA255_LOWPASS
+	struct bma255_i2c_data *priv = i2c_get_clientdata(client);
+#endif
+	u8 addr = BMA255_REG_DATAXLOW;
+	u8 buf[BMA255_DATA_LEN] = {0};
+	int err = 0;
+
+ err = hwmsen_read_block(client, addr, buf, BMA255_DATA_LEN);
+
+	if(NULL == client)
+	{
+		err = -EINVAL;
+	}
+	else if(err)
+	{
+ 		GSE_ERR("error: %d\n", err);
+	}
+	else
+	{
+		/* Convert sensor raw data to 16-bit integer */
+		data[BMA255_AXIS_X] = BMA255_GET_BITSLICE(buf[0], BMA255_ACC_X_LSB)
+                                |(BMA255_GET_BITSLICE(buf[1], BMA255_ACC_X_MSB)<<BMA255_ACC_X_LSB__LEN);
+		data[BMA255_AXIS_X] = data[BMA255_AXIS_X] << (sizeof(short)*8-(BMA255_ACC_X_LSB__LEN
+                                + BMA255_ACC_X_MSB__LEN));
+		data[BMA255_AXIS_X] = data[BMA255_AXIS_X] >> (sizeof(short)*8-(BMA255_ACC_X_LSB__LEN
+                                + BMA255_ACC_X_MSB__LEN));
+		data[BMA255_AXIS_Y] = BMA255_GET_BITSLICE(buf[2], BMA255_ACC_Y_LSB)
+                                | (BMA255_GET_BITSLICE(buf[3], BMA255_ACC_Y_MSB)<<BMA255_ACC_Y_LSB__LEN);
+		data[BMA255_AXIS_Y] = data[BMA255_AXIS_Y] << (sizeof(short)*8-(BMA255_ACC_Y_LSB__LEN
+                                + BMA255_ACC_Y_MSB__LEN));
+		data[BMA255_AXIS_Y] = data[BMA255_AXIS_Y] >> (sizeof(short)*8-(BMA255_ACC_Y_LSB__LEN
+                                + BMA255_ACC_Y_MSB__LEN));
+		data[BMA255_AXIS_Z] = BMA255_GET_BITSLICE(buf[4], BMA255_ACC_Z_LSB)
+                                | (BMA255_GET_BITSLICE(buf[5], BMA255_ACC_Z_MSB)<<BMA255_ACC_Z_LSB__LEN);
+		data[BMA255_AXIS_Z] = data[BMA255_AXIS_Z] << (sizeof(short)*8-(BMA255_ACC_Z_LSB__LEN
+                                + BMA255_ACC_Z_MSB__LEN));
+		data[BMA255_AXIS_Z] = data[BMA255_AXIS_Z] >> (sizeof(short)*8-(BMA255_ACC_Z_LSB__LEN
+                                + BMA255_ACC_Z_MSB__LEN));
+
+#ifdef CONFIG_BMA255_LOWPASS
+		if(atomic_read(&priv->filter))
+		{
+			if(atomic_read(&priv->fir_en) && !atomic_read(&priv->suspend))
+			{
+				int idx, firlen = atomic_read(&priv->firlen);
+				if(priv->fir.num < firlen)
+				{
+					priv->fir.raw[priv->fir.num][BMA255_AXIS_X] = data[BMA255_AXIS_X];
+					priv->fir.raw[priv->fir.num][BMA255_AXIS_Y] = data[BMA255_AXIS_Y];
+					priv->fir.raw[priv->fir.num][BMA255_AXIS_Z] = data[BMA255_AXIS_Z];
+					priv->fir.sum[BMA255_AXIS_X] += data[BMA255_AXIS_X];
+					priv->fir.sum[BMA255_AXIS_Y] += data[BMA255_AXIS_Y];
+					priv->fir.sum[BMA255_AXIS_Z] += data[BMA255_AXIS_Z];
+					if(atomic_read(&priv->trace) & BMA_TRC_FILTER)
+					{
+						GSE_LOG("add [%2d] [%5d %5d %5d] => [%5d %5d %5d]\n", priv->fir.num,
+							priv->fir.raw[priv->fir.num][BMA255_AXIS_X], priv->fir.raw[priv->fir.num][BMA255_AXIS_Y], priv->fir.raw[priv->fir.num][BMA255_AXIS_Z],
+							priv->fir.sum[BMA255_AXIS_X], priv->fir.sum[BMA255_AXIS_Y], priv->fir.sum[BMA255_AXIS_Z]);
+					}
+					priv->fir.num++;
+					priv->fir.idx++;
+				}
+				else
+				{
+					idx = priv->fir.idx % firlen;
+					priv->fir.sum[BMA255_AXIS_X] -= priv->fir.raw[idx][BMA255_AXIS_X];
+					priv->fir.sum[BMA255_AXIS_Y] -= priv->fir.raw[idx][BMA255_AXIS_Y];
+					priv->fir.sum[BMA255_AXIS_Z] -= priv->fir.raw[idx][BMA255_AXIS_Z];
+					priv->fir.raw[idx][BMA255_AXIS_X] = data[BMA255_AXIS_X];
+					priv->fir.raw[idx][BMA255_AXIS_Y] = data[BMA255_AXIS_Y];
+					priv->fir.raw[idx][BMA255_AXIS_Z] = data[BMA255_AXIS_Z];
+					priv->fir.sum[BMA255_AXIS_X] += data[BMA255_AXIS_X];
+					priv->fir.sum[BMA255_AXIS_Y] += data[BMA255_AXIS_Y];
+					priv->fir.sum[BMA255_AXIS_Z] += data[BMA255_AXIS_Z];
+					priv->fir.idx++;
+					data[BMA255_AXIS_X] = priv->fir.sum[BMA255_AXIS_X]/firlen;
+					data[BMA255_AXIS_Y] = priv->fir.sum[BMA255_AXIS_Y]/firlen;
+					data[BMA255_AXIS_Z] = priv->fir.sum[BMA255_AXIS_Z]/firlen;
+					if(atomic_read(&priv->trace) & BMA_TRC_FILTER)
+					{
+						GSE_LOG("add [%2d] [%5d %5d %5d] => [%5d %5d %5d] : [%5d %5d %5d]\n", idx,
+						priv->fir.raw[idx][BMA255_AXIS_X], priv->fir.raw[idx][BMA255_AXIS_Y], priv->fir.raw[idx][BMA255_AXIS_Z],
+						priv->fir.sum[BMA255_AXIS_X], priv->fir.sum[BMA255_AXIS_Y], priv->fir.sum[BMA255_AXIS_Z],
+						data[BMA255_AXIS_X], data[BMA255_AXIS_Y], data[BMA255_AXIS_Z]);
+					}
+				}
+			}
+		}
+#endif
+	}
+	return err;
+}
+/*----------------------------------------------------------------------------*/
+static int BMA255_ResetCalibration(struct i2c_client *client)
+{
+	struct bma255_i2c_data *obj = i2c_get_clientdata(client);
+#ifndef SW_CALIBRATION
+	u8 ofs[4]={0,0,0,0};
+#endif
+	int err;
+
+	#ifdef SW_CALIBRATION
+  err = 0;
+	#else
+		if(err = hwmsen_write_block(client, BMA255_REG_OFSX, ofs, 4))
+		{
+			GSE_ERR("error: %d\n", err);
+		}
+	#endif
+
+	memset(obj->cali_sw, 0x00, sizeof(obj->cali_sw));
+	memset(obj->offset, 0x00, sizeof(obj->offset));
+	return err;
+}
+/*----------------------------------------------------------------------------*/
+static int BMA255_CheckDeviceID(struct i2c_client *client)
+{
+	u8 databuf[2];
+	int res = 0;
+
+	memset(databuf, 0, sizeof(u8)*2);
+	databuf[0] = BMA255_REG_DEVID;
+
+	res = i2c_master_send(client, databuf, 0x1);
+	if(res <= 0)
+	{
+		res = i2c_master_send(client, databuf, 0x1);
+		if(res <= 0)
+		{
+			goto exit_BMA255_CheckDeviceID;
+		}
+	}
+
+	udelay(500);
+
+	databuf[0] = 0x0;
+	res = i2c_master_recv(client, databuf, 0x01);
+	if(res <= 0)
+	{
+		goto exit_BMA255_CheckDeviceID;
+	}
+
+	if(databuf[0]!=BMA255_FIXED_DEVID)
+	{
+		GSE_ERR("BMA255_CheckDeviceID %d failt!\n ", databuf[0]);
+		return BMA255_ERR_IDENTIFICATION;
+	}
+	else
+	{
+		GSE_LOG("BMA255_CheckDeviceID %d pass!\n ", databuf[0]);
+	}
+
+	exit_BMA255_CheckDeviceID:
+	if (res <= 0)
+	{
+		return BMA255_ERR_I2C;
+	}
+
+	return BMA255_SUCCESS;
+}
+/*----------------------------------------------------------------------------*/
+static int BMA255_SetPowerMode(struct i2c_client *client, bool enable)
+{
+	u8 databuf[2] = {0};
+	int res = 0;
+//	struct bma255_i2c_data *obj = i2c_get_clientdata(client);
+
+	GSE_FUN();
+
+	if(enable == sensor_power )
+	{
+		GSE_LOG("Sensor power status is newest!\n");
+		return BMA255_SUCCESS;
+	}
+
+	if(enable == TRUE)
+	{
+		databuf[1] = 0;
+	}
+	else
+	{
+		databuf[1] = BMA255_MEASURE_MODE;
+	}
+
+	databuf[0] = BMA255_REG_POWER_CTL;
+
+	res = i2c_master_send(client, databuf, 0x2);
+	if(res <= 0)
+	{
+		GSE_LOG("set power mode failed!\n");
+		return BMA255_ERR_I2C;
+	}
+	else
+	{
+		GSE_LOG("set power mode ok: 0x%02x\n", databuf[1]);
+	}
+	//GSE_LOG("BMA255_SetPowerMode ok!\n");
+
+	sensor_power = enable;
+	test_status = sensor_power;
+
+	mdelay(10);
+
+	return BMA255_SUCCESS;
+}
+/*----------------------------------------------------------------------------*/
+static int BMA255_SetDataFormat(struct i2c_client *client, u8 dataformat)
+{
+	struct bma255_i2c_data *obj = i2c_get_clientdata(client);
+	u8 databuf[2] = {0};
+	int res = 0;
+
+	databuf[1] = dataformat;
+	databuf[0] = BMA255_REG_DATA_FORMAT;
+
+	res = i2c_master_send(client, databuf, 0x2);
+	if(res <= 0)
+	{
+		return BMA255_ERR_I2C;
+	}
+	else
+	{
+		GSE_LOG("Set DataFormat: 0x%02x\n", dataformat);
+	}
+	//printk("BMA255_SetDataFormat OK! \n");
+
+	return BMA255_SetDataResolution(obj);
+}
+/*----------------------------------------------------------------------------*/
+static int BMA255_SetBWRate(struct i2c_client *client, u8 bwrate)
+{
+	u8 databuf[2] = {0};
+	int res = 0;
+
+	databuf[1] = bwrate;
+	databuf[0] = BMA255_REG_BW_RATE;
+
+	res = i2c_master_send(client, databuf, 0x2);
+	if(res <= 0)
+	{
+		return BMA255_ERR_I2C;
+	}
+	else
+	{
+		GSE_LOG("Set BWrate: 0x%02x\n", bwrate);
+	}
+
+	return BMA255_SUCCESS;
+}
+/*----------------------------------------------------------------------------*/
+static int BMA255_SetIntEnable(struct i2c_client *client, u8 intenable)
+{
+	int res = 0;
+
+	res = hwmsen_write_byte(client, BMA255_INT_REG_1, intenable);
+	if(res != BMA255_SUCCESS)
+	{
+		return res;
+	}
+	res = hwmsen_write_byte(client, BMA255_INT_REG_2, intenable);
+	if(res != BMA255_SUCCESS)
+	{
+		return res;
+	}
+	GSE_LOG("BMA255 interrupt was disabled\n");
+
+	/*for disable interrupt function*/
+
+	return BMA255_SUCCESS;
+}
+
+/*----------------------------------------------------------------------------*/
+static int bma255_init_client(struct i2c_client *client, int reset_cali)
+{
+	struct bma255_i2c_data *obj = i2c_get_clientdata(client);
+	int res = 0;
+	GSE_FUN();
+
+	res = BMA255_CheckDeviceID(client);
+	if(res != BMA255_SUCCESS)
+	{
+		return res;
+	}
+	printk("BMA255_CheckDeviceID ok \n");
+
+	res = BMA255_SetBWRate(client, BMA255_BW_100HZ);
+	if(res != BMA255_SUCCESS )
+	{
+		return res;
+	}
+	printk("BMA255_SetBWRate OK!\n");
+
+	res = BMA255_SetDataFormat(client, BMA255_RANGE_4G);
+	if(res != BMA255_SUCCESS)
+	{
+		return res;
+	}
+	printk("BMA255_SetDataFormat OK!\n");
+
+	gsensor_gain.x = gsensor_gain.y = gsensor_gain.z = obj->reso->sensitivity;
+
+	res = BMA255_SetIntEnable(client, 0x00);
+	if(res != BMA255_SUCCESS)
+	{
+		return res;
+	}
+	printk("BMA255 disable interrupt function!\n");
+
+	res = BMA255_SetPowerMode(client, false);
+	if(res != BMA255_SUCCESS)
+	{
+		return res;
+	}
+	printk("BMA255_SetPowerMode OK!\n");
+
+	if(0 != reset_cali)
+	{
+		/*reset calibration only in power on*/
+		res = BMA255_ResetCalibration(client);
+		if(res != BMA255_SUCCESS)
+		{
+			return res;
+		}
+	}
+	printk("bma255_init_client OK!\n");
+#ifdef CONFIG_BMA255_LOWPASS
+	memset(&obj->fir, 0x00, sizeof(obj->fir));
+#endif
+
+	mdelay(20);
+
+	return BMA255_SUCCESS;
+}
+/*----------------------------------------------------------------------------*/
+static int BMA255_ReadChipInfo(struct i2c_client *client, char *buf, int bufsize)
+{
+	u8 databuf[10];
+
+	memset(databuf, 0, sizeof(u8)*10);
+
+	if((NULL == buf)||(bufsize<=30))
+	{
+		return -1;
+	}
+
+	if(NULL == client)
+	{
+		*buf = 0;
+		return -2;
+	}
+
+	sprintf(buf, "BMA255 Chip");
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int BMA255_CompassReadData(struct i2c_client *client, char *buf, int bufsize)
+{
+	struct bma255_i2c_data *obj = (struct bma255_i2c_data*)i2c_get_clientdata(client);
+	u8 databuf[20];
+	int acc[BMA255_AXES_NUM];
+	int res = 0;
+	memset(databuf, 0, sizeof(u8)*10);
+
+	if(NULL == buf)
+	{
+		return -1;
+	}
+	if(NULL == client)
+	{
+		*buf = 0;
+		return -2;
+	}
+
+	if(sensor_power == FALSE)
+	{
+		res = BMA255_SetPowerMode(client, true);
+		if(res)
+		{
+			GSE_ERR("Power on bma255 error %d!\n", res);
+		}
+	}
+
+ res = BMA255_ReadData(client, obj->data);
+	if(res)
+	{
+		GSE_ERR("I2C error: ret value=%d", res);
+		return -3;
+	}
+	else
+	{
+		/*remap coordinate*/
+		acc[obj->cvt.map[BMA255_AXIS_X]] = obj->cvt.sign[BMA255_AXIS_X]*obj->data[BMA255_AXIS_X];
+		acc[obj->cvt.map[BMA255_AXIS_Y]] = obj->cvt.sign[BMA255_AXIS_Y]*obj->data[BMA255_AXIS_Y];
+		acc[obj->cvt.map[BMA255_AXIS_Z]] = obj->cvt.sign[BMA255_AXIS_Z]*obj->data[BMA255_AXIS_Z];
+
+		sprintf(buf, "%d %d %d", (s16)acc[BMA255_AXIS_X], (s16)acc[BMA255_AXIS_Y], (s16)acc[BMA255_AXIS_Z]);
+		if(atomic_read(&obj->trace) & BMA_TRC_IOCTL)
+		{
+			GSE_LOG("gsensor data for compass: %s!\n", buf);
+		}
+	}
+
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int BMA255_ReadSensorData(struct i2c_client *client, char *buf, int bufsize)
+{
+	struct bma255_i2c_data *obj = (struct bma255_i2c_data*)i2c_get_clientdata(client);
+	u8 databuf[20];
+	int acc[BMA255_AXES_NUM];
+	int res = 0;
+	memset(databuf, 0, sizeof(u8)*10);
+
+	if(NULL == buf)
+	{
+		return -1;
+	}
+	if(NULL == client)
+	{
+		*buf = 0;
+		return -2;
+	}
+
+	if(sensor_power == FALSE)
+	{
+		res = BMA255_SetPowerMode(client, true);
+		if(res)
+		{
+			GSE_ERR("Power on bma255 error %d!\n", res);
+		}
+	}
+
+	if(BMA255_ReadData(client, obj->data))
+	{
+		GSE_ERR("I2C error: ret value=%d", res);
+		return -3;
+	}
+	else
+	{
+		obj->data[BMA255_AXIS_X] += obj->cali_sw[BMA255_AXIS_X];
+		obj->data[BMA255_AXIS_Y] += obj->cali_sw[BMA255_AXIS_Y];
+		obj->data[BMA255_AXIS_Z] += obj->cali_sw[BMA255_AXIS_Z];
+
+		/*remap coordinate*/
+		acc[obj->cvt.map[BMA255_AXIS_X]] = obj->cvt.sign[BMA255_AXIS_X]*obj->data[BMA255_AXIS_X];
+		acc[obj->cvt.map[BMA255_AXIS_Y]] = obj->cvt.sign[BMA255_AXIS_Y]*obj->data[BMA255_AXIS_Y];
+		acc[obj->cvt.map[BMA255_AXIS_Z]] = obj->cvt.sign[BMA255_AXIS_Z]*obj->data[BMA255_AXIS_Z];
+
+		//Out put the mg
+		acc[BMA255_AXIS_X] = acc[BMA255_AXIS_X] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+		acc[BMA255_AXIS_Y] = acc[BMA255_AXIS_Y] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+		acc[BMA255_AXIS_Z] = acc[BMA255_AXIS_Z] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+		data_count++;
+		if(data_count > 100000)
+		{
+			data_count = 0;
+		}
+
+		sprintf(buf, "%04x %04x %04x %04x", acc[BMA255_AXIS_X], acc[BMA255_AXIS_Y], acc[BMA255_AXIS_Z], data_count);
+
+        /* Add for accel sensor data error debugging : start */
+        if((data_count%500)==0)
+            GSE_LOG("gsensor data: debug count = %d, x=%d, y=%d, z=%d!\n",data_count,acc[BMA255_AXIS_X], acc[BMA255_AXIS_Y], acc[BMA255_AXIS_Z]);
+        /* Add for accel sensor data error debugging : end */
+
+		if(atomic_read(&obj->trace) & BMA_TRC_IOCTL)
+		{
+			GSE_LOG("gsensor data: %s!\n", buf);
+		}
+	}
+
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int BMA255_ReadRawData(struct i2c_client *client, char *buf)
+{
+	struct bma255_i2c_data *obj = (struct bma255_i2c_data*)i2c_get_clientdata(client);
+	int res = 0;
+
+	if (!buf || !client)
+	{
+		return EINVAL;
+	}
+
+	if(BMA255_ReadData(client, obj->data))
+	{
+		GSE_ERR("I2C error: ret value=%d", res);
+		return EIO;
+	}
+	else
+	{
+		sprintf(buf, "BMA255_ReadRawData %04x %04x %04x", obj->data[BMA255_AXIS_X],
+			obj->data[BMA255_AXIS_Y], obj->data[BMA255_AXIS_Z]);
+	}
+
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int bma255_set_mode(struct i2c_client *client, unsigned char mode)
+{
+	int comres = 0;
+	unsigned char data[2] = {BMA255_EN_LOW_POWER__REG};
+
+	if ((client == NULL) || (mode >= 3))
+	{
+		return -1;
+	}
+
+	comres = hwmsen_read_block(client, BMA255_EN_LOW_POWER__REG, data+1, 1);
+	switch (mode) {
+	case BMA255_MODE_NORMAL:
+		data[1]  = BMA255_SET_BITSLICE(data[1], BMA255_EN_LOW_POWER, 0);
+		data[1]  = BMA255_SET_BITSLICE(data[1],	BMA255_EN_SUSPEND, 0);
+		break;
+	case BMA255_MODE_LOWPOWER:
+		data[1]  = BMA255_SET_BITSLICE(data[1],	BMA255_EN_LOW_POWER, 1);
+		data[1]  = BMA255_SET_BITSLICE(data[1],	BMA255_EN_SUSPEND, 0);
+		break;
+	case BMA255_MODE_SUSPEND:
+		data[1]  = BMA255_SET_BITSLICE(data[1],	BMA255_EN_LOW_POWER, 0);
+		data[1]  = BMA255_SET_BITSLICE(data[1],	BMA255_EN_SUSPEND, 1);
+		break;
+	default:
+		break;
+	}
+
+	comres = i2c_master_send(client, data, 2);
+
+	if(comres <= 0)
+	{
+		return BMA255_ERR_I2C;
+	}
+	else
+	{
+		return comres;
+	}
+}
+/*----------------------------------------------------------------------------*/
+static int bma255_get_mode(struct i2c_client *client, unsigned char *mode)
+{
+	int comres = 0;
+
+	if (client == NULL)
+	{
+		return -1;
+	}
+	comres = hwmsen_read_block(client,
+			BMA255_EN_LOW_POWER__REG, mode, 1);
+	*mode  = (*mode) >> 6;
+
+	return comres;
+}
+
+/*----------------------------------------------------------------------------*/
+static int bma255_set_range(struct i2c_client *client, unsigned char range)
+{
+	struct bma255_i2c_data *obj = (struct bma255_i2c_data*)i2c_get_clientdata(client);
+	int comres = 0;
+	unsigned char data[2] = {BMA255_RANGE_SEL__REG};
+
+	if (client == NULL)
+	{
+		return -1;
+	}
+	if (range == obj->range)
+	{
+		return 0;
+	}
+
+	comres = hwmsen_read_block(client,
+			BMA255_RANGE_SEL__REG, data+1, 1);
+
+	data[1]  = BMA255_SET_BITSLICE(data[1],
+			BMA255_RANGE_SEL, range);
+
+	comres = i2c_master_send(client, data, 2);
+	mdelay(1);
+
+	if(comres <= 0)
+	{
+		return BMA255_ERR_I2C;
+	}
+	else
+	{
+		obj->range = range;
+		return comres;
+	}
+}
+/*----------------------------------------------------------------------------*/
+static int bma255_get_range(struct i2c_client *client, unsigned char *range)
+{
+	int comres = 0;
+	unsigned char data;
+
+	if (client == NULL)
+	{
+		return -1;
+	}
+
+	comres = hwmsen_read_block(client, BMA255_RANGE_SEL__REG,	&data, 1);
+	*range = BMA255_GET_BITSLICE(data, BMA255_RANGE_SEL);
+
+	return comres;
+}
+/*----------------------------------------------------------------------------*/
+static int bma255_set_bandwidth(struct i2c_client *client, unsigned char bandwidth)
+{
+	int comres = 0;
+	unsigned char data[2] = {BMA255_BANDWIDTH__REG};
+
+	if (client == NULL)
+	{
+		return -1;
+	}
+
+	comres = hwmsen_read_block(client,
+			BMA255_BANDWIDTH__REG, data+1, 1);
+
+	data[1]  = BMA255_SET_BITSLICE(data[1],
+			BMA255_BANDWIDTH, bandwidth);
+
+	comres = i2c_master_send(client, data, 2);
+	mdelay(1);
+
+	if(comres <= 0)
+	{
+		return BMA255_ERR_I2C;
+	}
+	else
+	{
+		return comres;
+	}
+}
+/*----------------------------------------------------------------------------*/
+static int bma255_get_bandwidth(struct i2c_client *client, unsigned char *bandwidth)
+{
+	int comres = 0;
+	unsigned char data;
+
+	if (client == NULL)
+	{
+		return -1;
+	}
+
+	comres = hwmsen_read_block(client, BMA255_BANDWIDTH__REG, &data, 1);
+	data = BMA255_GET_BITSLICE(data, BMA255_BANDWIDTH);
+
+	if (data < 0x08)        //7.81Hz
+	{
+		*bandwidth = BMA255_BW_7_81HZ;
+	}
+	else if (data > 0x0f)   // 1000Hz
+	{
+		*bandwidth = BMA255_BW_1000HZ;
+	}
+	else
+	{
+		*bandwidth = data;
+	}
+	return comres;
+}
+/*----------------------------------------------------------------------------*/
+#ifdef BMA255_ACCEL_CALIBRATION
+static int bma255_set_offset_target(struct i2c_client *client, unsigned char channel, unsigned char offset)
+{
+	unsigned char data = 0;
+	int comres = 0;
+
+	switch (channel)
+	{
+		case BMA255_CUT_OFF:
+			comres = bma255_smbus_read_byte(client, BMA255_COMP_CUTOFF__REG, &data);
+			data = BMA255_SET_BITSLICE(data, BMA255_COMP_CUTOFF, offset);
+			comres = bma255_smbus_write_byte(client, BMA255_COMP_CUTOFF__REG, &data);
+			break;
+
+		case BMA255_OFFSET_TRIGGER_X:
+			comres = bma255_smbus_read_byte(client, BMA255_COMP_TARGET_OFFSET_X__REG, &data);
+			data = BMA255_SET_BITSLICE(data, BMA255_COMP_TARGET_OFFSET_X, offset);
+			comres = bma255_smbus_write_byte(client, BMA255_COMP_TARGET_OFFSET_X__REG, &data);
+			break;
+
+		case BMA255_OFFSET_TRIGGER_Y:
+			comres = bma255_smbus_read_byte(client, BMA255_COMP_TARGET_OFFSET_Y__REG, &data);
+			data = BMA255_SET_BITSLICE(data, BMA255_COMP_TARGET_OFFSET_Y, offset);
+			comres = bma255_smbus_write_byte(client, BMA255_COMP_TARGET_OFFSET_Y__REG, &data);
+			break;
+
+		case BMA255_OFFSET_TRIGGER_Z:
+			comres = bma255_smbus_read_byte(client, BMA255_COMP_TARGET_OFFSET_Z__REG, &data);
+			data = BMA255_SET_BITSLICE(data, BMA255_COMP_TARGET_OFFSET_Z, offset);
+			comres = bma255_smbus_write_byte(client, BMA255_COMP_TARGET_OFFSET_Z__REG, &data);
+			break;
+
+		default:
+			comres = -1;
+			break;
+	}
+
+	return comres;
+}
+
+static int bma255_get_cal_ready(struct i2c_client *client, unsigned char *calrdy)
+{
+	int comres = 0 ;
+	unsigned char data =0;
+
+	comres = bma255_smbus_read_byte(client, BMA255_FAST_CAL_RDY_S__REG, &data);
+	data = BMA255_GET_BITSLICE(data, BMA255_FAST_CAL_RDY_S);
+	*calrdy = data;
+
+	return comres;
+}
+
+static int bma255_set_cal_trigger(struct i2c_client *client, unsigned char caltrigger)
+{
+	int comres = 0;
+	unsigned char data = 0;
+	struct bma255_i2c_data *bma255 = i2c_get_clientdata(client);
+
+	if(atomic_read(&bma255->fast_calib_rslt) != 0)
+	{
+		atomic_set(&bma255->fast_calib_rslt, 0);
+		GSE_LOG(KERN_INFO "[set] bma2X2->fast_calib_rslt:%d\n",atomic_read(&bma255->fast_calib_rslt));
+	}
+
+	comres = bma255_smbus_read_byte(client, BMA255_CAL_TRIGGER__REG, &data);
+	data = BMA255_SET_BITSLICE(data, BMA255_CAL_TRIGGER, caltrigger);
+	comres = bma255_smbus_write_byte(client, BMA255_CAL_TRIGGER__REG, &data);
+
+	return comres;
+}
+
+static int bma255_set_offset_x(struct i2c_client *client, unsigned char offsetfilt)
+{
+	int comres = 0;
+	unsigned char data;
+
+	data =  offsetfilt;
+	comres = bma255_smbus_write_byte(client, BMA255_OFFSET_X_AXIS_REG, &data);
+
+	return comres;
+}
+
+static int bma255_get_offset_x(struct i2c_client *client, unsigned char *offsetfilt)
+{
+	int comres = 0;
+	unsigned char data = 0;
+
+	comres = bma255_smbus_read_byte(client, BMA255_OFFSET_X_AXIS_REG, &data);
+	*offsetfilt = data;
+
+	return comres;
+}
+
+static int bma255_set_offset_y(struct i2c_client *client, unsigned char offsetfilt)
+{
+	int comres = 0;
+	unsigned char data;
+
+	data =  offsetfilt;
+	comres = bma255_smbus_write_byte(client, BMA255_OFFSET_Y_AXIS_REG, &data);
+
+	return comres;
+}
+
+static int bma255_get_offset_y(struct i2c_client *client, unsigned char *offsetfilt)
+{
+	int comres = 0;
+	unsigned char data =0;
+
+	comres = bma255_smbus_read_byte(client, BMA255_OFFSET_Y_AXIS_REG, &data);
+	*offsetfilt = data;
+
+	return comres;
+}
+
+static int bma255_set_offset_z(struct i2c_client *client, unsigned char offsetfilt)
+{
+	int comres = 0;
+	unsigned char data;
+
+	data =  offsetfilt;
+	comres = bma255_smbus_write_byte(client, BMA255_OFFSET_Z_AXIS_REG, &data);
+
+	return comres;
+}
+
+static int bma255_get_offset_z(struct i2c_client *client, unsigned char *offsetfilt)
+{
+	int comres = 0;
+	unsigned char data = 0;
+
+	comres = bma255_smbus_read_byte(client, BMA255_OFFSET_Z_AXIS_REG, &data);
+	*offsetfilt = data;
+
+	return comres;
+}
+
+static int bma255_read_accel_xyz(struct i2c_client *client, struct bma255acc *acc)
+{
+	int comres = 0;
+	unsigned char data[6];
+
+	comres = bma255_smbus_read_byte_block(client, BMA255_ACC_X_LSB__REG, data, 6);
+
+	acc->x = BMA255_GET_BITSLICE(data[0], BMA255_ACC_X_LSB) | (BMA255_GET_BITSLICE(data[1], BMA255_ACC_X_MSB)<<(BMA255_ACC_X_LSB__LEN));
+	acc->x = acc->x << (sizeof(short)*8-(BMA255_ACC_X_LSB__LEN + BMA255_ACC_X_MSB__LEN));
+	acc->x = acc->x >> (sizeof(short)*8-(BMA255_ACC_X_LSB__LEN + BMA255_ACC_X_MSB__LEN));
+
+	acc->y = BMA255_GET_BITSLICE(data[2], BMA255_ACC_Y_LSB) | (BMA255_GET_BITSLICE(data[3], BMA255_ACC_Y_MSB)<<(BMA255_ACC_Y_LSB__LEN));
+	acc->y = acc->y << (sizeof(short)*8-(BMA255_ACC_Y_LSB__LEN + BMA255_ACC_Y_MSB__LEN));
+	acc->y = acc->y >> (sizeof(short)*8-(BMA255_ACC_Y_LSB__LEN + BMA255_ACC_Y_MSB__LEN));
+
+	acc->z = BMA255_GET_BITSLICE(data[4], BMA255_ACC_Z_LSB) | (BMA255_GET_BITSLICE(data[5], BMA255_ACC_Z_MSB)<<(BMA255_ACC_Z_LSB__LEN));
+	acc->z = acc->z << (sizeof(short)*8-(BMA255_ACC_Z_LSB__LEN + BMA255_ACC_Z_MSB__LEN));
+	acc->z = acc->z >> (sizeof(short)*8-(BMA255_ACC_Z_LSB__LEN + BMA255_ACC_Z_MSB__LEN));
+
+	return comres;
+}
+
+static ssize_t show_cali_value(struct device_driver *ddri, char *buf)
+{
+	struct i2c_client *client = bma255_i2c_client;
+    struct bma255_i2c_data *bma255 = i2c_get_clientdata(client);
+	unsigned char offset_x,offset_y,offset_z;
+
+	if(bma255_get_offset_x(bma255->client, &offset_x) < 0)
+		return -EINVAL;
+	if(bma255_get_offset_y(bma255->client, &offset_y) < 0)
+		return -EINVAL;
+	if(bma255_get_offset_z(bma255->client, &offset_z) < 0)
+		return -EINVAL;
+
+    GSE_LOG("offset_x: %d, offset_y: %d, offset_z: %d\n",offset_x,offset_y,offset_z);
+
+	return snprintf(buf, PAGE_SIZE, "%d %d %d \n", (unsigned int)offset_x, (unsigned int)offset_y, (unsigned int)offset_z);
+}
+
+#if defined(CALIBRATION_TO_FILE)
+static int bma255_calibration_save(int *cal)
+{
+   int fd;
+   int i;
+   int res;
+   char *fname = "/persist-lg/sensor/sensor_cal_data.txt";
+   mm_segment_t old_fs = get_fs();
+   char temp_str[5];
+
+   set_fs(KERNEL_DS);
+
+   fd = sys_open(fname,O_WRONLY|O_CREAT|S_IROTH, 0666);
+	if(fd< 0){
+           GSE_LOG("[%s] File Open Error !!!(%d)\n", __func__,fd);
+   		   sys_close(fd);
+	       return -EINVAL;
+		}
+   for(i=0;i<6;i++)
+   {
+   		memset(temp_str,0x00,sizeof(temp_str));
+   		sprintf(temp_str, "%d", cal[i]);
+	   	res = sys_write(fd,temp_str, sizeof(temp_str));
+	   
+   		if(res<0) {
+   			GSE_LOG("[%s] Write Error !!!\n", __func__);
+   			sys_close(fd);
+	 		return -EINVAL;
+   		}
+   	}
+   sys_fsync(fd);
+   sys_close(fd);
+
+   sys_chmod(fname, 0664);
+   set_fs(old_fs);
+   GSE_LOG("bma255_calibration_save Done.\n");
+   
+   return 0;
+}
+
+static int bma255_calibration_read(int *cal_read)
+{
+   int fd;
+   int i;  
+   int res;
+   char *fname = "/persist-lg/sensor/sensor_cal_data.txt";
+   mm_segment_t old_fs = get_fs();
+   char temp_str[5];
+   
+   set_fs(KERNEL_DS);
+
+   fd = sys_open(fname, O_RDONLY, 0);
+	if(fd< 0){
+         GSE_LOG("[%s] File Open Error !!!\n", __func__);
+	     sys_close(fd);
+     	 return -EINVAL;
+		}
+   for(i=0;i<6;i++)
+   {
+   		memset(temp_str,0x00,sizeof(temp_str));   
+        res = sys_read(fd, temp_str, sizeof(temp_str));
+         if(res<0){
+   			 GSE_LOG("[%s] Read Error !!!\n", __func__);
+              sys_close(fd);
+              return -EINVAL;
+         	}
+        sscanf(temp_str,"%d",&cal_read[i]);
+        GSE_LOG("bma255_calibration_read : cal_read[%d]=%d\n",i,cal_read[i]);
+   	}
+   sys_close(fd);
+   set_fs(old_fs);
+   GSE_LOG("bma255_calibration_read Done.\n");
+   
+   return 0;
+}
+
+/* Make gsensor cal file to save calibration data */
+/* 1. If file exist, do nothing                  */
+/* 2. If file does not exist, read cal data from misc2 (for L-OS upgrade model) */
+static int make_cal_data_file(void)
+{
+   int fd;
+   int i;
+   int res;
+   char *fname = "/persist-lg/sensor/sensor_cal_data.txt";
+   mm_segment_t old_fs = get_fs();
+   char temp_str[5];
+   int cal_misc[6]={0,};
+
+   set_fs(KERNEL_DS);
+   
+   fd = sys_open(fname, O_RDONLY, 0); /* Cal file exist check */
+   
+   if(fd==-2)
+   GSE_LOG("[%s] Open Cal File Error. Need to make file !!!(%d)\n", __func__,fd);
+   
+ 	if(fd< 0){
+	       fd = sys_open(fname,O_WRONLY|O_CREAT|S_IROTH, 0666);
+	     if(fd< 0){
+	       GSE_LOG("[%s] Open or Make Cal File Error !!!(%d)\n", __func__,fd);
+  		   sys_close(fd);
+	       return -EINVAL;
+		   }
+
+#if 0
+	     if(LGE_FacReadAccelerometerCalibration((unsigned int*)cal_misc) == TRUE)
+	     	{
+		     GSE_LOG("Read Cal from misc Old x: %d, y: %d, z: %d\n",cal_misc[3],cal_misc[4],cal_misc[5]);
+		     GSE_LOG("Read Cal from misc Now x: %d, y: %d, z: %d\n",cal_misc[0],cal_misc[1],cal_misc[2]);
+	     	}
+         else{
+		     GSE_LOG("Read Cal from misc Error !!!\n");
+         	}
+#endif
+
+		     /* set default cal */   
+			  if((cal_misc[0]==0)&&(cal_misc[1]==0)&&(cal_misc[2]==0))
+			  	{
+			  	 #if defined(TARGET_Y90)
+                   cal_misc[0]=3;
+                   cal_misc[1]=6;
+                   cal_misc[2]=5;
+                 #endif 
+		           GSE_LOG("Default Cal is set : %d, %d, %d\n",cal_misc[0],cal_misc[1],cal_misc[2]);					  	
+			  	}
+    	      /* set default cal */   
+
+		   for(i=0;i<6;i++)
+		   {
+		   		memset(temp_str,0x00,sizeof(temp_str));
+		   		sprintf(temp_str, "%d", cal_misc[i]);
+			   	res = sys_write(fd,temp_str, sizeof(temp_str));
+			   
+		   		if(res<0) {
+		   			GSE_LOG("[%s] Write Cal Error !!!\n", __func__);
+		   			sys_close(fd);
+			 		return -EINVAL;
+		   		}
+		   	}
+         GSE_LOG("make_cal_data_file Done.\n"); 
+		}
+	else
+         GSE_LOG("Sensor Cal File exist.\n"); 
+   
+   sys_fsync(fd);
+   sys_close(fd);
+
+   sys_chmod(fname, 0664);
+   set_fs(old_fs);
+   return 0;
+}
+#endif
+
+#if 1 //motion sensor cal backup
+static ssize_t show_cali_backup_value(struct device_driver *ddri, char *buf)
+{
+	struct i2c_client *client = bma255_i2c_client;
+    struct bma255_i2c_data *bma255 = i2c_get_clientdata(client);
+	unsigned char offset_x,offset_y,offset_z;
+	int cal_backup[6]={0,};
+//	int i;
+
+#if defined(CALIBRATION_TO_FILE)
+   if(bma255_calibration_read(cal_backup) == 0)
+		GSE_LOG("previous offset_x: %d,  offset_y: %d,  offset_z: %d\n",cal_backup[3],cal_backup[4],cal_backup[5]);
+   else
+        GSE_LOG("Fail to read previous gsensor cal value from Cal File\n");
+#else
+	if(LGE_FacReadAccelerometerCalibration((unsigned int*)cal_backup) == TRUE)
+		GSE_LOG("Read from LGPServer gsensor previous offset_x: %d,  offset_y: %d,  offset_z: %d\n",cal_backup[3],cal_backup[4],cal_backup[5]);
+	else
+        GSE_LOG("Fail to read previous gsensor cal value from LGPserver\n");
+#endif
+
+	if(bma255_get_offset_x(bma255->client, &offset_x) < 0)
+		return -EINVAL;
+	if(bma255_get_offset_y(bma255->client, &offset_y) < 0)
+		return -EINVAL;
+	if(bma255_get_offset_z(bma255->client, &offset_z) < 0)
+		return -EINVAL;
+
+    GSE_LOG("Current offset_x: %d, offset_y: %d, offset_z: %d\n",offset_x,offset_y,offset_z);
+
+	return snprintf(buf, PAGE_SIZE, "Old %d %d %d   Now %d %d %d \n", (unsigned int)cal_backup[3], (unsigned int)cal_backup[4], (unsigned int)cal_backup[5], (unsigned int)offset_x, (unsigned int)offset_y, (unsigned int)offset_z);
+}
+#endif
+
+static ssize_t store_cali_value(struct device_driver *ddri, char *buf, size_t count)
+{
+	struct i2c_client *client = bma255_i2c_client;
+	struct bma255_i2c_data *bma255 = i2c_get_clientdata(client);
+	int err =0;
+	int offset_x,offset_y,offset_z;
+	unsigned char offsets[3];
+//	int dat[BMA255_AXES_NUM];
+
+	if(!strncmp(buf, "rst", 3))
+	{
+		if(BMA255_ResetCalibration(client))
+		{
+			GSE_ERR("reset offset err = %d\n", err);
+		}
+	}
+	else if(3 == sscanf(buf, "%d %d %d", &offset_x, &offset_y, &offset_z))
+	{
+		GSE_LOG("store_cali_value: x=%d, y=%d, z=%d\n", offset_x, offset_y, offset_z);
+		offsets[0] = (unsigned char)offset_x;
+		offsets[1] = (unsigned char)offset_y;
+		offsets[2] = (unsigned char)offset_z;
+		if(bma255_set_offset_x(bma255->client, (unsigned char)offsets[0]) < 0)
+			return -EINVAL;
+		if(bma255_set_offset_y(bma255->client, (unsigned char)offsets[1]) < 0)
+			return -EINVAL;
+		if(bma255_set_offset_z(bma255->client, (unsigned char)offsets[2]) < 0)
+			return -EINVAL;
+		GSE_LOG("store_cali_value success\n");
+	}
+	else
+	{
+		GSE_ERR("invalid format\n");
+	}
+
+	return count;
+}
+
+static ssize_t bma255_fast_calibration_x_show(struct device_driver *ddri, char *buf)
+{
+	struct i2c_client *client = bma255_i2c_client;
+	struct bma255_i2c_data *bma255 = i2c_get_clientdata(client);
+
+	return sprintf(buf, "%d\n", atomic_read(&bma255->fast_calib_x_rslt));
+}
+
+static ssize_t bma255_fast_calibration_x_store(struct device_driver *ddri, char *buf, size_t count)
+{
+	unsigned long data;
+	signed char tmp;
+	unsigned char timeout = 0;
+	int error;
+	struct i2c_client *client =bma255_i2c_client;
+	struct bma255_i2c_data *bma255 = i2c_get_clientdata(client);
+	test_status = 4;	// calibration status
+ 
+ error = strict_strtoul(buf, 10, &data);
+	if (error)
+		return error;
+	atomic_set(&bma255->fast_calib_x_rslt, 0);
+
+    if(bma255_do_calibration() != BMA255_SUCCESS)
+    {
+    	atomic_set(&bma255->fast_calib_x_rslt, 0);
+    	return -EINVAL;
+    }
+
+	if (bma255_set_offset_target(bma255->client, 1, (unsigned char)data) < 0)
+		return -EINVAL;
+
+	if (bma255_set_cal_trigger(bma255->client, 1) < 0)
+		return -EINVAL;
+
+	do{
+		mdelay(2);
+		bma255_get_cal_ready(bma255->client, &tmp);
+
+		GSE_LOG(KERN_INFO "x wait 2ms cal ready flag is %d\n", tmp);
+
+		timeout++;
+		if (timeout == 50)
+		{
+			GSE_LOG(KERN_INFO "get fast calibration ready error\n");
+			return -EINVAL;
+		}
+	} while (tmp == 0);
+
+	atomic_set(&bma255->fast_calib_x_rslt, 1);
+	GSE_LOG(KERN_INFO "x axis fast calibration finished\n");
+
+	return count;
+}
+
+static ssize_t bma255_fast_calibration_y_show(struct device_driver *ddri, char *buf)
+{
+
+	struct i2c_client *client = bma255_i2c_client;
+	struct bma255_i2c_data *bma255 = i2c_get_clientdata(client);
+
+	return sprintf(buf, "%d\n", atomic_read(&bma255->fast_calib_y_rslt));
+}
+
+static ssize_t bma255_fast_calibration_y_store(struct device_driver *ddri, char *buf, size_t count)
+{
+	unsigned long data;
+	signed char tmp;
+	unsigned char timeout = 0;
+	int error;
+	struct i2c_client *client = bma255_i2c_client;
+	struct bma255_i2c_data *bma255 = i2c_get_clientdata(client);
+ 
+ error = strict_strtoul(buf, 10, &data);
+	if (error)
+		return error;
+	atomic_set(&bma255->fast_calib_y_rslt, 0);
+
+	if (bma255_set_offset_target(bma255->client, 2, (unsigned char)data) < 0)
+		return -EINVAL;
+
+	if (bma255_set_cal_trigger(bma255->client, 2) < 0)
+		return -EINVAL;
+
+	do {
+		mdelay(2);
+		bma255_get_cal_ready(bma255->client, &tmp);
+
+		GSE_LOG(KERN_INFO "y wait 2ms cal ready flag is %d\n", tmp);
+
+		timeout++;
+		if (timeout == 50)
+		{
+			GSE_LOG(KERN_INFO "get fast calibration ready error\n");
+			return -EINVAL;
+		}
+	} while (tmp == 0);
+
+	atomic_set(&bma255->fast_calib_y_rslt, 1);
+	GSE_LOG(KERN_INFO "y axis fast calibration finished\n");
+
+	return count;
+}
+
+static ssize_t bma255_fast_calibration_z_show(struct device_driver *ddri, char *buf)
+{
+	struct i2c_client *client = bma255_i2c_client;
+	struct bma255_i2c_data *bma255 = i2c_get_clientdata(client);
+
+	return sprintf(buf, "%d\n", atomic_read(&bma255->fast_calib_z_rslt));
+}
+
+static ssize_t bma255_fast_calibration_z_store(struct device_driver *ddri, char *buf, size_t count)
+{
+	unsigned long data;
+	signed char tmp;
+	unsigned char timeout = 0;
+	int error;
+	struct i2c_client *client = bma255_i2c_client;
+	struct bma255_i2c_data *bma255 = i2c_get_clientdata(client);
+
+ error = strict_strtoul(buf, 10, &data);
+	if (error)
+		return error;
+	atomic_set(&bma255->fast_calib_z_rslt, 0);
+
+	if (bma255_set_offset_target(bma255->client, 3, (unsigned char)data) < 0)
+		return -EINVAL;
+
+	if (bma255_set_cal_trigger(bma255->client, 3) < 0)
+		return -EINVAL;
+
+	do {
+		mdelay(2);
+		bma255_get_cal_ready(bma255->client, &tmp);
+
+		GSE_LOG(KERN_INFO " z wait 2ms cal ready flag is %d\n", tmp);
+
+		timeout++;
+		if (timeout == 50)
+		{
+			GSE_LOG(KERN_INFO "get fast calibration ready error\n");
+			return -EINVAL;
+		}
+	} while (tmp == 0);
+
+	atomic_set(&bma255->fast_calib_z_rslt, 1);
+	GSE_LOG(KERN_INFO "z axis fast calibration finished\n");
+
+	test_status = sensor_power;
+
+	return count;
+}
+
+/* LGE_BSP_COMMON LGE_CHANGE_S 140228 : Calibration for user Apps */
+static int bma255_runCalibration(void)
+{
+//	signed char tmp;
+//	unsigned char timeout = 0;
+	struct i2c_client *client = bma255_i2c_client;
+	struct bma255_i2c_data *bma255 = i2c_get_clientdata(client);
+	unsigned char backup_offset_x, backup_offset_y, backup_offset_z;
+	int res = 0;
+	int res2 = 0;
+	#if 0 //motion sensor cal backup
+	int cali[3];
+	#else
+	int cali[6];  
+	#endif
+
+	GSE_FUN();
+	if(bma255_get_offset_x(bma255->client, &backup_offset_x) < 0)
+		return FALSE;
+	if(bma255_get_offset_y(bma255->client, &backup_offset_y) < 0)
+		return FALSE;
+	if(bma255_get_offset_z(bma255->client, &backup_offset_z) < 0)
+		return FALSE;
+
+	GSE_LOG("backup_offset_x: %d, backup_offset_y: %d, backup_offset_z: %d\n",backup_offset_x,backup_offset_y,backup_offset_z);
+
+	if(sensor_power == FALSE)
+	{
+		res = BMA255_SetPowerMode(client, true);
+		if(res)
+		{
+			GSE_ERR("Power on bma255 error %d!\n", res);
+			return FALSE;
+		}
+	}
+
+	res2= BMA255_SetDataFormat(client, BMA255_RANGE_2G);
+	if (res2 < 0)//change range before calibration
+	{
+		GSE_ERR("SetDataFormat 2G error");
+	}
+	res = bma255_do_calibration();
+
+	res2 = BMA255_SetDataFormat(client, BMA255_RANGE_4G);
+	if (res2 < 0)//change range before calibration
+	{
+		GSE_ERR("SetDataFormat 4G error");
+	}
+
+	if(res != BMA255_SUCCESS)
+	{
+		GSE_LOG("Fail flatDetection, backup offset\n");
+		if(bma255_set_offset_x(bma255->client, (unsigned char)backup_offset_x) < 0)
+			return -EINVAL;
+		if(bma255_set_offset_y(bma255->client, (unsigned char)backup_offset_y) < 0)
+			return -EINVAL;
+		if(bma255_set_offset_z(bma255->client, (unsigned char)backup_offset_z) < 0)
+			return -EINVAL;
+		GSE_LOG("Recovery backup cal value: x=%d, y=%d, z=%d\n", backup_offset_x, backup_offset_y, backup_offset_z);
+
+		if(res==BMA255_ERR_SETUP_FAILURE)
+			return BMA255_ERR_SETUP_FAILURE;
+		else
+			return BMA255_ERR_STATUS;
+    }
+	else
+	{
+       #if 1 //motion sensor cal backup
+		cali[3] = (int)backup_offset_x;
+		cali[4] = (int)backup_offset_y;
+		cali[5] = (int)backup_offset_z;
+      #endif
+	
+        bma255_get_offset_x(bma255->client, &backup_offset_x);
+        bma255_get_offset_y(bma255->client, &backup_offset_y);
+        bma255_get_offset_z(bma255->client, &backup_offset_z);
+        GSE_LOG("new_offset_x: %d, new_offset_y: %d, new_offset_z: %d\n",(int)backup_offset_x,(int)backup_offset_y,(int)backup_offset_z);
+		cali[0] = (int)backup_offset_x;
+		cali[1] = (int)backup_offset_y;
+		cali[2] = (int)backup_offset_z;
+        #if defined(CALIBRATION_TO_FILE)
+         bma255_calibration_save(cali);
+        #else
+		 if(LGE_FacWriteAccelerometerCalibration((unsigned int*)cali) == TRUE)
+		 {
+		 	atomic_set(&bma255->fast_calib_rslt, 1);
+		 	GSE_LOG("Calibration factory write END\n");
+		 }
+		#endif
+	}
+
+	return BMA255_SUCCESS;
+}
+/* LGE_BSP_COMMON LGE_CHANGE_E 140228 : Calibration for user Apps */
+
+static int bma255_do_calibration(void)
+{
+	signed char tmp;
+	unsigned char timeout = 0;
+	unsigned int timeout_shaking = 0;
+	int sum[3] = {0, };
+	int err = 0;
+	struct i2c_client *client =bma255_i2c_client;
+	struct bma255_i2c_data *bma255 = i2c_get_clientdata(client);
+	struct bma255acc acc_cal;
+	struct bma255acc acc_cal_pre;
+
+	GSE_FUN();
+	test_status = 4;	// calibration status
+   /* set axis off set to zero */
+	if(bma255_set_offset_x(bma255->client, (unsigned char)0) < 0)
+		return -EINVAL;
+    if(bma255_set_offset_y(bma255->client, (unsigned char)0) < 0)
+ 		return -EINVAL;
+   	if(bma255_set_offset_z(bma255->client, (unsigned char)0) < 0)
+		return -EINVAL;
+   /* set axis off set to zero */
+
+	mdelay(20);
+
+	bma255_read_accel_xyz(bma255->client, &acc_cal_pre);
+	do{
+		mdelay(20);
+		bma255_read_accel_xyz(bma255->client, &acc_cal);
+
+		GSE_LOG(KERN_INFO "===============moved x=============== timeout = %d\n",timeout_shaking);
+		GSE_LOG(KERN_INFO "(%d, %d, %d) (%d, %d, %d)\n", acc_cal_pre.x,	acc_cal_pre.y,	acc_cal_pre.z, acc_cal.x,acc_cal.y,acc_cal.z );
+
+		if((abs(acc_cal.x - acc_cal_pre.x) > BMA255_SHAKING_DETECT_THRESHOLD)
+			|| (abs((acc_cal.y - acc_cal_pre.y)) > BMA255_SHAKING_DETECT_THRESHOLD)
+			|| (abs((acc_cal.z - acc_cal_pre.z)) > BMA255_SHAKING_DETECT_THRESHOLD))
+		{
+			atomic_set(&bma255->fast_calib_rslt, 0);
+			GSE_LOG(KERN_INFO "===============shaking x===============\n");
+			return -EINVAL;
+		}
+		else
+		{
+         /* Calibration zero-g offset check */
+            sum[BMA255_AXIS_X] += acc_cal.x;
+            sum[BMA255_AXIS_Y] += acc_cal.y;
+            sum[BMA255_AXIS_Z] += acc_cal.z;
+         /* Calibration zero-g offset check */
+
+			acc_cal_pre.x = acc_cal.x;
+			acc_cal_pre.y = acc_cal.y;
+			acc_cal_pre.z = acc_cal.z;
+		}
+		timeout_shaking++;
+		GSE_LOG(KERN_INFO "===============timeout_shaking: %d=============== \n",timeout_shaking);
+	} while(timeout_shaking < 10);
+
+	GSE_LOG(KERN_INFO "===============complete shaking check===============\n");
+#if 1  /* Calibration zero-g offset check */
+        // check zero-g offset
+        if((abs(sum[BMA255_AXIS_X]/CALIBRATION_DATA_AMOUNT) >TESTLIMIT_XY) ||
+            (abs(sum[BMA255_AXIS_Y]/CALIBRATION_DATA_AMOUNT) >TESTLIMIT_XY) ||
+            ((abs(sum[BMA255_AXIS_Z]/CALIBRATION_DATA_AMOUNT) > TESTLIMIT_Z_USL_LSB) || (abs(sum[BMA255_AXIS_Z]/CALIBRATION_DATA_AMOUNT) < TESTLIMIT_Z_LSL_LSB)))
+        {
+            GSE_LOG("Calibration zero-g offset check failed (%d, %d, %d)\n", sum[BMA255_AXIS_X]/CALIBRATION_DATA_AMOUNT,
+                                                                    sum[BMA255_AXIS_Y]/CALIBRATION_DATA_AMOUNT, sum[BMA255_AXIS_Z]/CALIBRATION_DATA_AMOUNT);
+			atomic_set(&bma255->fast_calib_rslt, 0);
+			return BMA255_ERR_SETUP_FAILURE;
+        }
+#endif  /* Calibration zero-g offset check */
+
+	GSE_LOG(KERN_INFO "===============complete zero-g check===============\n");
+
+	atomic_set(&bma255->fast_calib_x_rslt, 0);
+	if (bma255_set_offset_target(bma255->client, 1, (unsigned char)0) < 0)
+		return -EINVAL;
+	if (bma255_set_cal_trigger(bma255->client, 1) < 0)
+		return -EINVAL;
+	do{
+		mdelay(2);
+		bma255_get_cal_ready(bma255->client, &tmp);
+
+		GSE_LOG(KERN_INFO "x wait 2ms cal ready flag is %d\n", tmp);
+
+		timeout++;
+		if (timeout == 50)
+		{
+			GSE_LOG(KERN_INFO "get fast calibration ready error\n");
+			return -EINVAL;
+		}
+	} while (tmp == 0);
+	atomic_set(&bma255->fast_calib_x_rslt, 1);
+	GSE_LOG(KERN_INFO "===============x axis fast calibration finished\n");
+
+	atomic_set(&bma255->fast_calib_y_rslt, 0);
+	if (bma255_set_offset_target(bma255->client, 2, (unsigned char)0) < 0)
+		return -EINVAL;
+	if (bma255_set_cal_trigger(bma255->client, 2) < 0)
+		return -EINVAL;
+	do {
+		mdelay(2);
+		bma255_get_cal_ready(bma255->client, &tmp);
+
+		GSE_LOG(KERN_INFO "y wait 2ms cal ready flag is %d\n", tmp);
+
+		timeout++;
+		if (timeout == 50)
+		{
+			GSE_LOG(KERN_INFO "get fast calibration ready error\n");
+			return -EINVAL;
+		}
+	} while (tmp == 0);
+	atomic_set(&bma255->fast_calib_y_rslt, 1);
+	GSE_LOG(KERN_INFO "===============y axis fast calibration finished\n");
+
+	atomic_set(&bma255->fast_calib_z_rslt, 0);
+	if (bma255_set_offset_target(bma255->client, 3, (unsigned char)2) < 0)
+		return -EINVAL;
+	if (bma255_set_cal_trigger(bma255->client, 3) < 0)
+		return -EINVAL;
+	do {
+		mdelay(2);
+		bma255_get_cal_ready(bma255->client, &tmp);
+
+		GSE_LOG(KERN_INFO " z wait 2ms cal ready flag is %d\n", tmp);
+
+		timeout++;
+		if (timeout == 50)
+		{
+			GSE_LOG(KERN_INFO "get fast calibration ready error\n");
+			return -EINVAL;
+		}
+	} while (tmp == 0);
+	atomic_set(&bma255->fast_calib_z_rslt, 1);
+	GSE_LOG(KERN_INFO "===============z axis fast calibration finished\n");
+
+
+	test_status = sensor_power;
+
+	return err;
+}
+
+#endif
+
+#if 0
+static int bma255_get_selftest(struct i2c_client *client)
+{
+	int value = 0;
+    struct bma255_i2c_data *obj = i2c_get_clientdata(client);
+
+	return value;
+}
+#endif
+
+static ssize_t show_chipinfo_value(struct device_driver *ddri, char *buf)
+{
+	struct i2c_client *client = bma255_i2c_client;
+	char strbuf[BMA255_BUFSIZE]={0,};
+	if(NULL == client)
+	{
+		GSE_ERR("i2c client is null!!\n");
+		return 0;
+	}
+
+	BMA255_ReadChipInfo(client, strbuf, BMA255_BUFSIZE);
+	return snprintf(buf, PAGE_SIZE, "%s\n", strbuf);
+}
+
+#if 0
+static ssize_t gsensor_init(struct device_driver *ddri, char *buf, size_t count)
+{
+	struct i2c_client *client = bma255_i2c_client;
+	char strbuf[BMA255_BUFSIZE]={0,};
+
+	if(NULL == client)
+	{
+		GSE_ERR("i2c client is null!!\n");
+		return 0;
+	}
+	bma255_init_client(client, 1);
+	return snprintf(buf, PAGE_SIZE, "%s\n", strbuf);
+}
+#endif
+
+/*----------------------------------------------------------------------------*/
+/* g sensor opmode for compass tilt compensation
+ */
+static ssize_t show_cpsopmode_value(struct device_driver *ddri, char *buf)
+{
+	unsigned char data;
+
+	if (bma255_get_mode(bma255_i2c_client, &data) < 0)
+	{
+		return sprintf(buf, "Read error\n");
+	}
+	else
+	{
+		return sprintf(buf, "%d\n", data);
+	}
+}
+
+/*----------------------------------------------------------------------------*/
+/* g sensor opmode for compass tilt compensation
+ */
+static ssize_t store_cpsopmode_value(struct device_driver *ddri, char *buf, size_t count)
+{
+	unsigned long data;
+	int error;
+ 
+ error = strict_strtoul(buf, 10, &data);
+	if (error)
+	{
+		return error;
+	}
+	if (data == BMA255_MODE_NORMAL)
+	{
+		BMA255_SetPowerMode(bma255_i2c_client, true);
+	}
+	else if (data == BMA255_MODE_SUSPEND)
+	{
+		BMA255_SetPowerMode(bma255_i2c_client, false);
+	}
+	else if (bma255_set_mode(bma255_i2c_client, (unsigned char) data) < 0)
+	{
+		GSE_ERR("invalid content: '%s', length = %d\n", buf, count);
+	}
+
+	return count;
+}
+
+/*----------------------------------------------------------------------------*/
+/* g sensor range for compass tilt compensation
+ */
+static ssize_t show_cpsrange_value(struct device_driver *ddri, char *buf)
+{
+	unsigned char data;
+
+	if (bma255_get_range(bma255_i2c_client, &data) < 0)
+	{
+		return sprintf(buf, "Read error\n");
+	}
+	else
+	{
+		return sprintf(buf, "%d\n", data);
+	}
+}
+
+/*----------------------------------------------------------------------------*/
+/* g sensor range for compass tilt compensation
+ */
+static ssize_t store_cpsrange_value(struct device_driver *ddri, char *buf, size_t count)
+{
+	unsigned long data;
+	int error;
+ 
+ error = strict_strtoul(buf, 10, &data);
+	if (error)
+	{
+		return error;
+	}
+	if (bma255_set_range(bma255_i2c_client, (unsigned char) data) < 0)
+	{
+		GSE_ERR("invalid content: '%s', length = %d\n", buf, count);
+	}
+
+	return count;
+}
+/*----------------------------------------------------------------------------*/
+/* g sensor bandwidth for compass tilt compensation
+ */
+static ssize_t show_cpsbandwidth_value(struct device_driver *ddri, char *buf)
+{
+	unsigned char data;
+
+	if (bma255_get_bandwidth(bma255_i2c_client, &data) < 0)
+	{
+		return sprintf(buf, "Read error\n");
+	}
+	else
+	{
+		return sprintf(buf, "%d\n", data);
+	}
+}
+
+/*----------------------------------------------------------------------------*/
+/* g sensor bandwidth for compass tilt compensation
+ */
+static ssize_t store_cpsbandwidth_value(struct device_driver *ddri, char *buf, size_t count)
+{
+	unsigned long data;
+	int error;
+ 
+ error = strict_strtoul(buf, 10, &data);
+	if (error)
+	{
+		return error;
+	}
+	if (bma255_set_bandwidth(bma255_i2c_client, (unsigned char) data) < 0)
+	{
+		GSE_ERR("invalid content: '%s', length = %d\n", buf, count);
+	}
+
+	return count;
+}
+//jwseo_test
+#if 0
+static ssize_t show_limit_value(struct device_driver *ddri, char *buf)
+{
+	return sprintf ( buf, "%d\n", (int)TESTLIMIT_XY);
+}
+
+/*----------------------------------------------------------------------------*/
+/* g sensor range for compass tilt compensation
+ */
+static ssize_t store_limit_value(struct device_driver *ddri, char *buf, size_t count)
+{
+	unsigned long data;
+	int error;
+
+	if (error = strict_strtoul(buf, 10, &data))
+		return error;
+	
+	TESTLIMIT_XY = (int)data;
+
+	return count;
+}
+#endif
+
+/*----------------------------------------------------------------------------*/
+/* g sensor data for compass tilt compensation
+ */
+static ssize_t show_cpsdata_value(struct device_driver *ddri, char *buf)
+{
+	struct i2c_client *client = bma255_i2c_client;
+	char strbuf[BMA255_BUFSIZE]={0,};
+
+	if(NULL == client)
+	{
+		GSE_ERR("i2c client is null!!\n");
+		return 0;
+	}
+	BMA255_CompassReadData(client, strbuf, BMA255_BUFSIZE);
+	return snprintf(buf, PAGE_SIZE, "%s\n", strbuf);
+}
+
+/*----------------------------------------------------------------------------*/
+static ssize_t show_sensordata_value(struct device_driver *ddri, char *buf)
+{
+	struct i2c_client *client = bma255_i2c_client;
+	char strbuf[BMA255_BUFSIZE]={0,};
+
+	if(NULL == client)
+	{
+		GSE_ERR("i2c client is null!!\n");
+		return 0;
+	}
+	BMA255_ReadSensorData(client, strbuf, BMA255_BUFSIZE);
+	return snprintf(buf, PAGE_SIZE, "%s\n", strbuf);
+}
+
+#if 0
+static ssize_t show_sensorrawdata_value(struct device_driver *ddri, char *buf, size_t count)
+{
+	struct i2c_client *client = bma255_i2c_client;
+	char strbuf[BMA255_BUFSIZE]={0,};
+
+	if(NULL == client)
+	{
+		GSE_ERR("i2c client is null!!\n");
+		return 0;
+	}
+	BMA255_ReadRawData(client, strbuf);
+	return snprintf(buf, PAGE_SIZE, "%s\n", strbuf);
+}
+#endif
+
+/*----------------------------------------------------------------------------*/
+static ssize_t show_firlen_value(struct device_driver *ddri, char *buf)
+{
+#ifdef CONFIG_BMA255_LOWPASS
+	struct i2c_client *client = bma255_i2c_client;
+	struct bma255_i2c_data *obj = i2c_get_clientdata(client);
+	if(atomic_read(&obj->firlen))
+	{
+		int idx, len = atomic_read(&obj->firlen);
+		GSE_LOG("len = %2d, idx = %2d\n", obj->fir.num, obj->fir.idx);
+
+		for(idx = 0; idx < len; idx++)
+		{
+			GSE_LOG("[%5d %5d %5d]\n", obj->fir.raw[idx][BMA255_AXIS_X], obj->fir.raw[idx][BMA255_AXIS_Y], obj->fir.raw[idx][BMA255_AXIS_Z]);
+		}
+
+		GSE_LOG("sum = [%5d %5d %5d]\n", obj->fir.sum[BMA255_AXIS_X], obj->fir.sum[BMA255_AXIS_Y], obj->fir.sum[BMA255_AXIS_Z]);
+		GSE_LOG("avg = [%5d %5d %5d]\n", obj->fir.sum[BMA255_AXIS_X]/len, obj->fir.sum[BMA255_AXIS_Y]/len, obj->fir.sum[BMA255_AXIS_Z]/len);
+	}
+	return snprintf(buf, PAGE_SIZE, "%d\n", atomic_read(&obj->firlen));
+#else
+	return snprintf(buf, PAGE_SIZE, "not support\n");
+#endif
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t store_firlen_value(struct device_driver *ddri, char *buf, size_t count)
+{
+#ifdef CONFIG_BMA255_LOWPASS
+	struct i2c_client *client = bma255_i2c_client;
+	struct bma255_i2c_data *obj = i2c_get_clientdata(client);
+	int firlen;
+
+	if(1 != sscanf(buf, "%d", &firlen))
+	{
+		GSE_ERR("invallid format\n");
+	}
+	else if(firlen > C_MAX_FIR_LENGTH)
+	{
+		GSE_ERR("exceeds maximum filter length\n");
+	}
+	else
+	{
+		atomic_set(&obj->firlen, firlen);
+		if(NULL == firlen)
+		{
+			atomic_set(&obj->fir_en, 0);
+		}
+		else
+		{
+			memset(&obj->fir, 0x00, sizeof(obj->fir));
+			atomic_set(&obj->fir_en, 1);
+		}
+	}
+#endif
+	return count;
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t show_trace_value(struct device_driver *ddri, char *buf)
+{
+	ssize_t res;
+	struct bma255_i2c_data *obj = obj_i2c_data;
+	if (obj == NULL)
+	{
+		GSE_ERR("i2c_data obj is null!!\n");
+		return 0;
+	}
+
+	res = snprintf(buf, PAGE_SIZE, "0x%04X\n", atomic_read(&obj->trace));
+	return res;
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t store_trace_value(struct device_driver *ddri, char *buf, size_t count)
+{
+	struct bma255_i2c_data *obj = obj_i2c_data;
+	int trace;
+	if (obj == NULL)
+	{
+		GSE_ERR("i2c_data obj is null!!\n");
+		return 0;
+	}
+
+	if(1 == sscanf(buf, "0x%x", &trace))
+	{
+		atomic_set(&obj->trace, trace);
+	}
+	else
+	{
+		GSE_ERR("invalid content: '%s', length = %d\n", buf, count);
+	}
+
+	return count;
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t show_status_value(struct device_driver *ddri, char *buf)
+{
+	ssize_t len = 0;
+	struct bma255_i2c_data *obj = obj_i2c_data;
+	if (obj == NULL)
+	{
+		GSE_ERR("i2c_data obj is null!!\n");
+		return 0;
+	}
+
+	if(obj->hw)
+	{
+		len += snprintf(buf+len, PAGE_SIZE-len, "CUST: %d %d (%d %d)\n",
+	            obj->hw->i2c_num, obj->hw->direction, obj->hw->power_id, obj->hw->power_vol);
+	}
+	else
+	{
+		len += snprintf(buf+len, PAGE_SIZE-len, "CUST: NULL\n");
+	}
+	return len;
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t show_power_status_value(struct device_driver *ddri, char *buf)
+{
+	if(sensor_power)
+		GSE_LOG("G sensor is in work mode, sensor_power = %d\n", sensor_power);
+	else
+		GSE_LOG("G sensor is in standby mode, sensor_power = %d\n", sensor_power);
+
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t show_teststatus_value(struct device_driver *ddri, char *buf)
+{
+	struct i2c_client *client = bma255_i2c_client;
+
+	if(client == NULL)
+	{
+        GSE_ERR("i2c client is null!!\n");
+		return 0;
+	}
+
+	return snprintf(buf, PAGE_SIZE, "%d\n", test_status);
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t bma255_bandwidth_show(struct device_driver *ddri, char *buf)
+{
+	unsigned char data;
+	struct i2c_client *client = bma255_i2c_client;
+	struct bma255_i2c_data *bma255 = i2c_get_clientdata(client);
+
+	if (bma255_get_bandwidth(bma255->client, &data) < 0)
+		return sprintf(buf, "Read error\n");
+
+	return sprintf(buf, "%d\n", data);
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t bma255_bandwidth_store(struct device_driver *ddri, char *buf, size_t count)
+{
+	unsigned long data;
+	int error;
+	struct i2c_client *client = bma255_i2c_client;
+	struct bma255_i2c_data *bma255 = i2c_get_clientdata(client);
+
+ error = strict_strtoul(buf, 10, &data);
+	if (error)
+		return error;
+
+	if (bma255_set_bandwidth(bma255->client, (unsigned char) data) < 0)
+		return -EINVAL;
+
+	return count;
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t bma255_eeprom_writing_show(struct device_driver *ddri, char *buf)
+{
+	struct i2c_client *client = bma255_i2c_client;
+	struct bma255_i2c_data *bma255 = i2c_get_clientdata(client);
+
+	return sprintf(buf, "%d\n", atomic_read(&bma255->fast_calib_rslt));
+}
+
+static ssize_t bma255_eeprom_writing_store(struct device_driver *ddri, char *buf, size_t count)
+{
+	unsigned char offset_x, offset_y, offset_z;
+	struct i2c_client *client = bma255_i2c_client;
+	struct bma255_i2c_data *bma255 = i2c_get_clientdata(client);
+
+	if(bma255_get_offset_x(bma255->client, &offset_x) < 0)
+		return -EINVAL;
+	if(bma255_get_offset_y(bma255->client, &offset_y) < 0)
+		return -EINVAL;
+	if(bma255_get_offset_z(bma255->client, &offset_z) < 0)
+		return -EINVAL;
+
+	atomic_set(&bma255->fast_calib_rslt, 1);
+
+	return count;
+}
+
+static int bma255_soft_reset(struct i2c_client *client)
+{
+	int comres = 0;
+	unsigned char data = BMA255_EN_SOFT_RESET_VALUE;
+
+	comres = bma255_smbus_write_byte(client, BMA255_EN_SOFT_RESET__REG, &data);
+
+	return comres;
+}
+static ssize_t bma255_softreset_store(struct device *dev, struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct i2c_client *client = bma255_i2c_client;
+	struct bma255_i2c_data *bma255 = i2c_get_clientdata(client);
+
+	if (bma255_soft_reset(bma255->client) < 0)
+		return -EINVAL;
+
+	return count;
+}
+/*----------------------------------------------------------------------------*/
+/* LGE_BSP_COMMON LGE_CHANGE_S 140228 : Calibration for user Apps */
+static int selfCalibration = 1;
+static ssize_t bma255_runCalibration_store(struct device *dev, struct device_attribute *attr, const char *buf, size_t count)
+{
+	int res = 0;
+	res = bma255_runCalibration();
+	if(res == BMA255_SUCCESS)
+	{
+		selfCalibration = BMA255_SUCCESS;
+	}
+	else if(res == BMA255_ERR_SETUP_FAILURE)
+	{
+		selfCalibration = 2;  // cal fail(flat)
+	}
+	else
+	{
+		selfCalibration = 1;  // cal fail
+	}
+
+	return count;
+}
+
+static ssize_t bma255_runCalibration_show(struct device_driver *ddri, char *buf)
+{
+	return sprintf(buf, "%d\n", selfCalibration);
+}
+/* LGE_BSP_COMMON LGE_CHANGE_E 140228 : Calibration for user Apps */
+
+/*----------------------------------------------------------------------------*/
+/* for self test */
+static int bma255_set_selftest_st(struct i2c_client *client, unsigned char
+		selftest)
+{
+	int comres = 0;
+	unsigned char data;
+
+	comres = bma255_smbus_read_byte_block(client, BMA255_EN_SELF_TEST__REG,
+			&data, 1);
+	data = BMA255_SET_BITSLICE(data, BMA255_EN_SELF_TEST, selftest);
+	comres = bma255_smbus_write_byte(client, BMA255_EN_SELF_TEST__REG,
+			&data);
+	//comres = i2c_master_send(client, &data, 1);
+	GSE_LOG("selftest_st comres : %d\n",comres);
+	return comres;
+}
+
+static int bma255_set_selftest_stn(struct i2c_client *client, unsigned char stn)
+{
+	int comres = 0;
+	unsigned char data;
+
+	comres = bma255_smbus_read_byte_block(client, BMA255_NEG_SELF_TEST__REG,
+			&data, 1);
+	data = BMA255_SET_BITSLICE(data, BMA255_NEG_SELF_TEST, stn);
+	comres = bma255_smbus_write_byte(client, BMA255_NEG_SELF_TEST__REG,
+			&data);
+	//comres = i2c_master_send(client, &data, 1);
+	GSE_LOG("selftest_stN comres : %d\n",comres);
+	return comres;
+}
+
+/*
+Read:
+0 ------ success
+1 ------ x  axis failed
+2 ------ y  axis failed
+3 ------ x and y axis failed
+4 ------ z  axis failed
+5 ------ x and z axis failed
+6 ------ y and z axis failed
+7 ------ x, y and z axis failed
+*/
+static ssize_t bma255_SelfTest_store(struct device_driver *ddri, char *buf, size_t count)
+{
+	unsigned long data = 0;
+    int error = -1;
+	struct i2c_client *client = bma255_i2c_client;
+    struct bma255_i2c_data *obj = i2c_get_clientdata(client);
+	enum BMA_RANGE_ENUM range;
+	bool power_mode;
+	u8 bandwidth = 0;
+	u8 value = 0;
+	u8 amp = 0;
+	s16 value1[BMA255_AXES_NUM];
+	s16 value2[BMA255_AXES_NUM];
+	s16 diff = 0;
+	u8 result = 0;
+
+    error = strict_strtoul(buf, 10, &data);
+    if (error)
+    {
+        return error;
+    }
+    GSE_LOG("Self test CMD value : %d\n", (int)data);
+
+    if(data == 1)// self test start command
+    {
+		/*backup settings*/
+		range = obj->range;
+		power_mode = sensor_power;
+		bandwidth = obj->bandwidth;
+		/*Step 1:Soft Reset*/
+		bma255_soft_reset(obj->client);
+		/*Step 2:Clear Selftest Register*/
+		{
+			value = 0;
+			bma255_smbus_write_byte(obj->client, BMA255_SELF_TEST_REG,&value);
+		}
+
+		/*Step 3:Set to +/-4G Range*/
+		BMA255_SetDataFormat(obj->client, BMA255_RANGE_4G);
+		//bma255_set_range(obj->client, BMA_RANGE_4G);
+		/*Step 4:Set Amplitude of Deflection*/
+		{
+			value = 0;
+			amp = 1;
+			bma255_smbus_read_byte(obj->client, BMA255_SELF_TEST_AMP__REG,&value);
+			value = BMA255_SET_BITSLICE(value, BMA255_SELF_TEST_AMP, amp);
+			bma255_smbus_write_byte(obj->client, BMA255_SELF_TEST_AMP__REG,&value);
+		}
+
+		/*Step 5:X-axis Selftest*/
+		bma255_set_selftest_st(obj->client, 1);/*1 for x-axis*/
+		bma255_set_selftest_stn(obj->client, 0);/*positive direction*/
+		mdelay(10);
+  error = BMA255_ReadData(client, obj->data);
+		if(error)
+		{
+			GSE_ERR("I2C error: ret value=%d", error);
+			return EIO;
+		}
+		else
+		{
+			value1[BMA255_AXIS_X] = obj->data[BMA255_AXIS_X];
+			value1[BMA255_AXIS_Y] = obj->data[BMA255_AXIS_Y];
+			value1[BMA255_AXIS_Z] = obj->data[BMA255_AXIS_Z];
+		}
+		bma255_set_selftest_stn(obj->client, 1);/*negative direction*/
+		mdelay(10);
+  error = BMA255_ReadData(client, obj->data);
+		if(error)
+		{
+			GSE_ERR("I2C error: ret value=%d", error);
+			return EIO;
+		}
+		else
+		{
+			value2[BMA255_AXIS_X] = obj->data[BMA255_AXIS_X];
+			value2[BMA255_AXIS_Y] = obj->data[BMA255_AXIS_Y];
+			value2[BMA255_AXIS_Z] = obj->data[BMA255_AXIS_Z];
+		}
+		diff = value1[BMA255_AXIS_X]-value2[BMA255_AXIS_X];
+
+		GSE_LOG("diff x is %d,value1 is %d, value2 is %d\n", diff,
+				value1[BMA255_AXIS_X], value2[BMA255_AXIS_X]);
+
+		if (abs(diff) < 204)
+			result |= 1;
+
+		/*Step 6:Y-axis Selftest*/
+		bma255_set_selftest_st(obj->client, 2);/*2 for y-axis*/
+		bma255_set_selftest_stn(obj->client, 0);/*positive direction*/
+		mdelay(10);
+  error = BMA255_ReadData(client, obj->data);
+		if(error)
+		{
+			GSE_ERR("I2C error: ret value=%d", error);
+			return EIO;
+		}
+		else
+		{
+			value1[BMA255_AXIS_X] = obj->data[BMA255_AXIS_X];
+			value1[BMA255_AXIS_Y] = obj->data[BMA255_AXIS_Y];
+			value1[BMA255_AXIS_Z] = obj->data[BMA255_AXIS_Z];
+		}
+		bma255_set_selftest_stn(obj->client, 1);/*negative direction*/
+		mdelay(10);
+  error = BMA255_ReadData(client, obj->data);
+		if(error)
+		{
+			GSE_ERR("I2C error: ret value=%d", error);
+			return EIO;
+		}
+		else
+		{
+			value2[BMA255_AXIS_X] = obj->data[BMA255_AXIS_X];
+			value2[BMA255_AXIS_Y] = obj->data[BMA255_AXIS_Y];
+			value2[BMA255_AXIS_Z] = obj->data[BMA255_AXIS_Z];
+		}
+		diff = value1[BMA255_AXIS_Y]-value2[BMA255_AXIS_Y];
+		GSE_LOG("diff y is %d,value1 is %d, value2 is %d\n", diff,
+				value1[BMA255_AXIS_Y], value2[BMA255_AXIS_Y]);
+
+		if (abs(diff) < 204)
+			result |= 2;
+
+		/*Step 7:Z-axis Selftest*/
+		bma255_set_selftest_st(obj->client, 3);/*3 for z-axis*/
+		bma255_set_selftest_stn(obj->client, 0);/*positive direction*/
+		mdelay(10);
+  error = BMA255_ReadData(client, obj->data);
+		if(error)
+		{
+			GSE_ERR("I2C error: ret value=%d", error);
+			return EIO;
+		}
+		else
+		{
+			value1[BMA255_AXIS_X] = obj->data[BMA255_AXIS_X];
+			value1[BMA255_AXIS_Y] = obj->data[BMA255_AXIS_Y];
+			value1[BMA255_AXIS_Z] = obj->data[BMA255_AXIS_Z];
+		}
+		bma255_set_selftest_stn(obj->client, 1);/*negative direction*/
+		mdelay(10);
+  error = BMA255_ReadData(client, obj->data);
+		if(error)
+		{
+			GSE_ERR("I2C error: ret value=%d", error);
+			return EIO;
+		}
+		else
+		{
+			value2[BMA255_AXIS_X] = obj->data[BMA255_AXIS_X];
+			value2[BMA255_AXIS_Y] = obj->data[BMA255_AXIS_Y];
+			value2[BMA255_AXIS_Z] = obj->data[BMA255_AXIS_Z];
+		}
+		diff = value1[BMA255_AXIS_Z]-value2[BMA255_AXIS_Z];
+
+		GSE_LOG("diff z is %d,value1 is %d, value2 is %d\n", diff,
+				value1[BMA255_AXIS_Z], value2[BMA255_AXIS_Z]);
+
+		if (abs(diff) < 204)
+			result |= 4;
+
+		/*Step 8:Soft Reset*/
+		bma255_soft_reset(obj->client);
+		/*Sync sw settings to hw settings*/
+		obj->range = range;//fix for 4G
+		obj->bandwidth= bandwidth;
+
+		atomic_set(&obj->selftest, result);;
+
+		/*restore settings*/
+		BMA255_SetDataFormat(obj->client, range);
+		//bma255_set_range(obj->client, range);
+		bma255_set_bandwidth(obj->client, bandwidth);
+		BMA255_SetPowerMode(obj->client, power_mode);
+		GSE_LOG("self test finished. result : %d\n",result);
+    }
+	else // wrong input
+	{
+    	GSE_LOG("SelfTest command FAIL\n");
+    	return -EINVAL;
+	}
+	return count;
+}
+
+static ssize_t bma255_SelfTest_show(struct device_driver *ddri, char *buf)
+{
+    int selftest_rslt = 1;  // fail
+
+    struct i2c_client *client = bma255_i2c_client;
+    struct bma255_i2c_data *obj = i2c_get_clientdata(client);
+	if (obj == NULL) {
+		GSE_ERR("bma i2c data pointer is null\n");
+		return 0;
+	}
+
+//    if(atomic_read(&obj->selftest) == 1)  // selftest success
+	if(atomic_read(&obj->selftest) == 0)  // selftest success
+    {
+        selftest_rslt = 0;  // success
+    }
+    else
+    {
+    	GSE_LOG("Self Test Fail : %d\n",atomic_read(&obj->selftest));
+        selftest_rslt = 1;  // fail
+    }
+	return sprintf(buf, "%d\n", selftest_rslt);
+}
+/*----------------------------------------------------------------------------*/
+static DRIVER_ATTR(chipinfo,   S_IWUSR | S_IRUGO, show_chipinfo_value,   NULL);
+static DRIVER_ATTR(cpsdata,    S_IWUSR | S_IRUGO, show_cpsdata_value,    NULL);
+static DRIVER_ATTR(cpsopmode, S_IWUSR|S_IRUGO|S_IWGRP, show_cpsopmode_value, store_cpsopmode_value);
+static DRIVER_ATTR(cpsrange, S_IWUSR|S_IRUGO|S_IWGRP, show_cpsrange_value, store_cpsrange_value);
+static DRIVER_ATTR(cpsbandwidth, S_IWUSR|S_IRUGO|S_IWGRP, show_cpsbandwidth_value, store_cpsbandwidth_value);
+static DRIVER_ATTR(sensordata, S_IWUSR | S_IRUGO, show_sensordata_value,    NULL);
+static DRIVER_ATTR(cali, S_IWUSR|S_IRUGO|S_IWGRP, show_cali_value, store_cali_value);
+static DRIVER_ATTR(firlen, S_IWUSR|S_IRUGO|S_IWGRP, show_firlen_value, store_firlen_value);
+static DRIVER_ATTR(trace, S_IWUSR|S_IRUGO|S_IWGRP, show_trace_value, store_trace_value);
+static DRIVER_ATTR(status,     S_IRUGO, show_status_value,        NULL);
+static DRIVER_ATTR(powerstatus, S_IRUGO, show_power_status_value, NULL);
+//LGE_CHANGE_S : 2012-12-07 taebum81.kim@lge.com AT%SURV : (4)
+static DRIVER_ATTR(softreset, S_IWUSR|S_IWGRP, NULL, bma255_softreset_store);
+static DRIVER_ATTR(teststatus, S_IWUSR|S_IRUGO, show_teststatus_value, NULL);
+static DRIVER_ATTR(fast_calibration_x, S_IRUGO|S_IWUSR|S_IWGRP, bma255_fast_calibration_x_show, bma255_fast_calibration_x_store);
+static DRIVER_ATTR(fast_calibration_y, S_IRUGO|S_IWUSR|S_IWGRP, bma255_fast_calibration_y_show, bma255_fast_calibration_y_store);
+static DRIVER_ATTR(fast_calibration_z, S_IRUGO|S_IWUSR|S_IWGRP, bma255_fast_calibration_z_show, bma255_fast_calibration_z_store);
+static DRIVER_ATTR(run_fast_calibration, S_IRUGO|S_IWUSR|S_IWGRP, bma255_runCalibration_show, bma255_runCalibration_store);
+static DRIVER_ATTR(eeprom_writing, S_IRUGO|S_IWUSR|S_IWGRP, bma255_eeprom_writing_show, bma255_eeprom_writing_store);
+static DRIVER_ATTR(bandwidth, S_IRUGO|S_IWUSR|S_IWGRP, bma255_bandwidth_show, bma255_bandwidth_store);
+static DRIVER_ATTR(selftest, S_IRUGO|S_IWUSR|S_IWGRP, bma255_SelfTest_show, bma255_SelfTest_store);
+#if 1 //motion sensor cal backup
+static DRIVER_ATTR(cali_backup, S_IWUSR|S_IRUGO|S_IWGRP, show_cali_backup_value, NULL);
+#endif
+#if 0
+static DRIVER_ATTR(limit, S_IWUSR|S_IRUGO|S_IWGRP, show_limit_value, store_limit_value); //jwseo_test
+#endif
+//LGE_CHANGE_E : 2012-12-07 taebum81.kim@lge.com AT%SURV : (4)
+/*----------------------------------------------------------------------------*/
+static struct driver_attribute *bma255_attr_list[] = {
+	&driver_attr_chipinfo,     /*chip information*/
+	&driver_attr_sensordata,   /*dump sensor data*/
+	&driver_attr_cali,         /*show calibration data*/
+	&driver_attr_firlen,       /*filter length: 0: disable, others: enable*/
+	&driver_attr_trace,        /*trace log*/
+	&driver_attr_status,
+	&driver_attr_powerstatus,
+	&driver_attr_cpsdata,	/*g sensor data for compass tilt compensation*/
+	&driver_attr_cpsopmode,	/*g sensor opmode for compass tilt compensation*/
+	&driver_attr_cpsrange,	/*g sensor range for compass tilt compensation*/
+	&driver_attr_cpsbandwidth,	/*g sensor bandwidth for compass tilt compensation*/
+	&driver_attr_bandwidth,
+	&driver_attr_softreset,
+	&driver_attr_teststatus,
+	&driver_attr_fast_calibration_x,
+	&driver_attr_fast_calibration_y,
+	&driver_attr_fast_calibration_z,
+	&driver_attr_run_fast_calibration,
+	&driver_attr_eeprom_writing,
+    &driver_attr_selftest,
+#if 1 //motion sensor cal backup
+	&driver_attr_cali_backup,         /*show calibration backup data*/
+#endif
+#if 0
+    &driver_attr_limit,  //jwseo_test
+#endif
+};
+/*----------------------------------------------------------------------------*/
+static int bma255_create_attr(struct device_driver *driver)
+{
+	int idx, err = 0;
+	int num = (int)(sizeof(bma255_attr_list)/sizeof(bma255_attr_list[0]));
+	if (driver == NULL)
+	{
+		return -EINVAL;
+	}
+
+	for(idx = 0; idx < num; idx++)
+	{
+  err = driver_create_file(driver, bma255_attr_list[idx]);
+		if(err)
+		{
+			GSE_ERR("driver_create_file (%s) = %d\n", bma255_attr_list[idx]->attr.name, err);
+			break;
+		}
+	}
+	return err;
+}
+/*----------------------------------------------------------------------------*/
+static int bma255_delete_attr(struct device_driver *driver)
+{
+	int idx ,err = 0;
+	int num = (int)(sizeof(bma255_attr_list)/sizeof(bma255_attr_list[0]));
+
+	if(driver == NULL)
+	{
+		return -EINVAL;
+	}
+
+	for(idx = 0; idx < num; idx++)
+	{
+		driver_remove_file(driver, bma255_attr_list[idx]);
+	}
+
+	return err;
+}
+
+/*----------------------------------------------------------------------------*/
+int gsensor_operate(void* self, uint32_t command, void* buff_in, int size_in,
+		void* buff_out, int size_out, int* actualout)
+{
+	int err = 0;
+	int value, sample_delay;
+	struct bma255_i2c_data *priv = (struct bma255_i2c_data*)self;
+	hwm_sensor_data* gsensor_data;
+	char buff[BMA255_BUFSIZE];
+
+	switch (command)
+	{
+		case SENSOR_DELAY:
+			if((buff_in == NULL) || (size_in < sizeof(int)))
+			{
+				GSE_ERR("Set delay parameter error!\n");
+				err = -EINVAL;
+			}
+			else
+			{
+				value = *(int *)buff_in;
+				if(value <= 5)
+				{
+					sample_delay = BMA255_BW_200HZ;
+				}
+				else if(value <= 10)
+				{
+					sample_delay = BMA255_BW_100HZ;
+				}
+				else
+				{
+					sample_delay = BMA255_BW_50HZ;
+				}
+
+				err = BMA255_SetBWRate(priv->client, sample_delay);
+				if(err != BMA255_SUCCESS ) //0x2C->BW=100Hz
+				{
+					GSE_ERR("Set delay parameter error!\n");
+				}
+
+				if(value >= 50)
+				{
+					atomic_set(&priv->filter, 0);
+				}
+				else
+				{
+				#if defined(CONFIG_BMA255_LOWPASS)
+					priv->fir.num = 0;
+					priv->fir.idx = 0;
+					priv->fir.sum[BMA255_AXIS_X] = 0;
+					priv->fir.sum[BMA255_AXIS_Y] = 0;
+					priv->fir.sum[BMA255_AXIS_Z] = 0;
+					atomic_set(&priv->filter, 1);
+				#endif
+				}
+			}
+			break;
+
+		case SENSOR_ENABLE:
+			if((buff_in == NULL) || (size_in < sizeof(int)))
+			{
+				GSE_ERR("Enable sensor parameter error!\n");
+				err = -EINVAL;
+			}
+			else
+			{
+				value = *(int *)buff_in;
+				if(((value == 0) && (sensor_power == false)) ||((value == 1) && (sensor_power == true)))
+				{
+					GSE_LOG("Gsensor device have updated!, power: %d\n", sensor_power);
+				}
+				else
+				{
+					err = BMA255_SetPowerMode( priv->client, !sensor_power);
+				}
+			}
+			break;
+
+		case SENSOR_GET_DATA:
+			if((buff_out == NULL) || (size_out< sizeof(hwm_sensor_data)))
+			{
+				GSE_ERR("get sensor data parameter error!\n");
+				err = -EINVAL;
+			}
+			else
+			{
+				gsensor_data = (hwm_sensor_data *)buff_out;
+				BMA255_ReadSensorData(priv->client, buff, BMA255_BUFSIZE);
+				sscanf(buff, "%x %x %x", &gsensor_data->values[0],
+					&gsensor_data->values[1], &gsensor_data->values[2]);
+				gsensor_data->status = SENSOR_STATUS_ACCURACY_MEDIUM;
+				gsensor_data->value_divide = 1000;
+			}
+			break;
+		default:
+			GSE_ERR("gsensor operate function no this parameter %d!\n", command);
+			err = -1;
+			break;
+	}
+
+	return err;
+}
+
+/******************************************************************************
+ * Function Configuration
+******************************************************************************/
+static int bma255_open(struct inode *inode, struct file *file)
+{
+	file->private_data = bma255_i2c_client;
+
+	if(file->private_data == NULL)
+	{
+		GSE_ERR("null pointer!!\n");
+		return -EINVAL;
+	}
+	return nonseekable_open(inode, file);
+}
+/*----------------------------------------------------------------------------*/
+static int bma255_release(struct inode *inode, struct file *file)
+{
+	file->private_data = NULL;
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static long bma255_unlocked_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
+{
+	struct i2c_client *client = (struct i2c_client*)file->private_data;
+	struct bma255_i2c_data *obj = (struct bma255_i2c_data*)i2c_get_clientdata(client);
+	char strbuf[BMA255_BUFSIZE]={0,};
+	void __user *data;
+	SENSOR_DATA sensor_data;
+	long err = 0;
+    #if defined(CALIBRATION_TO_FILE)
+	int cali[6];
+    #endif
+//	uint32_t enable = 0;
+
+	GSE_FUN(f);
+	if(_IOC_DIR(cmd) & _IOC_READ)
+	{
+		err = !access_ok(VERIFY_WRITE, (void __user *)arg, _IOC_SIZE(cmd));
+	}
+	else if(_IOC_DIR(cmd) & _IOC_WRITE)
+	{
+		err = !access_ok(VERIFY_READ, (void __user *)arg, _IOC_SIZE(cmd));
+	}
+
+	if(err)
+	{
+		GSE_ERR("access error: %08X, (%2d, %2d)\n", cmd, _IOC_DIR(cmd), _IOC_SIZE(cmd));
+		return -EFAULT;
+	}
+
+	switch(cmd)
+	{
+#if 0
+		case GSENSOR_IOCTL_SET_ENABLE:
+			data = (void __user *) arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;
+			}
+
+			if(copy_from_user(&enable, data, sizeof(enable)))
+			{
+				return -EFAULT;
+			}
+			else
+			{
+				if(enable == 1)
+				{
+					BMA255_SetPowerMode(obj_i2c_data->client, 1);
+				}
+				else if(enable == 0)
+				{
+					BMA255_SetPowerMode(obj_i2c_data->client, 0);
+				}
+			}
+			break;
+#endif
+
+		case GSENSOR_IOCTL_INIT:
+			bma255_init_client(client, 0);
+			break;
+
+		case GSENSOR_IOCTL_READ_CHIPINFO:
+			data = (void __user *) arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;
+			}
+
+			BMA255_ReadChipInfo(client, strbuf, BMA255_BUFSIZE);
+			if(copy_to_user(data, strbuf, strlen(strbuf)+1))
+			{
+				err = -EFAULT;
+				break;
+			}
+			break;
+
+		case GSENSOR_IOCTL_READ_SENSORDATA:
+			data = (void __user *) arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;
+			}
+
+			BMA255_ReadSensorData(client, strbuf, BMA255_BUFSIZE);
+			if(copy_to_user(data, strbuf, strlen(strbuf)+1))
+			{
+				err = -EFAULT;
+				break;
+			}
+			break;
+
+		case GSENSOR_IOCTL_READ_GAIN:
+			data = (void __user *) arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;
+			}
+
+			if(copy_to_user(data, &gsensor_gain, sizeof(GSENSOR_VECTOR3D)))
+			{
+				err = -EFAULT;
+				break;
+			}
+			break;
+
+		case GSENSOR_IOCTL_READ_RAW_DATA:
+			data = (void __user *) arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;
+			}
+			BMA255_ReadRawData(client, strbuf);
+			if(copy_to_user(data, &strbuf, strlen(strbuf)+1))
+			{
+				err = -EFAULT;
+				break;
+			}
+			break;
+
+		case GSENSOR_IOCTL_SET_CALI:
+			data = (void __user*)arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;
+			}
+			if(copy_from_user(&sensor_data, data, sizeof(sensor_data)))
+			{
+				err = -EFAULT;
+				break;
+			}
+
+			//LGE_CHANGE_S : 2012-12-07 taebum81.kim@lge.com AT%SURV : (6)
+
+           #if defined(CALIBRATION_TO_FILE)
+				make_cal_data_file(); 
+				
+	            err = bma255_calibration_read(cali);
+				if(err!=0){
+				    GSE_LOG("Read Cal Fail from file !!!\n");
+					break;
+					}
+				else
+					{
+	                  sensor_data.x = cali[0];
+	                  sensor_data.y = cali[1];
+	                  sensor_data.z = cali[2];
+					}
+			#endif
+			
+            if((sensor_data.x==0)&&(sensor_data.y==0)&&(sensor_data.z==0))
+             {
+			    GSE_LOG("Read Cal Data x : %d / y : %d / z : %d\n",sensor_data.x,sensor_data.y,sensor_data.z);
+			    GSE_LOG("Read Cal Data all Zero, Do not set register\n");
+				err = -EINVAL;
+				break;
+             }
+						
+			if(bma255_set_offset_x(obj->client, (unsigned char)sensor_data.x) < 0){
+				err = -EINVAL;
+				break;
+				}
+			if(bma255_set_offset_y(obj->client, (unsigned char)sensor_data.y) < 0){
+				err = -EINVAL;
+				break;
+				}
+			if(bma255_set_offset_z(obj->client, (unsigned char)sensor_data.z) < 0){
+				err = -EINVAL;
+				break;
+				}
+
+			GSE_LOG("-------------- set sensor cal --------------\n");
+			GSE_LOG("x : %d / y : %d / z : %d\n",sensor_data.x,sensor_data.y,sensor_data.z);
+			//LGE_CHANGE_S : 2012-12-07 taebum81.kim@lge.com AT%SURV : (6)
+			break;
+
+		case GSENSOR_IOCTL_CLR_CALI:
+			err = BMA255_ResetCalibration(client);
+			break;
+
+		case GSENSOR_IOCTL_GET_CALI:
+			GSE_LOG("GSENSOR_IOCTL_GET_CALI\n");
+			break;
+
+		default:
+			GSE_ERR("unknown IOCTL: 0x%08x\n", cmd);
+			err = -ENOIOCTLCMD;
+			break;
+
+	}
+
+	return err;
+}
+
+
+/*----------------------------------------------------------------------------*/
+static struct file_operations bma255_fops = {
+	//.owner = THIS_MODULE,
+	.open = bma255_open,
+	.release = bma255_release,
+	.unlocked_ioctl = bma255_unlocked_ioctl,
+};
+/*----------------------------------------------------------------------------*/
+static struct miscdevice bma255_device = {
+	.minor = MISC_DYNAMIC_MINOR,
+	.name = "gsensor",
+	.fops = &bma255_fops,
+};
+/*----------------------------------------------------------------------------*/
+#ifndef CONFIG_HAS_EARLYSUSPEND
+/*----------------------------------------------------------------------------*/
+static int bma255_suspend(struct i2c_client *client, pm_message_t msg)
+{
+	struct bma255_i2c_data *obj = i2c_get_clientdata(client);
+	int err = 0;
+
+	GSE_FUN();
+
+	if(msg.event == PM_EVENT_SUSPEND)
+	{
+		if(obj == NULL)
+		{
+			GSE_ERR("null pointer!!\n");
+			return -EINVAL;
+		}
+		atomic_set(&obj->suspend, 1);
+		#if 0
+		if(err = BMA255_SetPowerMode(obj->client, false))
+		{
+			GSE_ERR("write power control fail!!\n");
+			return;
+		}
+		#endif
+		BMA255_power(obj->hw, 0);
+	}
+	return err;
+}
+/*----------------------------------------------------------------------------*/
+static int bma255_resume(struct i2c_client *client)
+{
+	struct bma255_i2c_data *obj = i2c_get_clientdata(client);
+	int err;
+
+    GSE_FUN();
+
+	if(obj == NULL)
+	{
+		GSE_ERR("null pointer!!\n");
+		return -EINVAL;
+	}
+
+	BMA255_power(obj->hw, 1);
+	#if 0//already call PowerManagerSer
+	if(err = bma255_init_client(client, 0))
+	{
+		GSE_ERR("initialize client fail!!\n");
+		return err;
+	}
+	#endif
+	atomic_set(&obj->suspend, 0);
+
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+#else /*CONFIG_HAS_EARLY_SUSPEND is defined*/
+/*----------------------------------------------------------------------------*/
+static void bma255_early_suspend(struct early_suspend *h)
+{
+	struct bma255_i2c_data *obj = container_of(h, struct bma255_i2c_data, early_drv);
+//	int err;
+
+	GSE_FUN();
+
+	if(obj == NULL)
+	{
+		GSE_ERR("null pointer!!\n");
+		return;
+	}
+	atomic_set(&obj->suspend, 1);
+	#if 0
+	if(err = BMA255_SetPowerMode(obj->client, false))
+	{
+		GSE_ERR("write power control fail!!\n");
+		return;
+	}
+
+	sensor_power = false;
+	#endif
+	BMA255_power(obj->hw, 0);
+}
+/*----------------------------------------------------------------------------*/
+static void bma255_late_resume(struct early_suspend *h)
+{
+	struct bma255_i2c_data *obj = container_of(h, struct bma255_i2c_data, early_drv);
+//	int err;
+
+	GSE_FUN();
+
+	if(obj == NULL)
+	{
+		GSE_ERR("null pointer!!\n");
+		return;
+	}
+
+	BMA255_power(obj->hw, 1);
+	#if 0//already call PowerManagerSer
+	if(err = bma255_init_client(obj->client, 0))
+	{
+		GSE_ERR("initialize client fail!!\n");
+		return;
+	}
+	#endif
+	atomic_set(&obj->suspend, 0);
+}
+/*----------------------------------------------------------------------------*/
+#endif /*CONFIG_HAS_EARLYSUSPEND*/
+/*----------------------------------------------------------------------------*/
+static int bma255_i2c_probe(struct i2c_client *client, const struct i2c_device_id *id)
+{
+	struct i2c_client *new_client;
+	struct bma255_i2c_data *obj;
+	struct hwmsen_object sobj;
+	int err = 0;
+
+	GSE_FUN();
+
+	if(!(obj = kzalloc(sizeof(*obj), GFP_KERNEL)))
+	{
+		err = -ENOMEM;
+		goto exit;
+	}
+
+	memset(obj, 0, sizeof(struct bma255_i2c_data));
+
+	obj->hw = get_cust_acc_hw();
+ err = hwmsen_get_convert(obj->hw->direction, &obj->cvt);
+	if(err)
+	{
+		GSE_ERR("invalid direction: %d\n", obj->hw->direction);
+		goto exit;
+	}
+
+	obj_i2c_data = obj;
+	obj->client = client;
+	new_client = obj->client;
+	i2c_set_clientdata(new_client,obj);
+
+	atomic_set(&obj->trace, 0);
+	atomic_set(&obj->suspend, 0);
+
+#ifdef CONFIG_BMA255_LOWPASS
+	if(obj->hw->firlen > C_MAX_FIR_LENGTH)
+	{
+		atomic_set(&obj->firlen, C_MAX_FIR_LENGTH);
+	}
+	else
+	{
+		atomic_set(&obj->firlen, obj->hw->firlen);
+	}
+
+	if(atomic_read(&obj->firlen) > 0)
+	{
+		atomic_set(&obj->fir_en, 1);
+	}
+
+#endif
+
+	bma255_i2c_client = new_client;
+
+ err = bma255_init_client(new_client, 1);
+	if(err)
+	{
+		GSE_ERR ( "failed to init BMA255 ( err = %d )\n", err );
+		goto exit_init_failed;
+	}
+
+ err = misc_register(&bma255_device);
+	if(err)
+	{
+		GSE_ERR("bma255_device register failed\n");
+		goto exit_misc_device_register_failed;
+	}
+ err = bma255_create_attr(&bma255_gsensor_driver.driver);
+	if(err)
+	{
+		GSE_ERR("create attribute err = %d\n", err);
+		goto exit_create_attr_failed;
+	}
+
+	sobj.self = obj;
+    sobj.polling = 1;
+    sobj.sensor_operate = gsensor_operate;
+ err = hwmsen_attach(ID_ACCELEROMETER, &sobj);
+	if(err)
+	{
+		GSE_ERR("attach fail = %d\n", err);
+		goto exit_kfree;
+	}
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+	obj->early_drv.level    = EARLY_SUSPEND_LEVEL_DISABLE_FB - 1,
+	obj->early_drv.suspend  = bma255_early_suspend,
+	obj->early_drv.resume   = bma255_late_resume,
+	register_early_suspend(&obj->early_drv);
+#endif
+
+	GSE_LOG("%s: OK\n", __func__);
+	return 0;
+
+exit_create_attr_failed:
+	misc_deregister(&bma255_device);
+exit_misc_device_register_failed:
+exit_init_failed:
+	//i2c_detach_client(new_client);
+exit_kfree:
+	kfree(obj);
+exit:
+	GSE_ERR("%s: err = %d\n", __func__, err);
+	return err;
+}
+
+/*----------------------------------------------------------------------------*/
+static int bma255_i2c_remove(struct i2c_client *client)
+{
+	int err = 0;
+ 
+ err = bma255_delete_attr(&bma255_gsensor_driver.driver);
+	if(err)
+	{
+		GSE_ERR("bma150_delete_attr fail: %d\n", err);
+	}
+ err = misc_deregister(&bma255_device); 
+	if(err)
+	{
+		GSE_ERR("misc_deregister fail: %d\n", err);
+	}
+ err = hwmsen_detach(ID_ACCELEROMETER); 
+	if(err)
+    {
+		GSE_ERR("hwmsen_detach fail: %d\n", err);
+    }
+
+    bma255_i2c_client = NULL;
+	i2c_unregister_device(client);
+	kfree(i2c_get_clientdata(client));
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int bma255_probe(struct platform_device *pdev)
+{
+	struct acc_hw *hw = get_cust_acc_hw();
+
+	GSE_FUN();
+
+	BMA255_power(hw, 1);
+	if(i2c_add_driver(&bma255_i2c_driver))
+	{
+		GSE_ERR("add driver error\n");
+		return -1;
+	}
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int bma255_remove(struct platform_device *pdev)
+{
+    struct acc_hw *hw = get_cust_acc_hw();
+
+    GSE_FUN();
+    BMA255_power(hw, 0);
+    i2c_del_driver(&bma255_i2c_driver);
+    return 0;
+}
+/*----------------------------------------------------------------------------*/
+static struct platform_driver bma255_gsensor_driver = {
+	.probe      = bma255_probe,
+	.remove     = bma255_remove,
+	.driver     = {
+		.name  = "gsensor",
+#ifdef CONFIG_OF
+		.of_match_table = gsensor_of_match,
+#endif
+	}
+};
+
+/*----------------------------------------------------------------------------*/
+static int __init bma255_init(void)
+{
+	struct acc_hw *hw = get_cust_acc_hw();
+	unsigned short bma255_i2c_addr = 0x0;
+
+	GSE_FUN();
+	bma255_i2c_addr = 0x11;
+	bma255_i2c_info.addr = bma255_i2c_addr;
+	i2c_register_board_info(hw->i2c_num, &bma255_i2c_info, 1);
+	if(platform_driver_register(&bma255_gsensor_driver))
+	{
+		GSE_ERR("failed to register driver");
+		return -ENODEV;
+	}
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static void __exit bma255_exit(void)
+{
+	GSE_FUN();
+	platform_driver_unregister(&bma255_gsensor_driver);
+}
+/*----------------------------------------------------------------------------*/
+module_init(bma255_init);
+module_exit(bma255_exit);
+/*----------------------------------------------------------------------------*/
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("BMA255 I2C driver");
+MODULE_AUTHOR("hongji.zhou@bosch-sensortec.com");
diff --git a/drivers/misc/mediatek/accelerometer/bma255-sdo1/bma255.h b/drivers/misc/mediatek/accelerometer/bma255-sdo1/bma255.h
new file mode 100644
index 000000000..1c85bcce6
--- /dev/null
+++ b/drivers/misc/mediatek/accelerometer/bma255-sdo1/bma255.h
@@ -0,0 +1,155 @@
+/* BMA255 motion sensor driver
+ *
+ *
+ * This software program is licensed subject to the GNU General Public License
+ * (GPL).Version 2,June 1991, available at http://www.fsf.org/copyleft/gpl.html
+
+ * (C) Copyright 2011 Bosch Sensortec GmbH
+ * All Rights Reserved
+ */
+
+#ifndef BMA255_H
+#define BMA255_H
+
+#include <linux/ioctl.h>
+
+/* 7-bit addr: 0x10 (SDO connected to GND); 0x11 (SDO connected to VDDIO) */
+#define BMA255_I2C_ADDR				0x11
+/* chip ID */
+#define BMA255_FIXED_DEVID			0xFA
+
+ /* BMA255 Register Map  (Please refer to BMA255 Specifications) */
+#define BMA255_REG_DEVID			0x00
+#define BMA255_REG_OFSX				0x16
+#define BMA255_REG_OFSX_HIGH		0x1A
+#define BMA255_REG_BW_RATE			0x10
+#define BMA255_BW_MASK				0x1f
+
+#define BMA255_BW_250HZ				0x0d
+#define BMA255_BW_125HZ				0x0c
+#define BMA255_BW_62_5HZ			0x0b
+#define BMA255_BW_31_25HZ			0x0a
+#define BMA255_BW_15_63HZ			0x09
+#define BMA255_BW_7_81HZ			0x08
+#define BMA255_BW_200HZ				0x0d
+#define BMA255_BW_100HZ				0x0c
+#define BMA255_BW_50HZ				0x0b
+#define BMA255_BW_25HZ				0x0a
+
+#define BMA255_REG_POWER_CTL		0x11
+#define BMA255_REG_DATA_FORMAT		0x0f
+#define BMA255_RANGE_MASK			0x0f
+#define BMA255_RANGE_2G				0x03
+#define BMA255_RANGE_4G				0x05
+#define BMA255_RANGE_8G				0x08
+#define BMA255_REG_DATAXLOW			0x02
+#define BMA255_REG_DATA_RESOLUTION	0x14
+#define BMA255_MEASURE_MODE			0x80
+#define BMA255_SELF_TEST           	0x32
+#define BMA255_SELF_TEST_AXIS_X		0x01
+#define BMA255_SELF_TEST_AXIS_Y		0x02
+#define BMA255_SELF_TEST_AXIS_Z		0x03
+#define BMA255_SELF_TEST_POSITIVE	0x00
+#define BMA255_SELF_TEST_NEGATIVE	0x04
+#define BMA255_INT_REG_1           	0x16
+#define BMA255_INT_REG_2          	0x17
+
+#define BMA255_X_AXIS_LSB_REG		0x02
+#define BMA255_X_AXIS_MSB_REG		0x03
+#define BMA255_Y_AXIS_LSB_REG		0x04
+#define BMA255_Y_AXIS_MSB_REG		0x05
+#define BMA255_Z_AXIS_LSB_REG		0x06
+#define BMA255_Z_AXIS_MSB_REG		0x07
+
+#define BMA255_EN_SOFT_RESET__POS	0
+#define BMA255_EN_SOFT_RESET__LEN	8
+#define BMA255_EN_SOFT_RESET__MSK	0xFF
+#define BMA255_EN_SOFT_RESET__REG	BMA255_RESET_REG
+
+#define BMA255_EEPROM_CTRL_REG		0x33
+#define BMA255_OFFSET_CTRL_REG		0x36
+#define BMA255_OFFSET_PARAMS_REG	0x37
+#define BMA255_OFFSET_X_AXIS_REG	0x38
+#define BMA255_OFFSET_Y_AXIS_REG	0x39
+#define BMA255_OFFSET_Z_AXIS_REG	0x3A
+
+#define BMA255_CUT_OFF				0
+#define BMA255_OFFSET_TRIGGER_X		1
+#define BMA255_OFFSET_TRIGGER_Y		2
+#define BMA255_OFFSET_TRIGGER_Z		3
+
+#define BMA255_FAST_CAL_RDY_S__POS	4
+#define BMA255_FAST_CAL_RDY_S__LEN	1
+#define BMA255_FAST_CAL_RDY_S__MSK	0x10
+#define BMA255_FAST_CAL_RDY_S__REG	BMA255_OFFSET_CTRL_REG
+
+#define BMA255_CAL_TRIGGER__POS		5
+#define BMA255_CAL_TRIGGER__LEN		2
+#define BMA255_CAL_TRIGGER__MSK		0x60
+#define BMA255_CAL_TRIGGER__REG		BMA255_OFFSET_CTRL_REG
+
+#define BMA255_COMP_CUTOFF__POS		0
+#define BMA255_COMP_CUTOFF__LEN		1
+#define BMA255_COMP_CUTOFF__MSK		0x01
+#define BMA255_COMP_CUTOFF__REG		BMA255_OFFSET_PARAMS_REG
+
+#define BMA255_COMP_TARGET_OFFSET_X__POS		1
+#define BMA255_COMP_TARGET_OFFSET_X__LEN		2
+#define BMA255_COMP_TARGET_OFFSET_X__MSK		0x06
+#define BMA255_COMP_TARGET_OFFSET_X__REG		BMA255_OFFSET_PARAMS_REG
+
+#define BMA255_COMP_TARGET_OFFSET_Y__POS		3
+#define BMA255_COMP_TARGET_OFFSET_Y__LEN		2
+#define BMA255_COMP_TARGET_OFFSET_Y__MSK		0x18
+#define BMA255_COMP_TARGET_OFFSET_Y__REG		BMA255_OFFSET_PARAMS_REG
+
+#define BMA255_COMP_TARGET_OFFSET_Z__POS		5
+#define BMA255_COMP_TARGET_OFFSET_Z__LEN		2
+#define BMA255_COMP_TARGET_OFFSET_Z__MSK		0x60
+#define BMA255_COMP_TARGET_OFFSET_Z__REG		BMA255_OFFSET_PARAMS_REG
+
+#define BMA255_SUCCESS					0
+#define BMA255_ERR_I2C					-1
+#define BMA255_ERR_STATUS				-3
+#define BMA255_ERR_SETUP_FAILURE		-4
+#define BMA255_ERR_GETGSENSORDATA		-5
+#define BMA255_ERR_IDENTIFICATION		-6
+
+#define BMA255_BUFSIZE					256
+
+/* soft reset */
+#define BMA255_RESET_REG                        0x14
+#define BMA255_EN_SOFT_RESET_VALUE        	0xB6
+
+#define BMA255_EN_SOFT_RESET__POS         0
+#define BMA255_EN_SOFT_RESET__LEN         8
+#define BMA255_EN_SOFT_RESET__MSK         0xFF
+#define BMA255_EN_SOFT_RESET__REG         BMA255_RESET_REG
+
+/* self test */
+#define BMA255_SELF_TEST_REG                        0x32
+
+#define BMA255_SELF_TEST_AMP__POS               4
+#define BMA255_SELF_TEST_AMP__LEN               1
+#define BMA255_SELF_TEST_AMP__MSK               0x10
+#define BMA255_SELF_TEST_AMP__REG               BMA255_SELF_TEST_REG
+
+#define BMA255_EN_SELF_TEST__POS                0
+#define BMA255_EN_SELF_TEST__LEN                2
+#define BMA255_EN_SELF_TEST__MSK                0x03
+#define BMA255_EN_SELF_TEST__REG                BMA255_SELF_TEST_REG
+
+#define BMA255_NEG_SELF_TEST__POS               2
+#define BMA255_NEG_SELF_TEST__LEN               1
+#define BMA255_NEG_SELF_TEST__MSK               0x04
+#define BMA255_NEG_SELF_TEST__REG               BMA255_SELF_TEST_REG
+
+/* bandwidth */
+#define BMA255_BW_7_81HZ			0x08
+#define BMA255_BW_15_63HZ			0x09
+#define BMA255_BW_62_50HZ			0x0B
+#define BMA255_BW_500HZ				0x0E
+#define BMA255_BW_1000HZ			0x0F
+
+#endif
+
diff --git a/drivers/misc/mediatek/accelerometer/da213/Makefile b/drivers/misc/mediatek/accelerometer/da213/Makefile
new file mode 100755
index 000000000..dd1a5de7a
--- /dev/null
+++ b/drivers/misc/mediatek/accelerometer/da213/Makefile
@@ -0,0 +1,4 @@
+include $(srctree)/drivers/misc/mediatek/Makefile.custom
+
+obj-y	:=  mir3da_core.o mir3da_cust.o
+
diff --git a/drivers/misc/mediatek/accelerometer/da213/mir3da_core.c b/drivers/misc/mediatek/accelerometer/da213/mir3da_core.c
new file mode 100644
index 000000000..95911e262
--- /dev/null
+++ b/drivers/misc/mediatek/accelerometer/da213/mir3da_core.c
@@ -0,0 +1,1757 @@
+/* Core file for MiraMEMS 3-Axis Accelerometer's driver.
+ *
+ * mir3da_core.c - Linux kernel modules for 3-Axis Accelerometer
+ *
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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.
+ *
+ */
+#include "mir3da_core.h"
+#include "mir3da_cust.h"
+
+#if MIR3DA_TV03_CALIBRATE
+	short x_off = 0;
+	short y_off = 0;
+	short z_off = 0;
+#endif
+
+#define MIR3DA_REG_PAGE(REG)                (((REG)>>8)&0xFF)
+#define MIR3DA_REG_ADDR(REG)                ((REG)&0xFF)
+
+#define MIR3DA_OFFSET_THRESHOLD             20
+#define PEAK_LVL                            800
+#define STICK_LSB                           2000
+#define AIX_HISTORY_SIZE                    3
+
+typedef struct reg_obj_s {
+
+    short               addr;
+    unsigned char       mask;
+    unsigned char       value;
+
+} reg_obj_t;
+
+struct gsensor_data_fmt_s {
+
+    unsigned char       msbw;
+    unsigned char       lsbw;
+    unsigned char       endian;                         /* 0: little endian; 1: big endian */
+};
+
+struct gsensor_data_obj_s {
+
+#define MIR3DA_DATA_LEN         6
+    reg_obj_t                   data_sect[MIR3DA_DATA_LEN];
+    struct gsensor_data_fmt_s   data_fmt;
+};
+
+struct gsensor_obj_s {
+
+    char                        asic[10];
+
+    reg_obj_t                   chip_id;
+    reg_obj_t                   mod_id;
+    reg_obj_t                   soft_reset;
+    reg_obj_t                   power;
+
+#define MIR3DA_INIT_SECT_LEN    11
+#define MIR3DA_OFF_SECT_LEN     MIR3DA_OFFSET_LEN
+#define MIR3DA_ODR_SECT_LEN	3
+
+    reg_obj_t                   init_sect[MIR3DA_INIT_SECT_LEN];
+    reg_obj_t                   offset_sect[MIR3DA_OFF_SECT_LEN];
+    reg_obj_t                   odr_sect[MIR3DA_ODR_SECT_LEN];
+
+    struct gsensor_data_obj_s   data;
+
+    int                         (*calibrate)(MIR_HANDLE handle, int z_dir);
+    int                         (*auto_calibrate)(MIR_HANDLE handle, int xyz[3]);
+    int                         (*int_ops)(MIR_HANDLE handle, mir_int_ops_t *ops);
+    int                         (*get_reg_data)(MIR_HANDLE handle, char *buf);
+};
+
+struct gsensor_drv_s {
+
+    struct general_op_s         *method;
+
+    struct gsensor_obj_s        *obj;
+};
+
+typedef enum _asic_type {
+	ASIC_NONE,
+	ASIC_2511,
+	ASIC_2512B,
+} asic_type;
+
+typedef enum _mems_type {
+	MEMS_NONE,
+	MEMS_T4,
+	MEMS_T9,
+	MEMS_TV03,
+} mems_type;
+
+typedef enum _package_type {
+	PACKAGE_NONE,
+	PACKAGE_2X2_12PIN,
+	PACKAGE_3X3_10PIN,
+	PACKAGE_3X3_16PIN,
+} package_type;
+
+struct  chip_info_s {
+	unsigned char    reg_value;
+	package_type     package;
+	asic_type        asic;
+	mems_type        mems;
+};
+
+struct  chip_info_s gsensor_chip_info;
+
+static struct chip_info_s         mir3da_chip_info_list[] = {
+	{0x00, PACKAGE_2X2_12PIN, ASIC_2512B, MEMS_TV03},
+	{0x01, PACKAGE_2X2_12PIN, ASIC_2511, MEMS_T4},
+	{0x02, PACKAGE_2X2_12PIN, ASIC_2511, MEMS_T9},
+	{0x03, PACKAGE_3X3_10PIN, ASIC_2511, MEMS_T4},
+	{0x04, PACKAGE_3X3_10PIN, ASIC_2511, MEMS_T9},
+	{0x05, PACKAGE_3X3_10PIN, ASIC_2511, MEMS_T4},
+	{0x06, PACKAGE_3X3_10PIN, ASIC_2511, MEMS_T9},
+	{0x07, PACKAGE_3X3_16PIN, ASIC_2511, MEMS_T4},
+	{0x08, PACKAGE_3X3_16PIN, ASIC_2511, MEMS_T9},
+	{0x09, PACKAGE_2X2_12PIN, ASIC_2511, MEMS_T4},
+	{0x0c, PACKAGE_2X2_12PIN, ASIC_2512B, MEMS_T9},
+	{0x33, PACKAGE_2X2_12PIN, ASIC_2511, MEMS_T9},
+	{0x34, PACKAGE_2X2_12PIN, ASIC_2511, MEMS_T9},
+	{0x35, PACKAGE_2X2_12PIN, ASIC_2511, MEMS_T9},
+};
+
+#define MIR3DA_NSA_INIT_SECTION                         { NSA_REG_G_RANGE,              0x03,   0x00    },                                  \
+							{ NSA_REG_POWERMODE_BW,         0xFF,   0x1e    },                                  \
+							{ NSA_REG_ODR_AXIS_DISABLE,     0xFF,   0x07    },                                  \
+							{ NSA_REG_INTERRUPT_SETTINGS2,  0xFF,   0x00    },                                  \
+							{ NSA_REG_INTERRUPT_MAPPING2,   0xFF,   0x00    },                                  \
+							{ NSA_REG_ENGINEERING_MODE,     0xFF,   0x83    },                                  \
+							{ NSA_REG_ENGINEERING_MODE,     0xFF,   0x69    },                                  \
+							{ NSA_REG_ENGINEERING_MODE,     0xFF,   0xBD    },                                  \
+							{ NSA_REG_INT_PIN_CONFIG,       0x0F,   0x05    },                                  \
+							{ -1,                           0x00,   0x00    },                                  \
+							{ -1,                           0x00,   0x00    },                                  \
+
+
+#define MIR3DA_NSA_OFFSET_SECTION                       { NSA_REG_COARSE_OFFSET_TRIM_X, 0xFF,   0x00    },                                  \
+							{ NSA_REG_COARSE_OFFSET_TRIM_Y, 0xFF,   0x00    },                                  \
+							{ NSA_REG_COARSE_OFFSET_TRIM_Z, 0xFF,   0x00    },                                  \
+							{ NSA_REG_FINE_OFFSET_TRIM_X,   0xFF,   0x00    },                                  \
+							{ NSA_REG_FINE_OFFSET_TRIM_Y,   0xFF,   0x00    },                                  \
+							{ NSA_REG_FINE_OFFSET_TRIM_Z,   0xFF,   0x00    },                                  \
+							{ NSA_REG_CUSTOM_OFFSET_X,      0xFF,   0x00    },                                  \
+							{ NSA_REG_CUSTOM_OFFSET_Y,      0xFF,   0x00    },                                  \
+							{ NSA_REG_CUSTOM_OFFSET_Z,      0xFF,   0x00    },                                  \
+
+#define MIR3DA_NSA_ODR_SECTION                          { NSA_REG_ODR_AXIS_DISABLE,     0x0F,   0x06    },                                  \
+							{ NSA_REG_ODR_AXIS_DISABLE,     0x0F,   0x07    },                                  \
+							{ NSA_REG_ODR_AXIS_DISABLE,     0x0F,   0x08    },                                  \
+
+
+#define MIR3DA_NSA_DATA_SECTION                       { { NSA_REG_ACC_X_LSB,            0xFF,   0x00    },                                  \
+							{ NSA_REG_ACC_X_MSB,            0xFF,   0x00    },                                  \
+							{ NSA_REG_ACC_Y_LSB,            0xFF,   0x00    },                                  \
+							{ NSA_REG_ACC_Y_MSB,            0xFF,   0x00    },                                  \
+							{ NSA_REG_ACC_Z_LSB,            0xFF,   0x00    },                                  \
+							{ NSA_REG_ACC_Z_MSB,            0xFF,   0x00    } },                                \
+							{ 8,                            4,      0       }
+
+
+static int NSA_NTO_calibrate(MIR_HANDLE handle, int z_dir);
+static int NSA_NTO_auto_calibrate(MIR_HANDLE handle, int xyz[3]);
+#if MIR3DA_AUTO_CALIBRATE
+static int mir3da_auto_calibrate(MIR_HANDLE handle, int x, int y, int z);
+#endif /* !MIR3DA_AUTO_CALIBRATE */
+static int NSA_interrupt_ops(MIR_HANDLE handle, mir_int_ops_t *ops);
+static int NSA_get_reg_data(MIR_HANDLE handle, char *buf);
+
+#define MIR_NSA_NTO                     { "NSA_NTO",    { NSA_REG_WHO_AM_I,             0xFF,   0x13    },                                  \
+							{ NSA_REG_FIFO_CTRL,            0xFF,   0x00    },                                  \
+							{ NSA_REG_SPI_I2C,              0x24,   0x24    },                                  \
+							{ NSA_REG_POWERMODE_BW,         0xC0,   0xC0    },                                  \
+							{ MIR3DA_NSA_INIT_SECTION                       },                                  \
+							{ MIR3DA_NSA_OFFSET_SECTION                     },                                  \
+							{ MIR3DA_NSA_ODR_SECTION			    },                                  \
+							{ MIR3DA_NSA_DATA_SECTION                       },                                  \
+							  NSA_NTO_calibrate                              ,                                  \
+							  NSA_NTO_auto_calibrate                         ,                                  \
+							  NSA_interrupt_ops                              ,                                  \
+							  NSA_get_reg_data                               ,                                  \
+					}
+/**************************************************************** COMMON ***************************************************************************/
+#define MIR3DA_GSENSOR_SCHEME           MIR3DA_SUPPORT_CHIP_LIST
+
+
+/* this level can be modified while runtime through system attribute */
+int                                 Log_level = DEBUG_ERR|DEBUG_ASSERT|DEBUG_MSG|DEBUG_FUNC|DEBUG_DATA;
+int                                gsensor_mod = -1;        /* Initial value */
+int                                gsensor_type = -1;        /* Initial value */
+static struct gsensor_obj_s         mir3da_gsensor[] = { MIR3DA_GSENSOR_SCHEME };
+struct gsensor_drv_s                mir3da_gsensor_drv;
+
+#define MI_DATA(format, ...)            if (DEBUG_DATA&Log_level) {mir3da_gsensor_drv.method->myprintf(MI_TAG format "\n", ## __VA_ARGS__); }
+#define MI_MSG(format, ...)             if (DEBUG_MSG&Log_level) {mir3da_gsensor_drv.method->myprintf(MI_TAG format "\n", ## __VA_ARGS__); }
+#define MI_ERR(format, ...)             if (DEBUG_ERR&Log_level) {mir3da_gsensor_drv.method->myprintf(MI_TAG format "\n", ## __VA_ARGS__); }
+#define MI_FUN                          if (DEBUG_FUNC&Log_level) {mir3da_gsensor_drv.method->myprintf(MI_TAG "%s is called, line: %d\n", __func__, __LINE__); }
+#define MI_ASSERT(expr)                 \
+	if (!(expr)) {\
+		mir3da_gsensor_drv.method->myprintf("Assertion failed! %s,%d,%s,%s\n",\
+			__FILE__, __LINE__, __func__, #expr);\
+	}
+
+#ifdef abs
+#undef abs
+#define abs(x) ({ long __x = (x); (__x < 0) ? -__x : __x; })
+#endif
+
+#if FILTER_AVERAGE_ENHANCE
+typedef struct FilterAverageContextTag {
+    int sample_l;
+    int sample_h;
+    int filter_param_l;
+    int filter_param_h;
+    int filter_threhold;
+
+    int refN_l;
+    int refN_h;
+
+} FilterAverageContext;
+
+typedef struct mir3da_core_ctx_s {
+    struct mir3da_filter_param_s    filter_param;
+    FilterAverageContext            tFac[3];
+} mir3da_core_ctx;
+
+static mir3da_core_ctx       core_ctx;
+#endif
+
+#if MIR3DA_SENS_TEMP_SOLUTION
+static int bSensZoom;
+#endif
+
+#if MIR3DA_OFFSET_TEMP_SOLUTION
+static int is_cali;
+char bLoad = FILE_CHECKING;
+static char readOffsetCnt =  -1;
+static unsigned char original_offset[9];
+static int mir3da_write_offset_to_file(unsigned char *offset);
+static int mir3da_read_offset_from_file(unsigned char *offset);
+void manual_load_cali_file(MIR_HANDLE handle);
+#endif /* !MIR3DA_OFFSET_TEMP_SOLUTION */
+
+#if MIR3DA_STK_TEMP_SOLUTION
+static short aixHistort[AIX_HISTORY_SIZE*3] = {0};
+static short aixHistoryIndex;
+char bxstk = 0;
+char bystk = 0;
+char bzstk = 0;
+
+static void addAixHistory(short x, short y, short z)
+{
+    aixHistort[aixHistoryIndex++] = x;
+    aixHistort[aixHistoryIndex++] = y;
+    aixHistort[aixHistoryIndex++] = z;
+    aixHistoryIndex = (aixHistoryIndex)%(AIX_HISTORY_SIZE*3);
+}
+
+static char isXStick(void){
+    int i;
+    for (i = 0; i < AIX_HISTORY_SIZE; i++) {
+	if ((abs(aixHistort[i*3]) < STICK_LSB) && (aixHistort[i*3] != 0)) {
+	    break;
+	}
+    }
+
+	if ((aixHistort[0] + aixHistort[3]+aixHistort[6]) == 0)
+		return 1;
+
+    return i == AIX_HISTORY_SIZE;
+}
+
+static char isYStick(void){
+    int i;
+    for (i = 0; i < AIX_HISTORY_SIZE; i++) {
+	if ((abs(aixHistort[i*3+1]) < STICK_LSB) && (aixHistort[i*3+1] != 0)) {
+	    break;
+	}
+    }
+
+	if ((aixHistort[1] + aixHistort[4]+aixHistort[7]) == 0)
+		return 1;
+
+    return i == AIX_HISTORY_SIZE;
+}
+
+static char isZStick(void){
+    int i;
+    for (i = 0; i < AIX_HISTORY_SIZE; i++) {
+	if ((abs(aixHistort[i*3+2]) < STICK_LSB) && (aixHistort[i*3+2] != 0)) {
+	    break;
+	}
+    }
+
+	if ((aixHistort[2] + aixHistort[5]+aixHistort[8]) == 0)
+		return 1;
+
+    return i == AIX_HISTORY_SIZE;
+}
+
+int squareRoot(int val)
+{
+    int r = 0;
+    int shift;
+
+    if (val < 0) {
+		return 0;
+    }
+
+    for (shift = 0; shift < 32; shift += 2) {
+		int x = 0x40000000l >> shift;
+
+		if (x + r <= val) {
+			val -= x + r;
+			r = (r >> 1) | x;
+		} else {
+			r = r >> 1;
+		}
+    }
+
+    return r;
+}
+#endif /* ! MIR3DA_STK_TEMP_SOLUTION */
+
+#if FILTER_AVERAGE_ENHANCE
+static short filter_average(short preAve, short sample, int paramN, int *refNum)
+{
+ #if FILTER_AVERAGE_EX
+    if (abs(sample-preAve) > PEAK_LVL  && *refNum < 3) {
+	 MI_DATA("Hit, sample = %d, preAve = %d, refN =%d\n", sample, preAve, *refNum);
+	 sample = preAve;
+	 (*refNum)++;
+    } else{
+	 if (*refNum == 3) {
+		preAve = sample;
+	 }
+
+	 *refNum  = 0;
+    }
+#endif
+
+    return preAve + (sample - preAve)/paramN;
+}
+
+static int filter_average_enhance(FilterAverageContext *fac, short sample)
+{
+    if (fac == 0) {
+	MI_ERR("0 parameter fac");
+	return 0;
+    }
+
+    if (fac->filter_param_l == fac->filter_param_h) {
+	fac->sample_l = fac->sample_h = filter_average(fac->sample_l, sample, fac->filter_param_l, &fac->refN_l);
+    } else{
+	fac->sample_l = filter_average(fac->sample_l, sample, fac->filter_param_l,  &fac->refN_l);
+	fac->sample_h = filter_average(fac->sample_h, sample, fac->filter_param_h, &fac->refN_h);
+	if (abs(fac->sample_l - fac->sample_h) > fac->filter_threhold) {
+	    MI_DATA("adjust, fac->sample_l = %d, fac->sample_h = %d\n", fac->sample_l, fac->sample_h);
+	    fac->sample_h = fac->sample_l;
+	}
+     }
+
+    return fac->sample_h;
+}
+#endif /* ! FILTER_AVERAGE_ENHANCE */
+
+int mir3da_register_read(MIR_HANDLE handle, short addr, unsigned char *data)
+{
+    unsigned char      cur_page;
+    int     res = 0;
+
+    /* check page */
+    if (MIR3DA_REG_PAGE(addr) > 0) {
+	res = mir3da_gsensor_drv.method->smi.read(handle, 0x0, &cur_page);
+	if (res != 0) {
+	    return res;
+	}
+
+	if (cur_page != MIR3DA_REG_PAGE(addr)) {
+	    res |= mir3da_gsensor_drv.method->smi.write(handle, 0x0, MIR3DA_REG_PAGE(addr));
+	    if (res != 0) {
+		return res;
+	    }
+	}
+    }
+
+    res = mir3da_gsensor_drv.method->smi.read(handle, MIR3DA_REG_ADDR(addr), data);
+
+    if (MIR3DA_REG_PAGE(addr) > 0) {
+	/* restore page NO. */
+	res |= mir3da_gsensor_drv.method->smi.write(handle, 0x0, cur_page);
+    }
+
+    return res;
+}
+
+int mir3da_register_read_continuously(MIR_HANDLE handle, short addr, unsigned char count, unsigned char *data)
+{
+    unsigned char      cur_page;
+    int     res = 0;
+
+    /* check page */
+    if (MIR3DA_REG_PAGE(addr) > 0) {
+	res = mir3da_gsensor_drv.method->smi.read(handle, 0x0, &cur_page);
+	if (res != 0) {
+	    return res;
+	}
+
+	if (cur_page != MIR3DA_REG_PAGE(addr)) {
+	    res |= mir3da_gsensor_drv.method->smi.write(handle, 0x0, MIR3DA_REG_PAGE(addr));
+	    if (res != 0) {
+		return res;
+	    }
+	}
+    }
+
+    res = (count == mir3da_gsensor_drv.method->smi.read_block(handle, MIR3DA_REG_ADDR(addr), count, data)) ? 0 : 1;
+
+    if (MIR3DA_REG_PAGE(addr) > 0) {
+	/* restore page NO. */
+	res |= mir3da_gsensor_drv.method->smi.write(handle, 0x0, cur_page);
+    }
+
+    return res;
+}
+
+int mir3da_register_write(MIR_HANDLE handle, short addr, unsigned char data)
+{
+    unsigned char      cur_page;
+    int     res = 0;
+
+    /* check page */
+    if (MIR3DA_REG_PAGE(addr) > 0) {
+	res = mir3da_gsensor_drv.method->smi.read(handle, 0x0, &cur_page);
+	if (res != 0) {
+	    return res;
+	}
+
+	if (cur_page != MIR3DA_REG_PAGE(addr)) {
+	    res |= mir3da_gsensor_drv.method->smi.write(handle, 0x0, MIR3DA_REG_PAGE(addr));
+	    if (res != 0) {
+		return res;
+	    }
+	}
+    }
+
+    res = mir3da_gsensor_drv.method->smi.write(handle, MIR3DA_REG_ADDR(addr), data);
+
+    if (MIR3DA_REG_PAGE(addr) > 0) {
+	/* restore page NO. */
+	res |= mir3da_gsensor_drv.method->smi.write(handle, 0x0, cur_page);
+    }
+
+    return res;
+}
+
+int mir3da_register_mask_write(MIR_HANDLE handle, short addr, unsigned char mask, unsigned char data)
+{
+    int     res = 0;
+    unsigned char      tmp_data;
+
+    res = mir3da_register_read(handle, addr, &tmp_data);
+    if (res) {
+	return res;
+    }
+
+    tmp_data &= ~mask;
+    tmp_data |= data & mask;
+    res = mir3da_register_write(handle, addr, tmp_data);
+
+    return res;
+}
+
+static int mir3da_read_raw_data(MIR_HANDLE handle, short *x, short *y, short *z)
+{
+    unsigned char    tmp_data[6] = {0};
+
+    if (mir3da_register_read_continuously(handle, mir3da_gsensor_drv.obj[gsensor_mod].data.data_sect[0].addr, 6, tmp_data) != 0) {
+	MI_ERR("i2c block read failed\n");
+	return -1;
+    }
+
+    *x = ((short)(tmp_data[1] << mir3da_gsensor_drv.obj[gsensor_mod].data.data_fmt.msbw | tmp_data[0])) >> (8-mir3da_gsensor_drv.obj[gsensor_mod].data.data_fmt.lsbw);
+    *y = ((short)(tmp_data[3] << mir3da_gsensor_drv.obj[gsensor_mod].data.data_fmt.msbw | tmp_data[2])) >> (8-mir3da_gsensor_drv.obj[gsensor_mod].data.data_fmt.lsbw);
+    *z = ((short)(tmp_data[5] << mir3da_gsensor_drv.obj[gsensor_mod].data.data_fmt.msbw | tmp_data[4])) >> (8-mir3da_gsensor_drv.obj[gsensor_mod].data.data_fmt.lsbw);
+
+    MI_DATA("mir3da_raw: x=%d, y=%d, z=%d",  *x, *y, *z);
+
+#if MIR3DA_SENS_TEMP_SOLUTION
+    if (bSensZoom == 1) {
+	*z = (*z)*5/4;
+	MI_DATA("SensZoom take effect, Zoomed Z = %d", *z);
+    }
+#endif
+    return 0;
+}
+
+static int remap[8][4] = {{0, 0, 0, 0},
+					{0, 1, 0, 1},
+					{1, 1, 0, 0},
+					{1, 0, 0, 1},
+					{1, 0, 1, 0},
+					{0, 0, 1, 1},
+					{0, 1, 1, 0},
+					{1, 1, 1, 1} };
+
+int mir3da_direction_remap(short *x, short *y, short *z, int direction)
+{
+	short temp = 0;
+
+	*x = *x - ((*x) * remap[direction][0]*2);
+	*y = *y - ((*y) * remap[direction][1]*2);
+	*z = *z - ((*z) * remap[direction][2]*2);
+
+	if (remap[direction][3]) {
+		temp = *x;
+		*x = *y;
+		*y = temp;
+	}
+
+	if (remap[direction][2])
+		return -1;
+
+	return 1;
+}
+
+int mir3da_read_data(MIR_HANDLE handle, short *x, short *y, short *z)
+{
+    int    rst = 0;
+
+#if MIR3DA_SUPPORT_MULTI_LAYOUT
+    short temp = 0;
+#endif
+
+#if MIR3DA_OFFSET_TEMP_SOLUTION
+    if (is_cali) {
+	*x = *y = *z = 0;
+	return 0;
+    }
+
+    manual_load_cali_file(handle);
+#endif
+
+    rst = mir3da_read_raw_data(handle, x, y, z);
+    if (rst != 0) {
+	MI_ERR("mir3da_read_raw_data failed, rst = %d", rst);
+	return rst;
+    }
+
+#if MIR3DA_TV03_CALIBRATE
+		if (gsensor_chip_info.mems == MEMS_TV03) {
+				*x += x_off;
+				*y += y_off;
+				*z += z_off;
+		}
+#endif
+
+#if MIR3DA_AUTO_CALIBRATE
+#if MIR3DA_SUPPORT_FAST_AUTO_CALI
+    if ((mir3da_gsensor_drv.method->support_fast_auto_cali() && (bLoad != FILE_EXIST)) || (bLoad == FILE_NO_EXIST))
+#else
+    if (bLoad == FILE_NO_EXIST)
+#endif
+    {
+	   mir3da_auto_calibrate(handle, *x, *y, *z);
+    }
+#endif
+
+#if MIR3DA_STK_TEMP_SOLUTION
+    addAixHistory(*x, *y, *z);
+
+    bxstk = isXStick();
+    bystk = isYStick();
+    bzstk = isZStick();
+
+    if ((bxstk + bystk + bzstk) < 2) {
+	if (bxstk)
+	*x = squareRoot(1024*1024 - (*y)*(*y) - (*z)*(*z));
+    if (bystk)
+	*y = squareRoot(1024*1024 - (*x)*(*x) - (*z)*(*z));
+    if (bzstk)
+	*z = squareRoot(1024*1024 - (*x)*(*x) - (*y)*(*y));
+    } else{
+	/* MI_ERR( "CHIP ERR !MORE STK!\n"); */
+	return 0;
+    }
+#endif
+
+
+#if FILTER_AVERAGE_ENHANCE
+    *x = filter_average_enhance(&core_ctx.tFac[0], *x);
+    *y = filter_average_enhance(&core_ctx.tFac[1], *y);
+    *z = filter_average_enhance(&core_ctx.tFac[2], *z);
+    MI_DATA("mir3da_filt: x=%d, y=%d, z=%d",  *x, *y, *z);
+#endif
+
+
+#if MIR3DA_SUPPORT_MULTI_LAYOUT
+	if (gsensor_chip_info.package == PACKAGE_2X2_12PIN) {
+		*x =  *x;
+		*y =  *z;
+		*z =  *z;
+	} else if (gsensor_chip_info.package == PACKAGE_3X3_10PIN) {
+		temp = *x;
+		*x = *y;
+		*y = temp;
+		*z =  *z;
+	} else if (gsensor_chip_info.package == PACKAGE_3X3_16PIN) {
+		temp = -1*(*x);
+		*x = -1*(*y);
+		*y = temp;
+		*z =  *z;
+	}
+#endif
+
+    return 0;
+}
+
+int cycle_read_xyz(MIR_HANDLE handle, int *x, int *y, int *z, int ncycle)
+{
+    unsigned int j = 0;
+    short raw_x, raw_y, raw_z;
+
+    *x = *y = *z = 0;
+
+    for (j = 0; j < ncycle; j++) {
+		raw_x = raw_y = raw_z = 0;
+		mir3da_read_raw_data(handle, &raw_x, &raw_y, &raw_z);
+
+		(*x) += raw_x;
+		(*y) += raw_y;
+		(*z) += raw_z;
+
+		mir3da_gsensor_drv.method->msdelay(5);
+    }
+
+    (*x) /= ncycle;
+    (*y) /= ncycle;
+    (*z) /= ncycle;
+
+    return 0;
+}
+
+int mir3da_read_offset(MIR_HANDLE handle, unsigned char *offset)
+{
+    int     i, res = 0;
+
+    for (i = 0; i < MIR3DA_OFF_SECT_LEN; i++) {
+	if (mir3da_gsensor_drv.obj[gsensor_mod].offset_sect[i].addr < 0) {
+	    break;
+	}
+
+	res = mir3da_register_read(handle, mir3da_gsensor_drv.obj[gsensor_mod].offset_sect[i].addr, &offset[i]);
+	if (res != 0) {
+	    return res;
+	}
+    }
+
+    return res;
+}
+
+int mir3da_write_offset(MIR_HANDLE handle, unsigned char *offset)
+{
+    int     i, res = 0;
+
+#if MIR3DA_TV03_CALIBRATE
+	if (gsensor_chip_info.mems == MEMS_TV03) {
+				x_off = ((short)(offset[1] << 8 | offset[0]));
+				y_off = ((short)(offset[3] << 8 | offset[2]));
+				z_off = ((short)(offset[5] << 8 | offset[4]));
+
+				if (abs(z_off) > 1500)
+						z_off = z_off - 2048;
+
+				return 0;
+	}
+#else
+	if (gsensor_chip_info.mems == MEMS_TV03) {
+			return 0;
+	}
+#endif
+
+    for (i = 0; i < MIR3DA_OFF_SECT_LEN; i++) {
+	if (mir3da_gsensor_drv.obj[gsensor_mod].offset_sect[i].addr < 0) {
+	    break;
+	}
+
+	res = mir3da_register_write(handle, mir3da_gsensor_drv.obj[gsensor_mod].offset_sect[i].addr, offset[i]);
+	if (res != 0) {
+	    return res;
+	}
+    }
+
+    return res;
+}
+
+#if MIR3DA_OFFSET_TEMP_SOLUTION
+static int mir3da_write_offset_to_file(unsigned char *offset)
+{
+    int     ret = 0;
+
+    if (NULL == mir3da_gsensor_drv.method->data_save)
+	return 0;
+
+    ret = mir3da_gsensor_drv.method->data_save(offset);
+
+    MI_MSG("====sensor_sync_write, offset = 0x%x,0x%x,0x%x,0x%x,0x%x,0x%x,0x%x,0x%x,0x%x", offset[0], offset[1], offset[2], offset[3], offset[4], offset[5], offset[6], offset[7], offset[8]);
+
+    return ret;
+}
+
+static int mir3da_read_offset_from_file(unsigned char *offset)
+{
+    int     ret = 0;
+    int     i = 0, sum = 0;
+
+    if (NULL == mir3da_gsensor_drv.method->data_get)
+	return -1;
+
+    ret = mir3da_gsensor_drv.method->data_get(offset);
+
+    for (i = 0; i < MIR3DA_OFF_SECT_LEN; i++) {
+	sum += offset[i];
+    }
+
+    if (sum == 0)
+       return -1;
+
+    MI_MSG("====sensor_sync_read, offset = 0x%x,0x%x,0x%x,0x%x,0x%x,0x%x,0x%x,0x%x,0x%x", offset[0], offset[1], offset[2], offset[3], offset[4], offset[5], offset[6], offset[7], offset[8]);
+
+    return ret;
+}
+
+void manual_load_cali_file(MIR_HANDLE handle)
+{
+    unsigned char  offset[MIR3DA_OFFSET_LEN] = {0};
+
+    if (bLoad == FILE_CHECKING) {
+		readOffsetCnt++;
+
+		if (readOffsetCnt%10 == 0) {
+			if (readOffsetCnt > 0)
+				readOffsetCnt = 0;
+
+			 MI_ERR("====444 manual_load_cali_file(), bLoad = %d, readOffsetCnt=%d.\n", bLoad, readOffsetCnt);
+
+		       if (mir3da_gsensor_drv.method->data_check()) {
+			     if (!mir3da_read_offset_from_file(offset)) {
+				     MI_ERR("========= WRITE OFFSET");
+				     mir3da_write_offset(handle, offset);
+				     bLoad = FILE_EXIST;
+			     } else{
+				     bLoad = FILE_NO_EXIST;
+			     }
+			} else{
+			     bLoad = FILE_CHECKING;
+			}
+	     }
+    }
+}
+
+static void mir3da_cali_off_to_lsb(int off, int *coarse, int coarse_step, int *fine, int fine_step)
+{
+	*coarse = off/coarse_step;
+	*fine = 100*(off-(*coarse)*coarse_step)/fine_step;
+
+	MI_MSG("off = %d; delta_coarse = %d; delta_fine = %d", off, *coarse, *fine);
+}
+
+#if MIR3DA_AUTO_CALIBRATE
+static int NSA_once_calibrate(MIR_HANDLE handle, int coarse_step[3], int fine_step[3], int xyz[3])
+{
+    int     coarse[3] = {0};
+    int     coarse_delta[3] = {0};
+    int     fine[3] = {0};
+    int     fine_delta[3] = {0};
+    int     target[3] = {0};
+    int     i;
+    unsigned char   offset_data[9] = {0};
+
+    if (mir3da_read_offset(handle, offset_data)) {
+	MI_ERR("Get old offset failed !");
+	return -1;
+    }
+    coarse[0] = offset_data[0] & 0x3f;
+    coarse[1] = offset_data[1] & 0x3f;
+    coarse[2] = offset_data[2] & 0x3f;
+    fine[0] = (((int)offset_data[0] << 2) & 0x300)|offset_data[3];
+    fine[1] = (((int)offset_data[1] << 2) & 0x300)|offset_data[4];
+    fine[2] = (((int)offset_data[2] << 2) & 0x300)|offset_data[5];
+
+    MI_MSG("Old coarse_x = %d; coarse_y = %d; coarse_z = %d; fine_x = %d; fine_y = %d; fine_z = %d;", coarse[0], coarse[1], coarse[2], fine[0], fine[1], fine[2]);
+
+    /* 0 means auto detect z direction assume z axis is verticle */
+    if ((abs(target[0]) + abs(target[1]) + abs(target[2])) == 0) {
+	target[2] = (xyz[2] > 0) ? 1024 : (-1024);
+    }
+
+    for (i = 0; i < 3; i++) {
+	coarse_step[i] *= coarse[i] >= 32 ? (-1) : 1;
+	mir3da_cali_off_to_lsb((xyz[i]-target[i]), &coarse_delta[i], coarse_step[i], &fine_delta[i], fine_step[i]);
+
+	coarse[i] += coarse_delta[i];
+	fine[i] += fine_delta[i];
+	offset_data[i] = coarse[i]|((fine[i]>>2)&0xc0);
+	offset_data[i+3] = fine[i]&0xFF;
+    }
+
+    if (mir3da_write_offset(handle, offset_data)) {
+	MI_ERR("Update offset failed !");
+	return -1;
+    }
+    /* Discard unstable data after offset register changed */
+    cycle_read_xyz(handle, &xyz[0], &xyz[1], &xyz[2], 5);
+    if (cycle_read_xyz(handle, &xyz[0], &xyz[1], &xyz[2], 10)) {
+	return -1;
+    }
+    MI_MSG("---calibrate_Done, x = %d, y = %d, z = %d, coarse_x = %d, coarse_y = %d, coarse_z = %d, fine_x = %d, fine_y = %d, fine_z = %d", xyz[0], xyz[1], xyz[2], coarse[0], coarse[1], coarse[2], fine[0], fine[1], fine[2]);
+
+    return mir3da_write_offset_to_file(offset_data);
+}
+#endif /* !MIR3DA_AUTO_CALIBRATE */
+
+static int NSA_calibrate(MIR_HANDLE handle, int coarse_step[3], int fine_step[3], int fine_max, int target[3])
+{
+    int             i = 0, j = 0;
+    unsigned char   ncycle = 20;
+    unsigned char   nLoop = 20;
+    unsigned char   offset_data[9] = {0};
+    unsigned char   fine_ok_map = 0;
+
+    int             xyz[3] = {0};
+    int             coarse[3] = {0};
+    int             coarse_delta[3] = {0};
+    int             fine[3] = {0};
+    int             fine_delta[3] = {0};
+
+    if ((abs(target[0]) + abs(target[1]) + abs(target[2])) != 0 && (abs(target[0]) + abs(target[1]) + abs(target[2])) != 1024) {
+	MI_ERR("Invalid argument !");
+	return -1;
+    }
+
+    /* 0 means auto detect z direction assume z axis is verticle */
+    if ((abs(target[0]) + abs(target[1]) + abs(target[2])) == 0) {
+       if (cycle_read_xyz(handle, &xyz[0], &xyz[1], &xyz[2], 5)) {
+	    MI_ERR("check z direction failed\n");
+	    return -1;
+       }
+       target[2] = (xyz[2] > 0) ? 1024 : (-1024);
+    }
+
+    MI_MSG("---Start Calibrate, trim target %d, %d, %d---\n", target[0], target[1], target[2]);
+
+    /* Stage1: Coarse tune once */
+    MI_MSG("---Stage1, coarse tune---");
+    /* change to 16G mode */
+    if (mir3da_register_mask_write(handle, NSA_REG_G_RANGE, 0x03, 3)) {
+	MI_ERR("i2c mask write failed !\n");
+	return -1;
+    }
+
+    /* reset coarse offset register */
+    mir3da_write_offset(handle, offset_data);
+    /* Discard unstable data after offset register changed */
+    cycle_read_xyz(handle, &xyz[0], &xyz[1], &xyz[2], 5);
+
+    if (cycle_read_xyz(handle, &xyz[0], &xyz[1], &xyz[2], ncycle)) {
+	goto EXIT_16G_MOD;
+    }
+
+    for (i = 0; i < 3; i++) {
+	/* check rule */
+	xyz[i] *= 8;
+
+	coarse[i] = ((xyz[i]-target[i]) > 0) ? 0 : 32;
+
+	MI_MSG("xyz[%d] = %d, coarse[%d] = 0x%x", i, xyz[i], i, coarse[i]);
+
+	coarse_step[i] *= coarse[i] >= 32 ? (-1) : 1;
+	mir3da_cali_off_to_lsb((xyz[i]-target[i]), &coarse_delta[i], coarse_step[i], &fine_delta[i], fine_step[i]);
+
+	coarse[i] += coarse_delta[i];
+	fine[i] += fine_delta[i];
+	mir3da_register_mask_write(handle, NSA_REG_COARSE_OFFSET_TRIM_X+i, 0x3f, (unsigned char)coarse[i]);
+    }
+
+    /* Discard unstable data after offset register changed */
+    cycle_read_xyz(handle, &xyz[0], &xyz[1], &xyz[2], 5);
+    if (cycle_read_xyz(handle, &xyz[0], &xyz[1], &xyz[2], 5)) {
+	return -1;
+    }
+    for (i = 0; i < 3; i++) {
+	fine[i] += (xyz[i] > 0) ? 0 : fine_max;
+	mir3da_register_write(handle, NSA_REG_FINE_OFFSET_TRIM_X+i, (unsigned char)(fine[i]&0xff));
+	mir3da_register_mask_write(handle, NSA_REG_COARSE_OFFSET_TRIM_X+i, 0xc0, (unsigned char)(0xc0&(fine[i]>>2)));
+    }
+
+EXIT_16G_MOD:
+    /* change back to 2G mode */
+    if (mir3da_register_mask_write(handle, NSA_REG_G_RANGE, 0x03, 0)) {
+	MI_ERR("i2c mask write failed !\n");
+	return -1;
+    }
+    /* Discard unstable data after offset register changed */
+    cycle_read_xyz(handle, &xyz[0], &xyz[1], &xyz[2], 5);
+    if (cycle_read_xyz(handle, &xyz[0], &xyz[1], &xyz[2], ncycle)) {
+	return -1;
+    }
+    MI_MSG("---Stage1, coarse tune done: x = %d, y = %d, z = %d, coarse_x = %d, coarse_y = %d, coarse_z = %d, fine_x = %d, fine_y = %d, fine_z = %d", xyz[0], xyz[1], xyz[2], coarse[0], coarse[1], coarse[2], fine[0], fine[1], fine[2]);
+
+    /* Stage2: Fine tune */
+    MI_MSG("---Stage2, Fine tune---");
+    for (i = 0; i < nLoop; i++) {
+
+	if (0x07 == (fine_ok_map & 0x07)) {
+	    break;
+	}
+	/* Discard unstable data after offset register changed */
+	cycle_read_xyz(handle, &xyz[0], &xyz[1], &xyz[2], 5);
+	MI_MSG("---Stage2, Fine loop %d", i);
+	if (cycle_read_xyz(handle, &xyz[0], &xyz[1], &xyz[2], ncycle)) {
+	    return -1;
+	}
+
+	for (j = 0; j < 3; j++) {
+	    MI_MSG("xyz[%d] = %d, caorse[%d] = 0x%x, fine[%d] = 0x%x", j, xyz[j], j, coarse[j], j, fine[j]);
+	    if (abs(xyz[j]-target[j]) < MIR3DA_OFFSET_THRESHOLD) {
+		fine_ok_map |= (1<<j);
+		offset_data[j] = coarse[j]|((fine[j]>>2)&0xc0);
+		offset_data[j+3] = fine[j];
+		continue;
+	    }
+	    mir3da_cali_off_to_lsb((xyz[j]-target[j]), &coarse_delta[j], coarse_step[j], &fine_delta[j], fine_step[j]);
+
+	    coarse[j] += coarse_delta[j];
+	    fine[j] += fine_delta[j];
+	    mir3da_register_write(handle, NSA_REG_FINE_OFFSET_TRIM_X+j, (unsigned char)(fine[j]&0xff));
+	    mir3da_register_mask_write(handle, NSA_REG_COARSE_OFFSET_TRIM_X+j, 0xFF, (unsigned char)(0xc0&(fine[j]>>2))|coarse[j]);
+	}
+    }
+    MI_MSG("---Stage2, Fine tune done: x = %d, y = %d, z = %d, coarse_x = %d, coarse_y = %d, coarse_z = %d, fine_x = %d, fine_y = %d, fine_z = %d", xyz[0], xyz[1], xyz[2], coarse[0], coarse[1], coarse[2], fine[0], fine[1], fine[2]);
+
+    if (0x07 == (fine_ok_map & 0x07)) {
+	goto SUCCESS_EXIT;
+    }
+#if MIR3DA_STK_TEMP_SOLUTION
+    if (0x03 == (fine_ok_map & 0x07)) {
+	goto SUCCESS_EXIT;
+    }
+#endif
+
+    MI_MSG("---calibrate Failed !---");
+    return -1;
+
+SUCCESS_EXIT:
+    MI_MSG("---calibrate OK !---");
+    return mir3da_write_offset_to_file(offset_data);
+}
+
+static int NSA_NTO_cali_step_calc(MIR_HANDLE handle, int coarse[3], int x100_fine[3], int x100_cust[3])
+{
+    int                i;
+    unsigned int       total_gain[3] = {0};
+    unsigned char      coarse_gain = 0;
+    unsigned char      fine_gain[3] = {0};
+    unsigned int       const coarse_gain_map[] = {1000, 1125, 1250, 1375, 500, 625, 750, 875};   /* *1000  */
+    unsigned char      const fine_dig_gain_map[] = {1, 2, 4, 8};
+
+    if (mir3da_register_read_continuously(handle, NSA_REG_SENSITIVITY_TRIM_X, 3, fine_gain) != 0) {
+	MI_ERR("i2c block read failed\n");
+	return -1;
+    }
+
+    if (mir3da_register_read(handle, NSA_REG_SENS_COARSE_TRIM, &coarse_gain) != 0) {
+	MI_ERR("i2c block read failed\n");
+	return -1;
+    }
+
+    for (i = 0; i < 3; i++) {
+	/* *100*1000 */
+	total_gain[i] = ((1000 + (fine_gain[i]&0x1F)*1000/32)/15) * fine_dig_gain_map[((fine_gain[i]>>5)&0x03)] * coarse_gain_map[coarse_gain&0x07];
+	coarse[i] = (int)(total_gain[i] * 500 / 100000);
+	x100_fine[i] = (int)(total_gain[i] * 293 / 100000);
+	x100_cust[i] = (int)(total_gain[i] * 390 / 100000);
+    }
+    MI_MSG("coarse_step_x = %d, coarse_step_y = %d, coarse_step_z = %d\n", coarse[0], coarse[1], coarse[2]);
+    MI_MSG("fine_step_x = %d, fine_step_y = %d, fine_step_z = %d\n", x100_fine[0], x100_fine[1], x100_fine[2]);
+    MI_MSG("custom_step_x = %d, custom_step_y = %d, custom_step_z = %d\n", x100_cust[0], x100_cust[1], x100_cust[2]);
+
+    return 0;
+}
+#endif /* !MIR3DA_OFFSET_TEMP_SOLUTION */
+
+static int NSA_NTO_calibrate(MIR_HANDLE handle, int z_dir)
+{
+    int     result = 0;
+
+#if MIR3DA_OFFSET_TEMP_SOLUTION
+		int     coarse_step[3] = {0};
+		int     fine_step[3] = {0};
+		int     custom_step[3] = {0};
+		int     target[3] = {0};
+
+   unsigned char     swap_plarity_old = 0;
+
+#if MIR3DA_TV03_CALIBRATE
+		int             xyz[3] = {0};
+		int             xyz_t[3] = {0};
+		unsigned char   offset_data[9] = {0};
+
+		if (gsensor_chip_info.mems == MEMS_TV03) {
+				if (cycle_read_xyz(handle, &xyz[0], &xyz[1], &xyz[2], 20)) {
+						MI_ERR("read data failed!\n");
+						return -1;
+				}
+
+				if (cycle_read_xyz(handle, &xyz_t[0], &xyz_t[1], &xyz_t[2], 20)) {
+						MI_ERR("read data failed!\n");
+						return -1;
+				}
+
+				if (abs(xyz[0]-xyz_t[0]) < 10 && abs(xyz[1]-xyz_t[1]) < 10	&& abs(xyz[2]-xyz_t[2]) < 10) {
+						xyz_t[0] = 0 - (xyz[0]+xyz_t[0])/2;
+						xyz_t[1] = 0 - (xyz[1]+xyz_t[1])/2;
+						xyz_t[2] =  1024 - (xyz[2]+xyz_t[2])/2;
+
+	    x_off = xyz_t[0];
+	    y_off = xyz_t[1];
+	    z_off = xyz_t[2];
+
+						if (abs(z_off) > 1500)
+								z_off = z_off - 2048;
+
+						offset_data[0] = xyz_t[0] & 0xFF;
+						offset_data[1] = (xyz_t[0] & 0xFF00) >> 8;
+						offset_data[2] = xyz_t[1] & 0xFF;
+						offset_data[3] = (xyz_t[1] & 0xFF00) >> 8;
+						offset_data[4] = xyz_t[2] & 0xFF;
+						offset_data[5] = (xyz_t[2] & 0xFF00) >> 8;
+
+						return mir3da_write_offset_to_file(offset_data);
+				} else{
+						MI_ERR("read data difference is too big!\n");
+						return -1;
+				}
+		}
+
+#endif
+
+    /* compute step */
+    if (NSA_NTO_cali_step_calc(handle, coarse_step, fine_step, custom_step)) {
+	MI_ERR("Compute step failed !");
+	return -1;
+    }
+    target[2] = z_dir*1024;
+
+    /* save swap/plarity old setting */
+    if (mir3da_register_read(handle, NSA_REG_SWAP_POLARITY, &swap_plarity_old)) {
+	MI_ERR("Get SWAP/PLARITY setting failed !");
+	return -1;
+    }
+
+	if (gsensor_chip_info.asic == ASIC_2512B) {
+			coarse_step[2] = 2 * coarse_step[2];
+			target[2] = ((swap_plarity_old & (1<<1)) == 0) ? (-target[2]) : target[2];
+			if (mir3da_register_mask_write(handle, NSA_REG_SWAP_POLARITY, 0x0F, 0x0E)) {
+					MI_ERR("Set Plarity failed !");
+					return -1;
+			}
+	} else if (gsensor_chip_info.asic == ASIC_2511) {
+		target[2] = ((swap_plarity_old & (1<<1)) != 0) ? (-target[2]) : target[2];
+			if (mir3da_register_mask_write(handle, NSA_REG_SWAP_POLARITY, 0x0F, 0x00)) {
+					MI_ERR("Set Plarity failed !");
+					return -1;
+			}
+	}
+
+    result = NSA_calibrate(handle, coarse_step, fine_step, 0x3ff, target);
+
+    /* Restore swap/plarity setting */
+    if (mir3da_register_mask_write(handle, NSA_REG_SWAP_POLARITY, 0x0F, swap_plarity_old&0x0F)) {
+	MI_ERR("Restore SWAP/PLARITY setting failed !");
+	return -1;
+    }
+#endif /* !MIR3DA_OFFSET_TEMP_SOLUTION */
+    return result;
+}
+
+static int NSA_NTO_auto_calibrate(MIR_HANDLE handle, int xyz[3])
+{
+    int     result = 0;
+
+#if MIR3DA_AUTO_CALIBRATE
+    int     coarse_step[3];
+    int     fine_step[3];
+    int     custom_step[3] = {0};
+   unsigned char     swap_plarity_old = 0;
+    int     temp = 0;
+
+    /* compute step */
+    if (NSA_NTO_cali_step_calc(handle, coarse_step, fine_step, custom_step)) {
+	MI_ERR("Compute step failed !");
+	return -1;
+    }
+
+    /* save swap/plarity old setting */
+    if (mir3da_register_read(handle, NSA_REG_SWAP_POLARITY, &swap_plarity_old)) {
+	MI_ERR("Get SWAP/PLARITY setting failed !");
+	return -1;
+    }
+
+		if (gsensor_chip_info.asic == ASIC_2512B) {
+				coarse_step[2] = 2 * coarse_step[2];
+
+				if ((swap_plarity_old & (1<<0))) {
+				   temp = xyz[0];
+				   xyz[0] = ((swap_plarity_old & (1<<2)) == 0) ? (-xyz[1]) : xyz[1];
+				   xyz[1] = ((swap_plarity_old & (1<<3)) == 0) ? (-temp) : temp;
+				} else{
+				   xyz[0] = ((swap_plarity_old & (1<<3)) == 0) ? (-xyz[0]) : xyz[0];
+				   xyz[1] = ((swap_plarity_old & (1<<2)) == 0) ? (-xyz[1]) : xyz[1];
+				}
+				xyz[2] = ((swap_plarity_old & (1<<1)) == 0) ? (-xyz[2]) : xyz[2];
+		} else if (gsensor_chip_info.asic == ASIC_2511) {
+				if ((swap_plarity_old & (1<<0))) {
+				   temp = xyz[0];
+				   xyz[0] = ((swap_plarity_old & (1<<2)) != 0) ? (-xyz[1]) : xyz[1];
+				   xyz[1] = ((swap_plarity_old & (1<<3)) != 0) ? (-temp) : temp;
+				} else{
+				   xyz[0] = ((swap_plarity_old & (1<<3)) != 0) ? (-xyz[0]) : xyz[0];
+				   xyz[1] = ((swap_plarity_old & (1<<2)) != 0) ? (-xyz[1]) : xyz[1];
+				}
+
+				xyz[2] = ((swap_plarity_old & (1<<1)) != 0) ? (-xyz[2]) : xyz[2];
+		}
+
+    result = NSA_once_calibrate(handle, coarse_step, fine_step, xyz);
+#endif /* !MIR3DA_AUTO_CALIBRATE */
+    return result;
+}
+
+int mir3da_calibrate(MIR_HANDLE handle, int z_dir)
+{
+    int     res = 0;
+#if MIR3DA_OFFSET_TEMP_SOLUTION
+
+#if (MIR3DA_TV03_CALIBRATE == 0)
+		if (gsensor_chip_info.mems == MEMS_TV03) {
+			return -1;
+		}
+#endif
+
+    if (is_cali)
+	return -1;
+    is_cali = 1;
+
+    /* restore original direction if last calibration was done in a wrong direction */
+
+		if (gsensor_chip_info.mems != MEMS_TV03) {
+	mir3da_write_offset(handle, original_offset);
+		}
+
+    res = mir3da_gsensor_drv.obj[gsensor_mod].calibrate(handle, z_dir);
+    if (res != 0) {
+			MI_ERR("Calibrate failed !");
+			if (gsensor_chip_info.mems != MEMS_TV03) {
+				mir3da_write_offset(handle, original_offset);
+			}
+    } else
+	  bLoad = FILE_EXIST;
+
+    is_cali = 0;
+#endif /* !MIR3DA_OFFSET_TEMP_SOLUTION */
+    return res;
+}
+
+#if MIR3DA_AUTO_CALIBRATE
+#define STABLE_CHECK_SAMPLE_NUM     10
+#define STABLE_CHECK_THRESHOLD      50000
+#define AUTO_CALI_THRESHOLD_XY      500
+#define AUTO_CALI_THRESHOLD_Z        500
+
+static unsigned char    stable_sample_cnt;
+static int              stable_sample_pow_sum[STABLE_CHECK_SAMPLE_NUM] = {0};
+static int              stable_sample_sum[3] = {0};
+
+static int mir3da_auto_cali_condition_confirm(int x, int y, int z, int ave_xyz[3])
+{
+    int    max = 0, min = 0;
+    int    i;
+		int x_ok = 0, y_ok = 0, z_ok = 0;
+
+    stable_sample_pow_sum[stable_sample_cnt] = x*x + y*y + z*z;
+    stable_sample_sum[0] += x;
+    stable_sample_sum[1] += y;
+    stable_sample_sum[2] += z;
+    stable_sample_cnt++;
+
+    MI_MSG("---stable_sample_cnt = %d", stable_sample_cnt);
+
+    if (stable_sample_cnt < STABLE_CHECK_SAMPLE_NUM)
+	return -1;
+    stable_sample_cnt = 0;
+
+    max = stable_sample_pow_sum[0];
+    min = stable_sample_pow_sum[0];
+    stable_sample_pow_sum[0] = 0;
+    for (i = 1; i < STABLE_CHECK_SAMPLE_NUM; i++) {
+	if (stable_sample_pow_sum[i] > max)
+	    max = stable_sample_pow_sum[i];
+	if (stable_sample_pow_sum[i] < min)
+	    min = stable_sample_pow_sum[i];
+	stable_sample_pow_sum[i] = 0;
+    }
+    MI_MSG("---max = %d; min = %d", max, min);
+
+    ave_xyz[0] = stable_sample_sum[0]/STABLE_CHECK_SAMPLE_NUM;
+    stable_sample_sum[0] = 0;
+    ave_xyz[1] = stable_sample_sum[1]/STABLE_CHECK_SAMPLE_NUM;
+    stable_sample_sum[1] = 0;
+    ave_xyz[2] = stable_sample_sum[2]/STABLE_CHECK_SAMPLE_NUM;
+    stable_sample_sum[2] = 0;
+
+		MI_MSG("ave_x = %d, ave_y = %d, ave_z = %d", ave_xyz[0], ave_xyz[1], ave_xyz[2]);
+		x_ok =	(abs(ave_xyz[0]) < AUTO_CALI_THRESHOLD_XY) ? 1:0;
+		y_ok =	(abs(ave_xyz[1]) < AUTO_CALI_THRESHOLD_XY) ? 1:0;
+		z_ok =	(abs(abs(ave_xyz[2])-1024) < AUTO_CALI_THRESHOLD_Z) ? 1:0;
+
+		if ((abs(max-min) > STABLE_CHECK_THRESHOLD) || ((x_ok + y_ok + z_ok) < 2)) {
+			return -1;
+		}
+
+		return 0;
+}
+
+static int mir3da_auto_calibrate(MIR_HANDLE handle, int x, int y, int z)
+{
+    int     res = 0;
+    int     xyz[3] = {0};
+
+    if (is_cali)
+	return -1;
+    is_cali = 1;
+
+	if (gsensor_chip_info.mems == MEMS_TV03) {
+		return -1;
+	}
+
+#if MIR3DA_SUPPORT_FAST_AUTO_CALI
+	if (mir3da_gsensor_drv.method->support_fast_auto_cali()) {
+		cycle_read_xyz(handle, &xyz[0], &xyz[1], &xyz[2], 5);
+	}
+    else{
+		if (mir3da_auto_cali_condition_confirm(x, y, z, xyz)) {
+			res = -1;
+			goto EXIT;
+		}
+	}
+#else
+	if (mir3da_auto_cali_condition_confirm(x, y, z, xyz)) {
+		res = -1;
+		goto EXIT;
+	}
+#endif
+
+    mir3da_write_offset(handle, original_offset);
+
+    res = mir3da_gsensor_drv.obj[gsensor_mod].auto_calibrate(handle, xyz);
+    if (res != 0) {
+	     MI_ERR("Calibrate failed !");
+		 mir3da_write_offset(handle, original_offset);
+    } else
+	    bLoad = FILE_EXIST;
+
+EXIT:
+    is_cali = 0;
+
+    return res;
+}
+#endif /* !MIR3DA_AUTO_CALIBRATE */
+
+static int NSA_interrupt_ops(MIR_HANDLE handle, mir_int_ops_t *ops)
+{
+	switch (ops->type) {
+	case INTERRUPT_OP_INIT:
+
+	    /* latch */
+	    mir3da_register_mask_write(handle, NSA_REG_INT_LATCH, 0x0f, ops->data.init.latch);
+	    /* active level & output mode */
+	    mir3da_register_mask_write(handle, NSA_REG_INT_PIN_CONFIG, 0x0f, ops->data.init.level|(ops->data.init.pin_mod<<1)|(ops->data.init.level<<2)|(ops->data.init.pin_mod<<3));
+
+	    break;
+
+	case INTERRUPT_OP_ENABLE:
+		switch (ops->data.int_src) {
+		case INTERRUPT_ACTIVITY:
+		    /* Enable active interrupt */
+		    mir3da_register_mask_write(handle, NSA_REG_INTERRUPT_SETTINGS1, 0x07, 0x07);
+		    break;
+		case INTERRUPT_CLICK:
+		    /* Enable single and double tap detect */
+		    mir3da_register_mask_write(handle, NSA_REG_INTERRUPT_SETTINGS1, 0x30, 0x30);
+		    break;
+	    }
+	    break;
+
+	case INTERRUPT_OP_CONFIG:
+
+
+		switch (ops->data.cfg.int_src) {
+		case INTERRUPT_ACTIVITY:
+
+		    mir3da_register_write(handle, NSA_REG_ACTIVE_THRESHOLD, ops->data.cfg.int_cfg.act.threshold);
+		    mir3da_register_mask_write(handle, NSA_REG_ACTIVE_DURATION, 0x03, ops->data.cfg.int_cfg.act.duration);
+
+		    /* Int mapping */
+		    if (ops->data.cfg.pin == INTERRUPT_PIN1) {
+				mir3da_register_mask_write(handle, NSA_REG_INTERRUPT_MAPPING1, (1<<2), (1<<2));
+		    } else if (ops->data.cfg.pin == INTERRUPT_PIN2) {
+				mir3da_register_mask_write(handle, NSA_REG_INTERRUPT_MAPPING3, (1<<2), (1<<2));
+		    }
+		    break;
+
+		case INTERRUPT_CLICK:
+
+		    mir3da_register_mask_write(handle, NSA_REG_TAP_THRESHOLD, 0x1f, ops->data.cfg.int_cfg.clk.threshold);
+		    mir3da_register_mask_write(handle, NSA_REG_TAP_DURATION, (0x03<<5)|(0x07), (ops->data.cfg.int_cfg.clk.quiet_time<<7)|(ops->data.cfg.int_cfg.clk.click_time<<6)|(ops->data.cfg.int_cfg.clk.window));
+
+		    if (ops->data.cfg.pin == INTERRUPT_PIN1) {
+				mir3da_register_mask_write(handle, NSA_REG_INTERRUPT_MAPPING1, 0x30, 0x30);
+		    } else if (ops->data.cfg.pin == INTERRUPT_PIN2) {
+				mir3da_register_mask_write(handle, NSA_REG_INTERRUPT_MAPPING3, 0x30, 0x30);
+		    }
+		    break;
+	    }
+	    break;
+
+	case INTERRUPT_OP_DISABLE:
+		switch (ops->data.int_src) {
+		case INTERRUPT_ACTIVITY:
+		    /* Enable active interrupt */
+		    mir3da_register_mask_write(handle, NSA_REG_INTERRUPT_SETTINGS1, 0x07, 0x00);
+		    break;
+
+		case INTERRUPT_CLICK:
+		    /* Enable single and double tap detect */
+		    mir3da_register_mask_write(handle, NSA_REG_INTERRUPT_SETTINGS1, 0x30, 0x00);
+		    break;
+	    }
+	    break;
+
+	default:
+	    MI_ERR("Unsupport operation !");
+    }
+    return 0;
+}
+
+int mir3da_interrupt_ops(MIR_HANDLE handle, mir_int_ops_t *ops)
+{
+    int res = 0;
+
+    res = mir3da_gsensor_drv.obj[gsensor_mod].int_ops(handle, ops);
+    return res;
+}
+
+#if FILTER_AVERAGE_ENHANCE
+int mir3da_get_filter_param(struct mir3da_filter_param_s *param)
+{
+    if (param == 0) {
+		MI_ERR("Invalid param!");
+		return -1;
+    }
+
+    param->filter_param_h = core_ctx.tFac[0].filter_param_h;
+    param->filter_param_l = core_ctx.tFac[0].filter_param_l;
+    param->filter_threhold = core_ctx.tFac[0].filter_threhold;
+
+    MI_MSG("FILTER param is get: filter_param_h = %d, filter_param_l = %d, filter_threhold = %d", param->filter_param_h, param->filter_param_l, param->filter_threhold);
+
+    return 0;
+}
+
+int mir3da_set_filter_param(struct mir3da_filter_param_s *param)
+{
+
+    if (param == 0) {
+		MI_ERR("Invalid param!");
+		return -1;
+    }
+
+    MI_MSG("FILTER param is set: filter_param_h = %d, filter_param_l = %d, filter_threhold = %d", param->filter_param_h, param->filter_param_l, param->filter_threhold);
+
+    core_ctx.tFac[1].filter_param_l = core_ctx.tFac[2].filter_param_l = core_ctx.tFac[0].filter_param_l = param->filter_param_l;
+    core_ctx.tFac[1].filter_param_h = core_ctx.tFac[2].filter_param_h = core_ctx.tFac[0].filter_param_h  = param->filter_param_h;
+    core_ctx.tFac[1].filter_threhold = core_ctx.tFac[2].filter_threhold = core_ctx.tFac[0].filter_threhold = param->filter_threhold;
+
+    return 0;
+}
+#endif /* #if FILTER_AVERAGE_ENHANCE */
+
+int mir3da_get_enable(MIR_HANDLE handle, char *enable)
+{
+	unsigned char              reg_data;
+    int             res = 0;
+
+    res = mir3da_register_read(handle, mir3da_gsensor_drv.obj[gsensor_mod].power.addr, &reg_data);
+    if (res != 0) {
+		return res;
+	}
+
+     *enable = (reg_data & mir3da_gsensor_drv.obj[gsensor_mod].power.mask) ? 0 : 1;
+
+    return res;
+}
+
+int mir3da_set_enable(MIR_HANDLE handle, char enable)
+{
+    int             res = 0;
+    unsigned char              reg_data = 0;
+
+    if (!enable) {
+		reg_data = mir3da_gsensor_drv.obj[gsensor_mod].power.value;
+    }
+
+    res = mir3da_register_mask_write(handle, mir3da_gsensor_drv.obj[gsensor_mod].power.addr, mir3da_gsensor_drv.obj[gsensor_mod].power.mask, reg_data);
+
+    return res;
+}
+
+static int NSA_get_reg_data(MIR_HANDLE handle, char *buf)
+{
+    int                 count = 0;
+    int                 i;
+    unsigned char       val;
+
+    count += mir3da_gsensor_drv.method->mysprintf(buf+count, "---------start---------");
+    for (i = 0; i <= 0xd2; i++) {
+		if (i%16 == 0)
+		    count += mir3da_gsensor_drv.method->mysprintf(buf+count, "\n%02x\t", i);
+		mir3da_register_read(handle, i, &val);
+		count += mir3da_gsensor_drv.method->mysprintf(buf+count, "%02X ", val);
+    }
+
+    count += mir3da_gsensor_drv.method->mysprintf(buf+count, "\n--------end---------\n");
+    return count;
+}
+
+int mir3da_get_reg_data(MIR_HANDLE handle, char *buf)
+{
+    return mir3da_gsensor_drv.obj[gsensor_mod].get_reg_data(handle, buf);
+}
+
+int mir3da_set_odr(MIR_HANDLE handle, int delay)
+{
+    int     res = 0;
+    int     odr = 0;
+
+    if (delay <= 5) {
+       odr = MIR3DA_ODR_200HZ;
+    } else if (delay <= 10) {
+       odr = MIR3DA_ODR_100HZ;
+    } else {
+       odr = MIR3DA_ODR_50HZ;
+    }
+
+    res = mir3da_register_mask_write(handle, mir3da_gsensor_drv.obj[gsensor_mod].odr_sect[odr].addr,
+			mir3da_gsensor_drv.obj[gsensor_mod].odr_sect[odr].mask, mir3da_gsensor_drv.obj[gsensor_mod].odr_sect[odr].value);
+    if (res != 0) {
+		return res;
+    }
+
+    return res;
+}
+
+static int mir3da_soft_reset(MIR_HANDLE handle)
+{
+    int             res = 0;
+    unsigned char   reg_data;
+
+    reg_data = mir3da_gsensor_drv.obj[gsensor_mod].soft_reset.value;
+    res = mir3da_register_mask_write(handle, mir3da_gsensor_drv.obj[gsensor_mod].soft_reset.addr, mir3da_gsensor_drv.obj[gsensor_mod].soft_reset.mask, reg_data);
+    mir3da_gsensor_drv.method->msdelay(5);
+
+    return res;
+}
+
+int mir3da_module_detect(PLAT_HANDLE handle)
+{
+    int             i, res = 0;
+    unsigned char   cid, mid;
+    int             is_find = -1;
+
+    /* Probe gsensor module */
+    for (i = 0; i < sizeof(mir3da_gsensor)/sizeof(mir3da_gsensor[0]); i++) {
+		res = mir3da_register_read(handle, mir3da_gsensor[i].chip_id.addr, &cid);
+		    if (res != 0) {
+			    return res;
+		    }
+
+		cid &= mir3da_gsensor[i].chip_id.mask;
+		if (mir3da_gsensor[i].chip_id.value == cid) {
+		    res = mir3da_register_read(handle, mir3da_gsensor[i].mod_id.addr, &mid);
+			if (res != 0) {
+				return res;
+			}
+
+		    mid &= mir3da_gsensor[i].mod_id.mask;
+		    if (mir3da_gsensor[i].mod_id.value == mid) {
+			MI_MSG("Found Gsensor MIR3DA !");
+			gsensor_mod = i;
+			is_find = 0;
+			break;
+		    }
+		}
+    }
+
+    return is_find;
+}
+
+int mir3da_parse_chip_info(PLAT_HANDLE handle)
+{
+	unsigned char i = 0;
+	unsigned char reg_value = -1;
+	char res =  -1;
+
+	if (-1 == gsensor_mod)
+		return res;
+
+	res = mir3da_register_read(handle, NSA_REG_CHIP_INFO, &reg_value);
+	if (res != 0) {
+		return res;
+	}
+
+  gsensor_chip_info.reg_value	 = reg_value;
+
+	if (!(reg_value&0xc0)) {
+	    gsensor_chip_info.asic = ASIC_2511;
+	    gsensor_chip_info.mems = MEMS_T9;
+	    gsensor_chip_info.package = PACKAGE_NONE;
+
+		for (i = 0; i < sizeof(mir3da_chip_info_list)/sizeof(mir3da_chip_info_list[0]); i++) {
+				if (reg_value == mir3da_chip_info_list[i].reg_value) {
+					gsensor_chip_info.package = mir3da_chip_info_list[i].package;
+					gsensor_chip_info.asic = mir3da_chip_info_list[i].asic;
+					gsensor_chip_info.mems = mir3da_chip_info_list[i].mems;
+					break;
+				}
+		}
+	} else {
+	    gsensor_chip_info.asic = ASIC_2512B;
+	    gsensor_chip_info.mems = MEMS_T9;
+	    gsensor_chip_info.package = PACKAGE_NONE;
+
+			gsensor_chip_info.package = (reg_value&0xc0)>>6;
+
+			if ((reg_value&0x38)>>3 == 0x01)
+				gsensor_chip_info.asic = ASIC_2512B;
+
+			res = mir3da_register_read(handle, NSA_REG_MEMS_OPTION, &reg_value);
+			if (res != 0) {
+					return res;
+			}
+
+			if (reg_value&0x80)
+				 gsensor_chip_info.mems = MEMS_TV03;
+			else
+				 gsensor_chip_info.mems = MEMS_T9;
+	}
+
+	return 0;
+}
+
+
+int mir3da_install_general_ops(struct general_op_s *ops)
+{
+    if (0 == ops) {
+		return -1;
+    }
+
+    mir3da_gsensor_drv.method = ops;
+    return 0;
+}
+
+MIR_HANDLE mir3da_core_init(PLAT_HANDLE handle)
+{
+    int             res = 0;
+
+#if FILTER_AVERAGE_ENHANCE
+int i = 0;
+#endif
+
+    mir3da_gsensor_drv.obj = mir3da_gsensor;
+
+    if (gsensor_mod < 0) {
+		res = mir3da_module_detect(handle);
+		if (res) {
+		     MI_ERR("Can't find Mir3da gsensor!!");
+		    return 0;
+		}
+
+		/* No miramems gsensor instance found */
+		if (gsensor_mod < 0) {
+		    return 0;
+		}
+    }
+
+    MI_MSG("Probe gsensor module: %s", mir3da_gsensor[gsensor_mod].asic);
+
+#if FILTER_AVERAGE_ENHANCE
+    /* configure default filter param */
+    for (i = 0; i < 3; i++) {
+		core_ctx.tFac[i].filter_param_l = 2;
+		core_ctx.tFac[i].filter_param_h = 8;
+		core_ctx.tFac[i].filter_threhold = 60;
+
+		core_ctx.tFac[i].refN_l = 0;
+		core_ctx.tFac[i].refN_h = 0;
+    }
+#endif
+
+    res = mir3da_chip_resume(handle);
+    if (res) {
+				MI_ERR("chip resume fail!!\n");
+				return 0;
+    }
+
+    return handle;
+}
+
+int mir3da_chip_resume(MIR_HANDLE handle)
+{
+    int     res = 0;
+    unsigned char      reg_data;
+    unsigned char      i = 0;
+
+    res = mir3da_soft_reset(handle);
+    if (res) {
+		MI_ERR("Do softreset failed !");
+		return res;
+    }
+
+    for (i = 0; i < MIR3DA_INIT_SECT_LEN; i++) {
+		if (mir3da_gsensor_drv.obj[gsensor_mod].init_sect[i].addr < 0) {
+		    break;
+		}
+
+		reg_data = mir3da_gsensor_drv.obj[gsensor_mod].init_sect[i].value;
+		res = mir3da_register_mask_write(handle, mir3da_gsensor_drv.obj[gsensor_mod].init_sect[i].addr, mir3da_gsensor_drv.obj[gsensor_mod].init_sect[i].mask, reg_data);
+		if (res != 0) {
+		    return res;
+		}
+    }
+
+    mir3da_gsensor_drv.method->msdelay(10);
+
+	if (gsensor_type < 0) {
+		gsensor_type = mir3da_parse_chip_info(handle);
+
+		if (gsensor_type < 0) {
+			MI_ERR("Can't parse Mir3da gsensor chipinfo!!");
+			return 0;
+		}
+	}
+
+	if (gsensor_chip_info.asic == ASIC_2512B) {
+
+		reg_data = mir3da_gsensor_drv.method->get_address(handle);
+
+
+		if (reg_data == 0x26 || reg_data == 0x4c) {
+
+			mir3da_register_mask_write(handle, NSA_REG_SENS_COMP, 0xc0, 0x00);
+		}
+	}
+
+
+#if MIR3DA_OFFSET_TEMP_SOLUTION
+    res = mir3da_read_offset(handle, original_offset);
+    if (res != 0) {
+		MI_ERR("Read offset failed !");
+		return res;
+    }
+
+    bLoad = FILE_CHECKING;
+    readOffsetCnt = -1;
+    manual_load_cali_file(handle);
+#endif
+
+
+    return res;
+}
+
+int mir3da_get_primary_offset(MIR_HANDLE handle, int *x, int *y, int *z)
+{
+    int     res = 0;
+    unsigned char      reg_data;
+    unsigned char      i = 0;
+    unsigned char      offset[9] = {0};
+
+    if (gsensor_chip_info.mems == MEMS_TV03) {
+				res = cycle_read_xyz(handle, x, y, z, 20);
+				if (res) {
+					MI_ERR("i2c block read failed\n");
+					return -1;
+				}
+				return 0;
+	}
+
+    res = mir3da_read_offset(handle, offset);
+    if (res != 0) {
+		MI_ERR("Read offset failed !");
+		return -1;
+    }
+
+    res = mir3da_soft_reset(handle);
+    if (res) {
+		MI_ERR("Do softreset failed !");
+		return -1;
+    }
+
+    for (i = 0; i < MIR3DA_INIT_SECT_LEN; i++) {
+		if (mir3da_gsensor_drv.obj[gsensor_mod].init_sect[i].addr < 0) {
+		    break;
+		}
+
+		reg_data = mir3da_gsensor_drv.obj[gsensor_mod].init_sect[i].value;
+		res = mir3da_register_mask_write(handle, mir3da_gsensor_drv.obj[gsensor_mod].init_sect[i].addr, mir3da_gsensor_drv.obj[gsensor_mod].init_sect[i].mask, reg_data);
+		if (res != 0) {
+		    MI_ERR("Write register[0x%x] error!", mir3da_gsensor_drv.obj[gsensor_mod].init_sect[i].addr);
+		    goto EXIT;
+		}
+    }
+
+    mir3da_gsensor_drv.method->msdelay(100);
+
+    res = cycle_read_xyz(handle, x, y, z, 20);
+    if (res) {
+	    MI_ERR("i2c block read failed\n");
+	    goto EXIT;
+    }
+
+    mir3da_write_offset(handle, offset);
+
+    return 0;
+
+EXIT:
+    mir3da_write_offset(handle, offset);
+    return -1;
+}
diff --git a/drivers/misc/mediatek/accelerometer/da213/mir3da_core.h b/drivers/misc/mediatek/accelerometer/da213/mir3da_core.h
new file mode 100644
index 000000000..6c3063745
--- /dev/null
+++ b/drivers/misc/mediatek/accelerometer/da213/mir3da_core.h
@@ -0,0 +1,289 @@
+/* Core header for MiraMEMS 3-Axis Accelerometer's driver.
+ *
+ * mir3da_core.h - Linux kernel modules for MiraMEMS 3-Axis Accelerometer
+ *
+ *
+ * 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 __MIR3DA_CORE_H__
+#define __MIR3DA_CORE_H__
+
+#define CUST_VER                            ""                                          /* for Custom debug version */
+#define CORE_VER                            "3.0.1_2015-03-04-21:21:30_"CUST_VER
+
+#define MIR3DA_SUPPORT_CHIP_LIST            MIR_NSA_NTO
+
+#define MIR3DA_BUFSIZE                      256
+
+#define MIR3DA_STK_TEMP_SOLUTION            1
+#define MIR3DA_OFFSET_TEMP_SOLUTION         1
+#if MIR3DA_OFFSET_TEMP_SOLUTION
+#define MIR3DA_AUTO_CALIBRATE               0
+#else
+#define MIR3DA_AUTO_CALIBRATE               0
+#endif /* !MIR3DA_OFFSET_TEMP_SOLUTION */
+#if MIR3DA_AUTO_CALIBRATE
+#define MIR3DA_SUPPORT_FAST_AUTO_CALI       0
+#else
+#define MIR3DA_SUPPORT_FAST_AUTO_CALI       0
+#endif
+#if MIR3DA_OFFSET_TEMP_SOLUTION
+#define MIR3DA_TV03_CALIBRATE               1
+#else
+#define MIR3DA_TV03_CALIBRATE               0
+#endif
+#define MIR3DA_SENS_TEMP_SOLUTION           1
+#define FILTER_AVERAGE_ENHANCE              0
+#define FILTER_AVERAGE_EX                   0
+#define MIR3DA_SUPPORT_MULTI_LAYOUT         0
+
+#define MIR3DA_OFFSET_LEN                   9
+
+typedef void *MIR_HANDLE;
+typedef void *PLAT_HANDLE;
+
+
+struct serial_manage_if_s {
+
+    int                         (*read)(PLAT_HANDLE handle, unsigned char addr, unsigned char *data);
+    int                         (*write)(PLAT_HANDLE handle, unsigned char addr, unsigned char data);
+    int                         (*read_block)(PLAT_HANDLE handle, unsigned char base_addr, unsigned char count, unsigned char *data);
+};
+
+struct general_op_s {
+
+    struct serial_manage_if_s   smi;
+
+    int                         (*data_save)(unsigned char *data);
+    int                         (*data_get)(unsigned char *data);
+    int                         (*data_check)(void);
+    int                         (*get_address)(PLAT_HANDLE handle);
+    int                         (*support_fast_auto_cali)(void);
+
+    int                         (*myprintf)(const char *fmt, ...);
+    int                         (*mysprintf)(char *buf, const char *fmt, ...);
+    void                        (*msdelay)(int ms);
+};
+
+#define MIR_GENERAL_OPS_DECLARE(OPS_HDL, SMI_RD, SMI_RDBL, SMI_WR, DAT_SAVE, DAT_GET, DAT_CHECK, GET_ADDRESS, SUPPORT_FAST_AUTO_CALI, MDELAY, MYPRINTF, MYSPRINTF) \
+																			\
+				struct general_op_s     OPS_HDL = { { SMI_RD, SMI_WR, SMI_RDBL }, DAT_SAVE, DAT_GET, DAT_CHECK, GET_ADDRESS, SUPPORT_FAST_AUTO_CALI, MYPRINTF, MYSPRINTF, MDELAY }
+
+enum interrupt_src {
+
+    INTERRUPT_ACTIVITY     = 1,
+    INTERRUPT_CLICK,
+
+};
+
+typedef enum _int_op_type {
+
+    INTERRUPT_OP_INIT,
+    INTERRUPT_OP_ENABLE,
+    INTERRUPT_OP_CONFIG,
+    INTERRUPT_OP_DISABLE,
+
+} mir_int_op_type;
+
+enum interrupt_pin {
+
+    INTERRUPT_PIN1,
+    INTERRUPT_PIN2,
+};
+
+enum pin_output_mode {
+
+    OUTPUT_MOD_PULL_PUSH,
+    OUTPUT_MOD_OD,
+};
+
+struct int_act_cfg_s {
+
+    unsigned char           threshold;
+    unsigned char           duration;
+};
+
+struct int_clk_cfg_s {
+
+    unsigned char                   threshold;
+    unsigned char                   click_time;     /* click time */
+    unsigned char                   quiet_time;     /* quiet time after click */
+    unsigned char                   window;         /* for second click time window */
+};
+
+typedef union _int_src_configuration {
+
+    struct int_act_cfg_s            act;
+    struct int_clk_cfg_s            clk;
+
+} mir_int_src_cfg_t;
+
+typedef struct _int_configuration {
+
+    enum interrupt_pin              pin;
+    enum interrupt_src              int_src;
+
+    mir_int_src_cfg_t               int_cfg;
+
+} mir_int_cfg_t;
+
+typedef struct _int_init_data {
+
+    enum pin_output_mode            pin_mod;
+
+    unsigned char                   level;      /* 1: high active, 0: low active */
+    unsigned char                   latch;          /* >0: latch time, 0: no latch */
+
+} mir_int_init_t;
+
+typedef union _int_op_data {
+
+    enum interrupt_src              int_src;
+
+    mir_int_init_t                  init;
+    mir_int_cfg_t                   cfg;
+
+} mir_int_op_data;
+
+typedef struct _int_operations {
+
+    mir_int_op_type                 type;
+    mir_int_op_data                 data;
+
+} mir_int_ops_t;
+
+/* Register define for NSA asic */
+#define NSA_REG_SPI_I2C                 0x00
+#define NSA_REG_WHO_AM_I                0x01
+#define NSA_REG_ACC_X_LSB               0x02
+#define NSA_REG_ACC_X_MSB               0x03
+#define NSA_REG_ACC_Y_LSB               0x04
+#define NSA_REG_ACC_Y_MSB               0x05
+#define NSA_REG_ACC_Z_LSB               0x06
+#define NSA_REG_ACC_Z_MSB               0x07
+#define NSA_REG_G_RANGE                 0x0f
+#define NSA_REG_ODR_AXIS_DISABLE        0x10
+#define NSA_REG_POWERMODE_BW            0x11
+#define NSA_REG_SWAP_POLARITY           0x12
+#define NSA_REG_FIFO_CTRL               0x14
+#define NSA_REG_INTERRUPT_SETTINGS1     0x16
+#define NSA_REG_INTERRUPT_SETTINGS2     0x17
+#define NSA_REG_INTERRUPT_MAPPING1      0x19
+#define NSA_REG_INTERRUPT_MAPPING2      0x1a
+#define NSA_REG_INTERRUPT_MAPPING3      0x1b
+#define NSA_REG_INT_PIN_CONFIG          0x20
+#define NSA_REG_INT_LATCH               0x21
+#define NSA_REG_ACTIVE_DURATION         0x27
+#define NSA_REG_ACTIVE_THRESHOLD        0x28
+#define NSA_REG_TAP_DURATION            0x2A
+#define NSA_REG_TAP_THRESHOLD           0x2B
+#define NSA_REG_CUSTOM_OFFSET_X         0x38
+#define NSA_REG_CUSTOM_OFFSET_Y         0x39
+#define NSA_REG_CUSTOM_OFFSET_Z         0x3a
+#define NSA_REG_ENGINEERING_MODE        0x7f
+#define NSA_REG_SENSITIVITY_TRIM_X      0x80
+#define NSA_REG_SENSITIVITY_TRIM_Y      0x81
+#define NSA_REG_SENSITIVITY_TRIM_Z      0x82
+#define NSA_REG_COARSE_OFFSET_TRIM_X    0x83
+#define NSA_REG_COARSE_OFFSET_TRIM_Y    0x84
+#define NSA_REG_COARSE_OFFSET_TRIM_Z    0x85
+#define NSA_REG_FINE_OFFSET_TRIM_X      0x86
+#define NSA_REG_FINE_OFFSET_TRIM_Y      0x87
+#define NSA_REG_FINE_OFFSET_TRIM_Z      0x88
+#define NSA_REG_SENS_COMP               0x8c
+#define NSA_REG_MEMS_OPTION             0x8f
+#define NSA_REG_CHIP_INFO               0xc0
+#define NSA_REG_SENS_COARSE_TRIM        0xd1
+
+#define MIR3DA_ODR_50HZ                  0
+#define MIR3DA_ODR_100HZ                 1
+#define MIR3DA_ODR_200HZ                 2
+
+#define MI_TAG                          "[accel] "
+enum{
+	DEBUG_ERR = 1,
+	DEBUG_ASSERT = 1<<1,
+	DEBUG_MSG = 1<<2,
+	DEBUG_FUNC = 1<<3,
+	DEBUG_DATA = 1<<4,
+};
+
+/* register operation */
+int mir3da_register_read(MIR_HANDLE handle, short reg, unsigned char *data);
+int mir3da_register_write(MIR_HANDLE handle, short reg, unsigned char data);
+int mir3da_register_read_continuously(MIR_HANDLE handle, short base_reg, unsigned char count, unsigned char *data);
+int mir3da_register_mask_write(MIR_HANDLE handle, short addr, unsigned char mask, unsigned char data);
+
+int mir3da_install_general_ops(struct general_op_s *ops);
+/* chip init */
+int mir3da_module_detect(PLAT_HANDLE handle);
+MIR_HANDLE mir3da_core_init(PLAT_HANDLE handle);
+
+/* data polling */
+int mir3da_read_data(MIR_HANDLE handle, short *x, short *y, short *z);
+
+/* filter configure */
+#if FILTER_AVERAGE_ENHANCE
+struct mir3da_filter_param_s {
+    int filter_param_l;
+    int filter_param_h;
+    int filter_threhold;
+};
+
+int mir3da_get_filter_param(struct mir3da_filter_param_s *param);
+int mir3da_set_filter_param(struct mir3da_filter_param_s *param);
+#endif
+
+#if MIR3DA_STK_TEMP_SOLUTION
+extern char bxstk;
+extern char bystk;
+extern char bzstk;
+extern  int squareRoot(int val);
+#endif
+
+enum {
+    GSENSOR_MOD_NSA_NTO = 0,
+};
+
+extern int  gsensor_mod;        /* Initial value */
+
+/* CALI */
+int mir3da_calibrate(MIR_HANDLE handle, int z_dir);
+
+/* calibration */
+#if MIR3DA_OFFSET_TEMP_SOLUTION
+enum file_check_statu {
+    FILE_NO_EXIST  ,
+    FILE_CHECKING  ,
+    FILE_EXIST,
+};
+extern char bLoad;
+void manual_load_cali_file(MIR_HANDLE handle);
+#endif
+
+/* Interrupt operations */
+int mir3da_interrupt_ops(MIR_HANDLE handle, mir_int_ops_t *ops);
+
+int cycle_read_xyz(MIR_HANDLE handle, int *x, int *y, int *z, int ncycle);
+
+int mir3da_read_offset(MIR_HANDLE handle, unsigned char *offst);
+int mir3da_write_offset(MIR_HANDLE handle, unsigned char *offset);
+
+int mir3da_set_enable(MIR_HANDLE handle, char bEnable);
+int mir3da_get_enable(MIR_HANDLE handle, char *bEnable);
+int mir3da_get_reg_data(MIR_HANDLE handle, char *buf);
+int mir3da_set_odr(MIR_HANDLE handle, int delay);
+int mir3da_direction_remap(short *x, short *y, short *z, int direction);
+
+int mir3da_chip_resume(MIR_HANDLE handle);
+int mir3da_get_primary_offset(MIR_HANDLE handle, int *x, int *y, int *z);
+#endif    /* __MIR3DA_CORE_H__ */
diff --git a/drivers/misc/mediatek/accelerometer/da213/mir3da_cust.c b/drivers/misc/mediatek/accelerometer/da213/mir3da_cust.c
new file mode 100644
index 000000000..07574a265
--- /dev/null
+++ b/drivers/misc/mediatek/accelerometer/da213/mir3da_cust.c
@@ -0,0 +1,1739 @@
+/*
+ *
+ * mir3da.c - Linux kernel modules for 3-Axis Accelerometer
+ *
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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.
+ *
+ */
+
+#include <linux/interrupt.h>
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include <linux/irq.h>
+#include <linux/miscdevice.h>
+#include <asm/uaccess.h>
+#include <linux/delay.h>
+#include <linux/input.h>
+#include <linux/workqueue.h>
+#include <linux/kobject.h>
+#include <linux/earlysuspend.h>
+#include <linux/platform_device.h>
+#include <asm/atomic.h>
+
+#include <cust_acc.h>
+#include <linux/hwmsensor.h>
+#include <linux/hwmsen_dev.h>
+#include <linux/sensors_io.h>
+#include <linux/hwmsen_helper.h>
+#include "mir3da_core.h"
+#include "mir3da_cust.h"
+
+#if  MIR3DA_SUPPORT_FAST_AUTO_CALI
+#include <mach/mt_boot.h>
+#endif
+
+
+#include <mach/mt_pm_ldo.h>
+#include <mach/mt_typedefs.h>
+#include <mach/mt_gpio.h>
+#include <mach/mt_boot.h>
+
+#define MIR3DA_ACC_NEW_ARCH	/*//compatialbe L version new sensor arch */
+
+#ifdef MIR3DA_ACC_NEW_ARCH
+#include <accel.h>
+#endif
+
+
+#define POWER_NONE_MACRO MT65XX_POWER_NONE
+
+
+#define MIR3DA_DRV_NAME			"mir3da"
+#define MIR3DA_MISC_NAME		"gsensor"
+#define MIR3DA_PLATFORM_NAME         "gsensor"
+
+
+/* #define MTK_ANDROID_23		0 */
+
+#define MIR3DA_AXIS_X				0
+#define MIR3DA_AXIS_Y				1
+#define MIR3DA_AXIS_Z				2
+#define MIR3DA_AXES_NUM			3
+
+#define MTK_AUTO_MODE			0
+
+struct scale_factor {
+    u8  whole;
+    u8  fraction;
+};
+
+struct data_resolution {
+    struct scale_factor scalefactor;
+    int                 sensitivity;
+};
+
+struct mir3da_i2c_data {
+    struct i2c_client		*client;
+    struct acc_hw			*hw;
+    struct hwmsen_convert	cvt;
+
+    struct data_resolution	*reso;
+    atomic_t				layout;
+	atomic_t			trace;
+    atomic_t				suspend;
+    atomic_t				selftest;
+    s16					cali_sw[MIR3DA_AXES_NUM+1];
+
+    s8					offset[MIR3DA_AXES_NUM+1];
+    s16					data[MIR3DA_AXES_NUM+1];
+
+#if defined(CONFIG_HAS_EARLYSUSPEND)
+    struct early_suspend	early_drv;
+#endif
+};
+
+static struct data_resolution mir3da_data_resolution[] = {
+    {{ 1, 0}, 1024},
+};
+
+static struct i2c_board_info      mir3da_i2c_boardinfo = { I2C_BOARD_INFO(MIR3DA_DRV_NAME, MIR3DA_I2C_ADDR>>1) };
+
+static bool sensor_power;
+static bool sensor_enable_status;
+
+static GSENSOR_VECTOR3D gsensor_gain, gsensor_offset;
+static MIR_HANDLE              mir_handle;
+static struct mir3da_i2c_data *mir3da_obj;
+
+
+extern int Log_level;
+/*----------------------------------------------------------------------------*/
+#define MI_DATA(format, ...)            if (DEBUG_DATA&Log_level) {printk(KERN_ERR MI_TAG format "\n", ## __VA_ARGS__); }
+#define MI_MSG(format, ...)             if (DEBUG_MSG&Log_level) {printk(KERN_ERR MI_TAG format "\n", ## __VA_ARGS__); }
+#define MI_ERR(format, ...)             if (DEBUG_ERR&Log_level) {printk(KERN_ERR MI_TAG format "\n", ## __VA_ARGS__); }
+#define MI_FUN                          if (DEBUG_FUNC&Log_level) {printk(KERN_ERR MI_TAG "%s is called, line: %d\n", __func__, __LINE__); }
+#define MI_ASSERT(expr)                 \
+	if (!(expr)) {\
+		printk(KERN_ERR "Assertion failed! %s,%d,%s,%s\n",\
+			__FILE__, __LINE__, __func__, #expr);\
+	}
+/*----------------------------------------------------------------------------*/
+#ifdef MIR3DA_ACC_NEW_ARCH
+static int mir3da_init_flag;
+static int mir3da_local_init(void);
+static int mir3da_local_remove(void);
+static int mir3da_open_report_data(int open);
+static int mir3da_enable_nodata(int en);
+static int mir3da_set_delay(u64 ns);
+static int mir3da_get_data(int *x, int *y, int *z, int *status);
+
+static struct acc_init_info mir3da_init_info = {
+    .name = MIR3DA_DRV_NAME,
+    .init = mir3da_local_init,
+    .uninit = mir3da_local_remove,
+};
+
+#else
+static int mir3da_platform_probe(struct platform_device *pdev);
+static int mir3da_platform_remove(struct platform_device *pdev);
+#ifdef CONFIG_OF
+static const struct of_device_id gsensor_of_match[] = {
+	{ .compatible = "mediatek,gsensor", },
+	{},
+};
+#endif
+
+static struct platform_driver mir3da_gsensor_driver = {
+    .driver     = {
+	.name  = MIR3DA_PLATFORM_NAME,
+       /* .owner = THIS_MODULE, */
+#ifdef CONFIG_OF
+		.of_match_table = gsensor_of_match,
+#endif
+
+    },
+    .probe      = mir3da_platform_probe,
+    .remove     = mir3da_platform_remove,
+};
+#endif
+/*----------------------------------------------------------------------------*/
+#if MIR3DA_OFFSET_TEMP_SOLUTION
+static char OffsetFileName[] = "/data/misc/miraGSensorOffset.txt";
+static char OffsetFolerName[] = "/data/misc/";
+static int bCalires = -1;
+#define OFFSET_STRING_LEN               26
+struct work_info {
+    char        tst1[20];
+    char        tst2[20];
+    char        buffer[OFFSET_STRING_LEN];
+    struct      workqueue_struct *wq;
+    struct      delayed_work read_work;
+    struct      delayed_work write_work;
+    struct      completion completion;
+    int         len;
+    int         rst;
+};
+
+static struct work_info m_work_info = {{0} };
+/*----------------------------------------------------------------------------*/
+static void sensor_write_work(struct work_struct *work)
+{
+    struct work_info *pWorkInfo;
+    struct file         *filep;
+    mm_segment_t         orgfs;
+    int                 ret;
+
+    orgfs = get_fs();
+    /* set_fs(KERNEL_DS); */
+    set_fs(get_ds());
+
+    pWorkInfo = container_of((struct delayed_work *)work, struct work_info, write_work);
+    if (pWorkInfo == NULL) {
+	    MI_ERR("get pWorkInfo failed!");
+	    return;
+    }
+
+    filep = filp_open(OffsetFileName, O_RDWR|O_CREAT, 0600);
+    if (IS_ERR(filep)) {
+		set_fs(orgfs);
+		MI_ERR("write, sys_open %s error!!.\n", OffsetFileName);
+		ret =  -1;
+    } else {
+		filep->f_op->write(filep, pWorkInfo->buffer, pWorkInfo->len, &filep->f_pos);
+		filp_close(filep, NULL);
+		ret = 0;
+    }
+
+    set_fs(orgfs);
+    pWorkInfo->rst = ret;
+    complete(&pWorkInfo->completion);
+}
+/*----------------------------------------------------------------------------*/
+static void sensor_read_work(struct work_struct *work)
+{
+    mm_segment_t orgfs;
+    struct file *filep;
+    int ret;
+    struct work_info *pWorkInfo;
+
+    orgfs = get_fs();
+   /* set_fs(KERNEL_DS); */
+    set_fs(get_ds());
+
+    pWorkInfo = container_of((struct delayed_work *)work, struct work_info, read_work);
+    if (pWorkInfo == NULL) {
+		MI_ERR("get pWorkInfo failed!");
+		return;
+    }
+
+    filep = filp_open(OffsetFileName, O_RDONLY, 0600);
+    if (IS_ERR(filep)) {
+		MI_ERR("read, sys_open %s error!!.\n", OffsetFileName);
+		set_fs(orgfs);
+		ret =  -1;
+    } else {
+		filep->f_op->read(filep, pWorkInfo->buffer,  sizeof(pWorkInfo->buffer), &filep->f_pos);
+		filp_close(filep, NULL);
+		set_fs(orgfs);
+		ret = 0;
+    }
+
+    pWorkInfo->rst = ret;
+    complete(&(pWorkInfo->completion));
+}
+/*----------------------------------------------------------------------------*/
+static int sensor_sync_read(u8 *offset)
+{
+    int     err;
+    int     off[MIR3DA_OFFSET_LEN] = {0};
+    struct work_info *pWorkInfo = &m_work_info;
+
+    init_completion(&pWorkInfo->completion);
+    queue_delayed_work(pWorkInfo->wq, &pWorkInfo->read_work, msecs_to_jiffies(0));
+    err = wait_for_completion_timeout(&pWorkInfo->completion, msecs_to_jiffies(2000));
+    if (err == 0) {
+		MI_ERR("wait_for_completion_timeout TIMEOUT");
+		return -1;
+    }
+
+    if (pWorkInfo->rst != 0) {
+		MI_ERR("work_info.rst  not equal 0");
+		return pWorkInfo->rst;
+    }
+
+    sscanf(m_work_info.buffer, "%x,%x,%x,%x,%x,%x,%x,%x,%x", &off[0], &off[1], &off[2], &off[3], &off[4], &off[5], &off[6], &off[7], &off[8]);
+
+    offset[0] = (u8)off[0];
+    offset[1] = (u8)off[1];
+    offset[2] = (u8)off[2];
+    offset[3] = (u8)off[3];
+    offset[4] = (u8)off[4];
+    offset[5] = (u8)off[5];
+    offset[6] = (u8)off[6];
+    offset[7] = (u8)off[7];
+    offset[8] = (u8)off[8];
+
+    return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int sensor_sync_write(u8 *off)
+{
+    int err = 0;
+    struct work_info *pWorkInfo = &m_work_info;
+
+    init_completion(&pWorkInfo->completion);
+
+    sprintf(m_work_info.buffer, "%x,%x,%x,%x,%x,%x,%x,%x,%x\n", off[0], off[1], off[2], off[3], off[4], off[5], off[6], off[7], off[8]);
+
+    pWorkInfo->len = sizeof(m_work_info.buffer);
+
+    queue_delayed_work(pWorkInfo->wq, &pWorkInfo->write_work, msecs_to_jiffies(0));
+    err = wait_for_completion_timeout(&pWorkInfo->completion, msecs_to_jiffies(2000));
+    if (err == 0) {
+		MI_ERR("wait_for_completion_timeout TIMEOUT");
+		return -1;
+    }
+
+    if (pWorkInfo->rst != 0) {
+		MI_ERR("work_info.rst  not equal 0");
+		return pWorkInfo->rst;
+    }
+
+    return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int check_califolder_exist(void)
+{
+    mm_segment_t     orgfs = 0;
+    struct  file *filep = NULL;
+
+#if 1
+    orgfs = get_fs();
+    /* set_fs(KERNEL_DS); */
+	set_fs(get_ds());
+
+	filep = filp_open(OffsetFolerName, O_RDONLY, 0600);
+    if (IS_ERR(filep)) {
+		MI_ERR("%s read, sys_open %s error!!.\n", __func__, OffsetFolerName);
+		set_fs(orgfs);
+		return 0;
+    }
+
+    filp_close(filep, NULL);
+
+    set_fs(orgfs);
+#endif
+
+    return 1;
+}
+/*----------------------------------------------------------------------------*/
+static int support_fast_auto_cali(void)
+{
+#if MIR3DA_SUPPORT_FAST_AUTO_CALI
+    return (FACTORY_BOOT == get_boot_mode()) ? 1 : 0;
+#else
+    return 0;
+#endif
+}
+#endif
+/*----------------------------------------------------------------------------*/
+static int get_address(PLAT_HANDLE handle)
+{
+    if (NULL == handle) {
+		MI_ERR("chip init failed !\n");
+		    return -1;
+    }
+
+	return ((struct i2c_client *)handle)->addr;
+}
+/*----------------------------------------------------------------------------*/
+static int mir3da_resetCalibration(struct i2c_client *client)
+{
+    struct mir3da_i2c_data *obj = i2c_get_clientdata(client);
+
+    MI_FUN;
+
+    memset(obj->cali_sw, 0x00, sizeof(obj->cali_sw));
+    return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int mir3da_readCalibration(struct i2c_client *client, int dat[MIR3DA_AXES_NUM])
+{
+    struct mir3da_i2c_data *obj = i2c_get_clientdata(client);
+
+    MI_FUN;
+
+    dat[obj->cvt.map[MIR3DA_AXIS_X]] = obj->cvt.sign[MIR3DA_AXIS_X]*obj->cali_sw[MIR3DA_AXIS_X];
+    dat[obj->cvt.map[MIR3DA_AXIS_Y]] = obj->cvt.sign[MIR3DA_AXIS_Y]*obj->cali_sw[MIR3DA_AXIS_Y];
+    dat[obj->cvt.map[MIR3DA_AXIS_Z]] = obj->cvt.sign[MIR3DA_AXIS_Z]*obj->cali_sw[MIR3DA_AXIS_Z];
+
+    return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int mir3da_writeCalibration(struct i2c_client *client, int dat[MIR3DA_AXES_NUM])
+{
+    struct mir3da_i2c_data *obj = i2c_get_clientdata(client);
+    int err = 0;
+    int cali[MIR3DA_AXES_NUM];
+
+
+    MI_FUN;
+    if (!obj || !dat) {
+		MI_ERR("null ptr!!\n");
+		return -EINVAL;
+    } else {
+		cali[obj->cvt.map[MIR3DA_AXIS_X]] = obj->cvt.sign[MIR3DA_AXIS_X]*obj->cali_sw[MIR3DA_AXIS_X];
+		cali[obj->cvt.map[MIR3DA_AXIS_Y]] = obj->cvt.sign[MIR3DA_AXIS_Y]*obj->cali_sw[MIR3DA_AXIS_Y];
+		cali[obj->cvt.map[MIR3DA_AXIS_Z]] = obj->cvt.sign[MIR3DA_AXIS_Z]*obj->cali_sw[MIR3DA_AXIS_Z];
+		cali[MIR3DA_AXIS_X] += dat[MIR3DA_AXIS_X];
+		cali[MIR3DA_AXIS_Y] += dat[MIR3DA_AXIS_Y];
+		cali[MIR3DA_AXIS_Z] += dat[MIR3DA_AXIS_Z];
+
+		obj->cali_sw[MIR3DA_AXIS_X] += obj->cvt.sign[MIR3DA_AXIS_X]*dat[obj->cvt.map[MIR3DA_AXIS_X]];
+		obj->cali_sw[MIR3DA_AXIS_Y] += obj->cvt.sign[MIR3DA_AXIS_Y]*dat[obj->cvt.map[MIR3DA_AXIS_Y]];
+		obj->cali_sw[MIR3DA_AXIS_Z] += obj->cvt.sign[MIR3DA_AXIS_Z]*dat[obj->cvt.map[MIR3DA_AXIS_Z]];
+    }
+
+    return err;
+}
+/*----------------------------------------------------------------------------*/
+static int mir3da_readChipInfo(struct i2c_client *client, char *buf, int bufsize)
+{
+    if ((NULL == buf) || (bufsize <= 30)) {
+		return -1;
+    }
+
+    if (NULL == client) {
+		*buf = 0;
+		return -2;
+    }
+
+    sprintf(buf, "%s\n", "mir3da");
+
+    return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int mir3da_setPowerMode(struct i2c_client *client, bool enable)
+{
+    int ret;
+
+    MI_MSG("mir3da_setPowerMode(), enable = %d", enable);
+    ret = mir3da_set_enable(client, enable);
+    if (ret == 0) {
+		sensor_power = enable;
+    }
+    return ret;
+}
+/*----------------------------------------------------------------------------*/
+static int mir3da_readSensorData(struct i2c_client *client, char *buf)
+{
+    struct mir3da_i2c_data *obj = (struct mir3da_i2c_data *)i2c_get_clientdata(client);
+    unsigned char databuf[20];
+    int acc[MIR3DA_AXES_NUM];
+    int res = 0;
+    memset(databuf, 0, sizeof(unsigned char)*10);
+
+    if (NULL == buf) {
+		return -1;
+    }
+    if (NULL == client) {
+		*buf = 0;
+		return -2;
+    }
+
+    if (sensor_power == false) {
+		res = mir3da_setPowerMode(client, true);
+		if (res) {
+		    MI_ERR("Power on mir3da error %d!\n", res);
+		}
+		msleep(20);
+    }
+
+	res = mir3da_read_data(client, &(obj->data[MIR3DA_AXIS_X]), &(obj->data[MIR3DA_AXIS_Y]), &(obj->data[MIR3DA_AXIS_Z]));
+    if (res != 0) {
+		MI_ERR("I2C error: ret value=%d", res);
+		return -3;
+    } else {
+	#if MIR3DA_OFFSET_TEMP_SOLUTION
+			 /* if( bLoad==FILE_NO_EXIST) */
+		if (bLoad != FILE_EXIST)
+		#endif
+		{
+		    obj->data[MIR3DA_AXIS_X] += obj->cali_sw[MIR3DA_AXIS_X];
+		    obj->data[MIR3DA_AXIS_Y] += obj->cali_sw[MIR3DA_AXIS_Y];
+		    obj->data[MIR3DA_AXIS_Z] += obj->cali_sw[MIR3DA_AXIS_Z];
+		}
+
+		acc[obj->cvt.map[MIR3DA_AXIS_X]] = obj->cvt.sign[MIR3DA_AXIS_X]*obj->data[MIR3DA_AXIS_X];
+		acc[obj->cvt.map[MIR3DA_AXIS_Y]] = obj->cvt.sign[MIR3DA_AXIS_Y]*obj->data[MIR3DA_AXIS_Y];
+		acc[obj->cvt.map[MIR3DA_AXIS_Z]] = obj->cvt.sign[MIR3DA_AXIS_Z]*obj->data[MIR3DA_AXIS_Z];
+
+		#if MIR3DA_OFFSET_TEMP_SOLUTION
+		/* if( bLoad == FILE_NO_EXIST) */
+		if (bLoad != FILE_EXIST)
+		#endif
+		{
+			if (abs(obj->cali_sw[MIR3DA_AXIS_Z]) > 1300)
+				acc[obj->cvt.map[MIR3DA_AXIS_Z]] = acc[obj->cvt.map[MIR3DA_AXIS_Z]] - 2048;
+		}
+
+#if MIR3DA_STK_TEMP_SOLUTION
+			if (bzstk)
+				acc[MIR3DA_AXIS_Z] = squareRoot(1024*1024 - acc[MIR3DA_AXIS_X]*acc[MIR3DA_AXIS_X] - acc[MIR3DA_AXIS_Y]*acc[MIR3DA_AXIS_Y]);
+#endif
+
+		MI_DATA("mir3da data map: %d, %d, %d!\n", acc[MIR3DA_AXIS_X], acc[MIR3DA_AXIS_Y], acc[MIR3DA_AXIS_Z]);
+
+		acc[MIR3DA_AXIS_X] = acc[MIR3DA_AXIS_X] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+		acc[MIR3DA_AXIS_Y] = acc[MIR3DA_AXIS_Y] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+		acc[MIR3DA_AXIS_Z] = acc[MIR3DA_AXIS_Z] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+
+		sprintf(buf, "%04x %04x %04x", acc[MIR3DA_AXIS_X], acc[MIR3DA_AXIS_Y], acc[MIR3DA_AXIS_Z]);
+
+		MI_DATA("mir3da data mg: x= %d, y=%d, z=%d\n",  acc[MIR3DA_AXIS_X], acc[MIR3DA_AXIS_Y], acc[MIR3DA_AXIS_Z]);
+    }
+
+    return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int mir3da_readRawData(struct i2c_client *client, char *buf)
+{
+	struct mir3da_i2c_data *obj = (struct mir3da_i2c_data *)i2c_get_clientdata(client);
+	int res = 0;
+
+	if (!buf || !client) {
+		return -EINVAL;
+	}
+
+       res = mir3da_read_data(client, &(obj->data[MIR3DA_AXIS_X]), &(obj->data[MIR3DA_AXIS_Y]), &(obj->data[MIR3DA_AXIS_Z]));
+	if (res) {
+		MI_ERR("I2C error: ret value=%d", res);
+		return -EIO;
+	} else {
+		sprintf(buf, "%04x %04x %04x", obj->data[MIR3DA_AXIS_X], obj->data[MIR3DA_AXIS_Y], obj->data[MIR3DA_AXIS_Z]);
+
+	}
+
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int mir3da_misc_open(struct inode *inode, struct file *file)
+{
+    file->private_data = mir3da_obj;
+
+    if (file->private_data == NULL) {
+		MI_ERR("null pointer!!\n");
+		return -EINVAL;
+    }
+    return nonseekable_open(inode, file);
+}
+
+/*****************************************
+ *** mc3xxx_release
+ *****************************************/
+static int mir3da_misc_release(struct inode *inode, struct file *file)
+{
+    file->private_data = NULL;
+    return 0;
+}
+
+static long mir3da_misc_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
+{
+    struct i2c_client *client = mir_handle;
+    struct mir3da_i2c_data *obj = (struct mir3da_i2c_data *)i2c_get_clientdata(client);
+    char strbuf[MIR3DA_BUFSIZE];
+    void __user *data;
+    SENSOR_DATA sensor_data;
+    int err = 0;
+    int cali[3] = {0};
+
+    if (_IOC_DIR(cmd) & _IOC_READ) {
+		err = !access_ok(VERIFY_WRITE, (void __user *)arg, _IOC_SIZE(cmd));
+    } else if (_IOC_DIR(cmd) & _IOC_WRITE) {
+		err = !access_ok(VERIFY_READ, (void __user *)arg, _IOC_SIZE(cmd));
+    }
+
+    if (err) {
+		MI_ERR("access error: %08X, (%2d, %2d)\n", cmd, _IOC_DIR(cmd), _IOC_SIZE(cmd));
+		return -EFAULT;
+    }
+
+	memset(strbuf, 0, sizeof(strbuf));
+    switch (cmd) {
+
+    case GSENSOR_IOCTL_INIT:
+		MI_MSG("IOCTRL --- GSENSOR_IOCTL_INIT\n");
+
+		err = mir3da_chip_resume(client);
+		if (err) {
+			MI_ERR("chip resume fail!!\n");
+			err = -EFAULT;
+		}
+	break;
+
+    case GSENSOR_IOCTL_READ_CHIPINFO:
+	MI_MSG("IOCTRL --- GSENSOR_IOCTL_READ_CHIPINFO\n");
+	data = (void __user *) arg;
+	if (data == NULL) {
+	    err = -EINVAL;
+	    break;
+	}
+
+		mir3da_readChipInfo(client, strbuf, MIR3DA_BUFSIZE);
+
+	if (copy_to_user(data, strbuf, strlen(strbuf)+1)) {
+	    err = -EFAULT;
+	    break;
+	}
+	break;
+
+    case GSENSOR_IOCTL_READ_SENSORDATA:
+	MI_MSG("IOCTRL --- GSENSOR_IOCTL_READ_SENSORDATA\n");
+	data = (void __user *) arg;
+	if (data == NULL) {
+	    err = -EINVAL;
+	    break;
+	}
+
+	mir3da_readSensorData(client, strbuf);
+	if (copy_to_user(data, strbuf, strlen(strbuf)+1)) {
+	    err = -EFAULT;
+	    break;
+	}
+	break;
+
+    case GSENSOR_IOCTL_READ_GAIN:
+	MI_MSG("IOCTRL --- GSENSOR_IOCTL_READ_GAIN\n");
+	data = (void __user *) arg;
+	if (data == NULL) {
+	    err = -EINVAL;
+	    break;
+	}
+
+	if (copy_to_user(data, &gsensor_gain, sizeof(GSENSOR_VECTOR3D))) {
+	    err = -EFAULT;
+	    break;
+	}
+	break;
+
+    case GSENSOR_IOCTL_READ_OFFSET:
+	MI_MSG("IOCTRL --- GSENSOR_IOCTL_READ_OFFSET\n");
+	data = (void __user *) arg;
+	if (data == NULL) {
+	    err = -EINVAL;
+	    break;
+	}
+
+	if (copy_to_user(data, &gsensor_offset, sizeof(GSENSOR_VECTOR3D))) 	{
+	    err = -EFAULT;
+	    break;
+	}
+	break;
+
+    case GSENSOR_IOCTL_READ_RAW_DATA:
+	MI_MSG("IOCTRL --- GSENSOR_IOCTL_READ_RAW_DATA\n");
+	data = (void __user *) arg;
+	if (data == NULL) {
+	    err = -EINVAL;
+	    break;
+	}
+	mir3da_readRawData(client, strbuf);
+	if (copy_to_user(data, strbuf, strlen(strbuf)+1)) {
+	    err = -EFAULT;
+	    break;
+	}
+	break;
+
+    case GSENSOR_IOCTL_SET_CALI:
+	MI_MSG("IOCTRL --- GSENSOR_IOCTL_SET_CALI\n");
+	data = (void __user *)arg;
+	if (data == NULL) {
+	    err = -EINVAL;
+	    break;
+	}
+
+	if (copy_from_user(&sensor_data, data, sizeof(sensor_data))) {
+	    err = -EFAULT;
+	    break;
+	}
+
+	if (atomic_read(&obj->suspend)) {
+	    MI_ERR("Perform calibration in suspend state!!\n");
+	    err = -EINVAL;
+	} else {
+	    cali[MIR3DA_AXIS_X] = sensor_data.x * obj->reso->sensitivity / GRAVITY_EARTH_1000;
+	    cali[MIR3DA_AXIS_Y] = sensor_data.y * obj->reso->sensitivity / GRAVITY_EARTH_1000;
+	    cali[MIR3DA_AXIS_Z] = sensor_data.z * obj->reso->sensitivity / GRAVITY_EARTH_1000;
+	    err = mir3da_writeCalibration(client, cali);
+	}
+	break;
+
+    case GSENSOR_IOCTL_CLR_CALI:
+	MI_MSG("IOCTRL --- GSENSOR_IOCTL_CLR_CALI\n");
+	err = mir3da_resetCalibration(client);
+	break;
+
+    case GSENSOR_IOCTL_GET_CALI:
+	MI_MSG("IOCTRL --- GSENSOR_IOCTL_GET_CALI\n");
+	data = (void __user *)arg;
+	if (data == NULL) {
+	    err = -EINVAL;
+	    break;
+	}
+		err = mir3da_readCalibration(client, cali);
+	if (err < 0) {
+	    break;
+	}
+
+	sensor_data.x = cali[MIR3DA_AXIS_X] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+	sensor_data.y = cali[MIR3DA_AXIS_Y] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+	sensor_data.z = cali[MIR3DA_AXIS_Z] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+	if (copy_to_user(data, &sensor_data, sizeof(sensor_data))) 	{
+	    err = -EFAULT;
+	    break;
+	}
+	break;
+
+    default:
+		MI_ERR("unknown IOCTL: 0x%08x\n", cmd);
+		err = -ENOIOCTLCMD;
+
+    }
+
+    return err;
+}
+#ifdef CONFIG_COMPAT
+static long mir3da_misc_compat_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
+{
+	long err = 0;
+
+	void __user *arg32 = compat_ptr(arg);
+
+	if (!file->f_op || !file->f_op->unlocked_ioctl) {
+		return -ENOTTY;
+	}
+
+	switch (cmd) {
+	case COMPAT_GSENSOR_IOCTL_INIT:
+
+			err = file->f_op->unlocked_ioctl(file, GSENSOR_IOCTL_INIT, (unsigned long)arg32);
+			if (err) {
+				MI_ERR("%s COMPAT_GSENSOR_IOCTL_INIT fail!!\n", __func__);
+			}
+			break;
+
+	case COMPAT_GSENSOR_IOCTL_READ_CHIPINFO:
+			err = file->f_op->unlocked_ioctl(file, GSENSOR_IOCTL_READ_CHIPINFO, (unsigned long)arg32);
+			if (err) {
+				MI_ERR("%s COMPAT_GSENSOR_IOCTL_READ_CHIPINFO fail!!\n", __func__);
+			}
+			break;
+
+	case COMPAT_GSENSOR_IOCTL_READ_SENSORDATA:
+			err = file->f_op->unlocked_ioctl(file, GSENSOR_IOCTL_READ_SENSORDATA, (unsigned long)arg32);
+			if (err) {
+				MI_ERR("%s COMPAT_GSENSOR_IOCTL_READ_SENSORDATA fail!!\n", __func__);
+			}
+			break;
+
+	case COMPAT_GSENSOR_IOCTL_READ_GAIN:
+			err = file->f_op->unlocked_ioctl(file, GSENSOR_IOCTL_READ_GAIN, (unsigned long)arg32);
+			if (err) {
+				MI_ERR("%s COMPAT_GSENSOR_IOCTL_READ_GAIN fail!!\n", __func__);
+			}
+			break;
+
+	case COMPAT_GSENSOR_IOCTL_READ_OFFSET:
+			err = file->f_op->unlocked_ioctl(file, GSENSOR_IOCTL_READ_OFFSET, (unsigned long)arg32);
+			if (err) {
+				MI_ERR("%s COMPAT_GSENSOR_IOCTL_READ_OFFSET fail!!\n", __func__);
+			}
+			break;
+
+	case COMPAT_GSENSOR_IOCTL_READ_RAW_DATA:
+			err = file->f_op->unlocked_ioctl(file, GSENSOR_IOCTL_READ_RAW_DATA, (unsigned long)arg32);
+			if (err) {
+				MI_ERR("%s COMPAT_GSENSOR_IOCTL_READ_RAW_DATA fail!!\n", __func__);
+			}
+
+			break;
+
+	case COMPAT_GSENSOR_IOCTL_SET_CALI:
+			err = file->f_op->unlocked_ioctl(file, GSENSOR_IOCTL_SET_CALI, (unsigned long)arg32);
+			if (err) {
+				MI_ERR("%s COMPAT_GSENSOR_IOCTL_SET_CALI fail!!\n", __func__);
+			}
+
+			break;
+
+	case COMPAT_GSENSOR_IOCTL_CLR_CALI:
+			err = file->f_op->unlocked_ioctl(file, GSENSOR_IOCTL_CLR_CALI, (unsigned long)arg32);
+			if (err) {
+				MI_ERR("%s COMPAT_GSENSOR_IOCTL_CLR_CALI fail!!\n", __func__);
+			}
+
+			break;
+
+	case COMPAT_GSENSOR_IOCTL_GET_CALI:
+			err = file->f_op->unlocked_ioctl(file, GSENSOR_IOCTL_GET_CALI, (unsigned long)arg32);
+			if (err) {
+				MI_ERR("%s COMPAT_GSENSOR_IOCTL_GET_CALI fail!!\n", __func__);
+			}
+			break;
+
+	default:
+			MI_ERR("unknown IOCTL: 0x%08x\n", cmd);
+	    err = -ENOIOCTLCMD;
+	}
+
+	return err;
+}
+#endif
+/*----------------------------------------------------------------------------*/
+static const struct file_operations mir3da_misc_fops = {
+	.owner = THIS_MODULE,
+		.open			= mir3da_misc_open,
+		.release		= mir3da_misc_release,
+	.unlocked_ioctl = mir3da_misc_ioctl,
+#ifdef CONFIG_COMPAT
+		.compat_ioctl = mir3da_misc_compat_ioctl,
+#endif
+};
+
+static struct miscdevice misc_mir3da = {
+	.minor = MISC_DYNAMIC_MINOR,
+	.name = MIR3DA_MISC_NAME,
+	.fops = &mir3da_misc_fops,
+};
+/*----------------------------------------------------------------------------*/
+static ssize_t mir3da_layout_show(struct device_driver *ddri, char *buf)
+{
+	struct mir3da_i2c_data *data = mir3da_obj;
+	if (NULL == data) {
+		printk(KERN_ERR "mir3da_i2c_data is null!!\n");
+		return -1;
+	}
+
+	return sprintf(buf, "(%d, %d)\n[%+2d %+2d %+2d]\n[%+2d %+2d %+2d]\n",
+		data->hw->direction, atomic_read(&data->layout),	data->cvt.sign[0], data->cvt.sign[1],
+		data->cvt.sign[2], data->cvt.map[0], data->cvt.map[1], data->cvt.map[2]);
+
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t mir3da_layout_store(struct device_driver *ddri, const char *buf, size_t count)
+{
+	int layout = 0;
+	struct mir3da_i2c_data *data = mir3da_obj;
+
+	if (NULL == data) {
+		printk(KERN_ERR "mir3da_i2c_data is null!!\n");
+		return count;
+	}
+
+	if (1 == sscanf(buf, "%d", &layout)) {
+		atomic_set(&data->layout, layout);
+		if (!hwmsen_get_convert(layout, &data->cvt)) {
+			printk(KERN_ERR "HWMSEN_GET_CONVERT function error!\r\n");
+		} else if (!hwmsen_get_convert(data->hw->direction, &data->cvt)) {
+			printk(KERN_ERR "invalid layout: %d, restore to %d\n", layout, data->hw->direction);
+		} else {
+			printk(KERN_ERR "invalid layout: (%d, %d)\n", layout, data->hw->direction);
+			hwmsen_get_convert(0, &data->cvt);
+		}
+	} else {
+		printk(KERN_ERR "invalid format = '%s'\n", buf);
+	}
+
+	return count;
+
+}
+
+static ssize_t mir3da_enable_show(struct device_driver *ddri, char *buf)
+{
+    int ret;
+    int bEnable;
+
+    MI_FUN;
+
+    ret = mir3da_get_enable(mir_handle, (char *)(&bEnable));
+    if (ret < 0) {
+		ret = -EINVAL;
+    } else {
+		ret = sprintf(buf, "%d\n", bEnable);
+    }
+
+    return ret;
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t mir3da_enable_store(struct device_driver *ddri, const char *buf, size_t count)
+{
+    int ret;
+    bool bEnable;
+    unsigned long enable;
+
+    if (buf == NULL) {
+		return -1;
+    }
+
+    enable = simple_strtoul(buf, NULL, 10);
+    bEnable = (enable > 0) ? true : false;
+
+    ret = mir3da_set_enable(mir_handle, bEnable);
+    if (ret < 0) {
+		ret = -EINVAL;
+    } else {
+		ret = count;
+    }
+
+    return ret;
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t mir3da_axis_data_show(struct device_driver *ddri, char *buf)
+{
+    int res;
+    short x, y, z;
+    int count = 0;
+
+    res = mir3da_read_data(mir_handle, &x, &y, &z);
+    if (res == 0)
+		count += sprintf(buf+count, "x= %d;y=%d;z=%d\n", x, y, z);
+    else
+		count += sprintf(buf+count, "reading failed!");
+
+    return count;
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t mir3da_reg_data_show(struct device_driver *ddri, char *buf)
+{
+    MIR_HANDLE          handle = mir_handle;
+
+    return mir3da_get_reg_data(handle, buf);
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t mir3da_reg_data_store(struct device_driver *ddri, const char *buf, size_t count)
+{
+    int                 addr, data;
+    int                 res;
+
+    sscanf(buf, "0x%x, 0x%x\n", &addr, &data);
+
+    res = mir3da_register_write(mir_handle, addr, data);
+
+    MI_ASSERT(res == 0);
+
+    MI_MSG("set[0x%x]->[0x%x]\n", addr, data);
+
+    return count;
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t mir3da_log_level_show(struct device_driver *ddri, char *buf)
+{
+    int ret;
+
+    ret = sprintf(buf, "%d\n", Log_level);
+
+    return ret;
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t mir3da_log_level_store(struct device_driver *ddri, const char *buf, size_t count)
+{
+    Log_level = simple_strtoul(buf, NULL, 10);
+    return count;
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t mir3da_primary_offset_show(struct device_driver *ddri, char *buf)
+{
+    int x = 0, y = 0, z = 0;
+
+    mir3da_get_primary_offset(mir_handle, &x, &y, &z);
+
+	  return sprintf(buf, "x=%d ,y=%d ,z=%d\n", x, y, z);
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t mir3da_version_show(struct device_driver *ddri, char *buf)
+{
+	return sprintf(buf, "%s_%s\n", DRI_VER, CORE_VER);
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t mir3da_vendor_show(struct device_driver *ddri, char *buf)
+{
+    return sprintf(buf, "%s\n", "MiraMEMS");
+}
+/*----------------------------------------------------------------------------*/
+#if MIR3DA_OFFSET_TEMP_SOLUTION
+static ssize_t mir3da_offset_show(struct device_driver *ddri, char *buf)
+{
+    ssize_t count = 0;
+
+    if (bLoad == FILE_EXIST)
+		count += sprintf(buf, "%s", m_work_info.buffer);
+    else
+		count += sprintf(buf, "%s", "Calibration file not exist!\n");
+
+    return count;
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t mir3da_calibrate_show(struct device_driver *ddri, char *buf)
+{
+    int ret;
+
+    ret = sprintf(buf, "%d\n", bCalires);
+    return ret;
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t mir3da_calibrate_store(struct device_driver *ddri, const char *buf, size_t count)
+{
+    signed char     z_dir = 0;
+
+    z_dir = simple_strtol(buf, NULL, 10);
+    bCalires = mir3da_calibrate(mir_handle, z_dir);
+
+    return count;
+}
+#endif
+/*----------------------------------------------------------------------------*/
+#if FILTER_AVERAGE_ENHANCE
+static ssize_t mir3da_average_enhance_show(struct device_driver *ddri, char *buf)
+{
+    int       ret = 0;
+    struct mir3da_filter_param_s    param = {0};
+
+    ret = mir3da_get_filter_param(&param);
+    ret |= sprintf(buf, "%d %d %d\n", param.filter_param_l, param.filter_param_h, param.filter_threhold);
+
+    return ret;
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t mir3da_average_enhance_store(struct device_driver *ddri, const char *buf, size_t count)
+{
+    int       ret = 0;
+    struct mir3da_filter_param_s    param = {0};
+
+    sscanf(buf, "%d %d %d\n", &param.filter_param_l, &param.filter_param_h, &param.filter_threhold);
+
+    ret = mir3da_set_filter_param(&param);
+
+    return count;
+}
+#endif
+/*----------------------------------------------------------------------------*/
+static DRIVER_ATTR(layout,          S_IRUGO | S_IWUGO,  mir3da_layout_show,             mir3da_layout_store);
+static DRIVER_ATTR(enable,          S_IRUGO | S_IWUGO,  mir3da_enable_show,             mir3da_enable_store);
+static DRIVER_ATTR(axis_data,       S_IRUGO | S_IWUGO,  mir3da_axis_data_show,          NULL);
+static DRIVER_ATTR(reg_data,        S_IWUGO | S_IRUGO,  mir3da_reg_data_show,           mir3da_reg_data_store);
+static DRIVER_ATTR(log_level,       S_IWUGO | S_IRUGO,  mir3da_log_level_show,          mir3da_log_level_store);
+static DRIVER_ATTR(primary_offset,  S_IWUGO | S_IRUGO,  mir3da_primary_offset_show,     NULL);
+static DRIVER_ATTR(vendor,          S_IWUGO | S_IRUGO,  mir3da_vendor_show,             NULL);
+static DRIVER_ATTR(version,         S_IWUGO | S_IRUGO,  mir3da_version_show,            NULL);
+#if MIR3DA_OFFSET_TEMP_SOLUTION
+static DRIVER_ATTR(offset,          S_IWUGO | S_IRUGO,  mir3da_offset_show,             NULL);
+static DRIVER_ATTR(calibrate_miraGSensor,       S_IWUGO | S_IRUGO,  mir3da_calibrate_show,          mir3da_calibrate_store);
+#endif
+#if FILTER_AVERAGE_ENHANCE
+static DRIVER_ATTR(average_enhance, S_IWUGO | S_IRUGO,  mir3da_average_enhance_show,    mir3da_average_enhance_store);
+#endif
+/*----------------------------------------------------------------------------*/
+static struct driver_attribute *mir3da_attributes[] = {
+	&driver_attr_layout,
+    &driver_attr_enable,
+    &driver_attr_axis_data,
+    &driver_attr_reg_data,
+    &driver_attr_log_level,
+    &driver_attr_primary_offset,
+    &driver_attr_vendor,
+    &driver_attr_version,
+#if MIR3DA_OFFSET_TEMP_SOLUTION
+    &driver_attr_offset,
+    &driver_attr_calibrate_miraGSensor,
+#endif
+#if FILTER_AVERAGE_ENHANCE
+    &driver_attr_average_enhance,
+#endif
+};
+/*----------------------------------------------------------------------------*/
+static int mir3da_create_attr(struct device_driver *driver)
+{
+    int idx, err = 0;
+    int num = (int)(sizeof(mir3da_attributes)/sizeof(mir3da_attributes[0]));
+    if (driver == NULL) {
+		return -EINVAL;
+    }
+
+    for (idx = 0; idx < num; idx++) {
+		err = driver_create_file(driver, mir3da_attributes[idx]);
+		if (err != 0) {
+		    MI_MSG("driver_create_file (%s) = %d\n", mir3da_attributes[idx]->attr.name, err);
+		    break;
+		}
+    }
+    return err;
+}
+/*----------------------------------------------------------------------------*/
+static int mir3da_delete_attr(struct device_driver *driver)
+{
+    int idx , err = 0;
+    int num = (int)(sizeof(mir3da_attributes)/sizeof(mir3da_attributes[0]));
+
+    if (driver == NULL) {
+		return -EINVAL;
+    }
+
+    for (idx = 0; idx < num; idx++) {
+		driver_remove_file(driver, mir3da_attributes[idx]);
+    }
+
+    return err;
+}
+
+/*----------------------------------------------------------------------------*/
+static void mir3da_power(struct acc_hw *hw, unsigned int on)
+{
+    static unsigned int power_on;
+
+    MI_MSG("power %s\n", on ? "on" : "off");
+
+
+    if (hw->power_id != POWER_NONE_MACRO) {
+		MI_MSG("power %s\n", on ? "on" : "off");
+		if (power_on == on) {
+		    MI_MSG("ignore power control: %d\n", on);
+		} else if (on) {
+		    if (!hwPowerOn(hw->power_id, hw->power_vol, "mir3da")) {
+				MI_ERR("power on fails!!\n");
+		    }
+		} else {
+		    if (!hwPowerDown(hw->power_id, "mir3da")) {
+				MI_ERR("power off fail!!\n");
+		    }
+		}
+    }
+
+    power_on = on;
+}
+
+#ifndef CONFIG_HAS_EARLYSUSPEND
+/*----------------------------------------------------------------------------*/
+static int mir3da_suspend(struct i2c_client *client, pm_message_t msg)
+{
+	struct mir3da_i2c_data *obj = i2c_get_clientdata(client);
+	int err = 0;
+	u8 dat;
+	MI_FUN;
+
+	if (msg.event == PM_EVENT_SUSPEND) {
+		if (obj == NULL) {
+			MI_ERR("null pointer!!\n");
+			return -EINVAL;
+		}
+
+		err = mir3da_set_enable(obj->client, false);
+		if (err) {
+		    return res;
+		}
+
+		mir3da_power(obj->hw, 0);
+	}
+	return err;
+}
+/*----------------------------------------------------------------------------*/
+static int mir3da_resume(struct i2c_client *client)
+{
+	struct mir3da_i2c_data *obj = i2c_get_clientdata(client);
+	int err;
+	MI_FUN;
+
+	if (obj == NULL) {
+		MI_ERR("null pointer!!\n");
+		return -EINVAL;
+	}
+
+	err = mir3da_chip_resume(obj->client);
+	if (err) {
+		MI_ERR("chip resume fail!!\n");
+		return err;
+	}
+
+	err = mir3da_setPowerMode(obj->client, sensor_enable_status);
+	if (err != 0) {
+		return err;
+	}
+
+	mir3da_power(obj->hw, 1);
+
+	atomic_set(&obj->suspend, 0);
+
+	return 0;
+}
+#else
+/*----------------------------------------------------------------------------*/
+static void mir3da_early_suspend(struct early_suspend *h)
+{
+	struct mir3da_i2c_data *obj = container_of(h, struct mir3da_i2c_data, early_drv);
+	int err;
+	MI_FUN;
+
+	if (obj == NULL) {
+		MI_ERR("null pointer!!\n");
+		return;
+	}
+	atomic_set(&obj->suspend, 1);
+
+	err = mir3da_setPowerMode(obj->client, false);
+	if (err) {
+		MI_ERR("write power control fail!!\n");
+		return;
+	}
+
+	sensor_power = false;
+
+	mir3da_power(obj->hw, 0);
+}
+
+/*----------------------------------------------------------------------------*/
+static void mir3da_late_resume(struct early_suspend *h)
+{
+	struct mir3da_i2c_data  *obj = container_of(h, struct mir3da_i2c_data, early_drv);
+	int                     err;
+
+	MI_FUN;
+
+	if (obj == NULL) {
+		MI_ERR("null pointer!!\n");
+		return;
+	}
+
+	err = mir3da_chip_resume(obj->client);
+	if (err) {
+		MI_ERR("chip resume fail!!\n");
+		return;
+	}
+	err = mir3da_setPowerMode(obj->client, sensor_enable_status);
+	if (err != 0) {
+		return;
+	}
+
+	mir3da_power(obj->hw, 1);
+
+	atomic_set(&obj->suspend, 0);
+}
+#endif
+/*----------------------------------------------------------------------------*/
+#ifdef MIR3DA_ACC_NEW_ARCH
+static int mir3da_open_report_data(int open)
+{
+	return 0;
+}
+static int mir3da_enable_nodata(int en)
+{
+	int err = 0;
+	int value = 0;
+	struct mir3da_i2c_data *obj = mir3da_obj;
+
+	if (NULL == obj) {
+		MI_ERR("mir3da_obj is null!\n");
+		return -1;
+	}
+	value = en;
+
+	MI_MSG("%s, value = %d\n", __func__, value);
+	if (value == 0) {
+		sensor_enable_status = false;
+	} else {
+		sensor_enable_status = true;
+	}
+	if (((value == 0) && (sensor_power == false)) || ((value == 1) && (sensor_power == true)))	{
+		MI_MSG("Gsensor device have updated!\n");
+	} else {
+		err = mir3da_setPowerMode(obj->client, sensor_enable_status);
+	}
+
+	return err;
+}
+static int mir3da_set_delay(u64 ns)
+{
+	int err = 0;
+	int value = (int)ns/1000/1000;
+	struct mir3da_i2c_data *obj = mir3da_obj;
+
+	if (NULL == obj) {
+		MI_ERR("mir3da_obj is null!\n");
+		return -1;
+	}
+
+	err = mir3da_set_odr(obj->client, value);
+	if (err) {
+		MI_ERR("mir3da_set_odr failed !\n");
+		err = -EINVAL;
+	}
+	return err;
+}
+static int mir3da_get_data(int *x, int *y, int *z, int *status)
+{
+	int err = 0;
+    char buff[MIR3DA_BUFSIZE];
+
+	struct mir3da_i2c_data *obj = mir3da_obj;
+
+	if (NULL == obj) {
+		MI_ERR("mir3da_obj is null!\n");
+		return -1;
+	}
+	memset(buff, 0, sizeof(buff));
+
+	err = mir3da_readSensorData(obj->client, buff);
+	if (err < 0) {
+		MI_ERR("%s call mir3da_readSensorData failed!err=%d\n", __func__, err);
+	} else {
+		sscanf(buff, "%x %x %x", x, y, z);
+		*status = SENSOR_STATUS_ACCURACY_MEDIUM;
+	}
+
+	return err;
+}
+
+#else
+static int mir3da_operate(void *self, uint32_t command, void *buff_in, int size_in,
+	void *buff_out, int size_out, int *actualout)
+{
+    int err = 0;
+    int value;
+    struct mir3da_i2c_data *priv = (struct mir3da_i2c_data *)self;
+    hwm_sensor_data *gsensor_data;
+    char buff[MIR3DA_BUFSIZE];
+
+	switch (command) {
+	case SENSOR_DELAY:
+	    MI_MSG("mir3da_operate, command is SENSOR_DELAY");
+
+			if ((buff_in == NULL) || (size_in < sizeof(int))) {
+				MI_ERR("Set delay parameter error!\n");
+				err = -EINVAL;
+			} else {
+				value = *(int *)buff_in;
+
+				err = mir3da_set_odr(priv->client, value);
+				if (err) {
+					MI_ERR("mir3da_set_odr failed !");
+					err = -EINVAL;
+				}
+			}
+	    break;
+
+	case SENSOR_ENABLE:
+	    MI_MSG("mir3da_operate enable gsensor\n");
+	    if ((buff_in == NULL) || (size_in < sizeof(int))) {
+			MI_ERR("mir3da_operate Enable sensor parameter error!\n");
+			err = -EINVAL;
+	    } else {
+			value = *(int *)buff_in;
+
+			MI_MSG("mir3da_operate, command is SENSOR_ENABLE, value = %d\n", value);
+
+			if (((value == 0) && (sensor_power == false)) || ((value == 1) && (sensor_power == true))) {
+			    MI_MSG("Gsensor device have updated!\n");
+			} else {
+			    err = mir3da_setPowerMode(priv->client, !sensor_power);
+			}
+	    }
+	    break;
+
+	case SENSOR_GET_DATA:
+	    MI_MSG("mir3da_operate, command is SENSOR_GET_DATA\n");
+	    if ((buff_out == NULL) || (size_out < sizeof(hwm_sensor_data))) {
+			MI_MSG("get sensor data parameter error!\n");
+			err = -EINVAL;
+	    } else {
+			gsensor_data = (hwm_sensor_data *)buff_out;
+
+			mir3da_readSensorData(priv->client, buff);
+			sscanf(buff, "%x %x %x", &gsensor_data->values[0],
+			&gsensor_data->values[1], &gsensor_data->values[2]);
+
+			gsensor_data->status = SENSOR_STATUS_ACCURACY_MEDIUM;
+			gsensor_data->value_divide = 1000;
+	    }
+	    break;
+	default:
+		MI_MSG("gsensor operate function no this parameter %d!\n", command);
+		err = -1;
+	    break;
+    }
+
+    return err;
+}
+#endif
+/*----------------------------------------------------------------------------*/
+int i2c_smbus_read(PLAT_HANDLE handle, u8 addr, u8 *data)
+{
+    int                 res = 0;
+    struct i2c_client   *client = (struct i2c_client *)handle;
+
+    *data = i2c_smbus_read_byte_data(client, addr);
+
+    return res;
+}
+/*----------------------------------------------------------------------------*/
+int i2c_smbus_read_block(PLAT_HANDLE handle, u8 addr, u8 count, u8 *data)
+{
+    int                 res = 0;
+    struct i2c_client   *client = (struct i2c_client *)handle;
+
+    res = i2c_smbus_read_i2c_block_data(client, addr, count, data);
+
+    return res;
+}
+/*----------------------------------------------------------------------------*/
+int i2c_smbus_write(PLAT_HANDLE handle, u8 addr, u8 data)
+{
+    int                 res = 0;
+    struct i2c_client   *client = (struct i2c_client *)handle;
+
+    res = i2c_smbus_write_byte_data(client, addr, data);
+
+    return res;
+}
+/*----------------------------------------------------------------------------*/
+void msdelay(int ms)
+{
+    mdelay(ms);
+}
+
+#if MIR3DA_OFFSET_TEMP_SOLUTION
+MIR_GENERAL_OPS_DECLARE(ops_handle, i2c_smbus_read, i2c_smbus_read_block, i2c_smbus_write, sensor_sync_write, sensor_sync_read, check_califolder_exist, get_address, support_fast_auto_cali, msdelay, printk, sprintf);
+#else
+MIR_GENERAL_OPS_DECLARE(ops_handle, i2c_smbus_read, i2c_smbus_read_block, i2c_smbus_write, NULL, NULL, NULL, get_address, NULL, msdelay, printk, sprintf);
+#endif
+/*----------------------------------------------------------------------------*/
+static int mir3da_probe(struct i2c_client *client, const struct i2c_device_id *id)
+{
+    int res = 0;
+    struct mir3da_i2c_data *obj;
+
+#ifdef MIR3DA_ACC_NEW_ARCH
+	struct acc_control_path ctl = {0};
+	struct acc_data_path dat = {0};
+#else
+	struct hwmsen_object sobj;
+#endif
+
+    MI_FUN;
+	obj = kzalloc(sizeof(*obj), GFP_KERNEL);
+
+    if (obj == NULL) {
+		MI_ERR("kzalloc failed!");
+		res = -ENOMEM;
+		goto exit;
+    }
+
+    memset(obj, 0, sizeof(struct mir3da_i2c_data));
+
+    obj->client = client;
+    i2c_set_clientdata(client, obj);
+
+	mir3da_obj = obj;
+    obj->hw = get_cust_acc_hw();
+
+	res = hwmsen_get_convert(obj->hw->direction, &obj->cvt);
+    if (res < 0) {
+		MI_ERR("invalid direction: %d\n", obj->hw->direction);
+		goto exit_kfree;
+    }
+
+    obj->reso = &mir3da_data_resolution[0];
+    gsensor_gain.x = gsensor_gain.y = gsensor_gain.z = obj->reso->sensitivity;
+
+    res = mir3da_resetCalibration(client);
+    if (res != 0) {
+	   return res;
+    }
+
+    atomic_set(&obj->trace, 0);
+    atomic_set(&obj->suspend, 0);
+	atomic_set(&obj->layout, obj->hw->direction);
+
+    if (mir3da_install_general_ops(&ops_handle)) {
+		MI_ERR("Install ops failed !\n");
+		goto exit_init_failed;
+    }
+
+#if MIR3DA_OFFSET_TEMP_SOLUTION
+    m_work_info.wq = create_singlethread_workqueue("oo");
+    if (NULL == m_work_info.wq) {
+		MI_ERR("Failed to create workqueue !");
+		goto exit_init_failed;
+    }
+
+    INIT_DELAYED_WORK(&m_work_info.read_work, sensor_read_work);
+    INIT_DELAYED_WORK(&m_work_info.write_work, sensor_write_work);
+#endif
+
+    mir_handle = mir3da_core_init(client);
+    if (NULL == mir_handle) {
+		MI_ERR("chip init failed !\n");
+		goto exit_init_failed;
+    }
+
+    res = misc_register(&misc_mir3da);
+    if (res) {
+		MI_ERR("%s: misc register failed !\n", __func__);
+		goto exit_misc_device_register_failed;
+    }
+
+#ifdef MIR3DA_ACC_NEW_ARCH
+	res = mir3da_create_attr(&(mir3da_init_info.platform_diver_addr->driver));
+#else
+	res = mir3da_create_attr(&mir3da_gsensor_driver.driver);
+#endif
+	if (res < 0) {
+		MI_ERR("create attribute res = %d\n", res);
+		goto exit_create_attr_failed;
+    }
+
+#ifdef MIR3DA_ACC_NEW_ARCH
+	ctl.open_report_data = mir3da_open_report_data;
+	ctl.enable_nodata = mir3da_enable_nodata;
+	ctl.set_delay = mir3da_set_delay;
+	ctl.is_report_input_direct = false;
+	ctl.is_use_common_factory = false;
+	ctl.is_support_batch = obj->hw->is_batch_supported;
+
+	res = acc_register_control_path(&ctl);
+	if (res < 0) {
+		MI_ERR("acc_register_control_path res = %d\n", res);
+		goto exit_create_attr_failed;
+	}
+
+	dat.get_data = mir3da_get_data;
+	dat.vender_div = 1000;
+
+	res = acc_register_data_path(&dat);
+	if (res < 0) {
+		MI_ERR("acc_register_data_path res = %d\n", res);
+		goto exit_create_attr_failed;
+	}
+#else
+    sobj.self = obj;
+    sobj.polling = 1;
+    sobj.sensor_operate = mir3da_operate;
+	res = hwmsen_attach(ID_ACCELEROMETER, &sobj);
+    if (res != 0) {
+		MI_ERR("attach fail = %d\n", res);
+		goto exit_kfree;
+    }
+#endif
+#ifdef CONFIG_HAS_EARLYSUSPEND
+	obj->early_drv.level    = EARLY_SUSPEND_LEVEL_DISABLE_FB - 1,
+	obj->early_drv.suspend  = mir3da_early_suspend,
+	obj->early_drv.resume   = mir3da_late_resume,
+	register_early_suspend(&obj->early_drv);
+#endif
+
+#ifdef MIR3DA_ACC_NEW_ARCH
+    mir3da_init_flag = 0;
+#endif
+	MI_ERR("%s: probe ok\n", __func__);
+
+    return res;
+exit_create_attr_failed:
+	misc_deregister(&misc_mir3da);
+exit_misc_device_register_failed:
+exit_init_failed:
+
+exit_kfree:
+	mir3da_obj = NULL;
+	kfree(obj);
+exit:
+#ifdef MIR3DA_ACC_NEW_ARCH
+	mir3da_init_flag = -1;
+#endif
+	MI_ERR("%s: err = %d\n", __func__, res);
+	return res;
+}
+/*----------------------------------------------------------------------------*/
+static int  mir3da_remove(struct i2c_client *client)
+{
+   int err = 0;
+
+#ifdef MIR3DA_ACC_NEW_ARCH
+	err = mir3da_delete_attr(&(mir3da_init_info.platform_diver_addr->driver));
+#else
+	err = mir3da_delete_attr(&mir3da_gsensor_driver.driver);
+#endif
+    if (err < 0) {
+		MI_ERR("mir3da_delete_attr fail: %d\n", err);
+    }
+
+#ifdef MIR3DA_ACC_NEW_ARCH
+		mir3da_init_flag = -1;
+#endif
+    misc_deregister(&misc_mir3da);
+    mir_handle = NULL;
+	mir3da_obj = NULL;
+    i2c_unregister_device(client);
+    kfree(i2c_get_clientdata(client));
+
+    return 0;
+}
+
+/*----------------------------------------------------------------------------*/
+#if 0
+static int mir3da_detect(struct i2c_client *new_client, int kind, struct i2c_board_info *info)
+{
+      struct i2c_adapter *adapter = new_client->adapter;
+
+      MI_MSG("mir3da_detect, bus[%d] addr[0x%x]\n", adapter->nr, new_client->addr);
+
+      if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_BYTE_DATA))
+		return -ENODEV;
+
+       strcpy(info->type, MIR3DA_DRV_NAME);
+
+       return 0;
+}
+#endif
+/*----------------------------------------------------------------------------*/
+static const struct i2c_device_id mir3da_id[] = {
+    { MIR3DA_DRV_NAME, 0 },
+    { }
+};
+MODULE_DEVICE_TABLE(i2c, mir3da_id);
+#if 0
+#if MTK_ANDROID_23
+static unsigned short mir3da_force[] = {0x00, MIR3DA_I2C_ADDR, I2C_CLIENT_END, I2C_CLIENT_END};
+static const unsigned short *const mir3da_forces[] = { mir3da_force, NULL };
+static struct i2c_client_address_data mir3da_addr_data = { .forces = mir3da_forces,};
+#endif
+#endif
+static struct i2c_driver mir3da_driver = {
+    .driver = {
+	.name    = MIR3DA_DRV_NAME,
+	.owner   = THIS_MODULE,
+    },
+    .probe       = mir3da_probe,
+    .remove      = mir3da_remove,
+   /* .detect	     = mir3da_detect, */
+#if !defined(CONFIG_HAS_EARLYSUSPEND)
+    .suspend     = mir3da_suspend,
+    .resume      = mir3da_resume,
+#endif
+    .id_table    = mir3da_id,
+#if 0
+#if MTK_ANDROID_23
+    .address_data = &mir3da_addr_data,
+#endif
+#endif
+};
+
+/*----------------------------------------------------------------------------*/
+#ifdef MIR3DA_ACC_NEW_ARCH
+static int mir3da_local_init(void)
+{
+    struct acc_hw *hw = get_cust_acc_hw();
+
+    MI_FUN;
+
+    mir3da_power(hw, 1);
+
+    if (i2c_add_driver(&mir3da_driver)) {
+		MI_ERR("add driver error\n");
+		return -1;
+    }
+
+	if (-1 == mir3da_init_flag) {
+		MI_ERR("add driver error;mir3da_init_flag = %d\n", mir3da_init_flag);
+		return -1;
+	}
+
+    return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int mir3da_local_remove(void)
+{
+    struct acc_hw *hw = get_cust_acc_hw();
+
+    MI_FUN;
+
+    mir3da_power(hw, 0);
+
+    i2c_del_driver(&mir3da_driver);
+	mir3da_init_flag = -1;
+    return 0;
+}
+/*----------------------------------------------------------------------------*/
+#else
+static int mir3da_platform_probe(struct platform_device *pdev)
+{
+    struct acc_hw *hw = get_cust_acc_hw();
+
+    MI_FUN;
+
+    mir3da_power(hw, 1);
+
+    if (i2c_add_driver(&mir3da_driver)) {
+		MI_ERR("add driver error\n");
+		return -1;
+    }
+
+    return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int mir3da_platform_remove(struct platform_device *pdev)
+{
+    struct acc_hw *hw = get_cust_acc_hw();
+
+    MI_FUN;
+
+    mir3da_power(hw, 0);
+
+    i2c_del_driver(&mir3da_driver);
+
+    return 0;
+}
+#endif
+/*----------------------------------------------------------------------------*/
+static int __init mir3da_init(void)
+{
+    struct acc_hw *hw = get_cust_acc_hw();
+
+    MI_FUN;
+
+    i2c_register_board_info(hw->i2c_num, &mir3da_i2c_boardinfo, 1);
+
+
+#ifdef MIR3DA_ACC_NEW_ARCH
+    acc_driver_add(&mir3da_init_info);
+#else
+    if (platform_driver_register(&mir3da_gsensor_driver)) {
+		MI_ERR("failed to register driver");
+		return -ENODEV;
+    }
+#endif
+
+    return 0;
+}
+/*----------------------------------------------------------------------------*/
+static void __exit mir3da_exit(void)
+{
+    MI_FUN;
+
+#ifndef MIR3DA_ACC_NEW_ARCH
+    platform_driver_unregister(&mir3da_gsensor_driver);
+#endif
+}
+/*----------------------------------------------------------------------------*/
+
+MODULE_AUTHOR("MiraMEMS <lschen@miramems.com>");
+MODULE_DESCRIPTION("MirMEMS 3-Axis Accelerometer driver");
+MODULE_LICENSE("GPL");
+MODULE_VERSION("1.0");
+
+module_init(mir3da_init);
+module_exit(mir3da_exit);
diff --git a/drivers/misc/mediatek/accelerometer/da213/mir3da_cust.h b/drivers/misc/mediatek/accelerometer/da213/mir3da_cust.h
new file mode 100644
index 000000000..e9e2f7450
--- /dev/null
+++ b/drivers/misc/mediatek/accelerometer/da213/mir3da_cust.h
@@ -0,0 +1,44 @@
+/*
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ */
+
+#ifndef __MIR3DA_H__
+#define __MIR3DA_H__
+
+#include <linux/ioctl.h>
+#include <linux/kernel.h>
+#include "mir3da_core.h"
+
+#define DRI_VER					        "1.0"
+
+#define MIR3DA_I2C_ADDR		                    0x4c/* 0x26<-> SD0=GND;0x27<-> SD0=High */
+
+#define MIR3DA_ACC_IOCTL_BASE                   88
+#define IOCTL_INDEX_BASE                        0x00
+
+#define MIR3DA_ACC_IOCTL_SET_DELAY              _IOW(MIR3DA_ACC_IOCTL_BASE, IOCTL_INDEX_BASE, int)
+#define MIR3DA_ACC_IOCTL_GET_DELAY              _IOR(MIR3DA_ACC_IOCTL_BASE, IOCTL_INDEX_BASE+1, int)
+#define MIR3DA_ACC_IOCTL_SET_ENABLE             _IOW(MIR3DA_ACC_IOCTL_BASE, IOCTL_INDEX_BASE+2, int)
+#define MIR3DA_ACC_IOCTL_GET_ENABLE             _IOR(MIR3DA_ACC_IOCTL_BASE, IOCTL_INDEX_BASE+3, int)
+#define MIR3DA_ACC_IOCTL_SET_G_RANGE            _IOW(MIR3DA_ACC_IOCTL_BASE, IOCTL_INDEX_BASE+4, int)
+#define MIR3DA_ACC_IOCTL_GET_G_RANGE            _IOR(MIR3DA_ACC_IOCTL_BASE, IOCTL_INDEX_BASE+5, int)
+
+#define MIR3DA_ACC_IOCTL_GET_COOR_XYZ           _IOW(MIR3DA_ACC_IOCTL_BASE, IOCTL_INDEX_BASE+22, int)
+#define MIR3DA_ACC_IOCTL_CALIBRATION            _IOW(MIR3DA_ACC_IOCTL_BASE, IOCTL_INDEX_BASE+23, int)
+#define MIR3DA_ACC_IOCTL_UPDATE_OFFSET          _IOW(MIR3DA_ACC_IOCTL_BASE, IOCTL_INDEX_BASE+24, int)
+
+/* int mir3da_install_ops(struct gsensor_op_s *method); */
+
+#endif /* !__MIR3DA_H__ */
diff --git a/drivers/misc/mediatek/accelerometer/inc/accel.h b/drivers/misc/mediatek/accelerometer/inc/accel.h
new file mode 100644
index 000000000..0f133b40b
--- /dev/null
+++ b/drivers/misc/mediatek/accelerometer/inc/accel.h
@@ -0,0 +1,130 @@
+
+#ifndef __ACC_H__
+#define __ACC_H__
+
+
+#include <linux/wakelock.h>
+#include <linux/interrupt.h>
+#include <linux/miscdevice.h>
+#include <linux/platform_device.h>
+#include <linux/input.h>
+#include <linux/workqueue.h>
+#include <linux/slab.h>
+#include <linux/module.h>
+#include <linux/hwmsensor.h>
+#include <linux/earlysuspend.h> 
+#include <linux/hwmsen_dev.h>
+
+
+#define ACC_TAG					"<ACCELEROMETER> "
+#define ACC_FUN(f)				printk(KERN_ERR ACC_TAG"%s\n", __func__)
+#define ACC_ERR(fmt, args...)		printk(KERN_ERR ACC_TAG"%s %d : "fmt, __func__, __LINE__, ##args)
+#define ACC_LOG(fmt, args...)		printk(KERN_ERR ACC_TAG fmt, ##args)
+#define ACC_VER(fmt, args...)   	printk(KERN_ERR ACC_TAG"%s: "fmt, __func__, ##args) //((void)0)
+
+#define OP_ACC_DELAY	0X01
+#define	OP_ACC_ENABLE	0X02
+#define	OP_ACC_GET_DATA	0X04
+
+#define ACC_INVALID_VALUE -1
+
+#define EVENT_TYPE_ACCEL_X          		ABS_X
+#define EVENT_TYPE_ACCEL_Y          		ABS_Y
+#define EVENT_TYPE_ACCEL_Z          		ABS_Z
+#define EVENT_TYPE_ACCEL_UPDATE              REL_X
+#define EVENT_TYPE_ACCEL_STATUS     ABS_WHEEL
+#define EVENT_TYPE_ACCEL_DIV        ABS_GAS
+
+
+#define ACC_VALUE_MAX (32767)
+#define ACC_VALUE_MIN (-32768)
+#define ACC_STATUS_MIN (0)
+#define ACC_STATUS_MAX (64)
+#define ACC_DIV_MAX (32767)
+#define ACC_DIV_MIN (1)
+#define ACC_AXIS_X 0
+#define ACC_AXIS_Y 1
+#define ACC_AXIS_Z 2
+
+#define MAX_CHOOSE_G_NUM 5
+#define ACC_AXES_NUM 3
+struct acc_control_path
+{
+	int (*open_report_data)(int open);//open data rerport to HAL
+	int (*enable_nodata)(int en);//only enable not report event to HAL
+	int (*set_delay)(u64 delay);
+	int (*access_data_fifo)(void);//version2.used for flush operate
+	bool is_report_input_direct;
+	bool is_support_batch;//version2.used for batch mode support flag
+	bool is_use_common_factory;
+	int (*acc_calibration)(int type, int cali[3]);//version3 sensor common layer factory mode API1
+};
+
+struct acc_data_path
+{
+	int (*get_data)(int *x,int *y, int *z,int *status);
+	int (*get_raw_data)(int *x,int *y, int *z);//version3 sensor common layer factory mode API2
+	int vender_div;
+};
+
+struct acc_init_info
+{
+    char *name;
+	int (*init)(void);
+	int (*uninit)(void);
+	struct platform_driver* platform_diver_addr;
+};
+
+struct acc_data{
+	hwm_sensor_data acc_data ;
+	int data_updata;
+	//struct mutex lock;
+};
+
+struct acc_drv_obj {
+    void *self;
+	int polling;
+	int (*acc_operate)(void* self, uint32_t command, void* buff_in, int size_in,
+		void* buff_out, int size_out, int* actualout);
+};
+
+struct acc_context {
+	struct input_dev   *idev;
+	struct miscdevice   mdev;
+	struct work_struct  report;
+	struct mutex acc_op_mutex;
+	atomic_t            delay; /*polling period for reporting input event*/
+	atomic_t            wake;  /*user-space request to wake-up, used with stop*/
+	struct timer_list   timer;  /* polling timer */
+	atomic_t            trace;
+
+	struct early_suspend    early_drv;
+	atomic_t                early_suspend;
+	//struct acc_drv_obj    drv_obj;
+	struct acc_data       drv_data;
+	int                   cali_sw[ACC_AXES_NUM+1];
+	struct acc_control_path   acc_ctl;
+	struct acc_data_path   acc_data;
+	bool			is_active_nodata;		// Active, but HAL don't need data sensor. such as orientation need
+	bool			is_active_data;		// Active and HAL need data .
+	bool is_first_data_after_enable;
+	bool is_polling_run;
+	bool is_batch_enable;	//version2.this is used for judging whether sensor is in batch mode
+};
+
+//driver API for internal  
+//extern int acc_enable_nodata(int enable);
+//extern int acc_attach(struct acc_drv_obj *obj);
+//driver API for third party vendor
+
+//for auto detect
+extern int acc_driver_add(struct acc_init_info* obj) ;
+extern int acc_data_report(int x, int y, int z,int status);
+extern int acc_register_control_path(struct acc_control_path *ctl);
+extern int acc_register_data_path(struct acc_data_path *data);
+
+
+
+
+
+#endif
diff --git a/drivers/misc/mediatek/accelerometer/inc/accel_factory.h b/drivers/misc/mediatek/accelerometer/inc/accel_factory.h
new file mode 100644
index 000000000..0a9fa1706
--- /dev/null
+++ b/drivers/misc/mediatek/accelerometer/inc/accel_factory.h
@@ -0,0 +1,39 @@
+#ifndef __ACC_FACTORY_H__
+#define __ACC_FACTORY_H__
+
+#include <linux/interrupt.h>
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include <linux/irq.h>
+#include <linux/miscdevice.h>
+#include <asm/uaccess.h>
+#include <linux/delay.h>
+#include <linux/input.h>
+#include <linux/workqueue.h>
+#include <linux/kobject.h>
+#include <linux/earlysuspend.h>
+#include <linux/platform_device.h>
+#include <asm/atomic.h>
+
+#include <cust_acc.h>
+#include <linux/hwmsensor.h>
+#include <linux/hwmsen_dev.h>
+#include <linux/sensors_io.h>
+#include <linux/hwmsen_helper.h>
+#include <linux/batch.h>
+
+#include <mach/mt_typedefs.h>
+#include <mach/mt_gpio.h>
+#include <mach/mt_pm_ldo.h>
+
+#include <accel.h>
+
+extern struct acc_context *acc_context_obj;
+
+#define SETCALI 1
+#define CLRCALI 2
+#define GETCALI 3
+
+int acc_factory_device_init(void);
+
+#endif
\ No newline at end of file
diff --git a/drivers/misc/mediatek/accelerometer/inc/cust_acc.h b/drivers/misc/mediatek/accelerometer/inc/cust_acc.h
new file mode 100644
index 000000000..66ffb3cfb
--- /dev/null
+++ b/drivers/misc/mediatek/accelerometer/inc/cust_acc.h
@@ -0,0 +1,22 @@
+#ifndef __CUST_ACC_H__
+#define __CUST_ACC_H__
+
+#include <linux/types.h>
+#define G_CUST_I2C_ADDR_NUM 2
+
+struct acc_hw {
+	int i2c_num;		/*!< the i2c bus used by the chip */
+	int direction;		/*!< the direction of the chip */
+	int power_id;		/*!< the VDD LDO ID of the chip, MT6516_POWER_NONE means the power is always on */
+	int power_vol;		/*!< the VDD Power Voltage used by the chip */
+	int firlen;		/*!< the length of low pass filter */
+	int (*power) (struct acc_hw *hw, unsigned int on, char *devname);
+	unsigned char i2c_addr[G_CUST_I2C_ADDR_NUM];	/*!< i2c address list,for chips which has different addresses with different HW layout */
+	int power_vio_id;	/*!< the VIO LDO ID of the chip, MT6516_POWER_NONE means the power is always on */
+	int power_vio_vol;	/*!< the VIO Power Voltage used by the chip */
+    bool is_batch_supported;
+};
+
+extern struct acc_hw *get_cust_acc_hw(void);
+struct acc_hw* get_accel_dts_func(const char *, struct acc_hw*);
+#endif
diff --git a/drivers/misc/mediatek/accelerometer/k2dh/Makefile b/drivers/misc/mediatek/accelerometer/k2dh/Makefile
new file mode 100755
index 000000000..b4a4c56df
--- /dev/null
+++ b/drivers/misc/mediatek/accelerometer/k2dh/Makefile
@@ -0,0 +1,4 @@
+include $(srctree)/drivers/misc/mediatek/Makefile.custom
+
+obj-y	:=  k2dh.o
+
diff --git a/drivers/misc/mediatek/accelerometer/k2dh/k2dh.c b/drivers/misc/mediatek/accelerometer/k2dh/k2dh.c
new file mode 100644
index 000000000..f732e0839
--- /dev/null
+++ b/drivers/misc/mediatek/accelerometer/k2dh/k2dh.c
@@ -0,0 +1,2689 @@
+/* K2DH motion sensor driver
+ *
+ *
+ * This software program is licensed subject to the GNU General Public License
+ * (GPL).Version 2,June 1991, available at http://www.fsf.org/copyleft/gpl.html
+
+ * (C) Copyright 2010 STMicroelectronics
+ * All Rights Reserved
+ */
+
+#include <linux/interrupt.h>
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include <linux/irq.h>
+#include <linux/miscdevice.h>
+#include <asm/uaccess.h>
+#include <linux/delay.h>
+#include <linux/input.h>
+#include <linux/workqueue.h>
+#include <linux/kobject.h>
+#include <linux/earlysuspend.h>
+#include <linux/platform_device.h>
+#include <asm/atomic.h>
+#include <linux/module.h>
+
+#define MT6582
+
+#ifdef MT6516
+#include <mach/mt6516_devs.h>
+#include <mach/mt6516_typedefs.h>
+#include <mach/mt6516_gpio.h>
+#include <mach/mt6516_pll.h>
+#endif
+
+#ifdef MT6573
+#include <mach/mt6573_devs.h>
+#include <mach/mt6573_typedefs.h>
+#include <mach/mt6573_gpio.h>
+#include <mach/mt6573_pll.h>
+#endif
+
+#ifdef MT6575
+#include <mach/mt_devs.h>
+#include <mach/mt_typedefs.h>
+#include <mach/mt_gpio.h>
+#include <mach/mt_pm_ldo.h>
+#endif
+
+#ifdef MT6577
+#include <mach/mt6577_devs.h>
+#include <mach/mt6577_typedefs.h>
+#include <mach/mt6577_gpio.h>
+#include <mach/mt6577_pm_ldo.h>
+#endif
+
+#ifdef MT6572
+#include <mach/devs.h>
+#include <mach/mt_typedefs.h>
+#include <mach/mt_gpio.h>
+#include <mach/mt_pm_ldo.h>
+#endif
+
+#ifdef MT6582
+#include <mach/devs.h>
+#include <mach/mt_typedefs.h>
+#include <mach/mt_gpio.h>
+#include <mach/mt_pm_ldo.h>
+#endif
+
+#ifdef MT6516
+#define POWER_NONE_MACRO MT6516_POWER_NONE
+#endif
+
+#ifdef MT6573
+#define POWER_NONE_MACRO MT65XX_POWER_NONE
+#endif
+
+#ifdef MT6575
+#define POWER_NONE_MACRO MT65XX_POWER_NONE
+#endif
+
+#ifdef MT6577
+#define POWER_NONE_MACRO MT65XX_POWER_NONE
+#endif
+
+#ifdef MT6572
+#define POWER_NONE_MACRO MT65XX_POWER_NONE
+#endif
+
+#ifdef MT6582
+#define POWER_NONE_MACRO MT65XX_POWER_NONE
+#endif
+
+
+#include <cust_acc.h>
+#include <linux/hwmsensor.h>
+#include <linux/hwmsen_dev.h>
+#include <linux/sensors_io.h>
+#include "k2dh.h"
+#include <linux/hwmsen_helper.h>
+
+/*----------------------------------------------------------------------------*/
+#define DEBUG 1
+/*----------------------------------------------------------------------------*/
+//#define CONFIG_K2DH_LOWPASS   /*apply low pass filter on output*/
+
+#define K2DH_AXIS_X             0
+#define K2DH_AXIS_Y             1
+#define K2DH_AXIS_Z             2
+#define K2DH_AXES_NUM      3
+#define K2DH_DATA_LEN       6
+
+#define K2DH_MODE_LOWPOWER                  0
+#define K2DH_MODE_NORMAL                        1
+#define K2DH_MODE_HIGH_RESOLUTION     2
+
+#define CALIBRATION_DATA_AMOUNT         10
+
+#define K2DH_ACCEL_CALIBRATION        // Accelerometer Sensor Calibration Enable Feature
+/*----------------------------------------------------------------------------*/
+#define HIGH_RESOLUTION                 0x08
+
+#define AXISDATA_REG                        0x28
+#define WHOAMI_K2DH_ACC               0x33    /*  Expctd content for WAI    */
+
+/*  CONTROL REGISTERS    */
+#define WHO_AM_I                                0x0F    /*    WhoAmI register        */
+#define TEMP_CFG_REG                      0x1F    /*    temper sens control reg    */
+/* ctrl 1: ODR3 ODR2 ODR ODR0 LPen Zenable Yenable Zenable */
+#define CTRL_REG1                              0x20    /*    control reg 1        */
+#define CTRL_REG2                              0x21    /*    control reg 2        */
+#define CTRL_REG3                              0x22    /*    control reg 3        */
+#define CTRL_REG4                              0x23    /*    control reg 4        */
+#define CTRL_REG5                              0x24    /*    control reg 5        */
+#define CTRL_REG6                              0x25    /*    control reg 6        */
+
+#define FIFO_CTRL_REG                      0x2E    /*    FiFo control reg    */
+
+#define INT_CFG1                                 0x30    /*    interrupt 1 config    */
+#define INT_SRC1                                 0x31    /*    interrupt 1 source    */
+#define INT_THS1                                  0x32    /*    interrupt 1 threshold    */
+#define INT_DUR1                                 0x33    /*    interrupt 1 duration    */
+
+#define INT_CFG2                                 0x34    /*    interrupt 2 config    */
+#define INT_SRC2                                 0x35    /*    interrupt 2 source    */
+#define INT_THS2                                 0x36    /*    interrupt 2 threshold    */
+#define INT_DUR2                                 0x37    /*    interrupt 2 duration    */
+
+#define TT_CFG                                    0x38    /*    tap config        */
+#define TT_SRC                                    0x39    /*    tap source        */
+#define TT_THS                                    0x3A    /*    tap threshold        */
+#define TT_LIM                                     0x3B    /*    tap time limit        */
+#define TT_TLAT                                   0x3C    /*    tap time latency    */
+#define TT_TW                                      0x3D    /*    tap time window        */
+/*    end CONTROL REGISTRES    */
+
+#define K2DH_ACC_ENABLE_ALL_AXES              0x07
+#define K2DH_ACC_ENABLE_ALL_AXES_MSK    0x07
+
+#define ODR_POWERDOWN_MODE                0x00  /* Power Down Mode */
+#define ODR1                0x10  /* 1Hz output data rate */
+#define ODR10              0x20  /* 10Hz output data rate */
+#define ODR25              0x30  /* 25Hz output data rate */
+#define ODR50              0x40  /* 50Hz output data rate */
+#define ODR100            0x50  /* 100Hz output data rate */
+#define ODR200            0x60  /* 200Hz output data rate */  // default speed
+#define ODR400            0x70  /* 400Hz output data rate */
+#define ODR1344          0x90  /* 1344Hz output data rate */
+
+#define I2C_RETRY_DELAY            5
+#define I2C_RETRIES                     5
+#define I2C_AUTO_INCREMENT    0x80
+
+#define K2DH_LP_EN_MSK      0x08
+#define K2DH_LP_EN_POS      3
+#define K2DH_LP_EN_REG      CTRL_REG1
+
+#define K2DH_HR_MSK             0x08
+#define K2DH_HR_POS             3
+#define K2DH_HR_REG             CTRL_REG4
+
+#define K2DH_FS_MSK             0x30
+#define K2DH_FS_POS             4
+#define K2DH_FS_REG             CTRL_REG4
+
+#define K2DH_ODR_MSK          0xF0
+#define K2DH_ODR_POS          4
+#define K2DH_ODR_REG          CTRL_REG1
+
+#ifdef K2DH_ACCEL_CALIBRATION
+#define K2DH_SHAKING_DETECT_THRESHOLD    (200)     //clubsh cal2 50 -> 200
+
+struct K2DHacc{
+    s16    x,
+        y,
+        z;
+} ;
+#endif
+
+#define SELF_TEST_2G_MAX_LSB        (360)
+#define SELF_TEST_2G_MIN_LSB        (17)
+#define TESTLIMIT_XY                            (175)
+#define TESTLIMIT_Z_USL_LSB             (1270)
+#define TESTLIMIT_Z_LSL_LSB             (778)
+
+#define READ_DATA_CHECK_CNT         (20)
+
+/*----------------------------------------------------------------------------*/
+static const struct i2c_device_id k2dh_i2c_id[] = {{K2DH_ACC_DEV_NAME,0},{}};
+static struct i2c_board_info __initdata k2dh_i2c_info ={ I2C_BOARD_INFO(K2DH_ACC_DEV_NAME, K2DH_ACC_I2C_SAD_L)};
+
+/*----------------------------------------------------------------------------*/
+static int k2dh_i2c_probe(struct i2c_client *client, const struct i2c_device_id *id);
+static int k2dh_i2c_remove(struct i2c_client *client);
+
+/*----------------------------------------------------------------------------*/
+typedef enum {
+    K2DH_TRC_FILTER  = 0x01,
+    K2DH_TRC_RAWDATA = 0x02,
+    K2DH_TRC_IOCTL   = 0x04,
+    K2DH_TRC_CALI    = 0X08,
+    K2DH_TRC_INFO    = 0X10,
+} K2DH_TRC;
+/*----------------------------------------------------------------------------*/
+struct scale_factor{
+    u8  whole;
+    u8  fraction;
+};
+/*----------------------------------------------------------------------------*/
+struct data_resolution {
+    struct scale_factor scalefactor;
+    int                 sensitivity;
+};
+/*----------------------------------------------------------------------------*/
+#ifdef CONFIG_K2DH_LOWPASS
+#define C_MAX_FIR_LENGTH (32)
+
+struct data_filter {
+    s16 raw[C_MAX_FIR_LENGTH][K2DH_AXES_NUM];
+    int sum[K2DH_AXES_NUM];
+    int num;
+    int idx;
+};
+#endif
+/*----------------------------------------------------------------------------*/
+struct k2dh_i2c_data {
+    struct i2c_client *client;
+    struct acc_hw *hw;
+    struct hwmsen_convert   cvt;
+
+    /*misc*/
+    struct data_resolution *reso;
+    atomic_t                trace;
+    atomic_t                suspend;
+    atomic_t                selftest_rslt;
+    atomic_t                filter;
+    s16                     cali_sw[K2DH_AXES_NUM+1];
+
+    /*data*/
+    //s8                      offset[K2DH_AXES_NUM+1];  /*+1: for 4-byte alignment*/
+    s16                     data[K2DH_AXES_NUM+1];
+
+#if defined(CONFIG_K2DH_LOWPASS)
+    atomic_t                firlen;
+    atomic_t                fir_en;
+    struct data_filter      fir;
+#endif
+    /*early suspend*/
+#if defined(CONFIG_HAS_EARLYSUSPEND)
+    struct early_suspend    early_drv;
+#endif
+
+#ifdef K2DH_ACCEL_CALIBRATION
+    atomic_t fast_calib_x_rslt;
+    atomic_t fast_calib_y_rslt;
+    atomic_t fast_calib_z_rslt;
+    atomic_t fast_calib_rslt;
+#endif
+};
+/*----------------------------------------------------------------------------*/
+static struct i2c_driver k2dh_i2c_driver = {
+    .driver = {
+        .name           = K2DH_ACC_DEV_NAME,
+    },
+    .probe              = k2dh_i2c_probe,
+    .remove                = k2dh_i2c_remove,
+#if !defined(CONFIG_HAS_EARLYSUSPEND)
+    .suspend            = k2dh_suspend,
+    .resume             = k2dh_resume,
+#endif
+    .id_table = k2dh_i2c_id,
+};
+
+/*----------------------------------------------------------------------------*/
+static struct i2c_client *k2dh_i2c_client = NULL;
+static struct platform_driver k2dh_gsensor_driver;
+static struct k2dh_i2c_data *obj_i2c_data = NULL;
+static bool sensor_power = true;
+static int test_status = 0;
+static UINT32 data_read_count = 0;
+static GSENSOR_VECTOR3D gsensor_gain;
+
+/*----------------------------------------------------------------------------*/
+#define GSE_TAG                  "[Gsensor] "
+#define GSE_FUN(f)               printk(KERN_ERR GSE_TAG"%s()\n", __FUNCTION__)
+#define GSE_ERR(fmt, args...)    printk(KERN_ERR GSE_TAG"[ERROR] %s() line=%d : "fmt, __FUNCTION__, __LINE__, ##args)
+#define GSE_LOG(fmt, args...)    printk(KERN_ERR GSE_TAG fmt, ##args)
+/*----------------------------------------------------------------------------*/
+static struct data_resolution K2DH_data_resolution[1] = {
+ /* combination by {FULL_RES,RANGE}*/
+    {{ 1, 0}, 1024},   // dataformat +/-2g  in 12-bit resolution;  { 1, 0} = 1.0= (2*2*1000)/(2^12);  1024 = (2^12)/(2*2)
+};
+/*--------------------K2DH power control function----------------------------------*/
+static void k2dh_power(struct acc_hw *hw, unsigned int on)
+{
+    static unsigned int power_on = 0;
+
+    //if(hw->power_id != POWER_NONE_MACRO)        // have externel LDO
+    {
+        GSE_LOG("power %s\n", on ? "on" : "off");
+        if(power_on == on)    // power status not change
+        {
+            GSE_LOG("ignore power control: %d\n", on);
+        }
+        else if(on)    // power on
+        {
+            if(!(hw->power(hw, true, "K2DH")))
+            {
+                GSE_ERR("power on fails!!\n");
+            }
+        }
+        else    // power off
+        {
+            if(!(hw->power(hw, false, "K2DH")))
+            {
+                GSE_ERR("power off fail!!\n");
+            }
+        }
+    }
+    power_on = on;
+}
+
+/*----------------------------------------------------------------------------*/
+static int k2dh_SetDataResolution(struct k2dh_i2c_data *obj)
+{
+/*set g sensor dataresolution here*/
+/*end of set dataresolution*/
+
+    obj->reso = &K2DH_data_resolution[0];
+    return 0;
+
+/*if you changed the measure range, for example call: K2DH_SetDataFormat(client, K2DH_ACC_G_4G),
+you must set the right value to K2DH_data_resolution*/
+}
+
+/*----------------------------------------------------------------------------*/
+static int k2dh_ResetCalibration(struct i2c_client *client)
+{
+    struct k2dh_i2c_data *obj = i2c_get_clientdata(client);
+    u8 ofs[4]={0,0,0,0};
+    int err = 0;
+
+    memset(obj->cali_sw, 0x00, sizeof(obj->cali_sw));
+        //memset(obj->offset, 0x00, sizeof(obj->offset));
+
+    return err;
+}
+
+static int k2dh_acc_i2c_read(struct i2c_client *client, u8 * buf, int len)
+{
+    int err = -1;
+    int tries = 0;
+
+    struct i2c_msg    msgs[] = {
+        {
+            .addr = client->addr,
+            .flags = client->flags & I2C_M_TEN,
+            .len = 1,
+            .buf = buf, },
+        {
+            .addr = client->addr,
+            .flags = (client->flags & I2C_M_TEN) | I2C_M_RD,
+            .len = len,
+            .buf = buf, },
+    };
+
+#if 1 // SENSOR_I2C_FIX
+	client->adapter->retries = I2C_RETRIES;
+	err = i2c_transfer(client->adapter, msgs, 2);
+#else
+    do {
+        err = i2c_transfer(client->adapter, msgs, 2);
+        if (err != 2)
+            msleep_interruptible(I2C_RETRY_DELAY);
+    } while ((err != 2) && (++tries < I2C_RETRIES));
+#endif
+
+    if (err != 2) {
+        GSE_ERR("read transfer error: %d\n", err);
+        err = -EIO;
+    } else {
+        err = 0;
+    }
+
+    return err;
+}
+
+static int k2dh_acc_i2c_write(struct i2c_client *client, u8 * buf, int len)
+{
+    int err = -1;
+    int tries = 0;
+
+    struct i2c_msg msgs[] = { { .addr = client->addr,
+            .flags = client->flags & I2C_M_TEN,
+            .len = len + 1, .buf = buf, }, };
+
+#if 1 // SENSOR_I2C_FIX
+	client->adapter->retries = I2C_RETRIES;
+	err = i2c_transfer(client->adapter, msgs, 1);
+#else
+    do {
+        err = i2c_transfer(client->adapter, msgs, 1);
+        if (err != 1)
+            msleep_interruptible(I2C_RETRY_DELAY);
+    } while ((err != 1) && (++tries < I2C_RETRIES));
+#endif
+
+    if (err != 1) {
+        GSE_ERR("write transfer error: %d\n", err);
+        err = -EIO;
+    } else {
+        err = 0;
+    }
+
+    return err;
+}
+
+static int k2dh_acc_register_write(struct i2c_client *client, u8 *buf, u8 reg_address, u8 new_value)
+{
+    int err = -1;
+
+    buf[0] = reg_address;
+    buf[1] = new_value;
+
+    err = k2dh_acc_i2c_write(client, buf, 1);
+    if(err != K2DH_SUCCESS)
+    {
+        return err;
+    }
+    return err;
+}
+
+static int k2dh_acc_register_read(struct i2c_client *client, u8 *buf, u8 reg_address)
+{
+    int err = -1;
+    buf[0] = (reg_address);
+    err = k2dh_acc_i2c_read(client, buf, 1);
+    return err;
+}
+
+static int k2dh_acc_register_update(struct i2c_client *client, u8 *buf, u8 reg_address, u8 mask, u8 new_bit_values)
+{
+    int err = -1;
+    u8 init_val;
+    u8 updated_val;
+
+    err = k2dh_acc_register_read(client, buf, reg_address);
+    if(err == K2DH_SUCCESS)
+    {
+        init_val = buf[0];
+        updated_val = ((mask & new_bit_values) | ((~mask) & init_val));
+        err = k2dh_acc_register_write(client, buf, reg_address, updated_val);
+    }
+    return err;
+}
+
+static int k2dh_CheckDeviceID(struct i2c_client *client)
+{
+    u8 databuf[2] = {0,0};
+    int res = -1;
+
+    GSE_FUN();
+
+    memset(databuf, 0, sizeof(u8)*2);
+    databuf[0] = WHO_AM_I;
+
+    res = k2dh_acc_i2c_read(client, databuf, 1);
+    if(res != K2DH_SUCCESS)
+    {
+        res = k2dh_acc_i2c_read(client, databuf, 1);
+        if(res != K2DH_SUCCESS)
+        {
+            goto exit_k2dh_CheckDeviceID;
+        }
+    }
+
+    //udelay(500);
+
+    if(databuf[0] != WHOAMI_K2DH_ACC)
+    {
+        GSE_ERR("device unknown. Expected: 0x%x, Replies: 0x%x\n", WHOAMI_K2DH_ACC, databuf[0]);
+        return K2DH_ERR_IDENTIFICATION;
+    }
+    else
+    {
+        GSE_LOG("k2dh_CheckDeviceID %d pass! \n", databuf[0]);
+    }
+
+    exit_k2dh_CheckDeviceID:
+    if(res != K2DH_SUCCESS)
+    {
+        return K2DH_ERR_I2C;
+    }
+
+    return K2DH_SUCCESS;
+}
+
+/*----------------------------------------------------------------------------*/
+static int k2dh_SetBWRate(struct i2c_client *client, u8 bwrate)
+{  // bwrate : ODRxxx
+    int res = -1;
+    u8 databuf[2] = {0,0};
+
+    u8 new_val = bwrate | K2DH_ACC_ENABLE_ALL_AXES;
+    u8 mask = K2DH_ODR_MSK | K2DH_ACC_ENABLE_ALL_AXES_MSK;
+
+    res = k2dh_acc_register_update(client, databuf, CTRL_REG1, mask, new_val);
+    if(res != K2DH_SUCCESS)
+    {
+        GSE_ERR("update odr failed, 0x%x, 0x%x : %d\n", databuf[0], databuf[1], res);
+        return K2DH_ERR_I2C;
+    }
+    else
+    {
+        GSE_LOG("Set BWrate : 0x%02x\n", bwrate);
+    }
+
+    return K2DH_SUCCESS;
+}
+
+/*----------------------------------------------------------------------------*/
+static int k2dh_SetPowerMode(struct i2c_client *client, bool enable)
+{  // power mode controled by operating Bandwidth(ODR-Hz)
+    GSE_FUN();
+
+    int res = -1;
+    struct k2dh_i2c_data *obj = i2c_get_clientdata(client);
+
+    if(enable == sensor_power )
+    {
+        GSE_LOG("Sensor power status is newest!\n");
+        return K2DH_SUCCESS;
+    }
+
+    if(enable == TRUE)
+    {
+        res = k2dh_SetBWRate(client, ODR200);  // operating mode as 200HZ
+    }
+    else
+    {
+        res = k2dh_SetBWRate(client, ODR_POWERDOWN_MODE);  // power down mode
+    }
+
+    if(res != K2DH_SUCCESS)
+    {
+        GSE_LOG("set power mode failed!\n");
+        return K2DH_ERR_I2C;
+    }
+    else
+    {
+        GSE_LOG("set power mode ok: ");
+    }
+
+    sensor_power = enable;
+    test_status = sensor_power;
+
+    //mdelay(20);
+
+    return K2DH_SUCCESS;
+}
+
+/*----------------------------------------------------------------------------*/
+static int k2dh_SetDataFormat(struct i2c_client *client, u8 dataformat)
+{  // dataformat : K2DH_ACC_G_2G . K2DH_ACC_G_4G. K2DH_ACC_G_8G. K2DH_ACC_G_16G
+
+    int res = -1;
+
+    u8 buf[2] = {0,0};
+    u8 updated_val = 0;
+    u8 init_val = 0;
+    u8 new_val = 0;
+    u8 mask = K2DH_ACC_FS_MASK | HIGH_RESOLUTION;
+
+    struct k2dh_i2c_data *obj = i2c_get_clientdata(client);
+
+
+    buf[0] = CTRL_REG4;
+    res = k2dh_acc_i2c_read(client, buf, 1);
+    if(res != K2DH_SUCCESS)
+    {
+        goto error;
+    }
+    else
+    {
+        init_val = buf[0];
+        new_val = dataformat | HIGH_RESOLUTION;
+        updated_val = ((mask & new_val) | ((~mask) & init_val));
+        buf[1] = updated_val;
+        buf[0] = CTRL_REG4;
+        res = k2dh_acc_i2c_write(client, buf, 1);
+        if(res != K2DH_SUCCESS)
+        {
+            goto error;
+        }
+        else
+        {
+            GSE_LOG("Set DataFormat: 0x%02x\n", dataformat);
+        }
+    }
+
+    return k2dh_SetDataResolution(obj);
+
+error:
+    GSE_ERR("update g range failed 0x%x, 0x%x: %d\n", buf[0], buf[1], res);
+
+    return res;
+}
+
+/*----------------------------------------------------------------------------*/
+static int k2dh_SetIntEnable(struct i2c_client *client, u8 intenable)
+{
+    int res = -1;
+    u8 databuf[2] = {0, 0};
+
+    databuf[1] = 0x00;
+    databuf[0] = INT_CFG1;
+
+    res = k2dh_acc_i2c_read(client, databuf, 1);
+    if(res != K2DH_SUCCESS)
+    {
+        GSE_ERR("Interrupt configuration failed, 0x%x, 0x%x : %d\n", databuf[0], databuf[1], res);
+        return K2DH_ERR_I2C;
+    }
+
+    databuf[0] = INT_CFG2;
+    res = k2dh_acc_i2c_read(client, databuf, 1);
+    if(res != K2DH_SUCCESS)
+    {
+        GSE_ERR("Interrupt configuration failed, 0x%x, 0x%x : %d\n", databuf[0], databuf[1], res);
+        return K2DH_ERR_I2C;
+    }
+    GSE_LOG("K2DH Interrupt was configured\n");
+
+    return K2DH_SUCCESS;
+}
+
+
+static int k2dh_init_client(struct i2c_client *client)
+{
+    int err = -1;
+    u8 buf[7] = {0};
+
+    buf[0] = CTRL_REG1;
+    buf[1] = K2DH_ACC_ENABLE_ALL_AXES;
+    err = k2dh_acc_i2c_write(client, buf, 1);
+    if (err < 0)
+        goto error;
+
+    buf[0] = TEMP_CFG_REG;
+    buf[1] = 0x00;
+    err = k2dh_acc_i2c_write(client, buf, 1);
+    if (err < 0)
+        goto error;
+
+    buf[0] = FIFO_CTRL_REG;
+    buf[1] = 0x00;
+    err = k2dh_acc_i2c_write(client, buf, 1);
+    if (err < 0)
+        goto error;
+
+    buf[0] = (I2C_AUTO_INCREMENT | TT_THS);
+    buf[1] = 0x00;
+    buf[2] = 0x00;
+    buf[3] = 0x00;
+    buf[4] = 0x00;
+    err = k2dh_acc_i2c_write(client, buf, 4);
+    if (err < 0)
+        goto error;
+    buf[0] = TT_CFG;
+    buf[1] = 0x00;
+    err = k2dh_acc_i2c_write(client, buf, 1);
+    if (err < 0)
+        goto error;
+
+    buf[0] = (I2C_AUTO_INCREMENT | INT_THS1);
+    buf[1] = 0x00;
+    buf[2] = 0x00;
+    err = k2dh_acc_i2c_write(client, buf, 2);
+    if (err < 0)
+        goto error;
+    buf[0] = INT_CFG1;
+    buf[1] = 0x00;
+    err = k2dh_acc_i2c_write(client, buf, 1);
+    if (err < 0)
+        goto error;
+
+    buf[0] = (I2C_AUTO_INCREMENT | INT_THS2);
+    buf[1] = 0x00;
+    buf[2] = 0x00;
+    err = k2dh_acc_i2c_write(client, buf, 2);
+    if (err < 0)
+        goto error;
+    buf[0] = INT_CFG2;
+    buf[1] = 0x00;
+    err = k2dh_acc_i2c_write(client, buf, 1);
+    if (err < 0)
+        goto error;
+
+    buf[0] = (I2C_AUTO_INCREMENT | CTRL_REG2);
+    buf[1] = 0x00;
+    buf[2] = 0x00;
+    buf[3] = 0x00;
+    buf[4] = 0x00;
+    buf[5] = 0x00;
+    err = k2dh_acc_i2c_write(client, buf, 5);
+    if (err < 0)
+        goto error;
+
+        GSE_LOG("k2dh_init_client : hw init done\n");
+        return K2DH_SUCCESS;
+
+error:
+    GSE_ERR("hw init error 0x%x, 0x%x: %d\n", buf[0], buf[1], err);
+
+    return err;
+}
+
+static int k2dh_acc_hw_init(struct i2c_client *client, int reset_cali)
+{
+    int res = -1;
+    struct k2dh_i2c_data *obj = i2c_get_clientdata(client);
+
+    GSE_FUN();
+
+    res = k2dh_CheckDeviceID(client);
+    if(res != K2DH_SUCCESS)
+    {
+            return res;
+    }
+
+    res = k2dh_init_client(client);
+    if(res != K2DH_SUCCESS)
+    {
+        return res;
+    }
+
+    res = k2dh_SetBWRate(client, ODR200);
+    if(res != K2DH_SUCCESS )
+    {
+        return res;
+    }
+
+    res = k2dh_SetDataFormat(client, K2DH_ACC_G_2G);
+    if(res != K2DH_SUCCESS)
+    {
+        return res;
+    }
+
+    gsensor_gain.x = gsensor_gain.y = gsensor_gain.z = obj->reso->sensitivity;
+
+    res = k2dh_SetIntEnable(client, 0x00);  // interrupt disable
+    if(res != K2DH_SUCCESS)
+    {
+        return res;
+    }
+
+    res = k2dh_SetPowerMode(client, false);
+    if(res != K2DH_SUCCESS)
+    {
+        return res;
+    }
+
+    if(0 != reset_cali)
+    {
+        /*reset calibration only in power on*/
+        res = k2dh_ResetCalibration(client);
+        if(res != K2DH_SUCCESS)
+        {
+            return res;
+        }
+    }
+
+#ifdef CONFIG_K2DH_LOWPASS
+    memset(&obj->fir, 0x00, sizeof(obj->fir));
+#endif
+
+    //mdelay(20);
+
+    return K2DH_SUCCESS;
+}
+/*----------------------------------------------------------------------------*/
+static int k2dh_ReadChipInfo(struct i2c_client *client, char *buf, int bufsize)
+{
+    u8 databuf[10] = {0};
+
+    memset(databuf, 0, sizeof(u8)*10);
+
+    if((NULL == buf)||(bufsize<=30))
+    {
+        return -1;
+    }
+
+    if(NULL == client)
+    {
+        *buf = 0;
+        return -2;
+    }
+
+    sprintf(buf, "K2DH Chip");
+    return 0;
+}
+/*----------------------------------------------------------------------------*/
+
+
+/*----------------------------------------------------------------------------*/
+#if 1
+static s16 prev_data[K2DH_AXES_NUM] = {0,0,0};
+static s16 check_cnt = 0;
+#endif
+
+static int k2dh_ReadData(struct i2c_client *client, s16 data[K2DH_AXES_NUM])
+{
+    struct k2dh_i2c_data *priv = i2c_get_clientdata(client);
+    u8 acc_data[K2DH_DATA_LEN] = {0};
+    int err = -1;
+
+    acc_data[0] = (I2C_AUTO_INCREMENT | AXISDATA_REG);
+
+    if(NULL == client)
+    {
+        err = -EINVAL;
+    }
+    else if((err = k2dh_acc_i2c_read(client, acc_data, K2DH_DATA_LEN)) != K2DH_SUCCESS)
+    {
+        GSE_ERR("I2C read error: %d\n", err);
+    }
+    else
+    {
+        /* Convert sensor raw data to 16-bit integer */
+        data[K2DH_AXIS_X] = (((s16) ((acc_data[1] << 8) | acc_data[0])) >> 4);
+        data[K2DH_AXIS_Y] = (((s16) ((acc_data[3] << 8) | acc_data[2])) >> 4);
+        data[K2DH_AXIS_Z] = (((s16) ((acc_data[5] << 8) | acc_data[4])) >> 4);
+
+#if 1
+        if((prev_data[K2DH_AXIS_X] == data[K2DH_AXIS_X]) && (prev_data[K2DH_AXIS_Y] == data[K2DH_AXIS_Y]) && (prev_data[K2DH_AXIS_Z] == data[K2DH_AXIS_Z]))
+        {
+            check_cnt++;
+            if(check_cnt >= READ_DATA_CHECK_CNT)  // read data same case
+            {
+                GSE_ERR("READ DATA SAME CASE : (%d %d %d)\n", data[K2DH_AXIS_X], data[K2DH_AXIS_Y],data[K2DH_AXIS_Z]);
+
+                // Try recovery the malfunction by change Power Down mode and ODR200
+                k2dh_SetBWRate(client, ODR_POWERDOWN_MODE);  // power down mode
+                msleep(100);                
+                k2dh_SetBWRate(client, ODR200);  // operating mode as 200HZ                
+                check_cnt = 0;
+            }
+        }
+        else
+        {
+            prev_data[K2DH_AXIS_X] =data[K2DH_AXIS_X];
+            prev_data[K2DH_AXIS_Y] =data[K2DH_AXIS_Y];
+            prev_data[K2DH_AXIS_Z] =data[K2DH_AXIS_Z];
+            check_cnt = 0;            
+        }
+#endif
+
+        if(atomic_read(&priv->trace) & K2DH_TRC_RAWDATA)
+        {
+            GSE_LOG("gsensor raw data: x=%d, y=%d, z=%d!\n", data[K2DH_AXIS_X], data[K2DH_AXIS_Y], data[K2DH_AXIS_Z]);
+        }
+#ifdef CONFIG_K2DH_LOWPASS
+        if(atomic_read(&priv->filter))
+        {
+            if(atomic_read(&priv->fir_en) && !atomic_read(&priv->suspend))
+            {
+                int idx, firlen = atomic_read(&priv->firlen);
+                if(priv->fir.num < firlen)
+                {
+                    priv->fir.raw[priv->fir.num][K2DH_AXIS_X] = data[K2DH_AXIS_X];
+                    priv->fir.raw[priv->fir.num][K2DH_AXIS_Y] = data[K2DH_AXIS_Y];
+                    priv->fir.raw[priv->fir.num][K2DH_AXIS_Z] = data[K2DH_AXIS_Z];
+                    priv->fir.sum[K2DH_AXIS_X] += data[K2DH_AXIS_X];
+                    priv->fir.sum[K2DH_AXIS_Y] += data[K2DH_AXIS_Y];
+                    priv->fir.sum[K2DH_AXIS_Z] += data[K2DH_AXIS_Z];
+                    if(atomic_read(&priv->trace) & K2DH_TRC_FILTER)
+                    {
+                        GSE_LOG("add [%2d] [%5d %5d %5d] => [%5d %5d %5d]\n", priv->fir.num,
+                            priv->fir.raw[priv->fir.num][K2DH_AXIS_X], priv->fir.raw[priv->fir.num][K2DH_AXIS_Y], priv->fir.raw[priv->fir.num][K2DH_AXIS_Z],
+                            priv->fir.sum[K2DH_AXIS_X], priv->fir.sum[K2DH_AXIS_Y], priv->fir.sum[K2DH_AXIS_Z]);
+                    }
+                    priv->fir.num++;
+                    priv->fir.idx++;
+                }
+                else
+                {
+                    idx = priv->fir.idx % firlen;
+                    priv->fir.sum[K2DH_AXIS_X] -= priv->fir.raw[idx][K2DH_AXIS_X];
+                    priv->fir.sum[K2DH_AXIS_Y] -= priv->fir.raw[idx][K2DH_AXIS_Y];
+                    priv->fir.sum[K2DH_AXIS_Z] -= priv->fir.raw[idx][K2DH_AXIS_Z];
+                    priv->fir.raw[idx][K2DH_AXIS_X] = data[K2DH_AXIS_X];
+                    priv->fir.raw[idx][K2DH_AXIS_Y] = data[K2DH_AXIS_Y];
+                    priv->fir.raw[idx][K2DH_AXIS_Z] = data[K2DH_AXIS_Z];
+                    priv->fir.sum[K2DH_AXIS_X] += data[K2DH_AXIS_X];
+                    priv->fir.sum[K2DH_AXIS_Y] += data[K2DH_AXIS_Y];
+                    priv->fir.sum[K2DH_AXIS_Z] += data[K2DH_AXIS_Z];
+                    priv->fir.idx++;
+                    data[K2DH_AXIS_X] = priv->fir.sum[K2DH_AXIS_X]/firlen;
+                    data[K2DH_AXIS_Y] = priv->fir.sum[K2DH_AXIS_Y]/firlen;
+                    data[K2DH_AXIS_Z] = priv->fir.sum[K2DH_AXIS_Z]/firlen;
+                    if(atomic_read(&priv->trace) & K2DH_TRC_FILTER)
+                    {
+                        GSE_LOG("add [%2d] [%5d %5d %5d] => [%5d %5d %5d] : [%5d %5d %5d]\n", idx,
+                        priv->fir.raw[idx][K2DH_AXIS_X], priv->fir.raw[idx][K2DH_AXIS_Y], priv->fir.raw[idx][K2DH_AXIS_Z],
+                        priv->fir.sum[K2DH_AXIS_X], priv->fir.sum[K2DH_AXIS_Y], priv->fir.sum[K2DH_AXIS_Z],
+                        data[K2DH_AXIS_X], data[K2DH_AXIS_Y], data[K2DH_AXIS_Z]);
+                    }
+                }
+            }
+        }
+#endif
+    }
+    return err;
+}
+ /*----------------------------------------------------------------------------*/
+static int k2dh_ReadSensorData(struct i2c_client *client, char *buf, int bufsize)
+{
+    struct k2dh_i2c_data *obj = (struct k2dh_i2c_data*)i2c_get_clientdata(client);
+    u8 databuf[20] = {0};
+    int acc[K2DH_AXES_NUM] = {0};
+    int res = -1;
+    memset(databuf, 0, sizeof(u8)*10);
+
+    if(NULL == buf)
+    {
+        return -1;
+    }
+    if(NULL == client)
+    {
+        *buf = 0;
+        return -2;
+    }
+
+    if(sensor_power == FALSE)
+    {
+        res = k2dh_SetPowerMode(client, true);
+        if(res)
+        {
+            GSE_ERR("Power on K2DH error %d!\n", res);
+        }
+    }
+
+    if(res = k2dh_ReadData(client, obj->data))
+    {
+        GSE_ERR("I2C error: ret value=%d", res);
+        return -3;
+    }
+    else
+    {
+        data_read_count++;
+        if(data_read_count >= 2147483647)
+        {
+            data_read_count =0;
+        }
+        
+        obj->data[K2DH_AXIS_X] -= obj->cali_sw[K2DH_AXIS_X];
+        obj->data[K2DH_AXIS_Y] -= obj->cali_sw[K2DH_AXIS_Y];
+        obj->data[K2DH_AXIS_Z] -= obj->cali_sw[K2DH_AXIS_Z];
+
+        //printk("cali_sw x=%d, y=%d, z=%d \n",obj->cali_sw[K2DH_AXIS_X],obj->cali_sw[K2DH_AXIS_Y],obj->cali_sw[K2DH_AXIS_Z]);
+
+        /*remap coordinate*/
+        acc[obj->cvt.map[K2DH_AXIS_X]] = obj->cvt.sign[K2DH_AXIS_X]*obj->data[K2DH_AXIS_X];
+        acc[obj->cvt.map[K2DH_AXIS_Y]] = obj->cvt.sign[K2DH_AXIS_Y]*obj->data[K2DH_AXIS_Y];
+        acc[obj->cvt.map[K2DH_AXIS_Z]] = obj->cvt.sign[K2DH_AXIS_Z]*obj->data[K2DH_AXIS_Z];
+        //printk("cvt x=%d, y=%d, z=%d \n",obj->cvt.sign[K2DH_AXIS_X],obj->cvt.sign[K2DH_AXIS_Y],obj->cvt.sign[K2DH_AXIS_Z]);
+
+        if(atomic_read(&obj->trace) & K2DH_TRC_RAWDATA)
+        {
+            //GSE_LOG("Mapped gsensor data: x=%d, y=%d, z=%d!\n", acc[K2DH_AXIS_X], acc[K2DH_AXIS_Y], acc[K2DH_AXIS_Z]);
+        }
+
+        //Out put the mg
+        //printk("mg acc=%d, GRAVITY=%d, sensityvity=%d \n",acc[K2DH_AXIS_X],GRAVITY_EARTH_1000,obj->reso->sensitivity);
+        acc[K2DH_AXIS_X] = acc[K2DH_AXIS_X] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+        acc[K2DH_AXIS_Y] = acc[K2DH_AXIS_Y] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+        acc[K2DH_AXIS_Z] = acc[K2DH_AXIS_Z] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+
+        sprintf(buf, "%04x %04x %04x %x", acc[K2DH_AXIS_X], acc[K2DH_AXIS_Y], acc[K2DH_AXIS_Z], data_read_count);
+        if(atomic_read(&obj->trace) & K2DH_TRC_IOCTL)
+        {
+            GSE_LOG("gsensor data: x=%d, y=%d, z=%d!\n", acc[K2DH_AXIS_X], acc[K2DH_AXIS_Y], acc[K2DH_AXIS_Z]);
+        }
+    }
+
+    return 0;
+}
+
+static int k2dh_read_accel_raw_data(struct k2dh_acc *acc)
+{
+        int err;
+        unsigned char acc_data[6] = {0};
+        s16 temp;
+
+        acc_data[0] = (I2C_AUTO_INCREMENT | AXISDATA_REG);
+}
+
+static int k2dh_do_calibration(void)
+{
+        struct K2DHacc acc_cal = {0, };
+        struct K2DHacc acc_cal_pre = {0, };
+        int sum[3] = {0, };
+        int err = 0;
+        int i;
+
+        struct i2c_client *client = k2dh_i2c_client;
+        struct k2dh_i2c_data *obj = i2c_get_clientdata(client);
+
+        err = k2dh_ReadData(client, &acc_cal_pre);
+        if(err < 0)
+        {
+            GSE_ERR("%s : k2dh_ReadData() failed\n", __func__);
+            return err;
+        }
+
+        for(i = 0; i < CALIBRATION_DATA_AMOUNT; i++)
+        {
+            mdelay(20);
+            err = k2dh_ReadData(client, &acc_cal);
+            if(err < 0)
+            {
+                GSE_ERR("%s : k2dh_ReadData() failed in the %dth loop\n", __func__, i);
+                return err;
+            }
+
+            GSE_LOG(KERN_INFO "===============moved x=============== timeout = %d\n", i);
+            GSE_LOG(KERN_INFO "(%d, %d, %d) (%d, %d, %d)\n", acc_cal_pre.x, acc_cal_pre.y,  acc_cal_pre.z, acc_cal.x,acc_cal.y,acc_cal.z );
+
+            if((abs(acc_cal.x - acc_cal_pre.x) > K2DH_SHAKING_DETECT_THRESHOLD)
+                || (abs((acc_cal.y - acc_cal_pre.y)) > K2DH_SHAKING_DETECT_THRESHOLD)
+                || (abs((acc_cal.z - acc_cal_pre.z)) > K2DH_SHAKING_DETECT_THRESHOLD))
+            {
+                atomic_set(&obj->fast_calib_rslt, 0);
+                GSE_LOG(KERN_INFO "===============shaking x===============\n");
+                return -EINVAL;                
+            }
+            else
+            {
+                sum[K2DH_AXIS_X] += acc_cal.x;
+                sum[K2DH_AXIS_Y] += acc_cal.y;
+                sum[K2DH_AXIS_Z] += acc_cal.z;
+
+                acc_cal_pre.x = acc_cal.x;
+                acc_cal_pre.y = acc_cal.y;
+                acc_cal_pre.z = acc_cal.z;
+            }
+
+            GSE_LOG("calibration sum data (%d, %d, %d)\n", sum[K2DH_AXIS_X], sum[K2DH_AXIS_Y], sum[K2DH_AXIS_Z]);            
+            GSE_LOG(KERN_INFO "===============timeout_shaking: %d=============== \n",i);            
+        }
+
+        GSE_LOG(KERN_INFO "===============complete shaking x check===============\n");        
+
+#if 1
+        // check zero-g offset
+        if((abs(sum[K2DH_AXIS_X]/CALIBRATION_DATA_AMOUNT) >TESTLIMIT_XY) ||
+            (abs(sum[K2DH_AXIS_Y]/CALIBRATION_DATA_AMOUNT) >TESTLIMIT_XY) ||
+            ((abs(sum[K2DH_AXIS_Z]/CALIBRATION_DATA_AMOUNT) > TESTLIMIT_Z_USL_LSB) || (abs(sum[K2DH_AXIS_Z]/CALIBRATION_DATA_AMOUNT) < TESTLIMIT_Z_LSL_LSB)))
+        {
+            GSE_LOG("Calibration zero-g offset check failed (%d, %d, %d)\n", sum[K2DH_AXIS_X]/CALIBRATION_DATA_AMOUNT, 
+                                                                    sum[K2DH_AXIS_Y]/CALIBRATION_DATA_AMOUNT, sum[K2DH_AXIS_Z]/CALIBRATION_DATA_AMOUNT);
+             atomic_set(&obj->fast_calib_rslt, 0);
+             return -EINVAL;                            
+        }        
+#endif
+
+        obj->cali_sw[K2DH_AXIS_X] = sum[K2DH_AXIS_X] / CALIBRATION_DATA_AMOUNT;  //K2DH(12bit) 0+-154
+        obj->cali_sw[K2DH_AXIS_Y] = sum[K2DH_AXIS_Y] / CALIBRATION_DATA_AMOUNT;  //K2DH(12bit) 0+-154
+
+        if(sum[K2DH_AXIS_Z] < 0)
+        {
+            obj->cali_sw[K2DH_AXIS_Z] = (sum[K2DH_AXIS_Z] / CALIBRATION_DATA_AMOUNT) + 1024;        // K2DH(12bit) 1024 +- 226
+        }
+        else
+        {
+            obj->cali_sw[K2DH_AXIS_Z] = (sum[K2DH_AXIS_Z] / CALIBRATION_DATA_AMOUNT) - 1024;        // K2DH(12bit) 1024 +- 226
+        }
+
+        GSE_LOG("%s : calibration data (%d, %d, %d)\n", __func__, obj->cali_sw[K2DH_AXIS_X], obj->cali_sw[K2DH_AXIS_Y], obj->cali_sw[K2DH_AXIS_Z]);
+
+        return err;
+}
+
+#if 1
+static int k2dh_get_selftest(struct i2c_client *client)
+{
+    int val, i;
+    int en_state = 0;
+
+    unsigned char x[8];
+    int NO_ST[3] = {0, 0, 0};
+    int NO_ST_ZOFF[3] = {0, 0, 0};
+    int ST[3] = {0, 0, 0};
+    s16 tmp = 0;
+
+    struct k2dh_i2c_data *obj = i2c_get_clientdata(client); 
+
+    // ODR setting
+    x[0] = CTRL_REG1;
+    x[1] = 0x67;  // 0x47
+    k2dh_acc_i2c_write(client, x, 1);
+
+    x[0] = (I2C_AUTO_INCREMENT | CTRL_REG4);
+    x[1] = 0x88;
+    x[2] = 0x00;
+    x[3] = 0x00;
+    k2dh_acc_i2c_write(client, x, 3);
+
+    mdelay(80);
+
+    x[0] = (I2C_AUTO_INCREMENT | AXISDATA_REG);
+    k2dh_acc_i2c_read(client, x, 6);
+
+    for(i = 0; i < 5; i++)
+    {
+        while(1)
+        {
+            x[0] = 0x27;  // status_reg
+            val = k2dh_acc_i2c_read(client, x, 1);
+            if(val < 0)
+            {
+                GSE_ERR("[SELFTEST] I2C fail point1\n");
+                goto ST_EXIT;
+            }
+            if(x[0] & 0x08)  // x,y,z asix new data Available
+            {
+                break;
+            }
+        }
+
+        x[0] = (I2C_AUTO_INCREMENT | AXISDATA_REG);
+        k2dh_acc_i2c_read(client, x, 6);
+
+        NO_ST_ZOFF[0] += (((s16) ((x[1] << 8) | x[0])) >> 4); // 12 bit resolution 1LSB= 0.997mg
+        NO_ST_ZOFF[1] += (((s16) ((x[3] << 8) | x[2])) >> 4);
+        NO_ST_ZOFF[2] += (((s16) ((x[5] << 8) | x[4])) >> 4);
+    
+        NO_ST[0] += (((s16) ((x[1] << 8) | x[0])) >> 6); //10 bit resolution 1LSB=4mg
+        NO_ST[1] += (((s16) ((x[3] << 8) | x[2])) >> 6);
+        NO_ST[2] += (((s16) ((x[5] << 8) | x[4])) >> 6);
+
+        if(atomic_read(&obj->trace) & K2DH_TRC_CALI)
+        {
+            GSE_LOG("[SELFTEST] NO_ST(%d) : %d, %d, %d\n", i, NO_ST[0], NO_ST[1], NO_ST[2]);
+        }
+    }
+
+    NO_ST_ZOFF[0] /= 5;
+    NO_ST_ZOFF[1] /= 5;
+    NO_ST_ZOFF[2] /= 5;
+
+    NO_ST[0] /= 5;
+    NO_ST[1] /= 5;
+    NO_ST[2] /= 5;
+
+    //if(atomic_read(&obj->trace) & K2DH_TRC_CALI)
+    {
+        GSE_LOG("[SELFTEST] AVE_NO_ST : %d, %d, %d\n", NO_ST[0], NO_ST[1], NO_ST[2]);
+    }
+
+    x[0] = CTRL_REG4;
+    x[1] = 0x8A; // ST enable
+    k2dh_acc_i2c_write(client, x, 1);
+
+    mdelay(80);
+
+    x[0] = (I2C_AUTO_INCREMENT | AXISDATA_REG);
+    k2dh_acc_i2c_read(client, x, 6);
+
+    for(i = 0; i < 5; i++)
+    {
+        while(1)
+        {
+            x[0] = 0x27;
+            val = k2dh_acc_i2c_read(client, x, 1);
+
+            if(val < 0)
+            {
+                GSE_ERR("[SELFTEST] I2C fail point2\n");
+                goto ST_EXIT;
+            }
+
+            if(x[0] & 0x08)
+            {
+                break;
+            }
+        }
+
+        x[0] = (I2C_AUTO_INCREMENT | AXISDATA_REG);
+        k2dh_acc_i2c_read(client, x, 6);
+
+        ST[0] += (((s16) ((x[1] << 8) | x[0])) >> 6);
+        ST[1] += (((s16) ((x[3] << 8) | x[2])) >> 6);
+        ST[2] += (((s16) ((x[5] << 8) | x[4])) >> 6);
+
+        if(atomic_read(&obj->trace) & K2DH_TRC_CALI)
+        {
+            GSE_LOG("[SELFTEST] ST(%d) : %d, %d, %d\n", i, ST[0], ST[1], ST[2]);
+        }        
+    }
+    ST[0] /= 5;
+    ST[1] /= 5;
+    ST[2] /= 5;    
+    //if(atomic_read(&obj->trace) & K2DH_TRC_CALI)
+    {
+        GSE_LOG("[SELFTEST] AVE_ST : %d, %d, %d\n", ST[0], ST[1], ST[2]);
+    }
+
+    for(val = 0, i = 0; i < 3; i++)
+    {
+        // calculate differece between SelfTest value and zoro g offset in 10bit resolution
+        ST[i] -= NO_ST[i];
+        ST[i] = abs(ST[i]);
+
+        // range compare of the self test
+        if((SELF_TEST_2G_MIN_LSB > ST[i]) || (ST[i] > SELF_TEST_2G_MAX_LSB))
+        {
+            GSE_ERR("[SELFTEST] ST[%d] : Out of range!! (%d)\n", i, ST[i]);
+            val = -1;
+        }
+    }
+
+    // check zero-g offset
+    if(val >= 0)
+    {
+        for(val = 1, i = 0; i < 3; i++)
+        {
+            if(i < 2)
+            {
+                if(abs(NO_ST_ZOFF[i]) > TESTLIMIT_XY)  // X, Y axis flat check
+                {
+                    GSE_ERR("[SELFTEST] NO_ST[%d] : Out of ZOffset!! (%d)\n", i, NO_ST_ZOFF[i]);
+                    val = -1;
+                }
+            }
+            else
+            {  // Z axis flat check
+                if((abs(NO_ST_ZOFF[i]) > TESTLIMIT_Z_USL_LSB) || (abs(NO_ST_ZOFF[i]) < TESTLIMIT_Z_LSL_LSB))
+                {
+                    GSE_ERR("[SELFTEST] NO_ST[%d] : Out of ZOffset!! (%d)\n", i, NO_ST[i]);
+                    val = -1;
+                }
+            }
+        }
+    }
+
+    //if(atomic_read(&obj->trace) & K2DH_TRC_CALI)
+    {
+        if(val >= 0)
+        {
+            GSE_LOG("[SELFTEST] OK!! val : %d, (%d, %d, %d) ||| (%d, %d, %d)\n", val, ST[0], ST[1], ST[2], NO_ST_ZOFF[0], NO_ST_ZOFF[1], NO_ST_ZOFF[2]);
+        }
+        else
+        {
+            GSE_LOG("[SELFTEST] NG!! val : %d, (%d, %d, %d) ||| (%d, %d, %d)\n", val, ST[0], ST[1], ST[2], NO_ST_ZOFF[0], NO_ST_ZOFF[1], NO_ST_ZOFF[2]);
+        }   
+    }            
+
+ST_EXIT:
+#if 0  // do not control ODR rate
+    x[0] = CTRL_REG1;
+    x[1] = 0x00;
+    k2dh_acc_i2c_write(client, x, 1);
+#endif
+
+    x[0] = CTRL_REG4;
+    x[1] = 0x08; // ST disable
+    k2dh_acc_i2c_write(client, x, 1);
+
+
+    x[0] = CTRL_REG5;
+    x[1] = 0x88;
+    k2dh_acc_i2c_write(client, x, 1);
+
+    return val;    
+}
+
+#endif
+
+
+static int k2dh_set_mode(struct i2c_client *client, unsigned char mode)
+{
+    int comres = -1;
+    unsigned char data[2] ={0, 0};
+    unsigned char LPen = 0;
+    unsigned char HR = 0;
+
+    if((client == NULL) || (mode >= 3))
+    {
+        return -1;
+    }
+
+    switch(mode)
+    {
+        case K2DH_MODE_LOWPOWER :
+            LPen = 1;
+            HR = 0;
+            break;
+        case K2DH_MODE_NORMAL:
+            LPen = 0;
+            HR =  0;
+            break;
+        case K2DH_MODE_HIGH_RESOLUTION :
+            LPen = 0;
+            HR = 1;
+            break;
+        default :
+            break;
+    }
+
+    comres = k2dh_acc_register_update(client, (u8 *) data, K2DH_LP_EN_REG, K2DH_LP_EN_MSK, LPen << K2DH_LP_EN_POS);
+    if(comres != K2DH_SUCCESS)
+    {
+        return comres;
+    }
+    else
+    {
+        comres = k2dh_acc_register_update(client, (u8 *) data, K2DH_HR_REG, K2DH_HR_MSK, HR << K2DH_HR_POS);
+        if(comres != K2DH_SUCCESS)
+        {
+            return comres;
+        }
+    }
+    GSE_LOG("k2dh_set_mode, mode : %d, LPen : %d, HR : %d\n", mode, LPen, HR);
+    return K2DH_SUCCESS;
+}
+
+static int k2dh_get_mode(struct i2c_client *client, unsigned char *mode)
+{
+    int comres = -1;
+    unsigned char data[2] ={0, 0};
+    unsigned char LPen = 0;
+    unsigned char HR = 0;
+
+    comres = k2dh_acc_register_read(client, data, K2DH_LP_EN_REG);
+    if(comres != K2DH_SUCCESS)
+    {
+        return comres;
+    }
+    else
+    {
+        LPen = (data[0]& K2DH_LP_EN_MSK)?1:0;
+        comres = k2dh_acc_register_read(client, data, K2DH_HR_REG);
+        if(comres != K2DH_SUCCESS)
+        {
+            return comres;
+        }
+        else
+        {
+            HR = (data[0]& K2DH_HR_MSK)?1:0;
+        }
+
+        if(LPen == 1 && HR == 0)
+        {
+            *mode = K2DH_MODE_LOWPOWER;
+        }
+        else if(LPen == 0 && HR == 0)
+        {
+            *mode = K2DH_MODE_NORMAL;
+        }
+        else if(LPen == 0 && HR == 1)
+        {
+            *mode = K2DH_MODE_HIGH_RESOLUTION;
+        }
+        else
+        {
+            *mode = -1;
+        }
+    }
+    GSE_LOG("k2dh_get_mode, LPen : %d, HR : %d\n", LPen, HR);
+    return comres;
+}
+
+static int k2dh_set_range(struct i2c_client *client, unsigned char range)
+{
+    int comres = -1;
+    unsigned char data[2] ={0, 0};
+
+    if((client == NULL) || (range >= 4))
+    {
+        return -1;
+    }
+
+    comres = k2dh_acc_register_update(client, (u8 *) data, K2DH_FS_REG, K2DH_FS_MSK, range << K2DH_FS_POS);
+    if(comres != K2DH_SUCCESS)
+    {
+        return comres;
+    }
+
+    GSE_LOG("k2dh_set_range, range : %d\n", range);
+    return K2DH_SUCCESS;
+
+}
+static int k2dh_get_range(struct i2c_client *client, unsigned char *range)
+{
+    int comres = -1;
+    unsigned char data[2] ={0, 0};
+
+    comres = k2dh_acc_register_read(client, data, K2DH_FS_REG);
+    if(comres != K2DH_SUCCESS)
+    {
+        return comres;
+    }
+    data[0] = (data[0] & K2DH_FS_MSK) >> K2DH_FS_POS;
+    *range = data[0];
+    GSE_LOG("k2dh_get_range, range : %d\n", data[0]);
+    return comres;
+}
+static int k2dh_set_bandwidth(struct i2c_client *client, unsigned char bandwidth)
+{
+    int comres = -1;
+    unsigned char data[2] ={0, 0};
+
+    if((client == NULL) || (bandwidth >= 10))
+    {
+        return -1;
+    }
+
+    comres = k2dh_acc_register_update(client, (u8 *) data, K2DH_ODR_REG, K2DH_ODR_MSK, bandwidth << K2DH_ODR_POS);
+    if(comres != K2DH_SUCCESS)
+    {
+        return comres;
+    }
+
+    GSE_LOG("k2dh_set_bandwidth, bandwidth : %d\n", bandwidth);
+    return K2DH_SUCCESS;
+}
+static int k2dh_get_bandwidth(struct i2c_client *client, unsigned char *bandwidth)
+{
+    int comres = -1;
+    unsigned char data[2] ={0, 0};
+
+    comres = k2dh_acc_register_read(client, data, K2DH_ODR_REG);
+    if(comres != K2DH_SUCCESS)
+    {
+        return comres;
+    }
+    data[0] = (data[0] & K2DH_ODR_MSK) >> K2DH_ODR_POS;
+    *bandwidth = data[0];
+    GSE_LOG("k2dh_get_bandwidth, bandwidth : %d\n", data[0]);
+    return comres;
+}
+
+/*----------------------------------------------------------------------------*/
+static ssize_t show_chipinfo_value(struct device_driver *ddri, char *buf)
+{
+    struct i2c_client *client = k2dh_i2c_client;
+    char strbuf[K2DH_BUFSIZE] = {0};
+    if(NULL == client)
+    {
+        GSE_ERR("i2c client is null!!\n");
+        return 0;
+    }
+
+    k2dh_ReadChipInfo(client, strbuf, K2DH_BUFSIZE);
+    return snprintf(buf, PAGE_SIZE, "%s\n", strbuf);
+}
+
+static ssize_t gsensor_init(struct device_driver *ddri, char *buf, size_t count)
+{
+    struct i2c_client *client = k2dh_i2c_client;
+    char strbuf[K2DH_BUFSIZE] = {0};
+
+    if(NULL == client)
+    {
+        GSE_ERR("i2c client is null!!\n");
+        return 0;
+    }
+    k2dh_acc_hw_init(client, 1);
+    return snprintf(buf, PAGE_SIZE, "%s\n", strbuf);
+}
+
+static ssize_t show_opmode_value(struct device_driver *ddri, char *buf)
+{
+    unsigned char data = 0;
+
+    if (k2dh_get_mode(k2dh_i2c_client, &data) < 0)
+    {
+        return sprintf(buf, "Read error\n");
+    }
+    else
+    {
+        if(data == 0)
+            return sprintf(buf, "0:LP MODE\n");
+        else if(data == 1)
+            return sprintf(buf, "1:Normal MODE\n");
+        else if(data == 2)
+            return sprintf(buf, "2:HR MODE\n");
+        else
+            return sprintf(buf, "Error\n");
+    }
+}
+
+static ssize_t store_opmode_value(struct device_driver *ddri, char *buf, size_t count)
+{
+    unsigned long data = 0;
+    int error = -1;
+
+    if (error = strict_strtoul(buf, 10, &data))
+    {
+        return error;
+    }
+
+    if (k2dh_set_mode(k2dh_i2c_client, (unsigned char) data) < 0)
+    {
+        GSE_ERR("invalid content: '%s', length = %d\n", buf, count);
+    }
+
+    return count;
+}
+
+static ssize_t show_range_value(struct device_driver *ddri, char *buf)
+{
+    unsigned char data = 0;
+
+    if (k2dh_get_range(k2dh_i2c_client, &data) < 0)
+    {
+        return sprintf(buf, "Read error\n");
+    }
+    else
+    {
+        return sprintf(buf, "%d\n", data);
+    }
+}
+
+static ssize_t store_range_value(struct device_driver *ddri, char *buf, size_t count)
+{
+    unsigned long data = 0;
+    int error = -1;
+
+    if (error = strict_strtoul(buf, 10, &data))
+    {
+        return error;
+    }
+    if (k2dh_set_range(k2dh_i2c_client, (unsigned char) data) < 0)
+    {
+        GSE_ERR("invalid content: '%s', length = %d\n", buf, count);
+    }
+
+    return count;
+}
+
+static ssize_t show_bandwidth_value(struct device_driver *ddri, char *buf)
+{
+    unsigned char data = 0;
+
+    if (k2dh_get_bandwidth(k2dh_i2c_client, &data) < 0)
+    {
+        return sprintf(buf, "Read error\n");
+    }
+    else
+    {
+        return sprintf(buf, "%d\n", data);
+    }
+}
+
+static ssize_t store_bandwidth_value(struct device_driver *ddri, char *buf, size_t count)
+{
+    unsigned long data = 0;
+    int error = -1;
+
+    if (error = strict_strtoul(buf, 10, &data))
+    {
+        return error;
+    }
+    if (k2dh_set_bandwidth(k2dh_i2c_client, (unsigned char) data) < 0)
+    {
+        GSE_ERR("invalid content: '%s', length = %d\n", buf, count);
+    }
+
+    return count;
+}
+
+static ssize_t show_sensordata_value(struct device_driver *ddri, char *buf)
+{
+    struct i2c_client *client = k2dh_i2c_client;
+    char strbuf[K2DH_BUFSIZE] = {0};;
+
+    if(NULL == client)
+    {
+        GSE_ERR("i2c client is null!!\n");
+        return 0;
+    }
+    k2dh_ReadSensorData(client, strbuf, K2DH_BUFSIZE);
+    return snprintf(buf, PAGE_SIZE, "%s\n", strbuf);
+}
+
+static ssize_t show_sensorrawdata_value(struct device_driver *ddri, char *buf)
+{
+	struct i2c_client *client = k2dh_i2c_client;
+	s16 data[K2DH_AXES_NUM];
+
+	if(NULL == client)
+	{
+		GSE_ERR("i2c client is null!!\n");
+		return 0;
+	}
+
+	k2dh_ReadData(client, data);
+	return snprintf(buf, PAGE_SIZE, "Read RawData : x=%04x, y=%04x, z=%04x\n", data[0], data[1], data[2]);
+}
+
+
+
+/*----------------------------------------------------------------------------*/
+static ssize_t show_trace_value(struct device_driver *ddri, char *buf)
+{
+    ssize_t res = -1;
+    struct k2dh_i2c_data *obj = obj_i2c_data;
+    if (obj == NULL)
+    {
+        GSE_ERR("i2c_data obj is null!!\n");
+        return 0;
+    }
+
+    res = snprintf(buf, PAGE_SIZE, "0x%04X\n", atomic_read(&obj->trace));
+    return res;
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t store_trace_value(struct device_driver *ddri, char *buf, size_t count)
+{
+    struct k2dh_i2c_data *obj = obj_i2c_data;
+    unsigned long data = 0;
+    int error = -1;    
+
+    if (obj == NULL)
+    {
+        GSE_ERR("i2c_data obj is null!!\n");
+        return 0;
+    }
+
+    if (error = strict_strtoul(buf, 10, &data))
+    {
+        return error;
+    }
+
+    atomic_set(&obj->trace, data);
+
+    //GSE_LOG("data : %d saved_data : %d , buf : %s, count : %d\n", data, atomic_read(&obj->trace), buf, count);
+
+    return count;
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t show_status_value(struct device_driver *ddri, char *buf)
+{
+    ssize_t len = 0;
+    struct k2dh_i2c_data *obj = obj_i2c_data;
+    if (obj == NULL)
+    {
+        GSE_ERR("i2c_data obj is null!!\n");
+        return 0;
+    }
+
+    if(obj->hw)
+    {
+        len += snprintf(buf+len, PAGE_SIZE-len, "CUST: %d %d (%d %d)\n", obj->hw->i2c_num, obj->hw->direction, obj->hw->power_id, obj->hw->power_vol);
+    }
+    else
+    {
+        len += snprintf(buf+len, PAGE_SIZE-len, "CUST: NULL\n");
+    }
+    return len;
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t show_power_status_value(struct device_driver *ddri, char *buf)
+{
+    if(sensor_power)
+        GSE_LOG("G sensor is in work mode, sensor_power = %d\n", sensor_power);
+    else
+        GSE_LOG("G sensor is in standby mode, sensor_power = %d\n", sensor_power);
+
+    return sprintf(buf, "%d\n", sensor_power);
+}
+
+static ssize_t show_teststatus_value(struct device_driver *ddri, char *buf)
+{
+    struct i2c_client *client = k2dh_i2c_client;
+    if(NULL == client)
+    {
+        GSE_ERR("i2c client is null!!\n");
+        return 0;
+    }
+    
+    return snprintf(buf, PAGE_SIZE, "%d\n", test_status);
+}
+
+
+#ifdef K2DH_ACCEL_CALIBRATION
+static int k2dh_read_accel_xyz(struct i2c_client *client, struct K2DHacc *acc)
+{
+    int comres = 0;
+    unsigned char data[6];
+
+    return comres;
+}
+
+static ssize_t show_cali_value(struct device_driver *ddri, char *buf)
+{
+    GSE_FUN();
+    struct i2c_client *client = k2dh_i2c_client;
+    struct k2dh_i2c_data *obj = i2c_get_clientdata(client);
+    int offset_x,offset_y,offset_z;
+
+    offset_x = obj->cali_sw[K2DH_AXIS_X];
+    offset_y = obj->cali_sw[K2DH_AXIS_Y];
+    offset_z = obj->cali_sw[K2DH_AXIS_Z];
+
+    GSE_LOG("offset_x: %d, offset_y: %d, offset_z: %d\n",offset_x,offset_y,offset_z);
+
+    return snprintf(buf, PAGE_SIZE, "%d %d %d \n", offset_x, offset_y, offset_z);
+}
+
+static ssize_t store_cali_value(struct device_driver *ddri, char *buf, size_t count)
+{
+    struct i2c_client *client = k2dh_i2c_client;
+    struct k2dh_i2c_data *obj = i2c_get_clientdata(client);
+    int err;
+    int offset_x,offset_y,offset_z;
+    int dat[K2DH_AXES_NUM];
+
+    if(!strncmp(buf, "rst", 3))
+    {
+        GSE_FUN();
+        if(err = k2dh_ResetCalibration(client))
+        {
+            GSE_ERR("reset offset err = %d\n", err);
+        }
+    }
+    else if(3 == sscanf(buf, "%d %d %d", &offset_x, &offset_y, &offset_z))
+    {
+        GSE_LOG("store_cali_value: x=%d, y=%d, z=%d\n", offset_x, offset_y, offset_z);
+
+        obj->cali_sw[K2DH_AXIS_X] = offset_x;
+        obj->cali_sw[K2DH_AXIS_Y] = offset_y;
+        obj->cali_sw[K2DH_AXIS_Z] = offset_z;
+    }
+    else
+    {
+        GSE_ERR("invalid format\n");
+    }
+
+    return count;
+}
+
+static ssize_t k2dh_fast_calibration_x_show(struct device_driver *ddri, char *buf)
+{
+    struct i2c_client *client = k2dh_i2c_client;
+    struct k2dh_i2c_data *k2dh = i2c_get_clientdata(client);
+
+    return sprintf(buf, "%d\n", atomic_read(&k2dh->fast_calib_x_rslt));
+}
+
+static ssize_t k2dh_fast_calibration_x_store(struct device_driver *ddri, char *buf, size_t count)
+{
+    unsigned long data;
+    signed char tmp;
+    unsigned char timeout = 0;
+    unsigned int timeout_shaking = 0;
+    int error;
+    struct i2c_client *client = k2dh_i2c_client;
+    struct k2dh_i2c_data *k2dh = i2c_get_clientdata(client);
+    struct K2DHacc acc_cal;
+    struct K2DHacc acc_cal_pre;
+
+    test_status = 4;  // calibration status
+
+    if (error = strict_strtoul(buf, 10, &data))
+        return error;
+
+    if(k2dh_do_calibration() != K2DH_SUCCESS)
+        {
+            atomic_set(&k2dh->fast_calib_x_rslt, 0);
+            return -EINVAL;
+        }
+
+    atomic_set(&k2dh->fast_calib_x_rslt, 1);
+    GSE_LOG(KERN_INFO "x axis fast calibration finished\n");
+
+    return count;
+}
+
+static ssize_t k2dh_fast_calibration_y_show(struct device_driver *ddri, char *buf)
+{
+
+    struct i2c_client *client = k2dh_i2c_client;
+    struct k2dh_i2c_data *k2dh = i2c_get_clientdata(client);
+
+    return sprintf(buf, "%d\n", atomic_read(&k2dh->fast_calib_y_rslt));
+}
+
+static ssize_t k2dh_fast_calibration_y_store(struct device_driver *ddri, char *buf, size_t count)
+{
+    unsigned long data;
+    int error;
+    struct i2c_client *client = k2dh_i2c_client;
+    struct k2dh_i2c_data *k2dh = i2c_get_clientdata(client);
+
+    if (error = strict_strtoul(buf, 10, &data))
+        return error;
+
+    atomic_set(&k2dh->fast_calib_y_rslt, 1);
+    GSE_LOG(KERN_INFO "y axis fast calibration finished\n");
+
+    return count;
+}
+
+static ssize_t k2dh_fast_calibration_z_show(struct device_driver *ddri, char *buf)
+{
+    struct i2c_client *client = k2dh_i2c_client;
+    struct k2dh_i2c_data *k2dh = i2c_get_clientdata(client);
+
+    return sprintf(buf, "%d\n", atomic_read(&k2dh->fast_calib_z_rslt));
+}
+
+static ssize_t k2dh_fast_calibration_z_store(struct device_driver *ddri, char *buf, size_t count)
+{
+    unsigned long data;
+    int error;
+    struct i2c_client *client = k2dh_i2c_client;
+    struct k2dh_i2c_data *k2dh = i2c_get_clientdata(client);
+
+    if (error = strict_strtoul(buf, 10, &data))
+        return error;
+
+    atomic_set(&k2dh->fast_calib_z_rslt, 1);
+    GSE_LOG(KERN_INFO "z axis fast calibration finished\n");
+
+    test_status = sensor_power;
+
+    return count;
+}
+
+#if 1
+static int k2dh_runCalibration(void)
+{
+    if(k2dh_do_calibration() != K2DH_SUCCESS)
+    {
+        return -EINVAL;
+    }
+    GSE_LOG(KERN_INFO "self calibration Done\n");
+
+    return K2DH_SUCCESS;
+}
+#endif
+
+static ssize_t k2dh_eeprom_writing_show(struct device_driver *ddri, char *buf)
+{
+    struct i2c_client *client = k2dh_i2c_client;
+    struct k2dh_i2c_data *k2dh = i2c_get_clientdata(client);
+
+    return sprintf(buf, "%d\n", atomic_read(&k2dh->fast_calib_rslt));
+}
+
+static ssize_t k2dh_eeprom_writing_store(struct device_driver *ddri, char *buf, size_t count)
+{
+    unsigned char offset_x,offset_y,offset_z;
+    struct i2c_client *client = k2dh_i2c_client;
+    struct k2dh_i2c_data *k2dh = i2c_get_clientdata(client);
+
+    atomic_set(&k2dh->fast_calib_rslt, 1);
+
+    return count;
+}
+
+static int k2dh_soft_reset(struct i2c_client *client)
+{
+    int comres = 0;
+    unsigned char data;
+
+    GSE_FUN();
+    // no soft_reset need
+
+    return comres;
+}
+static ssize_t k2dh_softreset_store(struct device *dev, struct device_attribute *attr, const char *buf, size_t count)
+{
+    struct i2c_client *client = k2dh_i2c_client;
+    struct k2dh_i2c_data *k2dh = i2c_get_clientdata(client);
+
+    if (k2dh_soft_reset(k2dh->client) < 0)
+        return -EINVAL;
+
+    return count;
+}
+
+#if 0
+static ssize_t k2dh_runCalibration_store(struct device_driver *ddri, char *buf, size_t count)
+{        
+        unsigned long data = 0;
+        int error = -1;
+        int cali[3];
+
+        
+        struct i2c_client *client = k2dh_i2c_client;
+        struct k2dh_i2c_data *obj = i2c_get_clientdata(client);
+        
+        if (error = strict_strtoul(buf, 10, &data))
+        {
+            return error;
+        }
+        GSE_LOG("Calibration CMD value : %d\n", (int)data);
+
+        if(data == 1 || data == 2)  // calibration start command
+{
+        if(k2dh_runCalibration() == K2DH_SUCCESS)
+        {
+                cali[0] = obj->cali_sw[0];
+                cali[1] = obj->cali_sw[1];
+                cali[2] = obj->cali_sw[2];                
+                if(LGE_FacWriteAccelerometerCalibration((unsigned int*)cali) == TRUE)
+                {
+                    atomic_set(&obj->fast_calib_rslt, 1);
+				}
+			}
+			else // wrong input
+			{
+				GSE_LOG("Calibration FAIL\n");
+				return -EINVAL;
+			}
+        }
+	return count;
+}
+
+static ssize_t k2dh_runCalibration_show(struct device_driver *ddri, char *buf)
+{
+    int selfCalibration = 1;  // fail
+
+    struct i2c_client *client = k2dh_i2c_client;
+    struct k2dh_i2c_data *obj = i2c_get_clientdata(client);
+
+    if(atomic_read(&obj->fast_calib_rslt) == 1)  // calibration success
+    {
+        selfCalibration = 0;  // success
+    }
+    else
+    {
+        selfCalibration = 1;  // fail
+    }
+	return sprintf(buf, "%d\n", selfCalibration);
+}
+
+#endif
+
+#endif  // END OF K2DH_ACCEL_CALIBRATION
+
+#if 1
+static ssize_t k2dh_SelfTest_store(struct device_driver *ddri, char *buf, size_t count)
+{        
+        unsigned long data = 0;
+        int error = -1;
+        
+        struct i2c_client *client = k2dh_i2c_client;
+        struct k2dh_i2c_data *obj = i2c_get_clientdata(client);
+        
+        if (error = strict_strtoul(buf, 10, &data))
+        {
+            return error;
+        }
+        GSE_LOG("Self test CMD value : %d\n", (int)data);
+
+        if(data == 1)  // self test start command
+        {
+            if(k2dh_get_selftest(client) >= 0)
+            {            
+                atomic_set(&obj->selftest_rslt, 1);
+			}
+			else
+			{
+                atomic_set(&obj->selftest_rslt, 0);
+			}
+        }
+		else // wrong input
+		{
+            GSE_LOG("SelfTest FAIL\n");
+            return -EINVAL;
+		}
+	return count;
+}
+
+static ssize_t k2dh_SelfTest_show(struct device_driver *ddri, char *buf)
+{
+    int selftest = 1;  // fail
+
+    struct i2c_client *client = k2dh_i2c_client;
+    struct k2dh_i2c_data *obj = i2c_get_clientdata(client);
+
+    if(atomic_read(&obj->selftest_rslt) == 1)  // selftest success
+    {
+        selftest = 0;  // success
+    }
+    else
+    {
+        selftest = 1;  // fail
+    }
+	return sprintf(buf, "%d\n", selftest);
+}
+#endif
+
+/*----------------------------------------------------------------------------*/
+static DRIVER_ATTR(chipinfo, S_IWUSR|S_IRUGO, show_chipinfo_value, NULL);
+static DRIVER_ATTR(opmode, S_IWUSR|S_IRUGO|S_IWGRP, show_opmode_value, store_opmode_value);
+static DRIVER_ATTR(range, S_IWUSR|S_IRUGO|S_IWGRP, show_range_value, store_range_value);
+static DRIVER_ATTR(bandwidth, S_IWUSR|S_IRUGO|S_IWGRP, show_bandwidth_value, store_bandwidth_value);
+static DRIVER_ATTR(sensordata, S_IWUSR|S_IRUGO, show_sensordata_value, NULL);
+static DRIVER_ATTR(sensorrawdata, S_IWUSR|S_IRUGO, show_sensorrawdata_value, NULL);
+static DRIVER_ATTR(trace, S_IWUSR|S_IRUGO|S_IWGRP, show_trace_value, store_trace_value);
+static DRIVER_ATTR(status, S_IRUGO, show_status_value, NULL);
+static DRIVER_ATTR(powerstatus, S_IRUGO, show_power_status_value, NULL);
+static DRIVER_ATTR(teststatus, S_IWUSR|S_IRUGO, show_teststatus_value, NULL);
+#ifdef K2DH_ACCEL_CALIBRATION
+static DRIVER_ATTR(softreset, S_IWUSR|S_IWGRP, NULL, k2dh_softreset_store);
+static DRIVER_ATTR(cali, S_IWUSR|S_IRUGO|S_IWGRP, show_cali_value, store_cali_value);
+static DRIVER_ATTR(fast_calibration_x, S_IRUGO|S_IWUSR|S_IWGRP, k2dh_fast_calibration_x_show, k2dh_fast_calibration_x_store);
+static DRIVER_ATTR(fast_calibration_y, S_IRUGO|S_IWUSR|S_IWGRP, k2dh_fast_calibration_y_show, k2dh_fast_calibration_y_store);
+static DRIVER_ATTR(fast_calibration_z, S_IRUGO|S_IWUSR|S_IWGRP, k2dh_fast_calibration_z_show, k2dh_fast_calibration_z_store);
+static DRIVER_ATTR(eeprom_writing, S_IRUGO|S_IWUSR|S_IWGRP, k2dh_eeprom_writing_show, k2dh_eeprom_writing_store);
+
+#if 0
+static DRIVER_ATTR(run_fast_calibration, S_IRUGO|S_IWUSR|S_IWGRP, k2dh_runCalibration_show, k2dh_runCalibration_store);
+#endif
+#endif
+#ifdef CONFIG_K2DH_LOWPASS
+static DRIVER_ATTR(firlen, S_IWUSR|S_IRUGO|S_IWGRP, show_firlen_value, store_firlen_value);
+#endif
+#if 1
+static DRIVER_ATTR(selftest, S_IRUGO|S_IWUSR|S_IWGRP, k2dh_SelfTest_show, k2dh_SelfTest_store);
+#endif
+
+
+/*----------------------------------------------------------------------------*/
+static struct driver_attribute *k2dh_attr_list[] = {
+    &driver_attr_chipinfo,     /*chip information*/
+    &driver_attr_opmode,
+    &driver_attr_range,
+    &driver_attr_bandwidth,
+    &driver_attr_sensordata,   /*dump sensor data*/
+    &driver_attr_sensorrawdata,
+    &driver_attr_trace,        /*trace log*/
+    &driver_attr_status,
+    &driver_attr_powerstatus,
+    &driver_attr_teststatus,
+#ifdef K2DH_ACCEL_CALIBRATION
+    &driver_attr_softreset,
+    &driver_attr_cali,         /*show calibration data*/
+    &driver_attr_fast_calibration_x,
+    &driver_attr_fast_calibration_y,
+    &driver_attr_fast_calibration_z,
+    &driver_attr_eeprom_writing,
+#if 0
+&driver_attr_run_fast_calibration,
+#endif
+#endif
+#ifdef CONFIG_K2DH_LOWPASS
+    &driver_attr_firlen,       /*filter length: 0: disable, others: enable*/
+#endif
+#if 1
+    &driver_attr_selftest,
+#endif
+};
+/*----------------------------------------------------------------------------*/
+static int k2dh_create_attr(struct device_driver *driver)
+{
+    int idx = 0, err = 0;
+    int num = (int)(sizeof(k2dh_attr_list)/sizeof(k2dh_attr_list[0]));
+    if (driver == NULL)
+    {
+        return -EINVAL;
+    }
+
+    for(idx = 0; idx < num; idx++)
+    {
+        if(err = driver_create_file(driver, k2dh_attr_list[idx]))
+        {
+            GSE_ERR("driver_create_file (%s) = %d\n", k2dh_attr_list[idx]->attr.name, err);
+            break;
+        }
+    }
+    return err;
+}
+/*----------------------------------------------------------------------------*/
+static int k2dh_delete_attr(struct device_driver *driver)
+{
+    int idx = 0 ,err = 0;
+    int num = (int)(sizeof(k2dh_attr_list)/sizeof(k2dh_attr_list[0]));
+
+    if(driver == NULL)
+    {
+        return -EINVAL;
+    }
+
+    for(idx = 0; idx < num; idx++)
+    {
+        driver_remove_file(driver, k2dh_attr_list[idx]);
+    }
+
+    return err;
+}
+
+/*----------------------------------------------------------------------------*/
+int gsensor_operate(void* self, uint32_t command, void* buff_in, int size_in, void* buff_out, int size_out, int* actualout)
+{
+    int err = 0;
+    int value = 0, sample_delay = 0;
+    struct k2dh_i2c_data *priv = (struct k2dh_i2c_data*)self;
+    hwm_sensor_data* gsensor_data;
+    char buff[K2DH_BUFSIZE] = {0};
+
+    //GSE_FUN(f);
+    switch (command)
+    {
+        case SENSOR_DELAY:
+            if((buff_in == NULL) || (size_in < sizeof(int)))
+            {
+                GSE_ERR("Set delay parameter error!\n");
+                err = -EINVAL;
+            }
+            else
+            {
+                value = *(int *)buff_in;
+
+#if 1
+                sample_delay = ODR200;  // 5ms
+#else
+                if(value >= 100)
+                {
+                    sample_delay = ODR10;  // 100ms
+                }
+                else if(value >= 40)
+                {
+                    sample_delay = ODR25;  // 40ms
+                }
+                else if(value >= 20)
+                {
+                    sample_delay = ODR50;  // 20ms
+                }
+                else if(value >= 10)
+                {
+                    sample_delay = ODR100;  // 10ms
+                }
+                else
+                {
+                    sample_delay = ODR200;  // 5ms
+                }
+#endif
+                err = k2dh_SetBWRate(priv->client, sample_delay);
+                if(err != K2DH_SUCCESS )
+                {
+                    GSE_ERR("Set delay parameter error!\n");
+                }
+
+                if(value >= 50)
+                {
+                    atomic_set(&priv->filter, 0);
+                }
+                else
+                {
+                #if defined(CONFIG_K2DH_LOWPASS)
+                    priv->fir.num = 0;
+                    priv->fir.idx = 0;
+                    priv->fir.sum[K2DH_AXIS_X] = 0;
+                    priv->fir.sum[K2DH_AXIS_Y] = 0;
+                    priv->fir.sum[K2DH_AXIS_Z] = 0;
+                    atomic_set(&priv->filter, 1);
+                #endif
+                }
+            }
+            break;
+
+        case SENSOR_ENABLE:
+            if((buff_in == NULL) || (size_in < sizeof(int)))
+            {
+                GSE_ERR("Enable sensor parameter error!\n");
+                err = -EINVAL;
+            }
+            else
+            {
+                value = *(int *)buff_in;
+                if(((value == 0) && (sensor_power == false)) || ((value == 1) && (sensor_power == true)))
+                {
+                    GSE_LOG("Gsensor device have updated!, power: %d\n", sensor_power);
+                }
+                else
+                {
+                    err = k2dh_SetPowerMode( priv->client, !sensor_power);
+                }
+            }
+            break;
+
+        case SENSOR_GET_DATA:
+            if((buff_out == NULL) || (size_out< sizeof(hwm_sensor_data)))
+            {
+                GSE_ERR("get sensor data parameter error!\n");
+                err = -EINVAL;
+            }
+            else
+            {
+                gsensor_data = (hwm_sensor_data *)buff_out;
+                k2dh_ReadSensorData(priv->client, buff, K2DH_BUFSIZE);
+                sscanf(buff, "%x %x %x", &gsensor_data->values[0], &gsensor_data->values[1], &gsensor_data->values[2]);
+                gsensor_data->status = SENSOR_STATUS_ACCURACY_MEDIUM;
+                gsensor_data->value_divide = 1000;
+            }
+            break;
+
+        default:
+            GSE_ERR("gsensor operate function no this parameter %d!\n", command);
+            err = -1;
+            break;
+    }
+
+    return err;
+}
+
+/******************************************************************************
+ * Function Configuration
+******************************************************************************/
+static int k2dh_open(struct inode *inode, struct file *file)
+{
+    file->private_data = k2dh_i2c_client;
+
+    if(file->private_data == NULL)
+    {
+        GSE_ERR("null pointer!!\n");
+        return -EINVAL;
+    }
+    return nonseekable_open(inode, file);
+}
+/*----------------------------------------------------------------------------*/
+static int k2dh_release(struct inode *inode, struct file *file)
+{
+    file->private_data = NULL;
+    return 0;
+}
+/*----------------------------------------------------------------------------*/
+static long k2dh_unlocked_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
+{
+    struct i2c_client *client = (struct i2c_client*)file->private_data;
+    struct k2dh_i2c_data *obj = (struct k2dh_i2c_data*)i2c_get_clientdata(client);
+    char strbuf[K2DH_BUFSIZE] = {0};
+    void __user *data;
+    SENSOR_DATA sensor_data;
+    long err = 0;
+    int cali[3] = {0};
+    uint32_t enable = 0;
+
+    //GSE_FUN(f);
+    if(_IOC_DIR(cmd) & _IOC_READ)
+    {
+        err = !access_ok(VERIFY_WRITE, (void __user *)arg, _IOC_SIZE(cmd));
+    }
+    else if(_IOC_DIR(cmd) & _IOC_WRITE)
+    {
+        err = !access_ok(VERIFY_READ, (void __user *)arg, _IOC_SIZE(cmd));
+    }
+
+    if(err)
+    {
+        GSE_ERR("access error: %08X, (%2d, %2d)\n", cmd, _IOC_DIR(cmd), _IOC_SIZE(cmd));
+        return -EFAULT;
+    }
+
+    switch(cmd)
+    {
+#if 1
+#if 0
+            case GSENSOR_IOCTL_SET_ENABLE:
+                GSE_LOG("GSENSOR_IOCTL_SET_ENABLE\n");
+                data = (void __user *) arg;
+                if(data == NULL)
+                {
+                    err = -EINVAL;
+                    break;
+                }
+                if(copy_from_user(&enable, data, sizeof(enable)))
+                {
+                    return -EFAULT;
+                }
+                else
+                {
+                    if(enable == 1)
+                    {
+                        k2dh_SetPowerMode( obj_i2c_data->client, 1);
+                    }
+                    else if(enable == 0)
+                    {
+                        k2dh_SetPowerMode( obj_i2c_data->client, 0);
+						GSE_LOG("coolpisoo, test 1: %d\n", data2[0]);
+						msleep(10);
+						k2dh_acc_register_read(client, data2, CTRL_REG2);
+						GSE_LOG("coolpisoo, test 2: %d\n", data2[0]);
+						msleep(10);
+						k2dh_acc_register_read(client, data2, CTRL_REG3);
+						GSE_LOG("coolpisoo, test 3: %d\n", data2[0]);
+						msleep(10);
+						k2dh_acc_register_read(client, data2, CTRL_REG4);
+						GSE_LOG("coolpisoo, test 4: %d\n", data2[0]);
+						msleep(10);
+						k2dh_acc_register_read(client, data2, CTRL_REG5);
+						GSE_LOG("coolpisoo, test 5: %d\n", data2[0]);
+						msleep(10);
+						k2dh_acc_register_read(client, data2, CTRL_REG6);
+						GSE_LOG("coolpisoo, test 6: %d\n", data2[0]);
+						msleep(10);
+						k2dh_acc_register_read(client, data2, FIFO_CTRL_REG);
+						GSE_LOG("coolpisoo, test 7: %d\n", data2[0]);
+						msleep(10);
+						k2dh_acc_register_read(client, data2, TEMP_CFG_REG);
+						GSE_LOG("coolpisoo, test 8: %d\n", data2[0]);
+						msleep(10);
+                    }
+                }
+                break;
+            case GSENSOR_IOCTL_GET_STATUS:
+                GSE_LOG("GSENSOR_IOCTL_GET_STATUS\n");
+                data = (void __user *) arg;
+                if(data == NULL)
+                {
+                    err = -EINVAL;
+                    break;
+                }
+                if(copy_to_user(data, &sensor_power, sizeof(sensor_power)))
+                {
+                    return -EFAULT;
+                }
+                break;    
+#endif /* 0 */
+        case GSENSOR_IOCTL_INIT:
+            k2dh_acc_hw_init(client, 0);
+            break;
+
+        case GSENSOR_IOCTL_READ_CHIPINFO:
+            data = (void __user *) arg;
+            if(data == NULL)
+            {
+                err = -EINVAL;
+                break;
+            }
+
+            k2dh_ReadChipInfo(client, strbuf, K2DH_BUFSIZE);
+            if(copy_to_user(data, strbuf, strlen(strbuf)+1))
+            {
+                err = -EFAULT;
+                break;
+            }
+            break;
+
+        case GSENSOR_IOCTL_READ_SENSORDATA:
+            data = (void __user *) arg;
+            if(data == NULL)
+            {
+                err = -EINVAL;
+                break;
+            }
+
+            k2dh_ReadSensorData(client, strbuf, K2DH_BUFSIZE);
+            if(copy_to_user(data, strbuf, strlen(strbuf)+1))
+            {
+                err = -EFAULT;
+                break;
+            }
+            break;
+
+        case GSENSOR_IOCTL_READ_GAIN:
+            data = (void __user *) arg;
+            if(data == NULL)
+            {
+                err = -EINVAL;
+                break;
+            }
+
+            if(copy_to_user(data, &gsensor_gain, sizeof(GSENSOR_VECTOR3D)))
+            {
+                err = -EFAULT;
+                break;
+            }
+            break;
+
+        case GSENSOR_IOCTL_READ_RAW_DATA:
+            data = (void __user *) arg;
+            if(data == NULL)
+            {
+                err = -EINVAL;
+                break;
+            }
+            //k2dh_ReadRawData(client, strbuf);
+            if(copy_to_user(data, &strbuf, strlen(strbuf)+1))
+            {
+                err = -EFAULT;
+                break;
+            }
+            break;
+
+        case GSENSOR_IOCTL_SET_CALI:
+            data = (void __user*)arg;
+            if(data == NULL)
+            {
+                err = -EINVAL;
+                break;
+            }
+            if(copy_from_user(&sensor_data, data, sizeof(sensor_data)))
+            {
+                err = -EFAULT;
+                break;
+            }
+
+            GSE_LOG("GSENSOR_IOCTL_SET_CALI, Cal data : 0x%x, 0x%x, 0x%x\n", sensor_data.x, sensor_data.y, sensor_data.z);
+            if((sensor_data.x == 0) && (sensor_data.y == 0) && (sensor_data.z == 0))
+            {  // temp defensive code for nvram_daemon ioctl
+                break;
+            }
+            
+             obj->cali_sw[K2DH_AXIS_X] = sensor_data.x;
+             obj->cali_sw[K2DH_AXIS_Y] = sensor_data.y;
+             obj->cali_sw[K2DH_AXIS_Z] = sensor_data.z; 
+			if(obj->cali_sw[K2DH_AXIS_X]>1000|| obj->cali_sw[K2DH_AXIS_X]<-1000||
+			obj->cali_sw[K2DH_AXIS_Y]>1000|| obj->cali_sw[K2DH_AXIS_Y]<-1000||
+			obj->cali_sw[K2DH_AXIS_Z]>1000|| obj->cali_sw[K2DH_AXIS_Z]<-1000){
+				GSE_LOG("Unnormal Cal Data");
+				obj->cali_sw[K2DH_AXIS_X] = 0;
+				obj->cali_sw[K2DH_AXIS_Y] = 0;
+				obj->cali_sw[K2DH_AXIS_Z] = 0;
+			}
+            break;
+
+        case GSENSOR_IOCTL_CLR_CALI:
+            err = k2dh_ResetCalibration(client);
+            break;
+
+        case GSENSOR_IOCTL_GET_CALI:
+            GSE_LOG("GSENSOR_IOCTL_GET_CALI\n");
+            break;
+#endif /* 0 */
+
+        default:
+            GSE_ERR("unknown IOCTL: 0x%08x\n", cmd);
+            err = -ENOIOCTLCMD;
+            break;
+    }
+
+    return err;
+}
+
+/*----------------------------------------------------------------------------*/
+static struct file_operations k2dh_fops = {
+    //.owner = THIS_MODULE,
+    .open = k2dh_open,
+    .release = k2dh_release,
+    .unlocked_ioctl = k2dh_unlocked_ioctl,
+};
+/*----------------------------------------------------------------------------*/
+static struct miscdevice k2dh_device = {
+    .minor = MISC_DYNAMIC_MINOR,
+    .name = "gsensor",
+    .fops = &k2dh_fops,
+};
+/*----------------------------------------------------------------------------*/
+#ifndef CONFIG_HAS_EARLYSUSPEND
+/*----------------------------------------------------------------------------*/
+static int k2dh_suspend(struct i2c_client *client, pm_message_t msg)
+{
+    GSE_FUN();
+
+    return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int k2dh_resume(struct i2c_client *client)
+{
+    GSE_FUN();
+
+    return 0;
+}
+/*----------------------------------------------------------------------------*/
+#else /*CONFIG_HAS_EARLY_SUSPEND is defined*/
+/*----------------------------------------------------------------------------*/
+static void k2dh_early_suspend(struct early_suspend *h)
+{
+    GSE_FUN();
+}
+/*----------------------------------------------------------------------------*/
+static void k2dh_late_resume(struct early_suspend *h)
+{
+    GSE_FUN();
+}
+
+/*----------------------------------------------------------------------------*/
+#endif /*CONFIG_HAS_EARLYSUSPEND*/
+/*----------------------------------------------------------------------------*/
+static int k2dh_i2c_probe(struct i2c_client *client, const struct i2c_device_id *id)
+{
+    struct i2c_client *new_client;
+    struct k2dh_i2c_data *obj;
+    struct hwmsen_object sobj;
+    int err = 0;
+    GSE_FUN();
+
+    if(!(obj = kzalloc(sizeof(*obj), GFP_KERNEL)))
+    {
+        err = -ENOMEM;
+        goto exit;
+    }
+
+    memset(obj, 0, sizeof(struct k2dh_i2c_data));
+
+    obj->hw = get_cust_acc_hw();
+    if(err = hwmsen_get_convert(obj->hw->direction, &obj->cvt))
+    {
+        GSE_ERR("invalid direction: %d\n", obj->hw->direction);
+        goto exit;
+    }
+
+    obj_i2c_data = obj;
+    obj->client = client;
+    new_client = obj->client;
+    i2c_set_clientdata(new_client,obj);
+
+    atomic_set(&obj->trace, 0);
+    atomic_set(&obj->suspend, 0);
+
+#ifdef CONFIG_K2DH_LOWPASS
+    if(obj->hw->firlen > C_MAX_FIR_LENGTH)
+    {
+        atomic_set(&obj->firlen, C_MAX_FIR_LENGTH);
+    }
+    else
+    {
+        atomic_set(&obj->firlen, obj->hw->firlen);
+    }
+
+    if(atomic_read(&obj->firlen) > 0)
+    {
+        atomic_set(&obj->fir_en, 1);
+    }
+
+#endif
+
+    k2dh_i2c_client = new_client;
+
+    if(err = k2dh_acc_hw_init(new_client, 1))
+    {
+        GSE_ERR ( "failed to init K2DH ( err = %d )\n", err );
+        goto exit_init_failed;
+    }
+
+    if(err = misc_register(&k2dh_device))
+    {
+        GSE_ERR("k2dh_device register failed\n");
+        goto exit_misc_device_register_failed;
+    }
+
+    if(err = k2dh_create_attr(&k2dh_gsensor_driver.driver))
+    {
+        GSE_ERR("create attribute err = %d\n", err);
+        goto exit_create_attr_failed;
+    }
+
+    sobj.self = obj;
+    sobj.polling = 1;
+    sobj.sensor_operate = gsensor_operate;
+    if(err = hwmsen_attach(ID_ACCELEROMETER, &sobj))
+    {
+        GSE_ERR("attach fail = %d\n", err);
+        goto exit_kfree;
+    }
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+    obj->early_drv.level    = EARLY_SUSPEND_LEVEL_DISABLE_FB - 1,
+    obj->early_drv.suspend  = k2dh_early_suspend,
+    obj->early_drv.resume   = k2dh_late_resume,
+    register_early_suspend(&obj->early_drv);
+#endif
+
+    GSE_LOG("%s: OK\n", __func__);
+    return 0;
+
+    exit_create_attr_failed:
+    misc_deregister(&k2dh_device);
+    exit_misc_device_register_failed:
+    exit_init_failed:
+    //i2c_detach_client(new_client);
+    exit_kfree:
+    kfree(obj);
+    exit:
+    GSE_ERR("%s: err = %d\n", __func__, err);
+    return err;
+}
+
+/*----------------------------------------------------------------------------*/
+static int k2dh_i2c_remove(struct i2c_client *client)
+{
+    int err = 0;
+
+    if(err = k2dh_delete_attr(&k2dh_gsensor_driver.driver))
+    {
+        GSE_ERR("k2dh_delete_attr fail: %d\n", err);
+    }
+
+    if(err = misc_deregister(&k2dh_device))
+    {
+        GSE_ERR("misc_deregister fail: %d\n", err);
+    }
+
+    if(err = hwmsen_detach(ID_ACCELEROMETER))
+
+
+    k2dh_i2c_client = NULL;
+    i2c_unregister_device(client);
+    kfree(i2c_get_clientdata(client));
+    return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int k2dh_probe(struct platform_device *pdev)
+{
+    struct acc_hw *hw = get_cust_acc_hw();
+
+    GSE_FUN();
+
+    if(i2c_add_driver(&k2dh_i2c_driver))
+    {
+        GSE_ERR("add driver error\n");
+        return -1;
+    }
+    return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int k2dh_remove(struct platform_device *pdev)
+{
+    struct acc_hw *hw = get_cust_acc_hw();
+
+    GSE_FUN();
+    k2dh_power(hw, 0);
+    i2c_del_driver(&k2dh_i2c_driver);
+    return 0;
+}
+/*----------------------------------------------------------------------------*/
+static struct platform_driver k2dh_gsensor_driver = {
+    .probe      = k2dh_probe,
+    .remove     = k2dh_remove,
+    .driver     = {
+        .name  = "gsensor",
+    }
+};
+
+/*----------------------------------------------------------------------------*/
+static int __init k2dh_init(void)
+{
+    struct acc_hw *hw = get_cust_acc_hw();
+
+    GSE_FUN();
+    i2c_register_board_info(hw->i2c_num, &k2dh_i2c_info, 1);
+    if(platform_driver_register(&k2dh_gsensor_driver))
+    {
+        GSE_ERR("failed to register driver");
+        return -ENODEV;
+    }
+    return 0;
+}
+/*----------------------------------------------------------------------------*/
+static void __exit k2dh_exit(void)
+{
+    GSE_FUN();
+    platform_driver_unregister(&k2dh_gsensor_driver);
+}
+/*----------------------------------------------------------------------------*/
+module_init(k2dh_init);
+module_exit(k2dh_exit);
+/*----------------------------------------------------------------------------*/
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("K2DH Accelerometer misc driver");
+MODULE_AUTHOR("STMcroelectronics");
diff --git a/drivers/misc/mediatek/accelerometer/k2dh/k2dh.h b/drivers/misc/mediatek/accelerometer/k2dh/k2dh.h
new file mode 100644
index 000000000..f39b636ee
--- /dev/null
+++ b/drivers/misc/mediatek/accelerometer/k2dh/k2dh.h
@@ -0,0 +1,47 @@
+/* BMA255 motion sensor driver
+ *
+ *
+ * This software program is licensed subject to the GNU General Public License
+ * (GPL).Version 2,June 1991, available at http://www.fsf.org/copyleft/gpl.html
+
+ * (C) Copyright 2011 Bosch Sensortec GmbH
+ * All Rights Reserved
+ */
+
+#ifndef __K2DH_H__
+#define  __K2DH_H__
+
+#include <linux/ioctl.h>
+
+#define SAD0L            0x00
+#define SAD0H            0x01
+#define K2DH_ACC_I2C_SADROOT    0x0C
+#define K2DH_ACC_I2C_SAD_L    ((K2DH_ACC_I2C_SADROOT<<1)|SAD0L)
+#define K2DH_ACC_I2C_SAD_H    ((K2DH_ACC_I2C_SADROOT<<1)|SAD0H)
+#define    K2DH_ACC_DEV_NAME    "K2DH_acc_misc"
+
+/************************************************/
+/*     Accelerometer defines section         */
+/************************************************/
+
+/* Accelerometer Sensor Full Scale */
+#define K2DH_ACC_FS_MASK        0x30
+#define K2DH_ACC_G_2G               0x00
+#define K2DH_ACC_G_4G               0x10
+#define K2DH_ACC_G_8G               0x20
+#define K2DH_ACC_G_16G              0x30
+
+
+/* Accelerometer Sensor Operating Mode */
+#define K2DH_ACC_ENABLE        0x01
+#define K2DH_ACC_DISABLE        0x00
+
+#define K2DH_SUCCESS                                0
+#define K2DH_ERR_I2C                                -1
+#define K2DH_ERR_STATUS                         -3
+#define K2DH_ERR_SETUP_FAILURE          -4
+#define K2DH_ERR_GETGSENSORDATA     -5
+#define K2DH_ERR_IDENTIFICATION         -6
+
+#define K2DH_BUFSIZE                    256
+#endif
diff --git a/drivers/misc/mediatek/accelerometer/kxtik1004/Makefile b/drivers/misc/mediatek/accelerometer/kxtik1004/Makefile
new file mode 100644
index 000000000..10684fd84
--- /dev/null
+++ b/drivers/misc/mediatek/accelerometer/kxtik1004/Makefile
@@ -0,0 +1,4 @@
+include $(srctree)/drivers/misc/mediatek/Makefile.custom
+
+obj-y	:=  kxtik1004.o
+
diff --git a/drivers/misc/mediatek/accelerometer/kxtik1004/kxtik1004.c b/drivers/misc/mediatek/accelerometer/kxtik1004/kxtik1004.c
new file mode 100644
index 000000000..f2cf76763
--- /dev/null
+++ b/drivers/misc/mediatek/accelerometer/kxtik1004/kxtik1004.c
@@ -0,0 +1,2106 @@
+/* KXTIK1004 motion sensor driver
+ *
+ *
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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.
+ *
+ */
+
+#include <linux/interrupt.h>
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include <linux/irq.h>
+#include <linux/miscdevice.h>
+#include <asm/uaccess.h>
+#include <linux/delay.h>
+#include <linux/input.h>
+#include <linux/workqueue.h>
+#include <linux/kobject.h>
+#include <linux/earlysuspend.h>
+#include <linux/platform_device.h>
+#include <asm/atomic.h>
+
+//#include <mach/mt_devs.h>
+#include <mach/mt_typedefs.h>
+#include <mach/mt_gpio.h>
+#include <mach/mt_pm_ldo.h>
+
+#define POWER_NONE_MACRO MT65XX_POWER_NONE
+
+#include <cust_acc.h>
+#include <linux/hwmsensor.h>
+#include <linux/hwmsen_dev.h>
+#include <linux/sensors_io.h>
+#include "kxtik1004.h"
+#include <linux/hwmsen_helper.h>
+/*----------------------------------------------------------------------------*/
+#define I2C_DRIVERID_KXTIK1004 150
+/*----------------------------------------------------------------------------*/
+#define DEBUG 1
+/*----------------------------------------------------------------------------*/
+//#define CONFIG_KXTIK1004_LOWPASS   /*apply low pass filter on output*/       
+#define SW_CALIBRATION
+
+/*----------------------------------------------------------------------------*/
+#define KXTIK1004_AXIS_X          0
+#define KXTIK1004_AXIS_Y          1
+#define KXTIK1004_AXIS_Z          2
+#define KXTIK1004_AXES_NUM        3
+#define KXTIK1004_DATA_LEN        6
+#define KXTIK1004_DEV_NAME        "KXTIK1004"
+/*----------------------------------------------------------------------------*/
+
+/*----------------------------------------------------------------------------*/
+static const struct i2c_device_id kxtik1004_i2c_id[] = {{KXTIK1004_DEV_NAME,0},{}};
+static struct i2c_board_info __initdata i2c_kxtik1004={ I2C_BOARD_INFO(KXTIK1004_DEV_NAME, (KXTIK1004_I2C_SLAVE_ADDR>>1))};
+/*the adapter id will be available in customization*/
+//static unsigned short kxtik1004_force[] = {0x00, KXTIK1004_I2C_SLAVE_ADDR, I2C_CLIENT_END, I2C_CLIENT_END};
+//static const unsigned short *const kxtik1004_forces[] = { kxtik1004_force, NULL };
+//static struct i2c_client_address_data kxtik1004_addr_data = { .forces = kxtik1004_forces,};
+
+/*----------------------------------------------------------------------------*/
+static int kxtik1004_i2c_probe(struct i2c_client *client, const struct i2c_device_id *id); 
+static int kxtik1004_i2c_remove(struct i2c_client *client);
+static int kxtik1004_i2c_detect(struct i2c_client *client, struct i2c_board_info *info);
+#ifndef USE_EARLY_SUSPEND
+static int kxtik1004_suspend(struct i2c_client *client, pm_message_t msg);
+static int kxtik1004_resume(struct i2c_client *client);
+#endif
+/*----------------------------------------------------------------------------*/
+typedef enum {
+    ADX_TRC_FILTER  = 0x01,
+    ADX_TRC_RAWDATA = 0x02,
+    ADX_TRC_IOCTL   = 0x04,
+    ADX_TRC_CALI	= 0X08,
+    ADX_TRC_INFO	= 0X10,
+} ADX_TRC;
+/*----------------------------------------------------------------------------*/
+struct scale_factor{
+    u8  whole;
+    u8  fraction;
+};
+/*----------------------------------------------------------------------------*/
+struct data_resolution {
+    struct scale_factor scalefactor;
+    int                 sensitivity;
+};
+/*----------------------------------------------------------------------------*/
+#define C_MAX_FIR_LENGTH (32)
+/*----------------------------------------------------------------------------*/
+struct data_filter {
+    s16 raw[C_MAX_FIR_LENGTH][KXTIK1004_AXES_NUM];
+    int sum[KXTIK1004_AXES_NUM];
+    int num;
+    int idx;
+};
+/*----------------------------------------------------------------------------*/
+struct kxtik1004_i2c_data {
+    struct i2c_client *client;
+    struct acc_hw *hw;
+    struct hwmsen_convert   cvt;
+    
+    /*misc*/
+    struct data_resolution *reso;
+    atomic_t                trace;
+    atomic_t                suspend;
+    atomic_t                selftest;
+	atomic_t				filter;
+    s16                     cali_sw[KXTIK1004_AXES_NUM+1];
+
+    /*data*/
+    s8                      offset[KXTIK1004_AXES_NUM+1];  /*+1: for 4-byte alignment*/
+    s16                     data[KXTIK1004_AXES_NUM+1];
+
+#if defined(CONFIG_KXTIK1004_LOWPASS)
+    atomic_t                firlen;
+    atomic_t                fir_en;
+    struct data_filter      fir;
+#endif 
+    /*early suspend*/
+#ifdef USE_EARLY_SUSPEND
+    struct early_suspend    early_drv;
+#endif     
+};
+/*----------------------------------------------------------------------------*/
+static struct i2c_driver kxtik1004_i2c_driver = {
+    .driver = {
+//        .owner          = THIS_MODULE,
+        .name           = KXTIK1004_DEV_NAME,
+    },
+	.probe      		= kxtik1004_i2c_probe,
+	.remove    			= kxtik1004_i2c_remove,
+	.detect				= kxtik1004_i2c_detect,
+#if !defined(USE_EARLY_SUSPEND)    
+    .suspend            = kxtik1004_suspend,
+    .resume             = kxtik1004_resume,
+#endif
+	.id_table = kxtik1004_i2c_id,
+//	.address_data = &kxtik1004_addr_data,
+};
+
+/*----------------------------------------------------------------------------*/
+static struct i2c_client *kxtik1004_i2c_client = NULL;
+static struct platform_driver kxtik1004_gsensor_driver;
+static struct kxtik1004_i2c_data *obj_i2c_data = NULL;
+static bool sensor_power = true;
+static int sensor_suspend = 0;
+static GSENSOR_VECTOR3D gsensor_gain;
+static char selftestRes[8]= {0}; 
+static DEFINE_MUTEX(kxtik1004_i2c_mutex);
+static DEFINE_MUTEX(kxtik1004_op_mutex);
+
+static bool enable_status = false;
+
+/*----------------------------------------------------------------------------*/
+#define GSE_TAG                  "[Gsensor] "
+#define GSE_FUN(f)               printk(KERN_INFO GSE_TAG"%s\n", __FUNCTION__)
+#define GSE_ERR(fmt, args...)    printk(KERN_ERR GSE_TAG"%s %d : "fmt, __FUNCTION__, __LINE__, ##args)
+#define GSE_LOG(fmt, args...)    printk(KERN_INFO GSE_TAG fmt, ##args)
+/*----------------------------------------------------------------------------*/
+static struct data_resolution kxtik1004_data_resolution[1] = {
+ /* combination by {FULL_RES,RANGE}*/
+    {{ 0, 9}, 1024}, // dataformat +/-2g  in 12-bit resolution;  { 3, 9} = 3.9 = (2*2*1000)/(2^12);  256 = (2^12)/(2*2)          
+};
+/*----------------------------------------------------------------------------*/
+static struct data_resolution kxtik1004_offset_resolution = {{15, 6}, 64};
+/*----------------------------------------------------------------------------*/
+static int kxt_i2c_read_block(struct i2c_client *client, u8 addr, u8 *data, u8 len)
+{
+    u8 beg = addr;
+	int err;
+	struct i2c_msg msgs[2]={{0},{0}};
+	
+	mutex_lock(&kxtik1004_i2c_mutex);
+	
+	msgs[0].addr = client->addr;
+	msgs[0].flags = 0;
+	msgs[0].len =1;
+	msgs[0].buf = &beg;
+
+	msgs[1].addr = client->addr;
+	msgs[1].flags = I2C_M_RD;
+	msgs[1].len =len;
+	msgs[1].buf = data;
+	
+	if (!client)
+	{
+	    mutex_unlock(&kxtik1004_i2c_mutex);
+		return -EINVAL;
+	}
+	else if (len > C_I2C_FIFO_SIZE) 
+	{
+		GSE_ERR(" length %d exceeds %d\n", len, C_I2C_FIFO_SIZE);
+		mutex_unlock(&kxtik1004_i2c_mutex);
+		return -EINVAL;
+	}
+	err = i2c_transfer(client->adapter, msgs, sizeof(msgs)/sizeof(msgs[0]));
+	if (err != 2) 
+	{
+		GSE_ERR("i2c_transfer error: (%d %p %d) %d\n",addr, data, len, err);
+		err = -EIO;
+	} 
+	else 
+	{
+		err = 0;
+	}
+	mutex_unlock(&kxtik1004_i2c_mutex);
+	return err;
+
+}
+
+static int kxt_i2c_write_block(struct i2c_client *client, u8 addr, u8 *data, u8 len)
+{   /*because address also occupies one byte, the maximum length for write is 7 bytes*/
+    int err, idx, num;
+    char buf[C_I2C_FIFO_SIZE];
+    err =0;
+	mutex_lock(&kxtik1004_i2c_mutex);
+    if (!client)
+    {
+        mutex_unlock(&kxtik1004_i2c_mutex);
+        return -EINVAL;
+    }
+    else if (len >= C_I2C_FIFO_SIZE) 
+	{        
+        GSE_ERR(" length %d exceeds %d\n", len, C_I2C_FIFO_SIZE);
+		mutex_unlock(&kxtik1004_i2c_mutex);
+        return -EINVAL;
+    }    
+
+    num = 0;
+    buf[num++] = addr;
+    for (idx = 0; idx < len; idx++)
+    {
+        buf[num++] = data[idx];
+    }
+
+    err = i2c_master_send(client, buf, num);
+    if (err < 0)
+	{
+        GSE_ERR("send command error!!\n");
+		mutex_unlock(&kxtik1004_i2c_mutex);
+        return -EFAULT;
+    } 
+	mutex_unlock(&kxtik1004_i2c_mutex);
+    return err;
+}
+/*----------------------------------------------------------------------------*/
+/*----------------------------------------------------------------------------*/
+static int KXTIK1004_SetPowerMode(struct i2c_client *client, bool enable);
+/*--------------------KXTIK1004 power control function----------------------------------*/
+static void KXTIK1004_power(struct acc_hw *hw, unsigned int on) 
+{
+	static unsigned int power_on = 0;
+
+	if(hw->power_id != POWER_NONE_MACRO)		// have externel LDO
+	{        
+		GSE_LOG("power %s\n", on ? "on" : "off");
+		if(power_on == on)	// power status not change
+		{
+			GSE_LOG("ignore power control: %d\n", on);
+		}
+		else if(on)	// power on
+		{
+			if(!hwPowerOn(hw->power_id, hw->power_vol, "KXTIK1004"))
+			{
+				GSE_ERR("power on fails!!\n");
+			}
+		}
+		else	// power off
+		{
+			if (!hwPowerDown(hw->power_id, "KXTIK1004"))
+			{
+				GSE_ERR("power off fail!!\n");
+			}			  
+		}
+	}
+	power_on = on;    
+}
+/*----------------------------------------------------------------------------*/
+
+/*----------------------------------------------------------------------------*/
+static int KXTIK1004_SetDataResolution(struct kxtik1004_i2c_data *obj)
+{
+	int err;
+	u8  databuf[2];
+
+	KXTIK1004_SetPowerMode(obj->client, false);
+
+	if(kxt_i2c_read_block(obj->client, KXTIK1004_REG_DATA_RESOLUTION, databuf, 0x01))
+	{
+		printk("kxtik1004 read Dataformat failt \n");
+		return KXTIK1004_ERR_I2C;
+	}
+
+	databuf[0] &= ~KXTIK1004_RANGE_DATA_RESOLUTION_MASK;
+	databuf[0] |= KXTIK1004_RANGE_DATA_RESOLUTION_MASK;//12bit
+
+	err = kxt_i2c_write_block(obj->client, KXTIK1004_REG_DATA_RESOLUTION, databuf, 0x1);
+
+	if(err < 0)
+	{
+		return KXTIK1004_ERR_I2C;
+	}
+
+	KXTIK1004_SetPowerMode(obj->client, true);
+
+	//kxtik1004_data_resolution[0] has been set when initialize: +/-2g  in 8-bit resolution:  15.6 mg/LSB*/   
+	obj->reso = &kxtik1004_data_resolution[0];
+
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int KXTIK1004_ReadData(struct i2c_client *client, s16 data[KXTIK1004_AXES_NUM])
+{
+	struct kxtik1004_i2c_data *priv = i2c_get_clientdata(client);        
+	u8 addr = KXTIK1004_REG_DATAX0;
+	u8 buf[KXTIK1004_DATA_LEN] = {0};
+	int err = 0;
+	int i;
+
+	if(NULL == client)
+	{
+		err = -EINVAL;
+	}
+	else if((err = kxt_i2c_read_block(client, addr, buf, 0x06)))
+	{
+		GSE_ERR("error: %d\n", err);
+	}
+	else
+	{
+		data[KXTIK1004_AXIS_X] = (s16)((buf[KXTIK1004_AXIS_X*2] >> 4) |
+		         (buf[KXTIK1004_AXIS_X*2+1] << 4));
+		data[KXTIK1004_AXIS_Y] = (s16)((buf[KXTIK1004_AXIS_Y*2] >> 4) |
+		         (buf[KXTIK1004_AXIS_Y*2+1] << 4));
+		data[KXTIK1004_AXIS_Z] = (s16)((buf[KXTIK1004_AXIS_Z*2] >> 4) |
+		         (buf[KXTIK1004_AXIS_Z*2+1] << 4));
+
+		for(i=0;i<3;i++)				
+		{								//because the data is store in binary complement number formation in computer system
+			if ( data[i] == 0x0800 )	//so we want to calculate actual number here
+				data[i]= -2048;			//10bit resolution, 512= 2^(12-1)
+			else if ( data[i] & 0x0800 )//transfor format
+			{							//printk("data 0 step %x \n",data[i]);
+				data[i] -= 0x1; 		//printk("data 1 step %x \n",data[i]);
+				data[i] = ~data[i]; 	//printk("data 2 step %x \n",data[i]);
+				data[i] &= 0x07ff;		//printk("data 3 step %x \n\n",data[i]);
+				data[i] = -data[i]; 	
+			}
+		}	
+
+
+		if(atomic_read(&priv->trace) & ADX_TRC_RAWDATA)
+		{
+			GSE_LOG("[%08X %08X %08X] => [%5d %5d %5d]\n", data[KXTIK1004_AXIS_X], data[KXTIK1004_AXIS_Y], data[KXTIK1004_AXIS_Z],
+		                               data[KXTIK1004_AXIS_X], data[KXTIK1004_AXIS_Y], data[KXTIK1004_AXIS_Z]);
+		}
+#ifdef CONFIG_KXTIK1004_LOWPASS
+		if(atomic_read(&priv->filter))
+		{
+			if(atomic_read(&priv->fir_en) && !atomic_read(&priv->suspend))
+			{
+				int idx, firlen = atomic_read(&priv->firlen);   
+				if(priv->fir.num < firlen)
+				{                
+					priv->fir.raw[priv->fir.num][KXTIK1004_AXIS_X] = data[KXTIK1004_AXIS_X];
+					priv->fir.raw[priv->fir.num][KXTIK1004_AXIS_Y] = data[KXTIK1004_AXIS_Y];
+					priv->fir.raw[priv->fir.num][KXTIK1004_AXIS_Z] = data[KXTIK1004_AXIS_Z];
+					priv->fir.sum[KXTIK1004_AXIS_X] += data[KXTIK1004_AXIS_X];
+					priv->fir.sum[KXTIK1004_AXIS_Y] += data[KXTIK1004IK_AXIS_Y];
+					priv->fir.sum[KXTIK1004_AXIS_Z] += data[KXTIK1004_AXIS_Z];
+					if(atomic_read(&priv->trace) & ADX_TRC_FILTER)
+					{
+						GSE_LOG("add [%2d] [%5d %5d %5d] => [%5d %5d %5d]\n", priv->fir.num,
+							priv->fir.raw[priv->fir.num][KXTIK1004_AXIS_X], priv->fir.raw[priv->fir.num][KXTIK1004_AXIS_Y], priv->fir.raw[priv->fir.num][KXTIK1004_AXIS_Z],
+							priv->fir.sum[KXTIK1004_AXIS_X], priv->fir.sum[KXTIK1004_AXIS_Y], priv->fir.sum[KXTIK1004_AXIS_Z]);
+					}
+					priv->fir.num++;
+					priv->fir.idx++;
+				}
+				else
+				{
+					idx = priv->fir.idx % firlen;
+					priv->fir.sum[KXTIK1004_AXIS_X] -= priv->fir.raw[idx][KXTIK1004_AXIS_X];
+					priv->fir.sum[KXTIK1004_AXIS_Y] -= priv->fir.raw[idx][KXTIK1004_AXIS_Y];
+					priv->fir.sum[KXTIK1004_AXIS_Z] -= priv->fir.raw[idx][KXTIK1004_AXIS_Z];
+					priv->fir.raw[idx][KXTIK1004_AXIS_X] = data[KXTIK1004_AXIS_X];
+					priv->fir.raw[idx][KXTIK1004_AXIS_Y] = data[KXTIK1004_AXIS_Y];
+					priv->fir.raw[idx][KXTIK1004_AXIS_Z] = data[KXTIK1004_AXIS_Z];
+					priv->fir.sum[KXTIK1004_AXIS_X] += data[KXTIK1004_AXIS_X];
+					priv->fir.sum[KXTIK1004_AXIS_Y] += data[KXTIK1004_AXIS_Y];
+					priv->fir.sum[KXTIK1004_AXIS_Z] += data[KXTIK1004_AXIS_Z];
+					priv->fir.idx++;
+					data[KXTIK1004_AXIS_X] = priv->fir.sum[KXTIK1004_AXIS_X]/firlen;
+					data[KXTIK1004_AXIS_Y] = priv->fir.sum[KXTIK1004_AXIS_Y]/firlen;
+					data[KXTIK1004_AXIS_Z] = priv->fir.sum[KXTIK1004_AXIS_Z]/firlen;
+					if(atomic_read(&priv->trace) & ADX_TRC_FILTER)
+					{
+						GSE_LOG("add [%2d] [%5d %5d %5d] => [%5d %5d %5d] : [%5d %5d %5d]\n", idx,
+						priv->fir.raw[idx][KXTIK1004_AXIS_X], priv->fir.raw[idx][KXTIK1004_AXIS_Y], priv->fir.raw[idx][KXTIK1004_AXIS_Z],
+						priv->fir.sum[KXTIK1004_AXIS_X], priv->fir.sum[KXTIK1004_AXIS_Y], priv->fir.sum[KXTIK1004_AXIS_Z],
+						data[KXTIK1004_AXIS_X], data[KXTIK1004_AXIS_Y], data[KXTIK1004_AXIS_Z]);
+					}
+				}
+			}
+		}	
+#endif         
+	}
+	return err;
+}
+/*----------------------------------------------------------------------------*/
+static int KXTIK1004_ReadOffset(struct i2c_client *client, s8 ofs[KXTIK1004_AXES_NUM])
+{    
+	int err = 0;
+
+	ofs[1]=ofs[2]=ofs[0]=0x00;
+
+	printk("offesx=%x, y=%x, z=%x",ofs[0],ofs[1],ofs[2]);
+	
+	return err;    
+}
+/*----------------------------------------------------------------------------*/
+static int KXTIK1004_ResetCalibration(struct i2c_client *client)
+{
+	struct kxtik1004_i2c_data *obj = i2c_get_clientdata(client);
+	//u8 ofs[4]={0,0,0,0};
+	int err = 0;
+
+	memset(obj->cali_sw, 0x00, sizeof(obj->cali_sw));
+	memset(obj->offset, 0x00, sizeof(obj->offset));
+	return err;    
+}
+/*----------------------------------------------------------------------------*/
+static int KXTIK1004_ReadCalibration(struct i2c_client *client, int dat[KXTIK1004_AXES_NUM])
+{
+    struct kxtik1004_i2c_data *obj = i2c_get_clientdata(client);
+    int  err = 0;
+    int mul;
+
+	#ifdef SW_CALIBRATION
+		mul = 0;//only SW Calibration, disable HW Calibration
+	#else
+	    if ((err = KXTIK1004_ReadOffset(client, obj->offset))) {
+        GSE_ERR("read offset fail, %d\n", err);
+        return err;
+    	}    
+    	mul = obj->reso->sensitivity/kxtik1004_offset_resolution.sensitivity;
+	#endif
+
+    dat[obj->cvt.map[KXTIK1004_AXIS_X]] = obj->cvt.sign[KXTIK1004_AXIS_X]*(obj->offset[KXTIK1004_AXIS_X]*mul * GRAVITY_EARTH_1000 / obj->reso->sensitivity + obj->cali_sw[KXTIK1004_AXIS_X]);
+    dat[obj->cvt.map[KXTIK1004_AXIS_Y]] = obj->cvt.sign[KXTIK1004_AXIS_Y]*(obj->offset[KXTIK1004_AXIS_Y]*mul * GRAVITY_EARTH_1000 / obj->reso->sensitivity + obj->cali_sw[KXTIK1004_AXIS_Y]);
+    dat[obj->cvt.map[KXTIK1004_AXIS_Z]] = obj->cvt.sign[KXTIK1004_AXIS_Z]*(obj->offset[KXTIK1004_AXIS_Z]*mul * GRAVITY_EARTH_1000 / obj->reso->sensitivity + obj->cali_sw[KXTIK1004_AXIS_Z]);                        
+                                       
+    return err;
+}
+/*----------------------------------------------------------------------------*/
+static int KXTIK1004_ReadCalibrationEx(struct i2c_client *client, int act[KXTIK1004_AXES_NUM], int raw[KXTIK1004_AXES_NUM])
+{  
+	/*raw: the raw calibration data; act: the actual calibration data*/
+	struct kxtik1004_i2c_data *obj = i2c_get_clientdata(client);
+	int err;
+	int mul;
+	err = 0;
+ 
+
+	#ifdef SW_CALIBRATION
+		mul = 0;//only SW Calibration, disable HW Calibration
+	#else
+		if((err = KXTIK1004_ReadOffset(client, obj->offset)))
+		{
+			GSE_ERR("read offset fail, %d\n", err);
+			return err;
+		}   
+		mul = obj->reso->sensitivity/kxtik1004_offset_resolution.sensitivity;
+	#endif
+	
+	raw[KXTIK1004_AXIS_X] = obj->offset[KXTIK1004_AXIS_X]*mul * GRAVITY_EARTH_1000 / obj->reso->sensitivity + obj->cali_sw[KXTIK1004_AXIS_X];
+	raw[KXTIK1004_AXIS_Y] = obj->offset[KXTIK1004_AXIS_Y]*mul * GRAVITY_EARTH_1000 / obj->reso->sensitivity + obj->cali_sw[KXTIK1004_AXIS_Y];
+	raw[KXTIK1004_AXIS_Z] = obj->offset[KXTIK1004_AXIS_Z]*mul * GRAVITY_EARTH_1000 / obj->reso->sensitivity + obj->cali_sw[KXTIK1004_AXIS_Z];
+
+	act[obj->cvt.map[KXTIK1004_AXIS_X]] = obj->cvt.sign[KXTIK1004_AXIS_X]*raw[KXTIK1004_AXIS_X];
+	act[obj->cvt.map[KXTIK1004_AXIS_Y]] = obj->cvt.sign[KXTIK1004_AXIS_Y]*raw[KXTIK1004_AXIS_Y];
+	act[obj->cvt.map[KXTIK1004_AXIS_Z]] = obj->cvt.sign[KXTIK1004_AXIS_Z]*raw[KXTIK1004_AXIS_Z];                        
+	                       
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int KXTIK1004_WriteCalibration(struct i2c_client *client, int dat[KXTIK1004_AXES_NUM])
+{
+	struct kxtik1004_i2c_data *obj = i2c_get_clientdata(client);
+	int err = 0;
+	int cali[KXTIK1004_AXES_NUM], raw[KXTIK1004_AXES_NUM];
+	//int lsb = kxtik1004_offset_resolution.sensitivity;
+	//int divisor = obj->reso->sensitivity/lsb;
+
+	if((err = KXTIK1004_ReadCalibrationEx(client, cali, raw)))	/*offset will be updated in obj->offset*/
+	{ 
+		GSE_ERR("read offset fail, %d\n", err);
+		return err;
+	}
+
+	GSE_LOG("OLDOFF: (%+3d %+3d %+3d): (%+3d %+3d %+3d) / (%+3d %+3d %+3d)\n", 
+		raw[KXTIK1004_AXIS_X], raw[KXTIK1004_AXIS_Y], raw[KXTIK1004_AXIS_Z],
+		obj->offset[KXTIK1004_AXIS_X], obj->offset[KXTIK1004_AXIS_Y], obj->offset[KXTIK1004_AXIS_Z],
+		obj->cali_sw[KXTIK1004_AXIS_X], obj->cali_sw[KXTIK1004_AXIS_Y], obj->cali_sw[KXTIK1004_AXIS_Z]);
+
+	/*calculate the real offset expected by caller*/
+	cali[KXTIK1004_AXIS_X] += dat[KXTIK1004_AXIS_X];
+	cali[KXTIK1004_AXIS_Y] += dat[KXTIK1004_AXIS_Y];
+	cali[KXTIK1004_AXIS_Z] += dat[KXTIK1004_AXIS_Z];
+
+	GSE_LOG("UPDATE: (%+3d %+3d %+3d)\n", 
+		dat[KXTIK1004_AXIS_X], dat[KXTIK1004_AXIS_Y], dat[KXTIK1004_AXIS_Z]);
+
+#ifdef SW_CALIBRATION
+	obj->cali_sw[KXTIK1004_AXIS_X] = obj->cvt.sign[KXTIK1004_AXIS_X]*(cali[obj->cvt.map[KXTIK1004_AXIS_X]]);
+	obj->cali_sw[KXTIK1004_AXIS_Y] = obj->cvt.sign[KXTIK1004_AXIS_Y]*(cali[obj->cvt.map[KXTIK1004_AXIS_Y]]);
+	obj->cali_sw[KXTIK1004_AXIS_Z] = obj->cvt.sign[KXTIK1004_AXIS_Z]*(cali[obj->cvt.map[KXTIK1004_AXIS_Z]]);	
+#else
+	obj->offset[KXTIK1004_AXIS_X] = (s8)(obj->cvt.sign[KXTIK1004_AXIS_X]*(cali[obj->cvt.map[KXTIK1004_AXIS_X]]) * obj->reso->sensitivity / GRAVITY_EARTH_1000/(divisor));
+	obj->offset[KXTIK1004_AXIS_Y] = (s8)(obj->cvt.sign[KXTIK1004_AXIS_Y]*(cali[obj->cvt.map[KXTIK1004_AXIS_Y]]) * obj->reso->sensitivity / GRAVITY_EARTH_1000/(divisor));
+	obj->offset[KXTIK1004_AXIS_Z] = (s8)(obj->cvt.sign[KXTIK1004_AXIS_Z]*(cali[obj->cvt.map[KXTIK1004_AXIS_Z]]) * obj->reso->sensitivity / GRAVITY_EARTH_1000/(divisor));
+
+	/*convert software calibration using standard calibration*/
+	obj->cali_sw[KXTIK1004_AXIS_X] = obj->cvt.sign[KXTIK1004_AXIS_X]*(cali[obj->cvt.map[KXTIK1004_AXIS_X]])%(divisor);
+	obj->cali_sw[KXTIK1004_AXIS_Y] = obj->cvt.sign[KXTIK1004_AXIS_Y]*(cali[obj->cvt.map[KXTIK1004_AXIS_Y]])%(divisor);
+	obj->cali_sw[KXTIK1004_AXIS_Z] = obj->cvt.sign[KXTIK1004_AXIS_Z]*(cali[obj->cvt.map[KXTIK1004_AXIS_Z]])%(divisor);
+
+	GSE_LOG("NEWOFF: (%+3d %+3d %+3d): (%+3d %+3d %+3d) / (%+3d %+3d %+3d)\n", 
+		obj->offset[KXTIK1004_AXIS_X]*divisor + obj->cali_sw[KXTIK1004_AXIS_X], 
+		obj->offset[KXTIK1004_AXIS_Y]*divisor + obj->cali_sw[KXTIK1004_AXIS_Y], 
+		obj->offset[KXTIK1004_AXIS_Z]*divisor + obj->cali_sw[KXTIK1004_AXIS_Z], 
+		obj->offset[KXTIK1004_AXIS_X], obj->offset[KXTIK1004_AXIS_Y], obj->offset[KXTIK1004_AXIS_Z],
+		obj->cali_sw[KXTIK1004_AXIS_X], obj->cali_sw[KXTIK1004_AXIS_Y], obj->cali_sw[KXTIK1004_AXIS_Z]);
+
+	if((err = kxt_i2c_write_block(obj->client, KXTIK1004_REG_OFSX, obj->offset, KXTIK1004_AXES_NUM))<0)
+	{
+		GSE_ERR("write offset fail: %d\n", err);
+		return err;
+	}
+#endif
+
+	return err;
+}
+/*----------------------------------------------------------------------------*/
+static int KXTIK1004_CheckDeviceID(struct i2c_client *client)
+{
+	u8 databuf[10];    
+	int res = 0;
+
+	memset(databuf, 0, sizeof(u8)*10);    
+
+	res = kxt_i2c_read_block(client, KXTIK1004_REG_DEVID, databuf, 0x1);
+	if(res < 0)
+	{
+		goto exit_KXTIK1004_CheckDeviceID;
+	}
+
+	if(databuf[0] == KXTIK1004_DEVICE_ID)
+	{
+		GSE_LOG("KXTIK1004_CheckDeviceID 0x%x pass!\n ", databuf[0]);
+	}
+	else if(databuf[0] == KXCJK1013_DEVICE_ID)
+	{
+		GSE_LOG("KXCJK1013_CheckDeviceID 0x%x pass!\n ", databuf[0]);/*because KXCJK-1013 use same driver with KXTIK1004*/
+	}
+	else
+	{
+		GSE_LOG("KXTIK1004_CheckDeviceID 0x%x failt!\n ", databuf[0]);
+		return KXTIK1004_ERR_IDENTIFICATION;
+	}
+
+	exit_KXTIK1004_CheckDeviceID:
+	if (res < 0)
+	{
+		return KXTIK1004_ERR_I2C;
+	}
+	
+	return KXTIK1004_SUCCESS;
+}
+/*----------------------------------------------------------------------------*/
+static int KXTIK1004_SetPowerMode(struct i2c_client *client, bool enable)
+{
+	u8 databuf[2];    
+	int res = 0;
+	u8 addr = KXTIK1004_REG_POWER_CTL;
+	//struct kxtik1004_i2c_data *obj = i2c_get_clientdata(client);
+	
+	
+	if(enable == sensor_power)
+	{
+		GSE_LOG("Sensor power status is newest!\n");
+		return KXTIK1004_SUCCESS;
+	}
+
+	if(kxt_i2c_read_block(client, addr, databuf, 0x01))
+	{
+		GSE_ERR("read power ctl register err!\n");
+		return KXTIK1004_ERR_I2C;
+	}
+
+	
+	if(enable == TRUE)
+	{
+		databuf[0] |= KXTIK1004_MEASURE_MODE;
+	}
+	else
+	{
+		databuf[0] &= ~KXTIK1004_MEASURE_MODE;
+	}
+
+	res = kxt_i2c_write_block(client, KXTIK1004_REG_POWER_CTL, databuf, 0x1);
+	if(res < 0)
+	{
+		return KXTIK1004_ERR_I2C;
+	}
+
+
+	GSE_LOG("KXTIK1004_SetPowerMode %d!\n ",enable);
+
+
+	sensor_power = enable;
+
+	mdelay(50);
+	
+	return KXTIK1004_SUCCESS;    
+}
+/*----------------------------------------------------------------------------*/
+static int KXTIK1004_SetDataFormat(struct i2c_client *client, u8 dataformat)
+{
+	struct kxtik1004_i2c_data *obj = i2c_get_clientdata(client);
+	u8 databuf[10];    
+	int res = 0;
+
+	memset(databuf, 0, sizeof(u8)*10);  
+
+	KXTIK1004_SetPowerMode(client, false);
+
+	if(kxt_i2c_read_block(client, KXTIK1004_REG_DATA_FORMAT, databuf, 0x01))
+	{
+		GSE_LOG("kxtik1004 read Dataformat failt \n");
+		return KXTIK1004_ERR_I2C;
+	}
+
+	databuf[0] &= ~KXTIK1004_RANGE_MASK;
+	databuf[0] |= dataformat;
+
+	res = kxt_i2c_write_block(client, KXTIK1004_REG_DATA_FORMAT, databuf, 0x1);
+	if(res < 0)
+	{
+		return KXTIK1004_ERR_I2C;
+	}
+
+	KXTIK1004_SetPowerMode(client, true);
+	
+	GSE_LOG("KXTIK1004_SetDataFormat OK! \n");
+	
+
+	return KXTIK1004_SetDataResolution(obj);    
+}
+/*----------------------------------------------------------------------------*/
+static int KXTIK1004_SetBWRate(struct i2c_client *client, u8 bwrate)
+{
+	u8 databuf[10]={0};    
+	int res = 0;
+
+	memset(databuf, 0, sizeof(u8)*10);    
+
+	if(kxt_i2c_read_block(client, KXTIK1004_REG_BW_RATE, databuf, 0x01))
+	{
+		GSE_LOG("kxtik1004 read rate failt \n");
+		return KXTIK1004_ERR_I2C;
+	}
+
+	databuf[0] &= 0xf8;
+	databuf[0] |= bwrate;
+
+	res = kxt_i2c_write_block(client, KXTIK1004_REG_BW_RATE, databuf, 0x1);
+	if(res < 0)
+	{
+		return KXTIK1004_ERR_I2C;
+	}
+	
+	GSE_LOG("KXTIK1004_SetBWRate OK! \n");
+	
+	return KXTIK1004_SUCCESS;    
+}
+/*----------------------------------------------------------------------------*/
+static int KXTIK1004_SetIntEnable(struct i2c_client *client, u8 intenable)
+{
+	u8 databuf[10];    
+	int res = 0;
+
+	memset(databuf, 0, sizeof(u8)*10);    
+	databuf[0] =  0x00;
+
+	res = kxt_i2c_write_block(client, KXTIK1004_REG_INT_ENABLE, databuf, 0x1);
+
+	if(res < 0)
+	{
+		return KXTIK1004_ERR_I2C;
+	}
+	
+	return KXTIK1004_SUCCESS;    
+}
+/*----------------------------------------------------------------------------*/
+static int kxtik1004_init_client(struct i2c_client *client, int reset_cali)
+{
+	struct kxtik1004_i2c_data *obj = i2c_get_clientdata(client);
+	int res = 0;
+
+	res = KXTIK1004_CheckDeviceID(client); 
+	if(res != KXTIK1004_SUCCESS)
+	{
+		return res;
+	}	
+
+	res = KXTIK1004_SetPowerMode(client, enable_status);
+	if(res != KXTIK1004_SUCCESS)
+	{
+		return res;
+	}
+	
+
+	res = KXTIK1004_SetBWRate(client, KXTIK1004_BW_100HZ);
+	if(res != KXTIK1004_SUCCESS ) //0x2C->BW=100Hz
+	{
+		return res;
+	}
+
+	res = KXTIK1004_SetDataFormat(client, KXTIK1004_RANGE_2G);
+	if(res != KXTIK1004_SUCCESS) //0x2C->BW=100Hz
+	{
+		return res;
+	}
+
+	gsensor_gain.x = gsensor_gain.y = gsensor_gain.z = obj->reso->sensitivity;
+
+
+	res = KXTIK1004_SetIntEnable(client, 0x00);        
+	if(res != KXTIK1004_SUCCESS)//0x2E->0x80
+	{
+		return res;
+	}
+
+	if(0 != reset_cali)
+	{ 
+		/*reset calibration only in power on*/
+		res = KXTIK1004_ResetCalibration(client);
+		if(res != KXTIK1004_SUCCESS)
+		{
+			return res;
+		}
+	}
+	GSE_LOG("kxtik1004_init_client OK!\n");
+#ifdef CONFIG_KXTIK1004_LOWPASS
+	memset(&obj->fir, 0x00, sizeof(obj->fir));  
+#endif
+
+	return KXTIK1004_SUCCESS;
+}
+/*----------------------------------------------------------------------------*/
+static int KXTIK1004_ReadChipInfo(struct i2c_client *client, char *buf, int bufsize)
+{
+	u8 databuf[10];    
+
+	memset(databuf, 0, sizeof(u8)*10);
+
+	if((NULL == buf)||(bufsize<=30))
+	{
+		return -1;
+	}
+	
+	if(NULL == client)
+	{
+		*buf = 0;
+		return -2;
+	}
+
+	sprintf(buf, "KXTIK1004 Chip");
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int KXTIK1004_ReadSensorData(struct i2c_client *client, char *buf, int bufsize)
+{
+	struct kxtik1004_i2c_data *obj = (struct kxtik1004_i2c_data*)i2c_get_clientdata(client);
+	u8 databuf[20];
+	int acc[KXTIK1004_AXES_NUM];
+	int res = 0;
+	memset(databuf, 0, sizeof(u8)*10);
+
+	if(NULL == buf)
+	{
+		return -1;
+	}
+	if(NULL == client)
+	{
+		*buf = 0;
+		return -2;
+	}
+		
+	if(sensor_suspend == 1)
+	{
+		//GSE_LOG("sensor in suspend read not data!\n");
+		return 0;
+	}
+
+	if((res = KXTIK1004_ReadData(client, obj->data)))
+	{        
+		GSE_ERR("I2C error: ret value=%d", res);
+		return -3;
+	}
+	else
+	{
+	#if 1
+		obj->data[KXTIK1004_AXIS_X] = obj->data[KXTIK1004_AXIS_X] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+		obj->data[KXTIK1004_AXIS_Y] = obj->data[KXTIK1004_AXIS_Y] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+		obj->data[KXTIK1004_AXIS_Z] = obj->data[KXTIK1004_AXIS_Z] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;		
+	#endif	
+		//printk("raw data x=%d, y=%d, z=%d \n",obj->data[KXTIK1004_AXIS_X],obj->data[KXTIK1004_AXIS_Y],obj->data[KXTIK1004_AXIS_Z]);
+		obj->data[KXTIK1004_AXIS_X] += obj->cali_sw[KXTIK1004_AXIS_X];
+		obj->data[KXTIK1004_AXIS_Y] += obj->cali_sw[KXTIK1004_AXIS_Y];
+		obj->data[KXTIK1004_AXIS_Z] += obj->cali_sw[KXTIK1004_AXIS_Z];
+		
+		//printk("cali_sw x=%d, y=%d, z=%d \n",obj->cali_sw[KXTIK1004_AXIS_X],obj->cali_sw[KXTIK1004_AXIS_Y],obj->cali_sw[KXTIK1004_AXIS_Z]);
+		
+		/*remap coordinate*/
+		acc[obj->cvt.map[KXTIK1004_AXIS_X]] = obj->cvt.sign[KXTIK1004_AXIS_X]*obj->data[KXTIK1004_AXIS_X];
+		acc[obj->cvt.map[KXTIK1004_AXIS_Y]] = obj->cvt.sign[KXTIK1004_AXIS_Y]*obj->data[KXTIK1004_AXIS_Y];
+		acc[obj->cvt.map[KXTIK1004_AXIS_Z]] = obj->cvt.sign[KXTIK1004_AXIS_Z]*obj->data[KXTIK1004_AXIS_Z];
+		//printk("cvt x=%d, y=%d, z=%d \n",obj->cvt.sign[KXTIK1004_AXIS_X],obj->cvt.sign[KXTIK1004_AXIS_Y],obj->cvt.sign[KXTIK1004_AXIS_Z]);
+
+
+		//GSE_LOG("Mapped gsensor data: %d, %d, %d!\n", acc[KXTIK1004_AXIS_X], acc[KXTIK1004_AXIS_Y], acc[KXTIK1004_AXIS_Z]);
+
+		//Out put the mg
+		//printk("mg acc=%d, GRAVITY=%d, sensityvity=%d \n",acc[KXTIK1004_AXIS_X],GRAVITY_EARTH_1000,obj->reso->sensitivity);
+#if 0
+		acc[KXTIK1004_AXIS_X] = acc[KXTIK1004_AXIS_X] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+		acc[KXTIK1004_AXIS_Y] = acc[KXTIK1004_AXIS_Y] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+		acc[KXTIK1004_AXIS_Z] = acc[KXTIK1004_AXIS_Z] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;		
+	#endif	
+	
+
+		sprintf(buf, "%04x %04x %04x", acc[KXTIK1004_AXIS_X], acc[KXTIK1004_AXIS_Y], acc[KXTIK1004_AXIS_Z]);
+		if(atomic_read(&obj->trace) & ADX_TRC_IOCTL)
+		{
+			GSE_LOG("gsensor data: %s!\n", buf);
+		}
+	}
+	
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int KXTIK1004_ReadRawData(struct i2c_client *client, char *buf)
+{
+	struct kxtik1004_i2c_data *obj = (struct kxtik1004_i2c_data*)i2c_get_clientdata(client);
+	int res = 0;
+
+	if (!buf || !client)
+	{
+		return EINVAL;
+	}
+	
+	if((res = KXTIK1004_ReadData(client, obj->data)))
+	{        
+		GSE_ERR("I2C error: ret value=%d", res);
+		return EIO;
+	}
+	else
+	{
+		sprintf(buf, "KXTIK1004_ReadRawData %04x %04x %04x", obj->data[KXTIK1004_AXIS_X], 
+			obj->data[KXTIK1004_AXIS_Y], obj->data[KXTIK1004_AXIS_Z]);
+	
+	}
+	
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int KXTIK1004_InitSelfTest(struct i2c_client *client)
+{
+	int res = 0;
+	u8  data,result;
+	
+	res = kxt_i2c_read_block(client, KXTIK1004_REG_CTL_REG3, &data, 0x1);
+	if(res < 0)
+	{
+		return res;
+	}
+//enable selftest bit
+	data |= KXTIK1004_SELF_TEST;
+	res = kxt_i2c_write_block(client, KXTIK1004_REG_CTL_REG3,  &data, 0x1);
+	if(res < 0) //0x2C->BW=100Hz
+	{
+		return res;
+	}
+//step 1
+	res = kxt_i2c_read_block(client, KXTIK1004_DCST_RESP, &result, 0x1);
+	if(res < 0)
+	{
+		return res;
+	}
+	GSE_LOG("step1: result = %x",result);
+	if(result != 0xaa)
+		return -EINVAL;
+
+//step 2
+	data |= KXTIK1004_SELF_TEST;
+	res = kxt_i2c_write_block(client, KXTIK1004_REG_CTL_REG3,  &data, 0x1);
+	if(res < 0) //0x2C->BW=100Hz
+	{
+		return res;
+	}
+//step 3
+	res = kxt_i2c_read_block(client, KXTIK1004_DCST_RESP, &result, 0x1);
+	if(res < 0)
+	{
+		return res;
+	}
+	GSE_LOG("step3: result = %x",result);
+	if(result != 0xAA)
+		return -EINVAL;
+		
+//step 4
+	res = kxt_i2c_read_block(client, KXTIK1004_DCST_RESP, &result, 0x1);
+	if(res < 0)
+	{
+		return res;
+	}
+	GSE_LOG("step4: result = %x",result);
+	if(result != 0x55)
+		return -EINVAL;
+	else
+		return KXTIK1004_SUCCESS;
+}
+/*----------------------------------------------------------------------------*/
+/*----------------------------------------------------------------------------*/
+static ssize_t show_chipinfo_value(struct device_driver *ddri, char *buf)
+{
+	struct i2c_client *client = kxtik1004_i2c_client;
+	char strbuf[KXTIK1004_BUFSIZE];
+	if(NULL == client)
+	{
+		GSE_ERR("i2c client is null!!\n");
+		return 0;
+	}
+	
+	KXTIK1004_ReadChipInfo(client, strbuf, KXTIK1004_BUFSIZE);
+	return snprintf(buf, PAGE_SIZE, "%s\n", strbuf);        
+}
+
+/*----------------------------------------------------------------------------*/
+static ssize_t show_sensordata_value(struct device_driver *ddri, char *buf)
+{
+	struct i2c_client *client = kxtik1004_i2c_client;
+	char strbuf[KXTIK1004_BUFSIZE];
+	
+	if(NULL == client)
+	{
+		GSE_ERR("i2c client is null!!\n");
+		return 0;
+	}
+	KXTIK1004_ReadSensorData(client, strbuf, KXTIK1004_BUFSIZE);
+	return snprintf(buf, PAGE_SIZE, "%s\n", strbuf);            
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t show_cali_value(struct device_driver *ddri, char *buf)
+{
+	struct i2c_client *client = kxtik1004_i2c_client;
+	struct kxtik1004_i2c_data *obj;
+	int err, len = 0, mul;
+	int tmp[KXTIK1004_AXES_NUM];
+
+	if(NULL == client)
+	{
+		GSE_ERR("i2c client is null!!\n");
+		return 0;
+	}
+
+	obj = i2c_get_clientdata(client);
+
+
+
+	if((err = KXTIK1004_ReadOffset(client, obj->offset)))
+	{
+		return -EINVAL;
+	}
+	else if((err = KXTIK1004_ReadCalibration(client, tmp)))
+	{
+		return -EINVAL;
+	}
+	else
+	{    
+		mul = obj->reso->sensitivity/kxtik1004_offset_resolution.sensitivity;
+		len += snprintf(buf+len, PAGE_SIZE-len, "[HW ][%d] (%+3d, %+3d, %+3d) : (0x%02X, 0x%02X, 0x%02X)\n", mul,                        
+			obj->offset[KXTIK1004_AXIS_X], obj->offset[KXTIK1004_AXIS_Y], obj->offset[KXTIK1004_AXIS_Z],
+			obj->offset[KXTIK1004_AXIS_X], obj->offset[KXTIK1004_AXIS_Y], obj->offset[KXTIK1004_AXIS_Z]);
+		len += snprintf(buf+len, PAGE_SIZE-len, "[SW ][%d] (%+3d, %+3d, %+3d)\n", 1, 
+			obj->cali_sw[KXTIK1004_AXIS_X], obj->cali_sw[KXTIK1004_AXIS_Y], obj->cali_sw[KXTIK1004_AXIS_Z]);
+
+		len += snprintf(buf+len, PAGE_SIZE-len, "[ALL]    (%+3d, %+3d, %+3d) : (%+3d, %+3d, %+3d)\n", 
+			obj->offset[KXTIK1004_AXIS_X]*mul + obj->cali_sw[KXTIK1004_AXIS_X],
+			obj->offset[KXTIK1004_AXIS_Y]*mul + obj->cali_sw[KXTIK1004_AXIS_Y],
+			obj->offset[KXTIK1004_AXIS_Z]*mul + obj->cali_sw[KXTIK1004_AXIS_Z],
+			tmp[KXTIK1004_AXIS_X], tmp[KXTIK1004_AXIS_Y], tmp[KXTIK1004_AXIS_Z]);
+		
+		return len;
+    }
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t store_cali_value(struct device_driver *ddri, const char *buf, size_t count)
+{
+	struct i2c_client *client = kxtik1004_i2c_client;  
+	int err, x, y, z;
+	int dat[KXTIK1004_AXES_NUM];
+
+	if(!strncmp(buf, "rst", 3))
+	{
+		if((err = KXTIK1004_ResetCalibration(client)))
+		{
+			GSE_ERR("reset offset err = %d\n", err);
+		}	
+	}
+	else if(3 == sscanf(buf, "0x%02X 0x%02X 0x%02X", &x, &y, &z))
+	{
+		dat[KXTIK1004_AXIS_X] = x;
+		dat[KXTIK1004_AXIS_Y] = y;
+		dat[KXTIK1004_AXIS_Z] = z;
+		if((err = KXTIK1004_WriteCalibration(client, dat)))
+		{
+			GSE_ERR("write calibration err = %d\n", err);
+		}		
+	}
+	else
+	{
+		GSE_ERR("invalid format\n");
+	}
+	
+	return count;
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t show_self_value(struct device_driver *ddri, char *buf)
+{
+	struct i2c_client *client = kxtik1004_i2c_client;
+
+	if(NULL == client)
+	{
+		GSE_ERR("i2c client is null!!\n");
+		return 0;
+	}
+
+    return snprintf(buf, 8, "%s\n", selftestRes);
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t store_self_value(struct device_driver *ddri, const char *buf, size_t count)
+{   /*write anything to this register will trigger the process*/
+	struct item{
+	s16 raw[KXTIK1004_AXES_NUM];
+	};
+	
+	struct i2c_client *client = kxtik1004_i2c_client;  
+	int res, num;
+	struct item *prv = NULL, *nxt = NULL;
+	//s32 avg_prv[KXTIK1004_AXES_NUM] = {0, 0, 0};
+	//s32 avg_nxt[KXTIK1004_AXES_NUM] = {0, 0, 0};
+	u8 data;
+
+
+	if(1 != sscanf(buf, "%d", &num))
+	{
+		GSE_ERR("parse number fail\n");
+		return count;
+	}
+	else if(num == 0)
+	{
+		GSE_ERR("invalid data count\n");
+		return count;
+	}
+
+	prv = kzalloc(sizeof(*prv) * num, GFP_KERNEL);
+	nxt = kzalloc(sizeof(*nxt) * num, GFP_KERNEL);
+	if (!prv || !nxt)
+	{
+		goto exit;
+	}
+
+
+	GSE_LOG("NORMAL:\n");
+	KXTIK1004_SetPowerMode(client,true); 
+
+	/*initial setting for self test*/
+	if(!KXTIK1004_InitSelfTest(client))
+	{
+		GSE_LOG("SELFTEST : PASS\n");
+		strcpy(selftestRes,"y");
+	}	
+	else
+	{
+		GSE_LOG("SELFTEST : FAIL\n");		
+		strcpy(selftestRes,"n");
+	}
+
+	res = kxt_i2c_read_block(client, KXTIK1004_REG_CTL_REG3, &data, 0x1);
+	if(res < 0)
+	{
+		return res;
+	}
+
+	data &= ~KXTIK1004_SELF_TEST;
+	res = kxt_i2c_write_block(client, KXTIK1004_REG_CTL_REG3,  &data, 0x1);
+	if(res < 0) //0x2C->BW=100Hz
+	{
+		return res;
+	}
+	
+	exit:
+	/*restore the setting*/    
+	kxtik1004_init_client(client, 0);
+	kfree(prv);
+	kfree(nxt);
+	return count;
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t show_selftest_value(struct device_driver *ddri, char *buf)
+{
+	struct i2c_client *client = kxtik1004_i2c_client;
+	struct kxtik1004_i2c_data *obj;
+
+	if(NULL == client)
+	{
+		GSE_ERR("i2c client is null!!\n");
+		return 0;
+	}
+
+	obj = i2c_get_clientdata(client);
+	return snprintf(buf, PAGE_SIZE, "%d\n", atomic_read(&obj->selftest));
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t store_selftest_value(struct device_driver *ddri, const char *buf, size_t count)
+{
+	struct kxtik1004_i2c_data *obj = obj_i2c_data;
+	int tmp;
+
+	if(NULL == obj)
+	{
+		GSE_ERR("i2c data obj is null!!\n");
+		return 0;
+	}
+	
+	
+	if(1 == sscanf(buf, "%d", &tmp))
+	{        
+		if(atomic_read(&obj->selftest) && !tmp)
+		{
+			/*enable -> disable*/
+			kxtik1004_init_client(obj->client, 0);
+		}
+		else if(!atomic_read(&obj->selftest) && tmp)
+		{
+			/*disable -> enable*/
+			KXTIK1004_InitSelfTest(obj->client);            
+		}
+		
+		GSE_LOG("selftest: %d => %d\n", atomic_read(&obj->selftest), tmp);
+		atomic_set(&obj->selftest, tmp); 
+	}
+	else
+	{ 
+		GSE_ERR("invalid content: '%s', length = %d\n", buf, count);   
+	}
+	return count;
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t show_firlen_value(struct device_driver *ddri, char *buf)
+{
+#ifdef CONFIG_KXTIK1004_LOWPASS
+	struct i2c_client *client = kxtik1004_i2c_client;
+	struct kxtik1004_i2c_data *obj = i2c_get_clientdata(client);
+	if(atomic_read(&obj->firlen))
+	{
+		int idx, len = atomic_read(&obj->firlen);
+		GSE_LOG("len = %2d, idx = %2d\n", obj->fir.num, obj->fir.idx);
+
+		for(idx = 0; idx < len; idx++)
+		{
+			GSE_LOG("[%5d %5d %5d]\n", obj->fir.raw[idx][KXTIK1004_AXIS_X], obj->fir.raw[idx][KXTIK1004_AXIS_Y], obj->fir.raw[idx][KXTIK1004_AXIS_Z]);
+		}
+		
+		GSE_LOG("sum = [%5d %5d %5d]\n", obj->fir.sum[KXTIK1004_AXIS_X], obj->fir.sum[KXTIK1004_AXIS_Y], obj->fir.sum[KXTIK1004_AXIS_Z]);
+		GSE_LOG("avg = [%5d %5d %5d]\n", obj->fir.sum[KXTIK1004_AXIS_X]/len, obj->fir.sum[KXTIK1004_AXIS_Y]/len, obj->fir.sum[KXTIK1004_AXIS_Z]/len);
+	}
+	return snprintf(buf, PAGE_SIZE, "%d\n", atomic_read(&obj->firlen));
+#else
+	return snprintf(buf, PAGE_SIZE, "not support\n");
+#endif
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t store_firlen_value(struct device_driver *ddri, const char *buf, size_t count)
+{
+#ifdef CONFIG_KXTIK1004_LOWPASS
+	struct i2c_client *client = kxtik1004_i2c_client;  
+	struct kxtik1004_i2c_data *obj = i2c_get_clientdata(client);
+	int firlen;
+
+	if(1 != sscanf(buf, "%d", &firlen))
+	{
+		GSE_ERR("invallid format\n");
+	}
+	else if(firlen > C_MAX_FIR_LENGTH)
+	{
+		GSE_ERR("exceeds maximum filter length\n");
+	}
+	else
+	{ 
+		atomic_set(&obj->firlen, firlen);
+		if(0 == firlen)
+		{
+			atomic_set(&obj->fir_en, 0);
+		}
+		else
+		{
+			memset(&obj->fir, 0x00, sizeof(obj->fir));
+			atomic_set(&obj->fir_en, 1);
+		}
+	}
+#endif    
+	return count;
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t show_trace_value(struct device_driver *ddri, char *buf)
+{
+	ssize_t res;
+	struct kxtik1004_i2c_data *obj = obj_i2c_data;
+	if (obj == NULL)
+	{
+		GSE_ERR("i2c_data obj is null!!\n");
+		return 0;
+	}
+	
+	res = snprintf(buf, PAGE_SIZE, "0x%04X\n", atomic_read(&obj->trace));     
+	return res;    
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t store_trace_value(struct device_driver *ddri, const char *buf, size_t count)
+{
+	struct kxtik1004_i2c_data *obj = obj_i2c_data;
+	int trace;
+	if (obj == NULL)
+	{
+		GSE_ERR("i2c_data obj is null!!\n");
+		return 0;
+	}
+	
+	if(1 == sscanf(buf, "0x%x", &trace))
+	{
+		atomic_set(&obj->trace, trace);
+	}	
+	else
+	{
+		GSE_ERR("invalid content: '%s', length = %d\n", buf, count);
+	}
+	
+	return count;    
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t show_status_value(struct device_driver *ddri, char *buf)
+{
+	ssize_t len = 0;    
+	struct kxtik1004_i2c_data *obj = obj_i2c_data;
+	if (obj == NULL)
+	{
+		GSE_ERR("i2c_data obj is null!!\n");
+		return 0;
+	}	
+	
+	if(obj->hw)
+	{
+		len += snprintf(buf+len, PAGE_SIZE-len, "CUST: %d %d (%d %d)\n", 
+	            obj->hw->i2c_num, obj->hw->direction, obj->hw->power_id, obj->hw->power_vol);   
+	}
+	else
+	{
+		len += snprintf(buf+len, PAGE_SIZE-len, "CUST: NULL\n");
+	}
+	return len;    
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t show_power_status_value(struct device_driver *ddri, char *buf)
+{
+	if(sensor_power)
+		GSE_LOG("G sensor is in work mode, sensor_power = %d\n", sensor_power);
+	else
+		GSE_LOG("G sensor is in standby mode, sensor_power = %d\n", sensor_power);
+
+	return 0;
+}
+
+/*----------------------------------------------------------------------------*/
+static DRIVER_ATTR(chipinfo,   S_IWUSR | S_IRUGO, show_chipinfo_value,      NULL);
+static DRIVER_ATTR(sensordata, S_IWUSR | S_IRUGO, show_sensordata_value,    NULL);
+static DRIVER_ATTR(cali,       S_IWUSR | S_IRUGO, show_cali_value,          store_cali_value);
+static DRIVER_ATTR(selftest, S_IWUSR | S_IRUGO, show_self_value,  store_self_value);
+static DRIVER_ATTR(self,   S_IWUSR | S_IRUGO, show_selftest_value,      store_selftest_value);
+static DRIVER_ATTR(firlen,     S_IWUSR | S_IRUGO, show_firlen_value,        store_firlen_value);
+static DRIVER_ATTR(trace,      S_IWUSR | S_IRUGO, show_trace_value,         store_trace_value);
+static DRIVER_ATTR(status,               S_IRUGO, show_status_value,        NULL);
+static DRIVER_ATTR(powerstatus,               S_IRUGO, show_power_status_value,        NULL);
+
+
+/*----------------------------------------------------------------------------*/
+static u8 i2c_dev_reg =0 ;
+
+static ssize_t show_register(struct device_driver *pdri, char *buf)
+{
+	//int input_value;
+		
+	GSE_LOG("i2c_dev_reg is 0x%2x \n", i2c_dev_reg);
+
+	return 0;
+}
+
+static ssize_t store_register(struct device_driver *ddri, const char *buf, size_t count)
+{
+	//unsigned long input_value;
+
+	i2c_dev_reg = simple_strtoul(buf, NULL, 16);
+	GSE_LOG("set i2c_dev_reg = 0x%2x \n", i2c_dev_reg);
+
+	return 0;
+}
+static ssize_t store_register_value(struct device_driver *ddri, const char *buf, size_t count)
+{
+	struct kxtik1004_i2c_data *obj = obj_i2c_data;
+	u8 databuf[2];  
+	unsigned long input_value;
+	int res;
+	
+	memset(databuf, 0, sizeof(u8)*2);    
+
+	input_value = simple_strtoul(buf, NULL, 16);
+	GSE_LOG("input_value = 0x%2lx \n", input_value);
+
+	if(NULL == obj)
+	{
+		GSE_ERR("i2c data obj is null!!\n");
+		return 0;
+	}
+
+	databuf[0] = input_value;
+	GSE_LOG("databuf[0]=0x%2x  databuf[1]=0x%2x \n", databuf[0],databuf[1]);
+
+	res = kxt_i2c_write_block(obj->client, i2c_dev_reg, databuf, 0x1);
+
+	if(res < 0)
+	{
+		return KXTIK1004_ERR_I2C;
+	}
+	return 0;
+	
+}
+
+static ssize_t show_register_value(struct device_driver *ddri, char *buf)
+{
+		struct kxtik1004_i2c_data *obj = obj_i2c_data;
+		u8 databuf[1];	
+		
+		memset(databuf, 0, sizeof(u8)*1);	 
+	
+		if(NULL == obj)
+		{
+			GSE_ERR("i2c data obj is null!!\n");
+			return 0;
+		}
+		
+		if(kxt_i2c_read_block(obj->client, i2c_dev_reg, databuf, 0x01))
+		{
+			GSE_ERR("read power ctl register err!\n");
+			return KXTIK1004_ERR_I2C;
+		}
+
+		GSE_LOG("i2c_dev_reg=0x%2x  data=0x%2x \n", i2c_dev_reg,databuf[0]);
+	
+		return 0;
+		
+}
+
+
+static DRIVER_ATTR(i2c,      S_IWUSR | S_IRUGO, show_register_value,         store_register_value);
+static DRIVER_ATTR(register,      S_IWUSR | S_IRUGO, show_register,         store_register);
+
+
+/*----------------------------------------------------------------------------*/
+static struct driver_attribute *kxtik1004_attr_list[] = {
+	&driver_attr_chipinfo,     /*chip information*/
+	&driver_attr_sensordata,   /*dump sensor data*/
+	&driver_attr_cali,         /*show calibration data*/
+	&driver_attr_self,         /*self test demo*/
+	&driver_attr_selftest,     /*self control: 0: disable, 1: enable*/
+	&driver_attr_firlen,       /*filter length: 0: disable, others: enable*/
+	&driver_attr_trace,        /*trace log*/
+	&driver_attr_status,
+	&driver_attr_powerstatus,
+	&driver_attr_register,
+	&driver_attr_i2c,
+};
+/*----------------------------------------------------------------------------*/
+static int kxtik1004_create_attr(struct device_driver *driver) 
+{
+	int idx, err = 0;
+	int num = (int)(sizeof(kxtik1004_attr_list)/sizeof(kxtik1004_attr_list[0]));
+	if (driver == NULL)
+	{
+		return -EINVAL;
+	}
+
+	for(idx = 0; idx < num; idx++)
+	{
+		if((err = driver_create_file(driver, kxtik1004_attr_list[idx])))
+		{            
+			GSE_ERR("driver_create_file (%s) = %d\n", kxtik1004_attr_list[idx]->attr.name, err);
+			break;
+		}
+	}    
+	return err;
+}
+/*----------------------------------------------------------------------------*/
+static int kxtik1004_delete_attr(struct device_driver *driver)
+{
+	int idx ,err = 0;
+	int num = (int)(sizeof(kxtik1004_attr_list)/sizeof(kxtik1004_attr_list[0]));
+
+	if(driver == NULL)
+	{
+		return -EINVAL;
+	}
+	
+
+	for(idx = 0; idx < num; idx++)
+	{
+		driver_remove_file(driver, kxtik1004_attr_list[idx]);
+	}
+	
+
+	return err;
+}
+
+/*----------------------------------------------------------------------------*/
+int gsensor_operate(void* self, uint32_t command, void* buff_in, int size_in,
+		void* buff_out, int size_out, int* actualout)
+{
+	int err = 0;
+	int value, sample_delay;	
+	struct kxtik1004_i2c_data *priv = (struct kxtik1004_i2c_data*)self;
+	hwm_sensor_data* gsensor_data;
+	char buff[KXTIK1004_BUFSIZE];
+	
+	//GSE_FUN(f);
+	switch (command)
+	{
+		case SENSOR_DELAY:
+			if((buff_in == NULL) || (size_in < sizeof(int)))
+			{
+				GSE_ERR("Set delay parameter error!\n");
+				err = -EINVAL;
+			}
+			else
+			{
+				value = *(int *)buff_in;
+				if(value <= 5)
+				{
+					sample_delay = KXTIK1004_BW_200HZ;
+				}
+				else if(value <= 10)
+				{
+					sample_delay = KXTIK1004_BW_100HZ;
+				}
+				else
+				{
+					sample_delay = KXTIK1004_BW_50HZ;
+				}
+				mutex_lock(&kxtik1004_op_mutex);
+				err = KXTIK1004_SetBWRate(priv->client, sample_delay);
+				if(err != KXTIK1004_SUCCESS ) //0x2C->BW=100Hz
+				{
+					GSE_ERR("Set delay parameter error!\n");
+				}
+				mutex_unlock(&kxtik1004_op_mutex);
+				if(value >= 50)
+				{
+					atomic_set(&priv->filter, 0);
+				}
+				else
+				{	
+				#if defined(CONFIG_KXTIK1004_LOWPASS)
+					priv->fir.num = 0;
+					priv->fir.idx = 0;
+					priv->fir.sum[KXTIK1004_AXIS_X] = 0;
+					priv->fir.sum[KXTIK1004_AXIS_Y] = 0;
+					priv->fir.sum[KXTIK1004_AXIS_Z] = 0;
+					atomic_set(&priv->filter, 1);
+				#endif
+				}
+			}
+			break;
+
+		case SENSOR_ENABLE:
+			if((buff_in == NULL) || (size_in < sizeof(int)))
+			{
+				GSE_ERR("Enable sensor parameter error!\n");
+				err = -EINVAL;
+			}
+			else
+			{
+				value = *(int *)buff_in;
+				mutex_lock(&kxtik1004_op_mutex);
+				GSE_LOG("Gsensor device enable function enable = %d, sensor_power = %d!\n",value,sensor_power);
+				if(((value == 0) && (sensor_power == false)) ||((value == 1) && (sensor_power == true)))
+				{
+					enable_status = sensor_power;
+					GSE_LOG("Gsensor device have updated!\n");
+				}
+				else
+				{
+					enable_status = !sensor_power;
+					err = KXTIK1004_SetPowerMode( priv->client, !sensor_power);
+					GSE_LOG("Gsensor not in suspend BMA222_SetPowerMode!, enable_status = %d\n",enable_status);
+				}
+				mutex_unlock(&kxtik1004_op_mutex);
+			}
+			break;
+
+		case SENSOR_GET_DATA:
+			if((buff_out == NULL) || (size_out< sizeof(hwm_sensor_data)))
+			{
+				GSE_ERR("get sensor data parameter error!\n");
+				err = -EINVAL;
+			}
+			else
+			{
+				mutex_lock(&kxtik1004_op_mutex);
+				gsensor_data = (hwm_sensor_data *)buff_out;
+				KXTIK1004_ReadSensorData(priv->client, buff, KXTIK1004_BUFSIZE);
+				sscanf(buff, "%x %x %x", &gsensor_data->values[0], 
+					&gsensor_data->values[1], &gsensor_data->values[2]);				
+				gsensor_data->status = SENSOR_STATUS_ACCURACY_MEDIUM;				
+				gsensor_data->value_divide = 1000;
+				mutex_unlock(&kxtik1004_op_mutex); 
+			}
+			break;
+		default:
+			GSE_ERR("gsensor operate function no this parameter %d!\n", command);
+			err = -1;
+			break;
+	}
+	
+	return err;
+}
+
+/****************************************************************************** 
+ * Function Configuration
+******************************************************************************/
+static int kxtik1004_open(struct inode *inode, struct file *file)
+{
+	file->private_data = kxtik1004_i2c_client;
+
+	if(file->private_data == NULL)
+	{
+		GSE_ERR("null pointer!!\n");
+		return -EINVAL;
+	}
+	return nonseekable_open(inode, file);
+}
+/*----------------------------------------------------------------------------*/
+static int kxtik1004_release(struct inode *inode, struct file *file)
+{
+	file->private_data = NULL;
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static long kxtik1004_unlocked_ioctl(struct file *file, unsigned int cmd,unsigned long arg)
+{
+	struct i2c_client *client = (struct i2c_client*)file->private_data;
+	struct kxtik1004_i2c_data *obj = (struct kxtik1004_i2c_data*)i2c_get_clientdata(client);	
+	char strbuf[KXTIK1004_BUFSIZE];
+	void __user *data;
+	SENSOR_DATA sensor_data;
+	long err = 0;
+	int cali[3];
+
+	//GSE_FUN(f);
+	if(_IOC_DIR(cmd) & _IOC_READ)
+	{
+		err = !access_ok(VERIFY_WRITE, (void __user *)arg, _IOC_SIZE(cmd));
+	}
+	else if(_IOC_DIR(cmd) & _IOC_WRITE)
+	{
+		err = !access_ok(VERIFY_READ, (void __user *)arg, _IOC_SIZE(cmd));
+	}
+
+	if(err)
+	{
+		GSE_ERR("access error: %08X, (%2d, %2d)\n", cmd, _IOC_DIR(cmd), _IOC_SIZE(cmd));
+		return -EFAULT;
+	}
+
+	switch(cmd)
+	{
+		case GSENSOR_IOCTL_INIT:
+			kxtik1004_init_client(client, 0);			
+			break;
+
+		case GSENSOR_IOCTL_READ_CHIPINFO:
+			data = (void __user *) arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}
+			
+			KXTIK1004_ReadChipInfo(client, strbuf, KXTIK1004_BUFSIZE);
+			if(copy_to_user(data, strbuf, strlen(strbuf)+1))
+			{
+				err = -EFAULT;
+				break;
+			}				 
+			break;	  
+
+		case GSENSOR_IOCTL_READ_SENSORDATA:
+			data = (void __user *) arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}
+			KXTIK1004_SetPowerMode(client,true);	
+			KXTIK1004_ReadSensorData(client, strbuf, KXTIK1004_BUFSIZE);
+			if(copy_to_user(data, strbuf, strlen(strbuf)+1))
+			{
+				err = -EFAULT;
+				break;	  
+			}				 
+			break;
+
+		case GSENSOR_IOCTL_READ_GAIN:
+			data = (void __user *) arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}			
+			
+			if(copy_to_user(data, &gsensor_gain, sizeof(GSENSOR_VECTOR3D)))
+			{
+				err = -EFAULT;
+				break;
+			}				 
+			break;
+
+		case GSENSOR_IOCTL_READ_RAW_DATA:
+			data = (void __user *) arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}
+			KXTIK1004_ReadRawData(client, strbuf);
+			if(copy_to_user(data, &strbuf, strlen(strbuf)+1))
+			{
+				err = -EFAULT;
+				break;	  
+			}
+			break;	  
+
+		case GSENSOR_IOCTL_SET_CALI:
+			data = (void __user*)arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}
+			if(copy_from_user(&sensor_data, data, sizeof(sensor_data)))
+			{
+				err = -EFAULT;
+				break;	  
+			}
+			if(atomic_read(&obj->suspend))
+			{
+				GSE_ERR("Perform calibration in suspend state!!\n");
+				err = -EINVAL;
+			}
+			else
+			{
+			#if 0
+				cali[KXTIK1004_AXIS_X] = sensor_data.x * obj->reso->sensitivity / GRAVITY_EARTH_1000;
+				cali[KXTIK1004_AXIS_Y] = sensor_data.y * obj->reso->sensitivity / GRAVITY_EARTH_1000;
+				cali[KXTIK1004_AXIS_Z] = sensor_data.z * obj->reso->sensitivity / GRAVITY_EARTH_1000;			  
+			#else
+			cali[KXTIK1004_AXIS_X] = sensor_data.x;
+			cali[KXTIK1004_AXIS_Y] = sensor_data.y;
+			cali[KXTIK1004_AXIS_Z] = sensor_data.z;			  
+					
+
+			#endif
+				err = KXTIK1004_WriteCalibration(client, cali);			 
+			}
+			break;
+
+		case GSENSOR_IOCTL_CLR_CALI:
+			err = KXTIK1004_ResetCalibration(client);
+			break;
+
+		case GSENSOR_IOCTL_GET_CALI:
+			data = (void __user*)arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}
+			if((err = KXTIK1004_ReadCalibration(client, cali)))
+			{
+				break;
+			}
+			#if 0
+			sensor_data.x = cali[KXTIK1004_AXIS_X] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+			sensor_data.y = cali[KXTIK1004_AXIS_Y] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+			sensor_data.z = cali[KXTIK1004_AXIS_Z] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+			#else
+			sensor_data.x = cali[KXTIK1004_AXIS_X];
+			sensor_data.y = cali[KXTIK1004_AXIS_Y];
+			sensor_data.z = cali[KXTIK1004_AXIS_Z];
+			
+			#endif
+			if(copy_to_user(data, &sensor_data, sizeof(sensor_data)))
+			{
+				err = -EFAULT;
+				break;
+			}		
+			break;
+		
+
+		default:
+			GSE_ERR("unknown IOCTL: 0x%08x\n", cmd);
+			err = -ENOIOCTLCMD;
+			break;
+			
+	}
+
+	return err;
+}
+
+
+/*----------------------------------------------------------------------------*/
+static struct file_operations kxtik1004_fops = {
+	.owner = THIS_MODULE,
+	.open = kxtik1004_open,
+	.release = kxtik1004_release,
+	.unlocked_ioctl = kxtik1004_unlocked_ioctl,
+};
+/*----------------------------------------------------------------------------*/
+static struct miscdevice kxtik1004_device = {
+	.minor = MISC_DYNAMIC_MINOR,
+	.name = "gsensor",
+	.fops = &kxtik1004_fops,
+};
+/*----------------------------------------------------------------------------*/
+#ifndef USE_EARLY_SUSPEND
+/*----------------------------------------------------------------------------*/
+static int kxtik1004_suspend(struct i2c_client *client, pm_message_t msg) 
+{
+	struct kxtik1004_i2c_data *obj = i2c_get_clientdata(client);    
+	int err = 0;
+	GSE_FUN();    
+    mutex_lock(&kxtik1004_op_mutex);
+	if(msg.event == PM_EVENT_SUSPEND)
+	{   
+		if(obj == NULL)
+		{
+			GSE_ERR("null pointer!!\n");
+			mutex_unlock(&kxtik1004_op_mutex);
+			return -EINVAL;
+		}
+		atomic_set(&obj->suspend, 1);
+		if((err = KXTIK1004_SetPowerMode(obj->client, false)))
+		{
+			GSE_ERR("write power control fail!!\n");
+			mutex_unlock(&kxtik1004_op_mutex);
+			return err;
+		}
+
+		KXTIK1004_power(obj->hw, 0);
+	}
+	sensor_suspend = 1;
+	mutex_unlock(&kxtik1004_op_mutex);
+	return err;
+}
+/*----------------------------------------------------------------------------*/
+static int kxtik1004_resume(struct i2c_client *client)
+{
+	struct kxtik1004_i2c_data *obj = i2c_get_clientdata(client);        
+	int err;
+	GSE_FUN();
+
+	if(obj == NULL)
+	{
+		GSE_ERR("null pointer!!\n");
+		return -EINVAL;
+	}
+	mutex_lock(&kxtik1004_op_mutex);
+
+	KXTIK1004_power(obj->hw, 1);
+	if((err = kxtik1004_init_client(client, 0)))
+	{
+		GSE_ERR("initialize client fail!!\n");
+		mutex_unlock(&kxtik1004_op_mutex);
+		return err;        
+	}
+	sensor_suspend = 0;
+	atomic_set(&obj->suspend, 0);
+    mutex_unlock(&kxtik1004_op_mutex);
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+#else /*CONFIG_HAS_EARLY_SUSPEND is defined*/
+/*----------------------------------------------------------------------------*/
+static void kxtik1004_early_suspend(struct early_suspend *h) 
+{
+	struct kxtik1004_i2c_data *obj = container_of(h, struct kxtik1004_i2c_data, early_drv);   
+	int err;
+	GSE_FUN();    
+
+	if(obj == NULL)
+	{
+		GSE_ERR("null pointer!!\n");
+		return;
+	}
+	atomic_set(&obj->suspend, 1); 
+	mutex_lock(&kxtik1004_op_mutex);
+	if((err = KXTIK1004_SetPowerMode(obj->client, false)))
+	{
+		GSE_ERR("write power control fail!!\n");
+		mutex_unlock(&kxtik1004_op_mutex);
+		return;
+	}
+
+	
+	sensor_suspend = 1;
+	mutex_unlock(&kxtik1004_op_mutex);
+	KXTIK1004_power(obj->hw, 0);
+}
+/*----------------------------------------------------------------------------*/
+static void kxtik1004_late_resume(struct early_suspend *h)
+{
+	struct kxtik1004_i2c_data *obj = container_of(h, struct kxtik1004_i2c_data, early_drv);         
+	int err;
+	GSE_FUN();
+
+	if(obj == NULL)
+	{
+		GSE_ERR("null pointer!!\n");
+		return;
+	}
+
+	KXTIK1004_power(obj->hw, 1);
+	mutex_lock(&kxtik1004_op_mutex);
+	if((err = kxtik1004_init_client(obj->client, 0)))
+	{
+		GSE_ERR("initialize client fail!!\n");
+		mutex_unlock(&kxtik1004_op_mutex);
+		return;        
+	}
+	sensor_suspend = 0;
+	mutex_unlock(&kxtik1004_op_mutex);
+	atomic_set(&obj->suspend, 0);    
+}
+/*----------------------------------------------------------------------------*/
+#endif /*CONFIG_HAS_EARLYSUSPEND*/
+/*----------------------------------------------------------------------------*/
+static int kxtik1004_i2c_detect(struct i2c_client *client, struct i2c_board_info *info) 
+{    
+	strcpy(info->type, KXTIK1004_DEV_NAME);
+	return 0;
+}
+
+/*----------------------------------------------------------------------------*/
+static int kxtik1004_i2c_probe(struct i2c_client *client, const struct i2c_device_id *id)
+{
+	struct i2c_client *new_client;
+	struct kxtik1004_i2c_data *obj;
+	struct hwmsen_object sobj;
+	int err = 0;
+	int retry = 0;
+	GSE_FUN();
+
+	if(!(obj = kzalloc(sizeof(*obj), GFP_KERNEL)))
+	{
+		err = -ENOMEM;
+		goto exit;
+	}
+	
+	memset(obj, 0, sizeof(struct kxtik1004_i2c_data));
+
+	obj->hw = get_cust_acc_hw();
+	
+	if((err = hwmsen_get_convert(obj->hw->direction, &obj->cvt)))
+	{
+		GSE_ERR("invalid direction: %d\n", obj->hw->direction);
+		goto exit;
+	}
+
+	obj_i2c_data = obj;
+	obj->client = client;
+	new_client = obj->client;
+	i2c_set_clientdata(new_client,obj);
+	
+	atomic_set(&obj->trace, 0);
+	atomic_set(&obj->suspend, 0);
+	
+#ifdef CONFIG_KXTIK1004_LOWPASS
+	if(obj->hw->firlen > C_MAX_FIR_LENGTH)
+	{
+		atomic_set(&obj->firlen, C_MAX_FIR_LENGTH);
+	}	
+	else
+	{
+		atomic_set(&obj->firlen, obj->hw->firlen);
+	}
+	
+	if(atomic_read(&obj->firlen) > 0)
+	{
+		atomic_set(&obj->fir_en, 1);
+	}
+	
+#endif
+
+	kxtik1004_i2c_client = new_client;	
+
+	for(retry = 0; retry < 3; retry++){
+	if((err = kxtik1004_init_client(new_client, 1)))
+	{
+			GSE_ERR("kxtik1004_device init cilent fail time: %d\n", retry);
+			continue;
+	}
+	}
+	if(err != 0)
+		goto exit_init_failed;
+	
+
+	if((err = misc_register(&kxtik1004_device)))
+	{
+		GSE_ERR("kxtik1004_device register failed\n");
+		goto exit_misc_device_register_failed;
+	}
+
+	if((err = kxtik1004_create_attr(&kxtik1004_gsensor_driver.driver)))
+	{
+		GSE_ERR("create attribute err = %d\n", err);
+		goto exit_create_attr_failed;
+	}
+
+	sobj.self = obj;
+    sobj.polling = 1;
+    sobj.sensor_operate = gsensor_operate;
+	if((err = hwmsen_attach(ID_ACCELEROMETER, &sobj)))
+	{
+		GSE_ERR("attach fail = %d\n", err);
+		goto exit_kfree;
+	}
+
+#ifdef USE_EARLY_SUSPEND
+	obj->early_drv.level    = EARLY_SUSPEND_LEVEL_STOP_DRAWING - 2,
+	obj->early_drv.suspend  = kxtik1004_early_suspend,
+	obj->early_drv.resume   = kxtik1004_late_resume,    
+	register_early_suspend(&obj->early_drv);
+#endif 
+
+	GSE_LOG("%s: OK\n", __func__);    
+	return 0;
+
+	exit_create_attr_failed:
+	misc_deregister(&kxtik1004_device);
+	exit_misc_device_register_failed:
+	exit_init_failed:
+	//i2c_detach_client(new_client);
+	exit_kfree:
+	kfree(obj);
+	exit:
+	GSE_ERR("%s: err = %d\n", __func__, err);        
+	return err;
+}
+
+/*----------------------------------------------------------------------------*/
+static int kxtik1004_i2c_remove(struct i2c_client *client)
+{
+	int err = 0;	
+	
+	if((err = kxtik1004_delete_attr(&kxtik1004_gsensor_driver.driver)))
+	{
+		GSE_ERR("kxtik1004_delete_attr fail: %d\n", err);
+	}
+	
+	if((err = misc_deregister(&kxtik1004_device)))
+	{
+		GSE_ERR("misc_deregister fail: %d\n", err);
+	}
+
+	if((err = hwmsen_detach(ID_ACCELEROMETER)))
+	    
+
+	kxtik1004_i2c_client = NULL;
+	i2c_unregister_device(client);
+	kfree(i2c_get_clientdata(client));
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int kxtik1004_probe(struct platform_device *pdev) 
+{
+	struct acc_hw *hw = get_cust_acc_hw();
+	GSE_FUN();
+
+	KXTIK1004_power(hw, 1);
+	if(i2c_add_driver(&kxtik1004_i2c_driver))
+	{
+		GSE_ERR("add driver error\n");
+		return -1;
+	}
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int kxtik1004_remove(struct platform_device *pdev)
+{
+    struct acc_hw *hw = get_cust_acc_hw();
+
+    GSE_FUN();    
+    KXTIK1004_power(hw, 0);    
+    i2c_del_driver(&kxtik1004_i2c_driver);
+    return 0;
+}
+/*----------------------------------------------------------------------------*/
+#if 1
+#ifdef CONFIG_OF
+static const struct of_device_id gsensor_of_match[] = {
+	{ .compatible = "mediatek,gsensor", },
+	{},
+};
+#endif
+
+static struct platform_driver kxtik1004_gsensor_driver = {
+	.probe      = kxtik1004_probe,
+	.remove     = kxtik1004_remove,    
+	.driver     = 
+	{
+		.name  = "gsensor",
+		.owner  = THIS_MODULE,
+        #ifdef CONFIG_OF
+		.of_match_table = gsensor_of_match,
+		#endif
+	}
+};
+#else
+
+static struct platform_driver kxtik1004_gsensor_driver = {
+	.probe      = kxtik1004_probe,
+	.remove     = kxtik1004_remove,    
+	.driver     = {
+		.name  = "gsensor",
+		.owner = THIS_MODULE,
+	}
+};
+#endif
+/*----------------------------------------------------------------------------*/
+static int __init kxtik1004_init(void)
+{
+	struct acc_hw *hw = get_cust_acc_hw();
+	GSE_FUN();	
+	GSE_LOG("%s: i2c_number=%d\n", __func__,hw->i2c_num);
+    if (hw->i2c_addr[0])
+    {
+        GSE_LOG("%s: i2c_slave_addr=%d\n", __func__,hw->i2c_addr[0]);
+        i2c_kxtik1004.addr = hw->i2c_addr[0]>>1;
+    }    
+	i2c_register_board_info(hw->i2c_num, &i2c_kxtik1004, 1);
+	if(platform_driver_register(&kxtik1004_gsensor_driver))
+	{
+		GSE_ERR("failed to register driver");
+		return -ENODEV;
+	}
+	return 0;    
+}
+/*----------------------------------------------------------------------------*/
+static void __exit kxtik1004_exit(void)
+{
+	GSE_FUN();
+	platform_driver_unregister(&kxtik1004_gsensor_driver);
+}
+/*----------------------------------------------------------------------------*/
+module_init(kxtik1004_init);
+module_exit(kxtik1004_exit);
+/*----------------------------------------------------------------------------*/
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("KXTIK1004 I2C driver");
+MODULE_AUTHOR("Dexiang.Liu@mediatek.com");
diff --git a/drivers/misc/mediatek/accelerometer/kxtik1004/kxtik1004.h b/drivers/misc/mediatek/accelerometer/kxtik1004/kxtik1004.h
new file mode 100644
index 000000000..17370c1c6
--- /dev/null
+++ b/drivers/misc/mediatek/accelerometer/kxtik1004/kxtik1004.h
@@ -0,0 +1,67 @@
+/* linux/drivers/hwmon/adxl345.c
+ *
+ * (C) Copyright 2008 
+ * MediaTek <www.mediatek.com>
+ *
+ * KXTIK1004 driver for MT6575
+ *
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA  KXTIK
+ */
+#ifndef KXTIK1004_H
+#define KXTIK1004_H
+	 
+#include <linux/ioctl.h>
+	 
+#define KXTIK1004_I2C_SLAVE_ADDR		0x1E
+	 
+ /* KXTIK1004 Register Map  (Please refer to KXTIK1004 Specifications) */
+#define KXTIK1004_REG_DEVID			0x0F
+#define	KXTIK1004_REG_BW_RATE			0x21
+#define KXTIK1004_REG_POWER_CTL		0x1B
+#define KXTIK1004_REG_CTL_REG3		0x1D
+#define KXTIK1004_DCST_RESP			0x0C
+#define KXTIK1004_REG_DATA_FORMAT		0x1B
+#define KXTIK1004_REG_DATA_RESOLUTION		0x1B
+#define KXTIK1004_RANGE_DATA_RESOLUTION_MASK	0x40
+#define KXTIK1004_REG_DATAX0			0x06	 
+#define KXTIK1004_FIXED_DEVID			0x12	 
+#define KXTIK1004_BW_200HZ				0x04
+#define KXTIK1004_BW_100HZ				0x03
+#define KXTIK1004_BW_50HZ				0x02	 
+#define KXTIK1004_MEASURE_MODE		0x80		 
+#define KXTIK1004_RANGE_MASK		0x18
+#define KXTIK1004_RANGE_2G			0x00
+#define KXTIK1004_RANGE_4G			0x08
+#define KXTIK1004_RANGE_8G			0x10
+#define KXTIK1004_REG_INT_ENABLE	0x1E
+
+#define KXTIK1004_SELF_TEST           0x10
+	 	 
+#define KXTIK1004_DEVICE_ID	 0x05
+#define KXCJK1013_DEVICE_ID	 0x11
+
+#define KXTIK1004_SUCCESS						0
+#define KXTIK1004_ERR_I2C						-1
+#define KXTIK1004_ERR_STATUS					-3
+#define KXTIK1004_ERR_SETUP_FAILURE				-4
+#define KXTIK1004_ERR_GETGSENSORDATA			-5
+#define KXTIK1004_ERR_IDENTIFICATION			-6
+	 
+	 
+	 
+#define KXTIK1004_BUFSIZE				256
+	 
+#endif
+
diff --git a/drivers/misc/mediatek/accelerometer/lsm6ds3/Makefile b/drivers/misc/mediatek/accelerometer/lsm6ds3/Makefile
new file mode 100755
index 000000000..1606807b8
--- /dev/null
+++ b/drivers/misc/mediatek/accelerometer/lsm6ds3/Makefile
@@ -0,0 +1,4 @@
+include $(srctree)/drivers/misc/mediatek/Makefile.custom
+
+obj-y	:=  lsm6ds3.o
+
diff --git a/drivers/misc/mediatek/accelerometer/lsm6ds3/lsm6ds3.c b/drivers/misc/mediatek/accelerometer/lsm6ds3/lsm6ds3.c
new file mode 100644
index 000000000..7ded5aa22
--- /dev/null
+++ b/drivers/misc/mediatek/accelerometer/lsm6ds3/lsm6ds3.c
@@ -0,0 +1,3055 @@
+/* ST LSM6DS3 Accelerometer and Gyroscope sensor driver
+ *
+ *
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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.
+ *
+ */
+
+#include <linux/interrupt.h>
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include <linux/irq.h>
+#include <linux/miscdevice.h>
+#include <asm/uaccess.h>
+#include <linux/delay.h>
+#include <linux/input.h>
+#include <linux/workqueue.h>
+#include <linux/kobject.h>
+#include <linux/earlysuspend.h>
+#include <linux/platform_device.h>
+#include <mach/mt_gpio.h>
+
+#include <linux/hwmsensor.h>
+#include <linux/hwmsen_dev.h>
+#include <linux/sensors_io.h>
+#include "lsm6ds3.h"
+#include <linux/hwmsen_helper.h>
+#include <linux/kernel.h>
+#include <mach/mt_pm_ldo.h>
+#include <cust_eint.h>
+#include <mach/eint.h>
+
+
+
+#define POWER_NONE_MACRO MT65XX_POWER_NONE
+
+#define LSM6DS3_NEW_ARCH				//kk and L compatialbe
+//#define LSM6DS3_STEP_COUNTER 			//it depends on the MACRO LSM6DS3_NEW_ARCH
+//#define LSM6DS3_TILT_FUNC 				//dependency on LSM6DS3_STEP_COUNTER
+//#define LSM6DS3_SIGNIFICANT_MOTION  	//dependency on LSM6DS3_STEP_COUNTER
+
+#ifndef LSM6DS3_NEW_ARCH		//new sensor type depend on new arch
+#undef LSM6DS3_STEP_COUNTER
+#undef LSM6DS3_TILT_FUNC
+#undef LSM6DS3_SIGNIFICANT_MOTION
+#endif
+
+
+#ifndef LSM6DS3_STEP_COUNTER		//significant_motion depend on step_counter
+#undef LSM6DS3_SIGNIFICANT_MOTION
+#endif
+
+
+#include <cust_acc.h>
+
+#ifdef LSM6DS3_NEW_ARCH
+#include <accel.h>
+#ifdef LSM6DS3_STEP_COUNTER //step counter
+#include <step_counter.h>
+#endif
+#ifdef LSM6DS3_TILT_FUNC //tilt detector
+#include <tilt_detector.h>
+#endif
+
+#endif
+/****************************************************************/
+#if defined(LSM6DS3_TILT_FUNC) //|| defined(LSM6DS3_SIGNIFICANT_MOTION)	
+#define GPIO_LSM6DS3_EINT_PIN GPIO_ALS_EINT_PIN		//eint gpio pin num
+#define GPIO_LSM6DS3_EINT_PIN_M_EINT GPIO_ALS_EINT_PIN_M_EINT	//eint mode
+#define CUST_EINT_LSM6DS3_NUM CUST_EINT_ALS_NUM		//eint num
+#define CUST_EINT_LSM6DS3_DEBOUNCE_CN CUST_EINT_ALS_DEBOUNCE_CN //debounce time
+#define CUST_EINT_LSM6DS3_TYPE CUST_EINT_ALS_TYPE	//eint trigger type
+#endif
+/*---------------------------------------------------------------------------*/
+#define DEBUG 1
+/*----------------------------------------------------------------------------*/
+#define CONFIG_LSM6DS3_LOWPASS   /*apply low pass filter on output*/       
+/*----------------------------------------------------------------------------*/
+#define LSM6DS3_AXIS_X          0
+#define LSM6DS3_AXIS_Y          1
+#define LSM6DS3_AXIS_Z          2
+#define LSM6DS3_ACC_AXES_NUM        3
+#define LSM6DS3_GYRO_AXES_NUM       3
+#define LSM6DS3_ACC_DATA_LEN        6   
+#define LSM6DS3_GYRO_DATA_LEN       6   
+#define LSM6DS3_ACC_DEV_NAME        "LSM6DS3_ACCEL"
+/*----------------------------------------------------------------------------*/
+static const struct i2c_device_id lsm6ds3_i2c_id[] = {{LSM6DS3_ACC_DEV_NAME,0},{}};
+static struct i2c_board_info __initdata i2c_lsm6ds3={ I2C_BOARD_INFO(LSM6DS3_ACC_DEV_NAME, (0xD4>>1))};
+
+
+/*----------------------------------------------------------------------------*/
+static int lsm6ds3_i2c_probe(struct i2c_client *client, const struct i2c_device_id *id);
+static int lsm6ds3_i2c_remove(struct i2c_client *client);
+//static int lsm6ds3_i2c_detect(struct i2c_client *client, int kind, struct i2c_board_info *info);
+static int LSM6DS3_init_client(struct i2c_client *client, bool enable);
+static int LSM6DS3_acc_SetPowerMode(struct i2c_client *client, bool enable);
+
+static int LSM6DS3_ReadAccRawData(struct i2c_client *client, s16 data[LSM6DS3_ACC_AXES_NUM]);
+#ifndef CONFIG_HAS_EARLYSUSPEND
+
+static int lsm6ds3_acc_suspend(struct i2c_client *client, pm_message_t msg);
+static int lsm6ds3_acc_resume(struct i2c_client *client);
+#endif
+static int LSM6DS3_acc_SetSampleRate(struct i2c_client *client, u8 sample_rate);
+
+static int LSM6DS3_acc_Enable_Func(struct i2c_client *client, LSM6DS3_ACC_GYRO_FUNC_EN_t newValue);
+static int LSM6DS3_Int_Ctrl(struct i2c_client *client, LSM6DS3_ACC_GYRO_INT_ACTIVE_t int_act, LSM6DS3_ACC_GYRO_INT_LATCH_CTL_t int_latch);
+
+#ifdef LSM6DS3_STEP_COUNTER //step counter
+static int LSM6DS3_acc_Enable_Pedometer_Func(struct i2c_client *client, bool enable);
+
+static int LSM6DS3_Write_PedoThreshold(struct i2c_client *client, u8 newValue);
+static int LSM6DS3_Reset_Pedo_Data(struct i2c_client *client, LSM6DS3_ACC_GYRO_PEDO_RST_STEP_t newValue);
+#ifdef LSM6DS3_SIGNIFICANT_MOTION
+
+static int LSM6DS3_Enable_SigMotion_Func(struct i2c_client *client, LSM6DS3_ACC_GYRO_SIGN_MOT_t newValue);
+#endif
+#endif
+#ifdef LSM6DS3_TILT_FUNC //tilt detector
+
+static int LSM6DS3_Enable_Tilt_Func(struct i2c_client *client, bool enable);
+static int LSM6DS3_Enable_Tilt_Func_On_Int(struct i2c_client *client, LSM6DS3_ACC_GYRO_ROUNT_INT_t tilt_int, bool enable);
+
+
+#endif
+
+
+
+/*----------------------------------------------------------------------------*/
+typedef enum {
+    ADX_TRC_FILTER  = 0x01,
+    ADX_TRC_RAWDATA = 0x02,
+    ADX_TRC_IOCTL   = 0x04,
+    ADX_TRC_CALI	= 0X08,
+    ADX_TRC_INFO	= 0X10,
+} ADX_TRC;
+/*----------------------------------------------------------------------------*/
+typedef enum {
+    ACCEL_TRC_FILTER  = 0x01,
+    ACCEL_TRC_RAWDATA = 0x02,
+    ACCEL_TRC_IOCTL   = 0x04,
+    ACCEL_TRC_CALI	= 0X08,
+    ACCEL_TRC_INFO	= 0X10,
+    ACCEL_TRC_DATA	= 0X20,
+} ACCEL_TRC;
+/*----------------------------------------------------------------------------*/
+struct scale_factor{
+    u8  whole;
+    u8  fraction;
+};
+/*----------------------------------------------------------------------------*/
+struct data_resolution {
+    struct scale_factor scalefactor;
+    int                 sensitivity;
+};
+/*----------------------------------------------------------------------------*/
+#define C_MAX_FIR_LENGTH (32)
+/*----------------------------------------------------------------------------*/
+struct data_filter {
+    s16 raw[C_MAX_FIR_LENGTH][LSM6DS3_ACC_AXES_NUM];
+    int sum[LSM6DS3_ACC_AXES_NUM];
+    int num;
+    int idx;
+};
+struct gyro_data_filter {
+    s16 raw[C_MAX_FIR_LENGTH][LSM6DS3_GYRO_AXES_NUM];
+    int sum[LSM6DS3_GYRO_AXES_NUM];
+    int num;
+    int idx;
+};
+/*----------------------------------------------------------------------------*/
+struct lsm6ds3_i2c_data {
+    struct i2c_client *client;
+	struct acc_hw *hw;
+    struct hwmsen_convert   cvt;
+    atomic_t 				layout;
+    /*misc*/
+    //struct data_resolution *reso;
+    struct work_struct	eint_work;				
+    atomic_t                trace;
+    atomic_t                suspend;
+    atomic_t                selftest;
+    atomic_t				filter;
+    s16                     cali_sw[LSM6DS3_GYRO_AXES_NUM+1];
+
+    /*data*/
+	s8                      offset[LSM6DS3_ACC_AXES_NUM+1];  /*+1: for 4-byte alignment*/
+    s16                     data[LSM6DS3_ACC_AXES_NUM+1];
+	
+	int 					sensitivity;
+	int 					sample_rate;
+
+#if defined(CONFIG_LSM6DS3_LOWPASS)
+    atomic_t                firlen;
+    atomic_t                fir_en;
+    struct data_filter      fir;
+#endif 
+    /*early suspend*/
+#if defined(CONFIG_HAS_EARLYSUSPEND)
+    struct early_suspend    early_drv;
+#endif     
+};
+/*----------------------------------------------------------------------------*/
+static struct i2c_driver lsm6ds3_i2c_driver = {
+    .driver = {
+        .owner          = THIS_MODULE,
+        .name           = LSM6DS3_ACC_DEV_NAME,
+    },
+	.probe      		= lsm6ds3_i2c_probe,
+	.remove    			= lsm6ds3_i2c_remove,
+#if !defined(CONFIG_HAS_EARLYSUSPEND)    
+    .suspend            = lsm6ds3_acc_suspend,
+    .resume             = lsm6ds3_acc_resume,
+#endif
+	.id_table = lsm6ds3_i2c_id,
+};
+#ifdef LSM6DS3_NEW_ARCH
+static int lsm6ds3_local_init(void);
+static int lsm6ds3_local_uninit(void);
+static int lsm6ds3_acc_init_flag = -1;
+static unsigned long lsm6ds3_init_flag_test = 0; //initial state
+static DEFINE_MUTEX(lsm6ds3_init_mutex);
+typedef enum {
+	LSM6DS3_ACC = 1,
+	LSM6DS3_STEP_C = 2,
+	LSM6DS3_TILT = 3,
+}LSM6DS3_INIT_TYPE;
+static struct acc_init_info  lsm6ds3_init_info = {
+	.name   = LSM6DS3_ACC_DEV_NAME,
+	.init   = lsm6ds3_local_init,
+	.uninit = lsm6ds3_local_uninit,
+};
+
+#ifdef LSM6DS3_STEP_COUNTER
+static int lsm6ds3_step_c_local_init(void);
+static int lsm6ds3_step_c_local_uninit(void);
+static struct step_c_init_info  lsm6ds3_step_c_init_info = {
+	.name   = "LSM6DS3_STEP_C",
+	.init   = lsm6ds3_step_c_local_init,
+	.uninit = lsm6ds3_step_c_local_uninit,
+};
+#endif
+#ifdef LSM6DS3_TILT_FUNC
+static int lsm6ds3_tilt_local_init(void);
+static int lsm6ds3_tilt_local_uninit(void);
+static struct tilt_init_info  lsm6ds3_tilt_init_info = {
+	.name   = "LSM6DS3_TILT",
+	.init   = lsm6ds3_tilt_local_init,
+	.uninit = lsm6ds3_tilt_local_uninit,
+};
+#endif
+
+#endif
+/*----------------------------------------------------------------------------*/
+static struct i2c_client *lsm6ds3_i2c_client = NULL;
+
+#ifndef LSM6DS3_NEW_ARCH
+static struct platform_driver lsm6ds3_driver;
+#endif
+
+static struct lsm6ds3_i2c_data *obj_i2c_data = NULL;
+static bool sensor_power = false;
+static bool enable_status = false;
+static bool pedo_enable_status = false;
+static bool tilt_enable_status = false;
+
+
+/*----------------------------------------------------------------------------*/
+
+#define GSE_TAG                  "[accel] "
+
+#define GSE_FUN(f)               printk(KERN_INFO GSE_TAG"%s\n", __FUNCTION__)
+#define GSE_ERR(fmt, args...)    printk(KERN_ERR GSE_TAG "%s %d : " fmt, __FUNCTION__, __LINE__, ##args)
+#define GSE_LOG(fmt, args...)    printk(KERN_INFO GSE_TAG "%s %d : " fmt, __FUNCTION__, __LINE__, ##args)
+
+/*----------------------------------------------------------------------------*/
+
+static void LSM6DS3_dumpReg(struct i2c_client *client)
+{
+  int i=0;
+  u8 addr = 0x10;
+  u8 regdata=0;
+  for(i=0; i<25 ; i++)
+  {
+    //dump all
+    hwmsen_read_byte(client,addr,&regdata);
+	HWM_LOG("Reg addr=%x regdata=%x\n",addr,regdata);
+	addr++;	
+  }
+}
+
+static void LSM6DS3_power(struct acc_hw *hw, unsigned int on) 
+{
+	static unsigned int power_on = 0;
+
+	if(hw->power_id != POWER_NONE_MACRO)		// have externel LDO
+	{        
+		GSE_LOG("power %s\n", on ? "on" : "off");
+		if(power_on == on)	// power status not change
+		{
+			GSE_LOG("ignore power control: %d\n", on);
+		}
+		else if(on)	// power on
+		{
+			if(!hwPowerOn(hw->power_id, hw->power_vol, "LSM6DS3"))
+			{
+				GSE_ERR("power on fails!!\n");
+			}
+		}
+		else	// power off
+		{
+			if (!hwPowerDown(hw->power_id, "LSM6DS3"))
+			{
+				GSE_ERR("power off fail!!\n");
+			}			  
+		}
+	}
+	power_on = on;    
+}
+/*----------------------------------------------------------------------------*/
+
+static int LSM6DS3_acc_write_rel_calibration(struct lsm6ds3_i2c_data *obj, int dat[LSM6DS3_GYRO_AXES_NUM])
+{
+    obj->cali_sw[LSM6DS3_AXIS_X] = obj->cvt.sign[LSM6DS3_AXIS_X]*dat[obj->cvt.map[LSM6DS3_AXIS_X]];
+    obj->cali_sw[LSM6DS3_AXIS_Y] = obj->cvt.sign[LSM6DS3_AXIS_Y]*dat[obj->cvt.map[LSM6DS3_AXIS_Y]];
+    obj->cali_sw[LSM6DS3_AXIS_Z] = obj->cvt.sign[LSM6DS3_AXIS_Z]*dat[obj->cvt.map[LSM6DS3_AXIS_Z]];
+#if DEBUG		
+		if(atomic_read(&obj->trace) & ACCEL_TRC_CALI)
+		{
+			GSE_LOG("test  (%5d, %5d, %5d) ->(%5d, %5d, %5d)->(%5d, %5d, %5d))\n", 
+				obj->cvt.sign[LSM6DS3_AXIS_X],obj->cvt.sign[LSM6DS3_AXIS_Y],obj->cvt.sign[LSM6DS3_AXIS_Z],
+				dat[LSM6DS3_AXIS_X], dat[LSM6DS3_AXIS_Y], dat[LSM6DS3_AXIS_Z],
+				obj->cvt.map[LSM6DS3_AXIS_X],obj->cvt.map[LSM6DS3_AXIS_Y],obj->cvt.map[LSM6DS3_AXIS_Z]);
+			GSE_LOG("write gyro calibration data  (%5d, %5d, %5d)\n", 
+				obj->cali_sw[LSM6DS3_AXIS_X],obj->cali_sw[LSM6DS3_AXIS_Y],obj->cali_sw[LSM6DS3_AXIS_Z]);
+		}
+#endif
+    return 0;
+}
+
+/*----------------------------------------------------------------------------*/
+static int LSM6DS3_acc_ResetCalibration(struct i2c_client *client)
+{
+	struct lsm6ds3_i2c_data *obj = i2c_get_clientdata(client);	
+
+	memset(obj->cali_sw, 0x00, sizeof(obj->cali_sw));
+	return 0;    
+}
+
+/*----------------------------------------------------------------------------*/
+static int LSM6DS3_acc_ReadCalibration(struct i2c_client *client, int dat[LSM6DS3_GYRO_AXES_NUM])
+{
+    struct lsm6ds3_i2c_data *obj = i2c_get_clientdata(client);
+
+    dat[obj->cvt.map[LSM6DS3_AXIS_X]] = obj->cvt.sign[LSM6DS3_AXIS_X]*obj->cali_sw[LSM6DS3_AXIS_X];
+    dat[obj->cvt.map[LSM6DS3_AXIS_Y]] = obj->cvt.sign[LSM6DS3_AXIS_Y]*obj->cali_sw[LSM6DS3_AXIS_Y];
+    dat[obj->cvt.map[LSM6DS3_AXIS_Z]] = obj->cvt.sign[LSM6DS3_AXIS_Z]*obj->cali_sw[LSM6DS3_AXIS_Z];
+
+#if DEBUG		
+		if(atomic_read(&obj->trace) & ACCEL_TRC_CALI)
+		{
+			GSE_LOG("Read gyro calibration data  (%5d, %5d, %5d)\n", 
+				dat[LSM6DS3_AXIS_X],dat[LSM6DS3_AXIS_Y],dat[LSM6DS3_AXIS_Z]);
+		}
+#endif
+                                       
+    return 0;
+}
+/*----------------------------------------------------------------------------*/
+
+static int LSM6DS3_acc_WriteCalibration(struct i2c_client *client, int dat[LSM6DS3_GYRO_AXES_NUM])
+{
+	struct lsm6ds3_i2c_data *obj = i2c_get_clientdata(client);
+	int err = 0;
+	int cali[LSM6DS3_GYRO_AXES_NUM];
+
+	GSE_FUN();
+	if(!obj || ! dat)
+	{
+		GSE_ERR("null ptr!!\n");
+		return -EINVAL;
+	}
+	else
+	{        		
+		cali[obj->cvt.map[LSM6DS3_AXIS_X]] = obj->cvt.sign[LSM6DS3_AXIS_X]*obj->cali_sw[LSM6DS3_AXIS_X];
+		cali[obj->cvt.map[LSM6DS3_AXIS_Y]] = obj->cvt.sign[LSM6DS3_AXIS_Y]*obj->cali_sw[LSM6DS3_AXIS_Y];
+		cali[obj->cvt.map[LSM6DS3_AXIS_Z]] = obj->cvt.sign[LSM6DS3_AXIS_Z]*obj->cali_sw[LSM6DS3_AXIS_Z]; 
+		cali[LSM6DS3_AXIS_X] += dat[LSM6DS3_AXIS_X];
+		cali[LSM6DS3_AXIS_Y] += dat[LSM6DS3_AXIS_Y];
+		cali[LSM6DS3_AXIS_Z] += dat[LSM6DS3_AXIS_Z];
+#if DEBUG		
+		if(atomic_read(&obj->trace) & ACCEL_TRC_CALI)
+		{
+			GSE_LOG("write gyro calibration data  (%5d, %5d, %5d)-->(%5d, %5d, %5d)\n", 
+				dat[LSM6DS3_AXIS_X], dat[LSM6DS3_AXIS_Y], dat[LSM6DS3_AXIS_Z],
+				cali[LSM6DS3_AXIS_X],cali[LSM6DS3_AXIS_Y],cali[LSM6DS3_AXIS_Z]);
+		}
+#endif
+		return LSM6DS3_acc_write_rel_calibration(obj, cali);
+	} 
+
+	return err;
+}
+/*----------------------------------------------------------------------------*/
+static int LSM6DS3_CheckDeviceID(struct i2c_client *client)
+{
+	u8 databuf[10];    
+	int res = 0;
+
+	memset(databuf, 0, sizeof(u8)*10);    
+	databuf[0] = LSM6DS3_FIXED_DEVID;    
+
+	res = hwmsen_read_byte(client,LSM6DS3_WHO_AM_I,databuf);
+    GSE_LOG(" LSM6DS3  id %x!\n",databuf[0]);
+	if(databuf[0]!=LSM6DS3_FIXED_DEVID)
+	{
+		return LSM6DS3_ERR_IDENTIFICATION;
+	}
+
+	if (res < 0)
+	{
+		return LSM6DS3_ERR_I2C;
+	}
+	
+	return LSM6DS3_SUCCESS;
+}
+
+#ifdef LSM6DS3_TILT_FUNC //tilt detector
+static int LSM6DS3_enable_tilt(struct i2c_client *client, bool enable)
+{
+	int res = 0;
+	struct lsm6ds3_i2c_data *obj = i2c_get_clientdata(client);//obj_i2c_data;
+	
+	if(enable)
+	{
+		//set ODR to 26 hz		
+		//res = LSM6DS3_acc_SetSampleRate(client, LSM6DS3_ACC_ODR_26HZ);
+		res = LSM6DS3_acc_SetSampleRate(client, obj->sample_rate);
+		if(LSM6DS3_SUCCESS == res)
+		{
+			GSE_LOG(" %s set 26hz odr to acc\n", __func__);
+		}			
+		
+		res = LSM6DS3_Enable_Tilt_Func(client, enable);
+		if(res != LSM6DS3_SUCCESS)
+		{
+			GSE_LOG(" LSM6DS3_Enable_Tilt_Func failed!\n");
+			return LSM6DS3_ERR_STATUS;
+		}
+		
+		res = LSM6DS3_acc_Enable_Func(client, LSM6DS3_ACC_GYRO_FUNC_EN_ENABLED);	
+		if(res != LSM6DS3_SUCCESS)
+		{
+			GSE_LOG(" LSM6DS3_acc_Enable_Func failed!\n");
+			return LSM6DS3_ERR_STATUS;
+		}		
+		
+		res = LSM6DS3_Enable_Tilt_Func_On_Int(client, LSM6DS3_ACC_GYRO_INT1, true);  //default route to INT1 	
+		if(res != LSM6DS3_SUCCESS)
+		{
+			GSE_LOG(" LSM6DS3_Enable_Tilt_Func_On_Int failed!\n");
+			return LSM6DS3_ERR_STATUS;
+		}	
+		mt_eint_unmask(CUST_EINT_LSM6DS3_NUM);
+	}
+	else
+	{
+		res = LSM6DS3_Enable_Tilt_Func(client, enable);
+		if(res != LSM6DS3_SUCCESS)
+		{
+			GSE_LOG(" LSM6DS3_Enable_Tilt_Func failed!\n");
+			return LSM6DS3_ERR_STATUS;
+		}
+		if(!enable_status && !pedo_enable_status)
+		{
+			res = LSM6DS3_acc_SetPowerMode(client, false);
+			if(res != LSM6DS3_SUCCESS)
+			{
+				GSE_LOG(" LSM6DS3_acc_SetPowerMode failed!\n");
+				return LSM6DS3_ERR_STATUS;
+			}
+		}
+		mt_eint_mask(CUST_EINT_LSM6DS3_NUM);
+	}
+//	tilt_enable_status = enable;
+	return LSM6DS3_SUCCESS;
+}
+#endif
+
+#ifdef LSM6DS3_STEP_COUNTER //step counter
+static int LSM6DS3_enable_pedo(struct i2c_client *client, bool enable)
+{
+//	u8 databuf[2] = {0};    
+	int res = 0;
+	struct lsm6ds3_i2c_data *obj = i2c_get_clientdata(client);//obj_i2c_data;
+
+	if(true == enable)
+	{	
+		//software reset
+		//set ODR to 26 hz		
+		//res = LSM6DS3_acc_SetSampleRate(client, LSM6DS3_ACC_ODR_26HZ);
+		res = LSM6DS3_acc_SetSampleRate(client, obj->sample_rate);
+		if(LSM6DS3_SUCCESS == res)
+		{
+			GSE_LOG(" %s set 26hz odr to acc\n", __func__);
+		}
+		//enable tilt feature and pedometer feature
+		res = LSM6DS3_acc_Enable_Pedometer_Func(client, enable);
+		if(res != LSM6DS3_SUCCESS)
+		{
+			GSE_LOG(" LSM6DS3_acc_Enable_Pedometer_Func failed!\n");
+			return LSM6DS3_ERR_STATUS;
+		}
+		
+		res = LSM6DS3_acc_Enable_Func(client, LSM6DS3_ACC_GYRO_FUNC_EN_ENABLED);	
+		if(res != LSM6DS3_SUCCESS)
+		{
+			GSE_LOG(" LSM6DS3_acc_Enable_Func failed!\n");
+			return LSM6DS3_ERR_STATUS;
+		}				
+		res = LSM6DS3_Write_PedoThreshold(client, 0x11);// set threshold to a certain value here
+		if(res != LSM6DS3_SUCCESS)
+		{
+			GSE_LOG(" LSM6DS3_Write_PedoThreshold failed!\n");
+			return LSM6DS3_ERR_STATUS;
+		}
+		res = LSM6DS3_Reset_Pedo_Data(client, LSM6DS3_ACC_GYRO_PEDO_RST_STEP_ENABLED);
+		
+		if(res != LSM6DS3_SUCCESS)
+		{
+			GSE_LOG(" LSM6DS3_Reset_Pedo_Data failed!\n");
+			return LSM6DS3_ERR_STATUS;
+		}
+	}
+	else
+	{
+		res = LSM6DS3_acc_Enable_Pedometer_Func(client, enable);
+		if(res != LSM6DS3_SUCCESS)
+		{
+			GSE_LOG(" LSM6DS3_acc_Enable_Func failed at disable pedo!\n");
+			return LSM6DS3_ERR_STATUS;
+		}
+		//do not turn off the func
+		if(!enable_status && !tilt_enable_status)
+		{
+			res = LSM6DS3_acc_SetPowerMode(client,false);
+			if(res != LSM6DS3_SUCCESS)
+			{
+				GSE_LOG(" LSM6DS3_acc_SetPowerMode failed at disable pedo!\n");
+				return LSM6DS3_ERR_STATUS;
+			}
+		}
+	}
+	return LSM6DS3_SUCCESS;
+}
+#endif
+
+static int LSM6DS3_acc_SetPowerMode(struct i2c_client *client, bool enable)
+{
+	u8 databuf[2] = {0};    
+	int res = 0;
+	struct lsm6ds3_i2c_data *obj = i2c_get_clientdata(client);//obj_i2c_data;
+
+	if(enable == sensor_power)
+	{
+		GSE_LOG("Sensor power status is newest!\n");
+		return LSM6DS3_SUCCESS;
+	}
+
+	if(hwmsen_read_byte(client, LSM6DS3_CTRL1_XL, databuf))
+	{
+		GSE_ERR("read lsm6ds3 power ctl register err!\n");
+		return LSM6DS3_ERR_I2C;
+	}
+	GSE_LOG("LSM6DS3_CTRL1_XL:databuf[0] =  %x!\n", databuf[0]);
+
+
+	if(true == enable)
+	{
+		databuf[0] &= ~LSM6DS3_ACC_ODR_MASK;//clear lsm6ds3 gyro ODR bits
+		databuf[0] |= obj->sample_rate;//LSM6DS3_ACC_ODR_104HZ; //default set 100HZ for LSM6DS3 acc
+	}
+	else
+	{
+		// do nothing
+		databuf[0] &= ~LSM6DS3_ACC_ODR_MASK;//clear lsm6ds3 acc ODR bits
+		databuf[0] |= LSM6DS3_ACC_ODR_POWER_DOWN;
+	}
+	databuf[1] = databuf[0];
+	databuf[0] = LSM6DS3_CTRL1_XL;    
+	res = i2c_master_send(client, databuf, 0x2);
+	if(res <= 0)
+	{
+		GSE_LOG("LSM6DS3 set power mode: ODR 100hz failed!\n");
+		return LSM6DS3_ERR_I2C;
+	}	
+	else
+	{
+		GSE_LOG("set LSM6DS3 gyro power mode:ODR 100HZ ok %d!\n", enable);
+	}	
+
+	sensor_power = enable;
+	
+	return LSM6DS3_SUCCESS;    
+}
+
+
+/*----------------------------------------------------------------------------*/
+static int LSM6DS3_acc_SetFullScale(struct i2c_client *client, u8 acc_fs)
+{
+	u8 databuf[2] = {0};    
+	int res = 0;
+	struct lsm6ds3_i2c_data *obj = i2c_get_clientdata(client);
+	
+	GSE_FUN();     
+		
+	if(hwmsen_read_byte(client, LSM6DS3_CTRL1_XL, databuf))
+	{
+		GSE_ERR("read LSM6DS3_CTRL1_XL err!\n");
+		return LSM6DS3_ERR_I2C;
+	}
+	else
+	{
+		GSE_LOG("read  LSM6DS3_CTRL1_XL register: 0x%x\n", databuf[0]);
+	}
+
+	databuf[0] &= ~LSM6DS3_ACC_RANGE_MASK;//clear 
+	databuf[0] |= acc_fs;
+	
+	databuf[1] = databuf[0];
+	databuf[0] = LSM6DS3_CTRL1_XL; 
+	
+	res = i2c_master_send(client, databuf, 0x2);
+	if(res <= 0)
+	{
+		GSE_ERR("write full scale register err!\n");
+		return LSM6DS3_ERR_I2C;
+	}
+	switch(acc_fs)
+	{
+		case LSM6DS3_ACC_RANGE_2g:
+			obj->sensitivity = LSM6DS3_ACC_SENSITIVITY_2G;
+			break;
+		case LSM6DS3_ACC_RANGE_4g:
+			obj->sensitivity = LSM6DS3_ACC_SENSITIVITY_4G;
+			break;
+		case LSM6DS3_ACC_RANGE_8g:
+			obj->sensitivity = LSM6DS3_ACC_SENSITIVITY_8G;
+			break;
+		case LSM6DS3_ACC_RANGE_16g:
+			obj->sensitivity = LSM6DS3_ACC_SENSITIVITY_16G;
+			break;
+		default:
+			obj->sensitivity = LSM6DS3_ACC_SENSITIVITY_2G;
+			break;
+	}
+
+	if(hwmsen_read_byte(client, LSM6DS3_CTRL9_XL, databuf))
+	{
+		GSE_ERR("read LSM6DS3_CTRL9_XL err!\n");
+		return LSM6DS3_ERR_I2C;
+	}
+	else
+	{
+		GSE_LOG("read  LSM6DS3_CTRL9_XL register: 0x%x\n", databuf[0]);
+	}
+
+	databuf[0] &= ~LSM6DS3_ACC_ENABLE_AXIS_MASK;//clear 
+	databuf[0] |= LSM6DS3_ACC_ENABLE_AXIS_X | LSM6DS3_ACC_ENABLE_AXIS_Y| LSM6DS3_ACC_ENABLE_AXIS_Z;
+	
+	databuf[1] = databuf[0];
+	databuf[0] = LSM6DS3_CTRL9_XL; 
+	
+	res = i2c_master_send(client, databuf, 0x2);
+	if(res <= 0)
+	{
+		GSE_ERR("write full scale register err!\n");
+		return LSM6DS3_ERR_I2C;
+	}
+
+	return LSM6DS3_SUCCESS;    
+}
+
+/*----------------------------------------------------------------------------*/
+// set the acc sample rate
+static int LSM6DS3_acc_SetSampleRate(struct i2c_client *client, u8 sample_rate)
+{
+	u8 databuf[2] = {0}; 
+	int res = 0;
+	GSE_FUN();    
+
+	res = LSM6DS3_acc_SetPowerMode(client, true);	//set Sample Rate will enable power and should changed power status
+	if(res != LSM6DS3_SUCCESS)	
+	{
+		return res;
+	}
+
+	if(hwmsen_read_byte(client, LSM6DS3_CTRL1_XL, databuf))
+	{
+		GSE_ERR("read acc data format register err!\n");
+		return LSM6DS3_ERR_I2C;
+	}
+	else
+	{
+		GSE_LOG("read  acc data format register: 0x%x\n", databuf[0]);
+	}
+
+	databuf[0] &= ~LSM6DS3_ACC_ODR_MASK;//clear 
+	databuf[0] |= sample_rate;
+	
+	databuf[1] = databuf[0];
+	databuf[0] = LSM6DS3_CTRL1_XL; 
+		
+	res = i2c_master_send(client, databuf, 0x2);
+	if(res <= 0)
+	{
+		GSE_ERR("write sample rate register err!\n");
+		return LSM6DS3_ERR_I2C;
+	}
+
+	return LSM6DS3_SUCCESS;    
+}
+
+#ifdef LSM6DS3_TILT_FUNC //tilt detector
+static int LSM6DS3_Enable_Tilt_Func(struct i2c_client *client, bool enable)
+{
+	u8 databuf[2] = {0}; 
+	int res = 0;
+	GSE_FUN();    
+	
+	if(hwmsen_read_byte(client, LSM6DS3_TAP_CFG, databuf))
+	{
+		GSE_ERR("read acc data format register err!\n");
+		return LSM6DS3_ERR_I2C;
+	}
+	else
+	{
+		GSE_LOG("read  acc data format register: 0x%x\n", databuf[0]);
+	}
+
+	if(enable)
+	{
+		databuf[0] &= ~LSM6DS3_TILT_EN_MASK;//clear 
+		databuf[0] |= LSM6DS3_ACC_GYRO_TILT_EN_ENABLED;			
+	}
+	else
+	{
+		databuf[0] &= ~LSM6DS3_TILT_EN_MASK;//clear 
+		databuf[0] |= LSM6DS3_ACC_GYRO_TILT_EN_DISABLED;		
+	}
+	
+	databuf[1] = databuf[0];
+	databuf[0] = LSM6DS3_TAP_CFG; 	
+	res = i2c_master_send(client, databuf, 0x2);
+	if(res < 0)
+	{
+		GSE_ERR("write enable tilt func register err!\n");
+		return LSM6DS3_ERR_I2C;
+	}
+
+	return LSM6DS3_SUCCESS; 
+}
+#endif
+
+#ifdef LSM6DS3_SIGNIFICANT_MOTION
+static int LSM6DS3_Enable_SigMotion_Func_On_Int(struct i2c_client *client, bool enable)
+{
+	u8 databuf[2] = {0}; 
+	int res = 0;
+	u8 op_reg = 0;
+	
+	LSM6DS3_ACC_GYRO_FUNC_EN_t func_enable;
+	LSM6DS3_ACC_GYRO_SIGN_MOT_t sigm_enable;
+	GSE_FUN();    
+	
+	if(enable)
+	{
+		func_enable = LSM6DS3_ACC_GYRO_FUNC_EN_ENABLED;
+		sigm_enable = LSM6DS3_ACC_GYRO_SIGN_MOT_ENABLED;
+		
+		res = LSM6DS3_acc_Enable_Func(client, func_enable);	
+		if(res != LSM6DS3_SUCCESS)
+		{
+			GSE_LOG(" LSM6DS3_acc_Enable_Func failed!\n");
+			return LSM6DS3_ERR_STATUS;
+		}	
+	}
+	else
+	{
+		//func_enable = LSM6DS3_ACC_GYRO_FUNC_EN_DISABLED;
+		sigm_enable = LSM6DS3_ACC_GYRO_SIGN_MOT_DISABLED;
+	}		
+	
+	res = LSM6DS3_Enable_SigMotion_Func(client, sigm_enable);	
+	if(res != LSM6DS3_SUCCESS)
+	{
+		GSE_LOG(" LSM6DS3_acc_Enable_Func failed!\n");
+		return LSM6DS3_ERR_STATUS;
+	}	
+	
+	//Config interrupt for significant motion
+
+	op_reg = LSM6DS3_INT1_CTRL;
+		
+	if(hwmsen_read_byte(client, op_reg, databuf))
+	{
+		GSE_ERR("%s read data format register err!\n", __func__);
+		return LSM6DS3_ERR_I2C;
+	}
+	else
+	{
+		GSE_LOG("read  acc data format register: 0x%x\n", databuf[0]);
+	}
+	
+	if(enable)
+	{
+		databuf[0] &= ~LSM6DS3_ACC_GYRO_INT_SIGN_MOT_MASK;//clear 
+		databuf[0] |= LSM6DS3_ACC_GYRO_INT_SIGN_MOT_ENABLED;			
+	}
+	else
+	{
+		databuf[0] &= ~LSM6DS3_ACC_GYRO_INT_SIGN_MOT_MASK;//clear 
+		databuf[0] |= LSM6DS3_ACC_GYRO_INT_SIGN_MOT_DISABLED;		
+	}
+	
+	databuf[1] = databuf[0];
+	databuf[0] = op_reg; 	
+	res = i2c_master_send(client, databuf, 0x2);
+	if(res < 0)
+	{
+		GSE_ERR("write enable tilt func register err!\n");
+		return LSM6DS3_ERR_I2C;
+	}	
+	res = LSM6DS3_Int_Ctrl(client, LSM6DS3_ACC_GYRO_INT_ACTIVE_LOW, LSM6DS3_ACC_GYRO_INT_LATCH);
+	if(res < 0)
+	{
+		GSE_ERR("write enable tilt func register err!\n");
+		return LSM6DS3_ERR_I2C;
+	}	
+	return LSM6DS3_SUCCESS; 
+}
+#endif
+static int LSM6DS3_Int_Ctrl(struct i2c_client *client, LSM6DS3_ACC_GYRO_INT_ACTIVE_t int_act, LSM6DS3_ACC_GYRO_INT_LATCH_CTL_t int_latch)
+{
+	u8 databuf[2] = {0}; 
+	int res = 0;
+	u8 op_reg = 0;
+	GSE_FUN();    
+
+	//config latch int or no latch
+	op_reg = LSM6DS3_TAP_CFG;
+	if(hwmsen_read_byte(client, op_reg, databuf))
+	{
+		GSE_ERR("%s read data format register err!\n", __func__);
+		return LSM6DS3_ERR_I2C;
+	}
+	else
+	{
+		GSE_LOG("read  acc data format register: 0x%x\n", databuf[0]);
+	}
+	
+	databuf[0] &= ~LSM6DS3_ACC_GYRO_INT_LATCH_CTL_MASK;//clear 
+	databuf[0] |= int_latch;			
+		
+	databuf[1] = databuf[0];
+	databuf[0] = op_reg; 	
+	res = i2c_master_send(client, databuf, 0x2);
+	if(res < 0)
+	{
+		GSE_ERR("write enable tilt func register err!\n");
+		return LSM6DS3_ERR_I2C;
+	}
+	// config high or low active
+	op_reg = LSM6DS3_CTRL3_C;
+	if(hwmsen_read_byte(client, op_reg, databuf))
+	{
+		GSE_ERR("%s read data format register err!\n", __func__);
+		return LSM6DS3_ERR_I2C;
+	}
+	else
+	{
+		GSE_LOG("read  acc data format register: 0x%x\n", databuf[0]);
+	}
+	
+	databuf[0] &= ~LSM6DS3_ACC_GYRO_INT_ACTIVE_MASK;//clear 
+	databuf[0] |= int_act;			
+		
+	databuf[1] = databuf[0];
+	databuf[0] = op_reg; 	
+	res = i2c_master_send(client, databuf, 0x2);
+	if(res <= 0)
+	{
+		GSE_ERR("write enable tilt func register err!\n");
+		return LSM6DS3_ERR_I2C;
+	}
+
+	return LSM6DS3_SUCCESS; 
+}
+
+#ifdef LSM6DS3_TILT_FUNC //tilt detector
+static int LSM6DS3_Enable_Tilt_Func_On_Int(struct i2c_client *client, LSM6DS3_ACC_GYRO_ROUNT_INT_t tilt_int, bool enable)
+{
+	u8 databuf[2] = {0}; 
+	int res = 0;
+	u8 op_reg = 0;
+	GSE_FUN();    
+	
+	if(LSM6DS3_ACC_GYRO_INT1 == tilt_int)
+	{
+		op_reg = LSM6DS3_MD1_CFG;
+	}
+	else if(LSM6DS3_ACC_GYRO_INT2 == tilt_int)
+	{
+		op_reg = LSM6DS3_MD2_CFG;
+	}
+	
+	if(hwmsen_read_byte(client, op_reg, databuf))
+	{
+		GSE_ERR("%s read data format register err!\n", __func__);
+		return LSM6DS3_ERR_I2C;
+	}
+	else
+	{
+		GSE_LOG("read  acc data format register: 0x%x\n", databuf[0]);
+	}
+	
+	if(enable)
+	{
+		databuf[0] &= ~LSM6DS3_ACC_GYRO_INT_TILT_MASK;//clear 
+		databuf[0] |= LSM6DS3_ACC_GYRO_INT_TILT_ENABLED;			
+	}
+	else
+	{
+		databuf[0] &= ~LSM6DS3_ACC_GYRO_INT_TILT_MASK;//clear 
+		databuf[0] |= LSM6DS3_ACC_GYRO_INT_TILT_DISABLED;		
+	}
+	
+	databuf[1] = databuf[0];
+	databuf[0] = op_reg; 	
+	res = i2c_master_send(client, databuf, 0x2);
+	if(res < 0)
+	{
+		GSE_ERR("write enable tilt func register err!\n");
+		return LSM6DS3_ERR_I2C;
+	}
+	res = LSM6DS3_Int_Ctrl(client, LSM6DS3_ACC_GYRO_INT_ACTIVE_LOW, LSM6DS3_ACC_GYRO_INT_LATCH);
+	if(res < 0)
+	{
+		GSE_ERR("write enable tilt func register err!\n");
+		return LSM6DS3_ERR_I2C;
+	}
+
+	return LSM6DS3_SUCCESS; 
+}
+#endif
+
+#ifdef LSM6DS3_STEP_COUNTER //step counter
+static int LSM6DS3_acc_Enable_Pedometer_Func(struct i2c_client *client, bool enable)
+{
+	u8 databuf[2] = {0}; 
+	int res = 0;
+	GSE_FUN();    
+	
+	if(hwmsen_read_byte(client, LSM6DS3_TAP_CFG, databuf))
+	{
+		GSE_ERR("read acc data format register err!\n");
+		return LSM6DS3_ERR_I2C;
+	}
+	else
+	{
+		GSE_LOG("read  acc data format register: 0x%x\n", databuf[0]);
+	}
+
+	if(enable)
+	{
+		databuf[0] &= ~LSM6DS3_PEDO_EN_MASK;//clear 
+		databuf[0] |= LSM6DS3_ACC_GYRO_PEDO_EN_ENABLED;			
+	}
+	else
+	{
+		databuf[0] &= ~LSM6DS3_PEDO_EN_MASK;//clear 
+		databuf[0] |= LSM6DS3_ACC_GYRO_PEDO_EN_DISABLED;		
+	}
+	
+	databuf[1] = databuf[0];
+	databuf[0] = LSM6DS3_TAP_CFG; 	
+	res = i2c_master_send(client, databuf, 0x2);
+	if(res < 0)
+	{
+		GSE_ERR("write enable pedometer func register err!\n");
+		return LSM6DS3_ERR_I2C;
+	}
+
+	return LSM6DS3_SUCCESS;    
+}
+
+#ifdef LSM6DS3_SIGNIFICANT_MOTION
+static int LSM6DS3_Set_SigMotion_Threshold(struct i2c_client *client, u8 SigMotion_Threshold)
+{
+	u8 databuf[2] = {0}; 
+	int res = 0;
+	GSE_FUN();    
+	
+	if(hwmsen_read_byte(client, LSM6DS3_FUNC_CFG_ACCESS, databuf))
+	{
+		GSE_ERR("%s read LSM6DS3_CTRL10_C register err!\n", __func__);
+		return LSM6DS3_ERR_I2C;
+	}
+	else
+	{
+		GSE_LOG("%s read acc data format register: 0x%x\n", __func__, databuf[0]);
+	}
+	databuf[0] = 0x80;
+	
+	databuf[1] = databuf[0];
+	databuf[0] = LSM6DS3_FUNC_CFG_ACCESS; 	
+	res = i2c_master_send(client, databuf, 0x2);
+	if(res <= 0)
+	{
+		GSE_ERR("%s write LSM6DS3_CTRL10_C register err!\n", __func__);
+		return LSM6DS3_ERR_I2C;
+	}
+	
+	databuf[1] = SigMotion_Threshold;
+	databuf[0] = LSM6DS3_SM_THS; 
+	
+	res = i2c_master_send(client, databuf, 0x2);
+	if(res <= 0)
+	{
+		GSE_ERR("%s write LSM6DS3_CTRL10_C register err!\n", __func__);
+		return LSM6DS3_ERR_I2C;
+	}
+	
+	databuf[1] = 0x00;
+	databuf[0] = LSM6DS3_FUNC_CFG_ACCESS;
+	res = i2c_master_send(client, databuf, 0x2);
+	if(res <= 0)
+	{
+		GSE_ERR("%s write LSM6DS3_CTRL10_C register err!\n", __func__);
+		return LSM6DS3_ERR_I2C;
+	}	
+	return LSM6DS3_SUCCESS;    
+}
+static int LSM6DS3_Enable_SigMotion_Func(struct i2c_client *client, LSM6DS3_ACC_GYRO_SIGN_MOT_t newValue)
+{
+	u8 databuf[2] = {0}; 
+	int res = 0;
+	GSE_FUN();    
+	
+	if(hwmsen_read_byte(client, LSM6DS3_CTRL10_C, databuf))
+	{
+		GSE_ERR("%s read LSM6DS3_CTRL10_C register err!\n", __func__);
+		return LSM6DS3_ERR_I2C;
+	}
+	else
+	{
+		GSE_LOG("%s read acc data format register: 0x%x\n", __func__, databuf[0]);
+	}
+	databuf[0] &= ~LSM6DS3_ACC_GYRO_SIGN_MOT_MASK;//clear 
+	databuf[0] |= newValue;
+	
+	databuf[1] = databuf[0];
+	databuf[0] = LSM6DS3_CTRL10_C; 	
+	res = i2c_master_send(client, databuf, 0x2);
+	if(res <= 0)
+	{
+		GSE_ERR("%s write LSM6DS3_CTRL10_C register err!\n", __func__);
+		return LSM6DS3_ERR_I2C;
+	}
+
+	return LSM6DS3_SUCCESS;    
+}
+#endif
+#endif
+static int LSM6DS3_acc_Enable_Func(struct i2c_client *client, LSM6DS3_ACC_GYRO_FUNC_EN_t newValue)
+{
+	u8 databuf[2] = {0}; 
+	int res = 0;
+	GSE_FUN();    
+	
+	if(hwmsen_read_byte(client, LSM6DS3_CTRL10_C, databuf))
+	{
+		GSE_ERR("%s read LSM6DS3_CTRL10_C register err!\n", __func__);
+		return LSM6DS3_ERR_I2C;
+	}
+	else
+	{
+		GSE_LOG("%s read acc data format register: 0x%x\n", __func__, databuf[0]);
+	}
+	databuf[0] &= ~LSM6DS3_ACC_GYRO_FUNC_EN_MASK;//clear 
+	databuf[0] |= newValue;
+	
+	databuf[1] = databuf[0];
+	databuf[0] = LSM6DS3_CTRL10_C; 	
+	res = i2c_master_send(client, databuf, 0x2);
+	if(res <= 0)
+	{
+		GSE_ERR("%s write LSM6DS3_CTRL10_C register err!\n", __func__);
+		return LSM6DS3_ERR_I2C;
+	}
+
+	return LSM6DS3_SUCCESS;    
+}
+
+#ifdef LSM6DS3_STEP_COUNTER //step counter
+static int LSM6DS3_W_Open_RAM_Page(struct i2c_client *client, LSM6DS3_ACC_GYRO_RAM_PAGE_t newValue)
+{
+	u8 databuf[2] = {0}; 
+	int res = 0;
+	GSE_FUN();    
+	
+	if(hwmsen_read_byte(client, LSM6DS3_RAM_ACCESS, databuf))
+	{
+		GSE_ERR("%s read LSM6DS3_RAM_ACCESS register err!\n", __func__);
+		return LSM6DS3_ERR_I2C;
+	}
+	else
+	{
+		GSE_LOG("%s read acc data format register: 0x%x\n", __func__, databuf[0]);
+	}
+	databuf[0] &= ~LSM6DS3_RAM_PAGE_MASK;//clear 
+	databuf[0] |= newValue;
+	
+	databuf[1] = databuf[0];
+	databuf[0] = LSM6DS3_RAM_ACCESS; 	
+	res = i2c_master_send(client, databuf, 0x2);
+	if(res <= 0)
+	{
+		GSE_ERR("%s write LSM6DS3_RAM_ACCESS register err!\n", __func__);
+		return LSM6DS3_ERR_I2C;
+	}
+
+	return LSM6DS3_SUCCESS;
+}
+
+static int LSM6DS3_Write_PedoThreshold(struct i2c_client *client, u8 newValue)
+{
+	u8 databuf[2] = {0}; 
+	int res = 0;
+	GSE_FUN();    
+	
+	res = LSM6DS3_W_Open_RAM_Page(client, LSM6DS3_ACC_GYRO_RAM_PAGE_ENABLED);
+	if(LSM6DS3_SUCCESS != res)
+	{
+		return res;
+	}
+	if(hwmsen_read_byte(client, LSM6DS3_CONFIG_PEDO_THS_MIN, databuf))
+	{
+		GSE_ERR("%s read LSM6DS3_CTRL10_C register err!\n", __func__);
+		return LSM6DS3_ERR_I2C;
+	}
+	else
+	{
+		GSE_LOG("%s read acc data format register: 0x%x\n", __func__, databuf[0]);
+	}
+	
+	databuf[0] &= ~0x1F; 
+	databuf[0] |= (newValue & 0x1F);
+	
+	databuf[1] = databuf[0];
+	databuf[0] = LSM6DS3_CONFIG_PEDO_THS_MIN; 	
+	res = i2c_master_send(client, databuf, 0x2);
+	if(res <= 0)
+	{
+		GSE_ERR("%s write LSM6DS3_CTRL10_C register err!\n", __func__);
+		return LSM6DS3_ERR_I2C;
+	}
+	
+	databuf[0] = 0x14;
+	databuf[1] = 0x6e; 
+	res = i2c_master_send(client, databuf, 0x2);
+	if(res <= 0)
+	{
+		GSE_ERR("%s write LSM6DS3_CTRL10_C register err!\n", __func__);
+		return LSM6DS3_ERR_I2C;
+	}
+	res = LSM6DS3_W_Open_RAM_Page(client, LSM6DS3_ACC_GYRO_RAM_PAGE_DISABLED);
+	if(LSM6DS3_SUCCESS != res)
+	{
+		GSE_ERR("%s write LSM6DS3_W_Open_RAM_Page failed!\n", __func__);
+		return res;
+	}
+	
+	return LSM6DS3_SUCCESS; 
+}
+static int LSM6DS3_Reset_Pedo_Data(struct i2c_client *client, LSM6DS3_ACC_GYRO_PEDO_RST_STEP_t newValue)
+{
+	u8 databuf[2] = {0}; 
+	int res = 0;
+	GSE_FUN();    
+	
+	if(hwmsen_read_byte(client, LSM6DS3_CTRL10_C, databuf))
+	{
+		GSE_ERR("%s read LSM6DS3_CTRL10_C register err!\n", __func__);
+		return LSM6DS3_ERR_I2C;
+	}
+	else
+	{
+		GSE_LOG("%s read acc LSM6DS3_CTRL10_C data format register: 0x%x\n", __func__, databuf[0]);
+	}
+	databuf[0] &= ~LSM6DS3_PEDO_RST_STEP_MASK;//clear 
+	databuf[0] |= newValue;
+	
+	databuf[1] = databuf[0];
+	databuf[0] = LSM6DS3_CTRL10_C; 	
+	res = i2c_master_send(client, databuf, 0x2);
+	if(res <= 0)
+	{
+		GSE_ERR("%s write LSM6DS3_CTRL10_C register err!\n", __func__);
+		return LSM6DS3_ERR_I2C;
+	}
+
+	return LSM6DS3_SUCCESS;
+}
+static int LSM6DS3_Get_Pedo_DataReg(struct i2c_client *client, u16 *Value)
+{
+	u8 databuf[2] = {0}; 
+	GSE_FUN();    
+	
+	if(hwmsen_read_block(client, LSM6DS3_STEP_COUNTER_L, databuf, 2))
+	{
+		GSE_ERR("LSM6DS3 read acc data  error\n");
+		return -2;
+	}
+
+	*Value = (databuf[1]<<8)|databuf[0];	
+
+	return LSM6DS3_SUCCESS;
+}
+#endif
+/*----------------------------------------------------------------------------*/
+static int LSM6DS3_ReadAccData(struct i2c_client *client, char *buf, int bufsize)
+{
+	struct lsm6ds3_i2c_data *obj = (struct lsm6ds3_i2c_data*)i2c_get_clientdata(client);
+	u8 databuf[20];
+	int acc[LSM6DS3_ACC_AXES_NUM];
+	int res = 0;
+	memset(databuf, 0, sizeof(u8)*10);
+
+	if(NULL == buf)
+	{
+		return -1;
+	}
+	if(NULL == client)
+	{
+		*buf = 0;
+		return -2;
+	}
+
+	if(sensor_power == false)
+	{
+		res = LSM6DS3_acc_SetPowerMode(client, true);
+		if(res)
+		{
+			GSE_ERR("Power on lsm6ds3 error %d!\n", res);
+		}
+		msleep(20);
+	}
+	
+	res = LSM6DS3_ReadAccRawData(client, obj->data);
+	if(res < 0)
+	{        
+		GSE_ERR("I2C error: ret value=%d", res);
+		return -3;
+	}
+	else
+	{
+	#if 1
+		obj->data[LSM6DS3_AXIS_X] = (long)(obj->data[LSM6DS3_AXIS_X]) * obj->sensitivity*GRAVITY_EARTH_1000/(1000*1000); //NTC
+		obj->data[LSM6DS3_AXIS_Y] = (long)(obj->data[LSM6DS3_AXIS_Y]) * obj->sensitivity*GRAVITY_EARTH_1000/(1000*1000);
+		obj->data[LSM6DS3_AXIS_Z] = (long)(obj->data[LSM6DS3_AXIS_Z]) * obj->sensitivity*GRAVITY_EARTH_1000/(1000*1000);
+		
+		obj->data[LSM6DS3_AXIS_X] += obj->cali_sw[LSM6DS3_AXIS_X];
+		obj->data[LSM6DS3_AXIS_Y] += obj->cali_sw[LSM6DS3_AXIS_Y];
+		obj->data[LSM6DS3_AXIS_Z] += obj->cali_sw[LSM6DS3_AXIS_Z];
+		
+		/*remap coordinate*/
+		acc[obj->cvt.map[LSM6DS3_AXIS_X]] = obj->cvt.sign[LSM6DS3_AXIS_X]*obj->data[LSM6DS3_AXIS_X];
+		acc[obj->cvt.map[LSM6DS3_AXIS_Y]] = obj->cvt.sign[LSM6DS3_AXIS_Y]*obj->data[LSM6DS3_AXIS_Y];
+		acc[obj->cvt.map[LSM6DS3_AXIS_Z]] = obj->cvt.sign[LSM6DS3_AXIS_Z]*obj->data[LSM6DS3_AXIS_Z];
+
+		//GSE_LOG("Mapped gsensor data: %d, %d, %d!\n", acc[LSM6DS3_AXIS_X], acc[LSM6DS3_AXIS_Y], acc[LSM6DS3_AXIS_Z]);
+
+		//Out put the mg
+		/*
+		acc[LSM6DS3_AXIS_X] = acc[LSM6DS3_AXIS_X] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+		acc[LSM6DS3_AXIS_Y] = acc[LSM6DS3_AXIS_Y] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+		acc[LSM6DS3_AXIS_Z] = acc[LSM6DS3_AXIS_Z] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;		
+		*/
+	#endif
+
+
+		sprintf(buf, "%04x %04x %04x", acc[LSM6DS3_AXIS_X], acc[LSM6DS3_AXIS_Y], acc[LSM6DS3_AXIS_Z]);
+	
+		if(atomic_read(&obj->trace) & ADX_TRC_IOCTL)//atomic_read(&obj->trace) & ADX_TRC_IOCTL
+		{
+			//GSE_LOG("gsensor data: %s!\n", buf);
+			GSE_LOG("raw data:obj->data:%04x %04x %04x\n", obj->data[LSM6DS3_AXIS_X], obj->data[LSM6DS3_AXIS_Y], obj->data[LSM6DS3_AXIS_Z]);
+			GSE_LOG("acc:%04x %04x %04x\n", acc[LSM6DS3_AXIS_X], acc[LSM6DS3_AXIS_Y], acc[LSM6DS3_AXIS_Z]);
+	
+			//LSM6DS3_dumpReg(client);
+		}
+	}
+	
+	return 0;
+}
+static int LSM6DS3_ReadAccRawData(struct i2c_client *client, s16 data[LSM6DS3_ACC_AXES_NUM])
+{
+	int err = 0;
+	char databuf[6] = {0};
+
+	if(NULL == client)
+	{
+		err = -EINVAL;
+	}	
+	else
+	{
+		if(hwmsen_read_block(client, LSM6DS3_OUTX_L_XL, databuf, 6))
+		{
+			GSE_ERR("LSM6DS3 read acc data  error\n");
+			return -2;
+		}
+		else
+		{
+			data[LSM6DS3_AXIS_X] = (s16)((databuf[LSM6DS3_AXIS_X*2+1] << 8) | (databuf[LSM6DS3_AXIS_X*2]));
+			data[LSM6DS3_AXIS_Y] = (s16)((databuf[LSM6DS3_AXIS_Y*2+1] << 8) | (databuf[LSM6DS3_AXIS_Y*2]));
+			data[LSM6DS3_AXIS_Z] = (s16)((databuf[LSM6DS3_AXIS_Z*2+1] << 8) | (databuf[LSM6DS3_AXIS_Z*2]));	
+		}      
+	}
+	return err;
+}
+
+/*----------------------------------------------------------------------------*/
+static int LSM6DS3_ReadChipInfo(struct i2c_client *client, char *buf, int bufsize)
+{
+	u8 databuf[10];    
+
+	memset(databuf, 0, sizeof(u8)*10);
+
+	if((NULL == buf)||(bufsize<=30))
+	{
+		return -1;
+	}
+	
+	if(NULL == client)
+	{
+		*buf = 0;
+		return -2;
+	}
+
+	sprintf(buf, "LSM6DS3 Chip");
+	return 0;
+}
+
+/*----------------------------------------------------------------------------*/
+static ssize_t show_chipinfo_value(struct device_driver *ddri, char *buf)
+{
+	struct i2c_client *client = lsm6ds3_i2c_client;
+	char strbuf[LSM6DS3_BUFSIZE];
+	if(NULL == client)
+	{
+		GSE_ERR("i2c client is null!!\n");
+		return 0;
+	}
+	
+	LSM6DS3_ReadChipInfo(client, strbuf, LSM6DS3_BUFSIZE);
+	return snprintf(buf, PAGE_SIZE, "%s\n", strbuf);        
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t show_sensordata_value(struct device_driver *ddri, char *buf)
+{
+	struct i2c_client *client = lsm6ds3_i2c_client;
+	char strbuf[LSM6DS3_BUFSIZE];
+	int x,y,z;
+	
+	if(NULL == client)
+	{
+		GSE_ERR("i2c client is null!!\n");
+		return 0;
+	}
+	
+	LSM6DS3_ReadAccData(client, strbuf, LSM6DS3_BUFSIZE);
+	sscanf(strbuf, "%x %x %x", &x, &y, &z);	
+	return snprintf(buf, PAGE_SIZE, "%d, %d, %d\n", x,y,z);            
+}
+static ssize_t show_sensorrawdata_value(struct device_driver *ddri, char *buf)
+{
+	struct i2c_client *client = lsm6ds3_i2c_client;
+	s16 data[LSM6DS3_ACC_AXES_NUM] = {0};
+	
+	if(NULL == client)
+	{
+		GSE_ERR("i2c client is null!!\n");
+		return 0;
+	}
+	
+	LSM6DS3_ReadAccRawData(client, data);
+	return snprintf(buf, PAGE_SIZE, "%x,%x,%x\n", data[0],data[1],data[2]);            
+}
+
+/*----------------------------------------------------------------------------*/
+static ssize_t show_trace_value(struct device_driver *ddri, char *buf)
+{
+	ssize_t res;
+	struct lsm6ds3_i2c_data *obj = obj_i2c_data;
+	
+	if (obj == NULL)
+	{
+		GSE_ERR("i2c_data obj is null!!\n");
+		return 0;
+	}
+	
+	res = snprintf(buf, PAGE_SIZE, "0x%04X\n", atomic_read(&obj->trace));     
+	return res;    
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t store_trace_value(struct device_driver *ddri, const char *buf, size_t count)
+{
+	struct lsm6ds3_i2c_data *obj = obj_i2c_data;
+	int trace;
+	if (obj == NULL)
+	{
+		GSE_ERR("i2c_data obj is null!!\n");
+		return count;
+	}
+	
+	if(1 == sscanf(buf, "0x%x", &trace))
+	{
+		atomic_set(&obj->trace, trace);
+	}	
+	else
+	{
+		GSE_ERR("invalid content: '%s', length = %zu\n", buf, count);
+	}
+	
+	return count;    
+}
+static ssize_t show_chipinit_value(struct device_driver *ddri, char *buf)
+{
+	ssize_t res;
+	struct lsm6ds3_i2c_data *obj = obj_i2c_data;
+	if (obj == NULL)
+	{
+		GSE_ERR("i2c_data obj is null!!\n");
+		return 0;
+	}
+	
+	res = snprintf(buf, PAGE_SIZE, "0x%04X\n", atomic_read(&obj->trace));     
+	return res;    
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t store_chipinit_value(struct device_driver *ddri, const char *buf, size_t count)
+{
+	struct lsm6ds3_i2c_data *obj = obj_i2c_data;
+
+	if (obj == NULL)
+	{
+		GSE_ERR("i2c_data obj is null!!\n");
+		return count;
+	}
+	
+	LSM6DS3_init_client(obj->client, true);
+	LSM6DS3_dumpReg(obj->client);
+	
+	return count;    
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t show_status_value(struct device_driver *ddri, char *buf)
+{
+	ssize_t len = 0;    
+	struct lsm6ds3_i2c_data *obj = obj_i2c_data;
+	if (obj == NULL)
+	{
+		GSE_ERR("i2c_data obj is null!!\n");
+		return 0;
+	}	
+	
+	if(obj->hw)
+	{
+		len += snprintf(buf+len, PAGE_SIZE-len, "CUST: i2c_num=%d, direction=%d, sensitivity = %d,(power_id=%d, power_vol=%d)\n", 
+	            obj->hw->i2c_num, obj->hw->direction, obj->sensitivity, obj->hw->power_id, obj->hw->power_vol);   
+	LSM6DS3_dumpReg(obj->client);
+	}
+	else
+	{
+		len += snprintf(buf+len, PAGE_SIZE-len, "CUST: NULL\n");
+	}
+	return len;    
+}
+static ssize_t show_layout_value(struct device_driver *ddri, char *buf)
+{
+	struct lsm6ds3_i2c_data *data = obj_i2c_data;
+	if(NULL == data)
+	{
+		printk(KERN_ERR "lsm6ds3_i2c_data is null!!\n");
+		return -1;
+	}
+
+	return sprintf(buf, "(%d, %d)\n[%+2d %+2d %+2d]\n[%+2d %+2d %+2d]\n",
+		data->hw->direction,atomic_read(&data->layout),	data->cvt.sign[0], data->cvt.sign[1],
+		data->cvt.sign[2],data->cvt.map[0], data->cvt.map[1], data->cvt.map[2]);
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t store_layout_value(struct device_driver *ddri, const char *buf, size_t count)
+{
+	int layout = 0;
+	struct lsm6ds3_i2c_data *data = obj_i2c_data;
+
+	if(NULL == data)
+	{
+		printk(KERN_ERR "lsm6ds3_i2c_data is null!!\n");
+		return count;
+	}
+
+	
+
+	if(1 == sscanf(buf, "%d", &layout))
+	{
+		atomic_set(&data->layout, layout);
+		if(!hwmsen_get_convert(layout, &data->cvt))
+		{
+			printk(KERN_ERR "HWMSEN_GET_CONVERT function error!\r\n");
+		}
+		else if(!hwmsen_get_convert(data->hw->direction, &data->cvt))
+		{
+			printk(KERN_ERR "invalid layout: %d, restore to %d\n", layout, data->hw->direction);
+		}
+		else
+		{
+			printk(KERN_ERR "invalid layout: (%d, %d)\n", layout, data->hw->direction);
+			hwmsen_get_convert(0, &data->cvt);
+		}
+	}
+	else
+	{
+		printk(KERN_ERR "invalid format = '%s'\n", buf);
+	}
+
+	return count;
+}
+
+/*----------------------------------------------------------------------------*/
+
+static DRIVER_ATTR(chipinfo,             S_IRUGO, show_chipinfo_value,      NULL);
+static DRIVER_ATTR(sensorrawdata,           S_IRUGO, show_sensorrawdata_value,    NULL);
+static DRIVER_ATTR(sensordata,           S_IRUGO, show_sensordata_value,    NULL);
+static DRIVER_ATTR(trace,      S_IWUGO | S_IRUGO, show_trace_value,         store_trace_value);
+static DRIVER_ATTR(chipinit,      S_IWUGO | S_IRUGO, show_chipinit_value,         store_chipinit_value);
+static DRIVER_ATTR(status,               S_IRUGO, show_status_value,        NULL);
+static DRIVER_ATTR(layout,      S_IRUGO | S_IWUSR, show_layout_value, store_layout_value);
+
+/*----------------------------------------------------------------------------*/
+static struct driver_attribute *LSM6DS3_attr_list[] = {
+	&driver_attr_chipinfo,     /*chip information*/
+	&driver_attr_sensordata,   /*dump sensor data*/	
+	&driver_attr_sensorrawdata,   /*dump sensor raw data*/	
+	&driver_attr_trace,        /*trace log*/
+	&driver_attr_status,  
+	&driver_attr_chipinit,
+	&driver_attr_layout,
+};
+/*----------------------------------------------------------------------------*/
+static int lsm6ds3_create_attr(struct device_driver *driver) 
+{
+	int idx, err = 0;
+	int num = (int)(sizeof(LSM6DS3_attr_list)/sizeof(LSM6DS3_attr_list[0]));
+	if (driver == NULL)
+	{
+		return -EINVAL;
+	}
+
+	for(idx = 0; idx < num; idx++)
+	{
+		if(0 != (err = driver_create_file(driver,  LSM6DS3_attr_list[idx])))
+		{            
+			GSE_ERR("driver_create_file (%s) = %d\n",  LSM6DS3_attr_list[idx]->attr.name, err);
+			break;
+		}
+	}    
+	return err;
+}
+/*----------------------------------------------------------------------------*/
+static int lsm6ds3_delete_attr(struct device_driver *driver)
+{
+	int idx ,err = 0;
+	int num = (int)(sizeof( LSM6DS3_attr_list)/sizeof( LSM6DS3_attr_list[0]));
+
+	if(driver == NULL)
+	{
+		return -EINVAL;
+	}	
+
+	for(idx = 0; idx < num; idx++)
+	{
+		driver_remove_file(driver,  LSM6DS3_attr_list[idx]);
+	}
+	return err;
+}
+static int LSM6DS3_Set_RegInc(struct i2c_client *client, bool inc)
+{
+	u8 databuf[2] = {0};    
+	int res = 0;
+	//GSE_FUN();     
+	
+	if(hwmsen_read_byte(client, LSM6DS3_CTRL3_C, databuf))
+	{
+		GSE_ERR("read LSM6DS3_CTRL3_XL err!\n");
+		return LSM6DS3_ERR_I2C;
+	}
+	else
+	{
+		GSE_LOG("read  LSM6DS3_CTRL3_C register: 0x%x\n", databuf[0]);
+	}
+	if(inc)
+	{
+		databuf[0] |= LSM6DS3_CTRL3_C_IFINC;
+		
+		databuf[1] = databuf[0];
+		databuf[0] = LSM6DS3_CTRL3_C; 
+		
+		res = i2c_master_send(client, databuf, 0x2);
+		if(res <= 0)
+		{
+			GSE_ERR("write full scale register err!\n");
+			return LSM6DS3_ERR_I2C;
+		}
+	}
+
+	return LSM6DS3_SUCCESS;    
+}
+
+/*----------------------------------------------------------------------------*/
+static int LSM6DS3_init_client(struct i2c_client *client, bool enable)
+{
+	struct lsm6ds3_i2c_data *obj = i2c_get_clientdata(client);
+	int res = 0;
+	GSE_FUN();	
+    GSE_LOG(" lsm6ds3 addr %x!\n",client->addr);
+	res = LSM6DS3_CheckDeviceID(client);
+	if(res != LSM6DS3_SUCCESS)
+	{
+		return res;
+	}
+
+	res = LSM6DS3_Set_RegInc(client, true);
+	if(res != LSM6DS3_SUCCESS) 
+	{
+		return res;
+	}
+
+	res = LSM6DS3_acc_SetFullScale(client,LSM6DS3_ACC_RANGE_2g);//we have only this choice
+	if(res != LSM6DS3_SUCCESS) 
+	{
+		return res;
+	}
+
+	//res = LSM6DS3_acc_SetSampleRate(client, LSM6DS3_ACC_ODR_104HZ);
+	res = LSM6DS3_acc_SetSampleRate(client, obj->sample_rate);
+	if(res != LSM6DS3_SUCCESS ) 
+	{
+		return res;
+	}
+	
+	res = LSM6DS3_acc_SetPowerMode(client, enable);
+	if(res != LSM6DS3_SUCCESS)
+	{
+		return res;
+	}
+
+	GSE_LOG("LSM6DS3_init_client OK!\n");
+	//acc setting
+		
+#ifdef CONFIG_LSM6DS3_LOWPASS
+	memset(&obj->fir, 0x00, sizeof(obj->fir));  
+#endif
+
+	return LSM6DS3_SUCCESS;
+}
+/*----------------------------------------------------------------------------*/
+#ifdef LSM6DS3_NEW_ARCH
+static int lsm6ds3_open_report_data(int open)
+{
+    //should queuq work to report event if  is_report_input_direct=true
+	
+    return 0;
+}
+
+// if use  this typ of enable , Gsensor only enabled but not report inputEvent to HAL
+
+static int lsm6ds3_enable_nodata(int en)
+{
+	int value = en;
+	int err = 0;
+	struct lsm6ds3_i2c_data *priv = obj_i2c_data;
+
+	if(priv == NULL)
+	{
+		GSE_ERR("obj_i2c_data is NULL!\n");
+		return -1;
+	}
+
+	if(value == 1)
+	{
+		enable_status = true;
+	}
+	else
+	{
+		enable_status = false;
+		priv->sample_rate = LSM6DS3_ACC_ODR_104HZ; //default rate
+	}
+	GSE_LOG("enable value=%d, sensor_power =%d\n",value,sensor_power);
+	
+	if(((value == 0) && (sensor_power == false)) ||((value == 1) && (sensor_power == true)))
+	{
+		GSE_LOG("Gsensor device have updated!\n");
+	}
+	else if(!pedo_enable_status && !tilt_enable_status)
+	{
+		err = LSM6DS3_acc_SetPowerMode( priv->client, enable_status);					
+	}
+
+    GSE_LOG("%s OK!\n",__FUNCTION__);
+    return err;
+}
+
+static int lsm6ds3_set_delay(u64 ns)
+{
+    int value =0;
+	int err = 0;
+    value = (int)ns/1000/1000;
+	int sample_delay;
+	struct lsm6ds3_i2c_data *priv = obj_i2c_data;
+
+	if(priv == NULL)
+	{
+		GSE_ERR("obj_i2c_data is NULL!\n");
+		return -1;
+	}
+					
+	if(value <= 5)
+	{
+		sample_delay = LSM6DS3_ACC_ODR_208HZ;
+	}
+	else if(value <= 10)
+	{
+		sample_delay = LSM6DS3_ACC_ODR_104HZ;
+	}
+	else
+	{
+		sample_delay = LSM6DS3_ACC_ODR_52HZ;
+	}
+	priv->sample_rate = sample_delay;
+	err = LSM6DS3_acc_SetSampleRate(priv->client, sample_delay);
+	if(err != LSM6DS3_SUCCESS ) 
+	{
+		GSE_ERR("Set delay parameter error!\n");
+	}
+
+	if(value >= 50)
+	{
+		atomic_set(&priv->filter, 0);
+	}
+	else
+	{					
+		priv->fir.num = 0;
+		priv->fir.idx = 0;
+		priv->fir.sum[LSM6DS3_AXIS_X] = 0;
+		priv->fir.sum[LSM6DS3_AXIS_Y] = 0;
+		priv->fir.sum[LSM6DS3_AXIS_Z] = 0;
+		atomic_set(&priv->filter, 1);
+	}
+
+    GSE_LOG("%s (%d), chip only use 1024HZ \n",__FUNCTION__, value);
+    return 0;
+}
+
+static int lsm6ds3_get_data(int* x ,int* y,int* z, int* status)
+{
+    char buff[LSM6DS3_BUFSIZE];
+	struct lsm6ds3_i2c_data *priv = obj_i2c_data;
+		
+	if(priv == NULL)
+	{
+		GSE_ERR("obj_i2c_data is NULL!\n");
+		return -1;
+	}
+	if(atomic_read(&priv->trace) & ACCEL_TRC_DATA)
+	{
+		GSE_LOG("%s (%d),  \n",__FUNCTION__,__LINE__);
+	}
+	memset(buff, 0, sizeof(buff));
+	LSM6DS3_ReadAccData(priv->client, buff, LSM6DS3_BUFSIZE);
+	
+	sscanf(buff, "%x %x %x", x, y, z);				
+	*status = SENSOR_STATUS_ACCURACY_MEDIUM;				
+
+    return 0;
+}
+#ifdef LSM6DS3_TILT_FUNC
+static int lsm6ds3_tilt_open_report_data(int open)
+{
+	int res = 0;
+	struct lsm6ds3_i2c_data *priv = obj_i2c_data;
+	
+	if(1 == open)
+	{
+		tilt_enable_status = true;
+		res = LSM6DS3_enable_tilt(priv->client, true);
+		if(LSM6DS3_SUCCESS != res)
+		{
+			GSE_ERR("%s run LSM6DS3_enable_tilt to true failed!\n", __func__);
+		}
+	}
+	else if(0 == open)
+	{
+		tilt_enable_status = false;
+		res = LSM6DS3_enable_tilt(priv->client, false);
+		if(LSM6DS3_SUCCESS != res)
+		{
+			GSE_ERR("%s run LSM6DS3_enable_tilt to false failed!\n", __func__);
+		}
+	}
+	
+	return res;
+}
+#endif
+
+#ifdef LSM6DS3_SIGNIFICANT_MOTION
+static int lsm6ds3_step_c_enable_significant(int en)
+{
+	int res =0;
+	struct lsm6ds3_i2c_data *priv = obj_i2c_data;
+	
+	if(1 == en)
+	{
+		pedo_enable_status = true;
+		res = LSM6DS3_Set_SigMotion_Threshold(priv->client, 0x08);
+		if(LSM6DS3_SUCCESS != res)
+		{
+			GSE_ERR("%s run LSM6DS3_Set_SigMotion_Threshold to fail!\n", __func__);
+		}
+		//res = LSM6DS3_acc_SetSampleRate(priv->client, LSM6DS3_ACC_ODR_26HZ);
+		res = LSM6DS3_acc_SetSampleRate(priv->client, priv->sample_rate);
+		if(LSM6DS3_SUCCESS != res)
+		{
+			GSE_ERR("%s run LSM6DS3_Set_SigMotion_Threshold to fail!\n", __func__);
+		}
+		res = LSM6DS3_Enable_SigMotion_Func_On_Int(priv->client, true); //default route to INT2
+		if(LSM6DS3_SUCCESS != res)
+		{
+			GSE_ERR("%s run LSM6DS3_Enable_SigMotion_Func_On_Int to fail!\n", __func__);
+		}
+		
+		res = LSM6DS3_acc_SetFullScale(priv->client, LSM6DS3_ACC_RANGE_2g);
+		if(LSM6DS3_SUCCESS != res)
+		{
+			GSE_ERR("%s run LSM6DS3_Enable_SigMotion_Func_On_Int to fail!\n", __func__);
+		}
+		
+		mt_eint_unmask(CUST_EINT_LSM6DS3_NUM);
+	}
+	else if(0 == en)
+	{
+		pedo_enable_status = false;
+		res = LSM6DS3_Enable_SigMotion_Func_On_Int(priv->client, false);
+		if(LSM6DS3_SUCCESS != res)
+		{
+			GSE_ERR("%s run LSM6DS3_Enable_SigMotion_Func_On_Int to fail!\n", __func__);
+		}
+		if(!enable_status && !tilt_enable_status)
+		{
+			res = LSM6DS3_acc_SetPowerMode(priv->client, false);
+			if(LSM6DS3_SUCCESS != res)
+			{
+				GSE_ERR("%s run LSM6DS3_acc_SetPowerMode to fail!\n", __func__);
+			}
+		}
+		
+		mt_eint_mask(CUST_EINT_LSM6DS3_NUM);
+	}
+	
+	return res;
+}
+#endif
+
+#ifdef LSM6DS3_STEP_COUNTER //step counter
+static int lsm6ds3_step_c_open_report_data(int open)
+{
+	
+	return LSM6DS3_SUCCESS;
+}
+static int lsm6ds3_step_c_enable_nodata(int en)
+{
+	int res =0;
+	int value = en;
+	int err = 0;
+	struct lsm6ds3_i2c_data *priv = obj_i2c_data;
+
+	if(priv == NULL)
+	{
+		GSE_ERR("%s obj_i2c_data is NULL!\n", __func__);
+		return -1;
+	}
+
+	if(value == 1)
+	{
+		pedo_enable_status = true;
+		res = LSM6DS3_enable_pedo(priv->client, true);
+		if(LSM6DS3_SUCCESS != res)
+		{
+			GSE_LOG("LSM6DS3_enable_pedo failed at open action!\n");
+			return res;
+		}
+	}
+	else
+	{
+		pedo_enable_status = false;
+		res = LSM6DS3_enable_pedo(priv->client, false);
+		if(LSM6DS3_SUCCESS != res)
+		{
+			GSE_LOG("LSM6DS3_enable_pedo failed at close action!\n");
+			return res;
+		}
+
+	}
+	
+	GSE_LOG("lsm6ds3_step_c_enable_nodata OK!\n");
+    return err;
+}
+static int lsm6ds3_step_c_enable_step_detect(int en)
+{
+	return lsm6ds3_step_c_enable_nodata(en);
+}
+
+static int lsm6ds3_step_c_set_delay(u64 delay)
+{
+	
+	return 0;
+}
+static int lsm6ds3_step_c_get_data(u64 *value, int *status)
+{
+	int err = 0;
+	u16 pedo_data = 0;
+	
+	struct lsm6ds3_i2c_data *priv = obj_i2c_data;
+	err = LSM6DS3_Get_Pedo_DataReg(priv->client, &pedo_data);
+	*value = (u64)pedo_data;
+	*status = SENSOR_STATUS_ACCURACY_MEDIUM;
+	
+	return err;
+}
+static int lsm6ds3_step_c_get_data_step_d(u64 *value, int *status)
+{
+	return 0;
+}
+static int lsm6ds3_step_c_get_data_significant(u64 *value, int *status)
+{
+	return 0;
+}
+#endif
+#else
+static int LSM6DS3_acc_operate(void* self, uint32_t command, void* buff_in, int size_in,
+		void* buff_out, int size_out, int* actualout)
+{
+	int err = 0;
+	int value, sample_delay;	
+	struct lsm6ds3_i2c_data *priv = (struct lsm6ds3_i2c_data*)self;
+	hwm_sensor_data* gsensor_data;
+	char buff[LSM6DS3_BUFSIZE];
+	
+	//GSE_FUN(f);
+	switch (command)
+	{
+		case SENSOR_DELAY:
+			if((buff_in == NULL) || (size_in < sizeof(int)))
+			{
+				GSE_ERR("Set delay parameter error!\n");
+				err = -EINVAL;
+			}
+			else
+			{
+				value = *(int *)buff_in;
+				if(value <= 5)
+				{
+					sample_delay = LSM6DS3_ACC_ODR_208HZ;
+				}
+				else if(value <= 10)
+				{
+					sample_delay = LSM6DS3_ACC_ODR_104HZ;
+				}
+				else
+				{
+					sample_delay = LSM6DS3_ACC_ODR_52HZ;
+				}
+				
+				priv->sample_rate = sample_delay;
+				LSM6DS3_acc_SetSampleRate(priv->client, sample_delay);
+				if(err != LSM6DS3_SUCCESS ) 
+				{
+					GSE_ERR("Set delay parameter error!\n");
+				}
+
+				if(value >= 50)
+				{
+					atomic_set(&priv->filter, 0);
+				}
+				else
+				{					
+					priv->fir.num = 0;
+					priv->fir.idx = 0;
+					priv->fir.sum[LSM6DS3_AXIS_X] = 0;
+					priv->fir.sum[LSM6DS3_AXIS_Y] = 0;
+					priv->fir.sum[LSM6DS3_AXIS_Z] = 0;
+					atomic_set(&priv->filter, 1);
+				}
+			}
+			break;
+
+		case SENSOR_ENABLE:
+			if((buff_in == NULL) || (size_in < sizeof(int)))
+			{
+				GSE_ERR("Enable sensor parameter error!\n");
+				err = -EINVAL;
+			}
+			else
+			{
+			    
+				value = *(int *)buff_in;
+				if(value == 1)
+				{
+					enable_status = true;
+				}
+				else
+				{
+					enable_status = false;
+					priv->sample_rate = LSM6DS3_ACC_ODR_104HZ; //default rate
+				}
+				GSE_LOG("enable value=%d, sensor_power =%d\n",value,sensor_power);
+				
+				if(((value == 0) && (sensor_power == false)) ||((value == 1) && (sensor_power == true)))
+				{
+					GSE_LOG("Gsensor device have updated!\n");
+				}
+				else if(!pedo_enable_status && !tilt_enable_status)
+				{
+					err = LSM6DS3_acc_SetPowerMode( priv->client, enable_status);					
+				}
+
+			}
+			break;
+
+		case SENSOR_GET_DATA:
+			if((buff_out == NULL) || (size_out< sizeof(hwm_sensor_data)))
+			{
+				GSE_ERR("get sensor data parameter error!\n");
+				err = -EINVAL;
+			}
+			else
+			{
+				gsensor_data = (hwm_sensor_data *)buff_out;
+				LSM6DS3_ReadAccData(priv->client, buff, LSM6DS3_BUFSIZE);
+				
+				sscanf(buff, "%x %x %x", &gsensor_data->values[0], 
+					&gsensor_data->values[1], &gsensor_data->values[2]);				
+				gsensor_data->status = SENSOR_STATUS_ACCURACY_MEDIUM;				
+				gsensor_data->value_divide = 1000;
+			}
+			break;
+		default:
+			GSE_ERR("gsensor operate function no this parameter %d!\n", command);
+			err = -1;
+			break;
+	}
+	
+	return err;
+}
+#endif
+
+/****************************************************************************** 
+ * Function Configuration
+******************************************************************************/
+static int lsm6ds3_open(struct inode *inode, struct file *file)
+{
+	file->private_data = lsm6ds3_i2c_client;
+
+	if(file->private_data == NULL)
+	{
+		GSE_ERR("null pointer!!\n");
+		return -EINVAL;
+	}
+	return nonseekable_open(inode, file);
+}
+/*----------------------------------------------------------------------------*/
+static int lsm6ds3_release(struct inode *inode, struct file *file)
+{
+	file->private_data = NULL;
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static long lsm6ds3_acc_unlocked_ioctl(struct file *file, unsigned int cmd,
+       unsigned long arg)
+{
+	struct i2c_client *client = (struct i2c_client*)file->private_data;
+	struct lsm6ds3_i2c_data *obj = (struct lsm6ds3_i2c_data*)i2c_get_clientdata(client);	
+	char strbuf[LSM6DS3_BUFSIZE];
+	void __user *data;
+	SENSOR_DATA sensor_data;
+	int err = 0;
+	int cali[3];
+
+	//GSE_FUN(f);
+	if(_IOC_DIR(cmd) & _IOC_READ)
+	{
+		err = !access_ok(VERIFY_WRITE, (void __user *)arg, _IOC_SIZE(cmd));
+	}
+	else if(_IOC_DIR(cmd) & _IOC_WRITE)
+	{
+		err = !access_ok(VERIFY_READ, (void __user *)arg, _IOC_SIZE(cmd));
+	}
+
+	if(err)
+	{
+		GSE_ERR("access error: %08X, (%2d, %2d)\n", cmd, _IOC_DIR(cmd), _IOC_SIZE(cmd));
+		return -EFAULT;
+	}
+
+	switch(cmd)
+	{
+		case GSENSOR_IOCTL_INIT:			
+			break;
+
+		case GSENSOR_IOCTL_READ_CHIPINFO:
+			data = (void __user *) arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}
+						
+			LSM6DS3_ReadChipInfo(client, strbuf, LSM6DS3_BUFSIZE);
+			
+			if(copy_to_user(data, strbuf, strlen(strbuf)+1))
+			{
+				err = -EFAULT;
+				break;
+			}				 
+			break;	  
+
+		case GSENSOR_IOCTL_READ_SENSORDATA:
+			data = (void __user *) arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}
+			
+			LSM6DS3_ReadAccData(client, strbuf, LSM6DS3_BUFSIZE);
+			
+			if(copy_to_user(data, strbuf, strlen(strbuf)+1))
+			{
+				err = -EFAULT;
+				break;	  
+			}				 
+			break;
+
+		case GSENSOR_IOCTL_READ_GAIN:
+			data = (void __user *) arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}			
+
+			break;
+
+		case GSENSOR_IOCTL_READ_OFFSET:
+			data = (void __user *) arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}
+
+			break;
+
+		case GSENSOR_IOCTL_READ_RAW_DATA:
+			data = (void __user *) arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}
+			
+			LSM6DS3_ReadAccRawData(client, (s16 *)strbuf);
+			if(copy_to_user(data, strbuf, strlen(strbuf)+1))
+			{
+				err = -EFAULT;
+				break;	  
+			}
+			break;	  
+
+		case GSENSOR_IOCTL_SET_CALI:
+			data = (void __user*)arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}
+			if(copy_from_user(&sensor_data, data, sizeof(sensor_data)))
+			{
+				err = -EFAULT;
+				break;	  
+			}
+			if(atomic_read(&obj->suspend))
+			{
+				GSE_ERR("Perform calibration in suspend state!!\n");
+				err = -EINVAL;
+			}
+			else
+			{
+		#if 0
+			cali[LSM6DS3_AXIS_X] = (s64)(sensor_data.x) * 1000*1000/(obj->sensitivity*GRAVITY_EARTH_1000); //NTC
+			cali[LSM6DS3_AXIS_Y] = (s64)(sensor_data.y) * 1000*1000/(obj->sensitivity*GRAVITY_EARTH_1000);
+			cali[LSM6DS3_AXIS_Z] = (s64)(sensor_data.z) * 1000*1000/(obj->sensitivity*GRAVITY_EARTH_1000);
+		#else
+			cali[LSM6DS3_AXIS_X] = (s64)(sensor_data.x);
+			cali[LSM6DS3_AXIS_Y] = (s64)(sensor_data.y);	
+			cali[LSM6DS3_AXIS_Z] = (s64)(sensor_data.z);	
+		#endif
+				err = LSM6DS3_acc_WriteCalibration(client, cali);			 
+			}
+			break;
+
+		case GSENSOR_IOCTL_CLR_CALI:
+			err = LSM6DS3_acc_ResetCalibration(client);
+			break;
+
+		case GSENSOR_IOCTL_GET_CALI:
+			data = (void __user*)arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}
+			err = LSM6DS3_acc_ReadCalibration(client, cali);
+			if(err < 0)
+			{
+				break;
+			}
+					
+		#if 0
+			sensor_data.x = (s64)(cali[LSM6DS3_AXIS_X]) * obj->sensitivity*GRAVITY_EARTH_1000/(1000*1000); //NTC
+			sensor_data.y = (s64)(cali[LSM6DS3_AXIS_Y]) * obj->sensitivity*GRAVITY_EARTH_1000/(1000*1000);
+			sensor_data.z = (s64)(cali[LSM6DS3_AXIS_Z]) * obj->sensitivity*GRAVITY_EARTH_1000/(1000*1000);
+		#else
+			sensor_data.x = (s64)(cali[LSM6DS3_AXIS_X]);
+			sensor_data.y = (s64)(cali[LSM6DS3_AXIS_Y]);
+			sensor_data.z = (s64)(cali[LSM6DS3_AXIS_Z]);
+		#endif
+			if(copy_to_user(data, &sensor_data, sizeof(sensor_data)))
+			{
+				err = -EFAULT;
+				break;
+			}		
+			break;
+		
+		default:
+			GSE_ERR("unknown IOCTL: 0x%08x\n", cmd);
+			err = -ENOIOCTLCMD;
+			break;
+			
+	}
+
+	return err;
+}
+#ifdef CONFIG_COMPAT
+static long lsm6ds3_acc_compat_ioctl(struct file *file, unsigned int cmd,
+       unsigned long arg)
+{
+    long err = 0;
+
+	void __user *arg32 = compat_ptr(arg);
+	
+	if (!file->f_op || !file->f_op->unlocked_ioctl)
+		return -ENOTTY;
+	
+    switch (cmd)
+    {
+        case COMPAT_GSENSOR_IOCTL_READ_SENSORDATA:
+            if (arg32 == NULL)
+            {
+                err = -EINVAL;
+                break;    
+            }
+		
+		    err = file->f_op->unlocked_ioctl(file, GSENSOR_IOCTL_READ_SENSORDATA, (unsigned long)arg32);
+		    if (err){
+		        GSE_ERR("GSENSOR_IOCTL_READ_SENSORDATA unlocked_ioctl failed.");
+		        return err;
+		    }
+			break;
+			
+        case COMPAT_GSENSOR_IOCTL_SET_CALI:
+            if (arg32 == NULL)
+            {
+                err = -EINVAL;
+                break;    
+            }
+		
+		    err = file->f_op->unlocked_ioctl(file, GSENSOR_IOCTL_SET_CALI, (unsigned long)arg32);
+		    if (err){
+		        GSE_ERR("GSENSOR_IOCTL_SET_CALI unlocked_ioctl failed.");
+		        return err;
+		    }
+			break;
+			
+        case COMPAT_GSENSOR_IOCTL_GET_CALI:
+            if (arg32 == NULL)
+            {
+                err = -EINVAL;
+                break;    
+            }
+		
+		    err = file->f_op->unlocked_ioctl(file, GSENSOR_IOCTL_GET_CALI, (unsigned long)arg32);
+		    if (err){
+		        GSE_ERR("GSENSOR_IOCTL_GET_CALI unlocked_ioctl failed.");
+		        return err;
+		    }
+			break;
+			
+        case COMPAT_GSENSOR_IOCTL_CLR_CALI:
+            if (arg32 == NULL)
+            {
+                err = -EINVAL;
+                break;    
+            }
+		
+		    err = file->f_op->unlocked_ioctl(file, GSENSOR_IOCTL_CLR_CALI, (unsigned long)arg32);
+		    if (err){
+		        GSE_ERR("GSENSOR_IOCTL_CLR_CALI unlocked_ioctl failed.");
+		        return err;
+		    }
+			break;
+
+        default:
+            GSE_ERR("unknown IOCTL: 0x%08x\n", cmd);
+            err = -ENOIOCTLCMD;
+        break;
+
+    }
+
+    return err;
+}
+#endif
+
+/*----------------------------------------------------------------------------*/
+static struct file_operations lsm6ds3_acc_fops = {
+	.owner = THIS_MODULE,
+	.open = lsm6ds3_open,
+	.release = lsm6ds3_release,
+	.unlocked_ioctl = lsm6ds3_acc_unlocked_ioctl,
+#ifdef CONFIG_COMPAT
+	.compat_ioctl = lsm6ds3_acc_compat_ioctl,
+#endif
+};
+/*----------------------------------------------------------------------------*/
+static struct miscdevice lsm6ds3_acc_device = {
+	.minor = MISC_DYNAMIC_MINOR,
+	.name = "gsensor",
+	.fops = &lsm6ds3_acc_fops,
+};
+/*----------------------------------------------------------------------------*/
+#ifndef CONFIG_HAS_EARLYSUSPEND
+/*----------------------------------------------------------------------------*/
+static int lsm6ds3_acc_suspend(struct i2c_client *client, pm_message_t msg) 
+{
+	struct lsm6ds3_i2c_data *obj = i2c_get_clientdata(client);    
+	GSE_FUN();    
+	int err = 0;
+	
+	if(msg.event == PM_EVENT_SUSPEND)
+	{   
+		if(obj == NULL)
+		{
+			GSE_ERR("null pointer!!\n");
+			return -EINVAL;
+		}
+		atomic_set(&obj->suspend, 1);
+
+		if(pedo_enable_status  || tilt_enable_status)
+		{
+			return 0;
+		}
+		err = LSM6DS3_acc_SetPowerMode(obj->client, false);
+		if(err)
+		{
+			GSE_ERR("write power control fail!!\n");
+			return err;
+		}
+		
+		sensor_power = false;
+		
+		LSM6DS3_power(obj->hw, 0);
+
+	}
+	return err;
+}
+/*----------------------------------------------------------------------------*/
+static int lsm6ds3_acc_resume(struct i2c_client *client)
+{
+	struct lsm6ds3_i2c_data *obj = i2c_get_clientdata(client);        
+	int err;
+	GSE_FUN();
+
+	if(obj == NULL)
+	{
+		GSE_ERR("null pointer!!\n");
+		return -1;
+	}
+
+	if(pedo_enable_status  || tilt_enable_status)
+	{
+		atomic_set(&obj->suspend, 0);
+		return 0;
+	}
+	LSM6DS3_power(obj->hw, 1);
+	err = LSM6DS3_acc_SetPowerMode(obj->client, enable_status);
+	if(err)
+	{
+		GSE_ERR("initialize client fail! err code %d!\n", err);
+		return err ;        
+	}
+	atomic_set(&obj->suspend, 0);  
+
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+#else /*CONFIG_HAS_EARLY_SUSPEND is defined*/
+/*----------------------------------------------------------------------------*/
+static void lsm6ds3_early_suspend(struct early_suspend *h) 
+{
+	struct lsm6ds3_i2c_data *obj = container_of(h, struct lsm6ds3_i2c_data, early_drv);   
+	int err;
+	GSE_FUN();    
+
+	if(obj == NULL)
+	{
+		GSE_ERR("null pointer!!\n");
+		return;
+	}
+	atomic_set(&obj->suspend, 1);
+
+	if(pedo_enable_status  || tilt_enable_status)
+	{
+		return;
+	}
+	err = LSM6DS3_acc_SetPowerMode(obj->client, false);
+	if(err)
+	{
+		GSE_ERR("write power control fail!!\n");
+		return;
+	}
+
+	sensor_power = false;
+	
+	LSM6DS3_power(obj->hw, 0);
+}
+/*----------------------------------------------------------------------------*/
+static void lsm6ds3_late_resume(struct early_suspend *h)
+{
+	struct lsm6ds3_i2c_data *obj = container_of(h, struct lsm6ds3_i2c_data, early_drv);         
+	int err;
+	GSE_FUN();
+
+	if(obj == NULL)
+	{
+		GSE_ERR("null pointer!!\n");
+		return;
+	}
+	
+	if(pedo_enable_status  || tilt_enable_status)
+	{
+		atomic_set(&obj->suspend, 0);
+		return;
+	}
+
+	LSM6DS3_power(obj->hw, 1);
+	
+	err = LSM6DS3_acc_SetPowerMode(obj->client, enable_status);
+
+	if(err)
+	{
+		GSE_ERR("initialize client fail! err code %d!\n", err);
+		return;        
+	}
+	atomic_set(&obj->suspend, 0);    
+}
+#endif /*CONFIG_HAS_EARLYSUSPEND*/
+
+#ifdef LSM6DS3_TILT_FUNC
+static void lsm6ds3_eint_work(struct work_struct *work)
+{
+	u8 databuf[2] = {0}; 
+	struct lsm6ds3_i2c_data *obj = obj_i2c_data;
+
+	if(obj == NULL)
+	{
+		GSE_ERR("obj_i2c_data is null pointer!!\n");
+		goto lsm6ds3_eint_work_exit;
+	}	
+	
+	if(hwmsen_read_byte(obj->client, LSM6DS3_FUNC_SRC, databuf))
+	{
+		GSE_ERR("%s read LSM6DS3_CTRL10_C register err!\n", __func__);
+		goto lsm6ds3_eint_work_exit;
+	}
+
+	if(atomic_read(&obj->trace) & ACCEL_TRC_DATA)
+	{
+		GSE_LOG("%s read acc data format register: 0x%x\n", __func__, databuf[0]);		
+	}
+
+	if(LSM6DS3_SIGNICANT_MOTION_INT_STATUS & databuf[0])
+	{
+#ifdef LSM6DS3_STEP_COUNTER
+#ifdef LSM6DS3_SIGNIFICANT_MOTION
+		//add the action when receive the significant motion
+		step_notify(TYPE_SIGNIFICANT);
+#endif
+	}
+	else if(LSM6DS3_STEP_DETECT_INT_STATUS & databuf[0])
+	{
+		//add the action when receive step detection interrupt
+		step_notify(TYPE_STEP_DETECTOR);
+#endif
+	}
+	
+#ifdef LSM6DS3_TILT_FUNC
+	else if(LSM6DS3_TILT_INT_STATUS & databuf[0])
+	{
+		//add the action when receive the tilt interrupt
+		tilt_notify();
+	}
+#endif
+
+lsm6ds3_eint_work_exit:
+	mt_eint_unmask(CUST_EINT_LSM6DS3_NUM);
+}
+#endif
+/*----------------------------------------------------------------------------*/
+
+/*----------------------------------------------------------------------------*/
+static int lsm6ds3_i2c_probe(struct i2c_client *client, const struct i2c_device_id *id)
+{
+	struct i2c_client *new_client;
+	struct lsm6ds3_i2c_data *obj;
+
+#ifdef LSM6DS3_NEW_ARCH
+
+#else
+	struct hwmsen_object gyro_sobj;
+	struct hwmsen_object acc_sobj;
+#endif
+	int err = 0;
+
+	GSE_FUN();
+    
+	if(!(obj = kzalloc(sizeof(*obj), GFP_KERNEL)))
+	{
+		err = -ENOMEM;
+		goto exit;
+	}
+	
+	memset(obj, 0, sizeof(struct lsm6ds3_i2c_data));
+	
+#ifdef LSM6DS3_TILT_FUNC
+	INIT_WORK(&obj->eint_work, lsm6ds3_eint_work);
+#endif
+
+	obj->hw = get_cust_acc_hw(); 
+	obj->sample_rate = LSM6DS3_ACC_ODR_104HZ;
+	
+	atomic_set(&obj->layout, obj->hw->direction);
+	err = hwmsen_get_convert(obj->hw->direction, &obj->cvt);
+	if(err)
+	{
+		GSE_ERR("invalid direction: %d\n", obj->hw->direction);
+		goto exit_kfree;
+	}
+
+	obj_i2c_data = obj;
+	obj->client = client;
+	new_client = obj->client;
+	i2c_set_clientdata(new_client,obj);
+	
+	atomic_set(&obj->trace, 0);
+	atomic_set(&obj->suspend, 0);
+	
+	lsm6ds3_i2c_client = new_client;	
+	err = LSM6DS3_init_client(new_client, false);
+	if(err)
+	{
+		goto exit_init_failed;
+	}
+	
+	err = misc_register(&lsm6ds3_acc_device);
+	if(err)
+	{
+		GSE_ERR("lsm6ds3_gyro_device misc register failed!\n");
+		goto exit_misc_device_register_failed;
+	}
+#ifdef LSM6DS3_NEW_ARCH
+
+#else
+	err = lsm6ds3_create_attr(&lsm6ds3_driver.driver);
+#endif
+	if(err)
+	{
+		GSE_ERR("lsm6ds3 create attribute err = %d\n", err);
+		goto exit_create_attr_failed;
+	}	
+
+#ifdef LSM6DS3_NEW_ARCH
+
+#else
+	acc_sobj.self = obj;
+    acc_sobj.polling = 1;
+    acc_sobj.sensor_operate = LSM6DS3_acc_operate;
+	err = hwmsen_attach(ID_ACCELEROMETER, &acc_sobj);
+	if(err)
+	{
+		GSE_ERR("hwmsen_attach Accelerometer fail = %d\n", err);
+		goto exit_kfree;
+	}
+#endif	
+
+#ifdef CONFIG_HAS_EARLYSUSPEND
+	obj->early_drv.level    = EARLY_SUSPEND_LEVEL_DISABLE_FB - 1,
+	obj->early_drv.suspend  = lsm6ds3_early_suspend,
+	obj->early_drv.resume   = lsm6ds3_late_resume,    
+	register_early_suspend(&obj->early_drv);
+#endif 
+#ifdef LSM6DS3_NEW_ARCH
+	lsm6ds3_acc_init_flag = 0;
+#endif
+	GSE_LOG("%s: OK\n", __func__);    
+	return 0;
+
+exit_create_attr_failed:
+	misc_deregister(&lsm6ds3_acc_device);
+exit_misc_device_register_failed:
+exit_init_failed:
+exit_kfree:
+	kfree(obj);
+exit:
+#ifdef LSM6DS3_NEW_ARCH
+	lsm6ds3_acc_init_flag = -1;
+#endif
+	GSE_ERR("%s: err = %d\n", __func__, err);        
+	return err;
+}
+
+/*----------------------------------------------------------------------------*/
+static int lsm6ds3_i2c_remove(struct i2c_client *client)
+{
+	int err = 0;	
+
+#ifdef LSM6DS3_NEW_ARCH		
+	if(test_bit(LSM6DS3_ACC, &lsm6ds3_init_flag_test))
+	{
+		err = lsm6ds3_delete_attr(&(lsm6ds3_init_info.platform_diver_addr->driver));
+	}
+	lsm6ds3_acc_init_flag = -1;
+#else
+	err = lsm6ds3_delete_attr(&lsm6ds3_driver.driver);
+#endif
+	if(err)
+	{
+		GSE_ERR("lsm6ds3_i2c_remove fail: %d\n", err);
+	}
+
+	err = misc_deregister(&lsm6ds3_acc_device);
+	if(err)
+	{
+		GSE_ERR("misc_deregister lsm6ds3_gyro_device fail: %d\n", err);
+	}
+
+	lsm6ds3_i2c_client = NULL;
+	i2c_unregister_device(client);
+	kfree(i2c_get_clientdata(client));
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+#ifdef LSM6DS3_NEW_ARCH
+static int lsm6ds3_local_init_common(void)
+{
+	struct acc_hw *accel_hw = get_cust_acc_hw();
+	//GSE_FUN();
+	
+	LSM6DS3_power(accel_hw, 1);
+	
+	if(i2c_add_driver(&lsm6ds3_i2c_driver))
+	{
+		GSE_ERR("add driver error\n");
+		return -1;
+	}
+
+	return 0;
+}
+static int lsm6ds3_local_init(void)
+{
+	int res = 0;
+	struct acc_control_path ctl={0};
+    struct acc_data_path data={0};
+	struct lsm6ds3_i2c_data *obj = NULL; 
+
+	mutex_lock(&lsm6ds3_init_mutex);
+	
+	set_bit(LSM6DS3_ACC, &lsm6ds3_init_flag_test);
+
+	if((0==test_bit(LSM6DS3_STEP_C, &lsm6ds3_init_flag_test)) \
+		&& (0 == test_bit(LSM6DS3_TILT, &lsm6ds3_init_flag_test)))
+	{
+		res = lsm6ds3_local_init_common();
+		if(res < 0)
+		{
+			goto lsm6ds3_local_init_failed;
+		}
+		
+	}
+
+
+	if(lsm6ds3_acc_init_flag == -1)
+	{
+		mutex_unlock(&lsm6ds3_init_mutex);
+		GSE_ERR("%s init failed!\n", __FUNCTION__);
+		return -1;
+	}
+	else
+	{
+		obj = obj_i2c_data;
+		if(NULL == obj)
+		{
+			GSE_ERR("i2c_data obj is null!!\n");
+			goto lsm6ds3_local_init_failed;
+		}
+		
+		res = lsm6ds3_create_attr(&(lsm6ds3_init_info.platform_diver_addr->driver));
+		if(res < 0)
+		{
+			goto lsm6ds3_local_init_failed;
+		}
+		ctl.open_report_data= lsm6ds3_open_report_data;
+	    ctl.enable_nodata = lsm6ds3_enable_nodata;
+	    ctl.set_delay  = lsm6ds3_set_delay;
+	    ctl.is_report_input_direct = false;
+	    ctl.is_support_batch = obj->hw->is_batch_supported;
+
+	    res = acc_register_control_path(&ctl);
+	    if(res)
+	    {
+	         GSE_ERR("register acc control path err\n");
+			 goto lsm6ds3_local_init_failed;
+
+	    }
+
+	    data.get_data = lsm6ds3_get_data;
+	    data.vender_div = 1000;
+	    res = acc_register_data_path(&data);
+	    if(res)
+	    {
+	        GSE_ERR("register acc data path err= %d\n", res);
+			goto lsm6ds3_local_init_failed;
+
+	    }
+	}
+	mutex_unlock(&lsm6ds3_init_mutex);
+	return 0;
+lsm6ds3_local_init_failed:
+	GSE_ERR("%s init failed\n", __FUNCTION__);
+	mutex_unlock(&lsm6ds3_init_mutex);
+	return res;
+
+}
+static int lsm6ds3_local_uninit(void)
+{
+	struct acc_hw *accel_hw = get_cust_acc_hw();
+	clear_bit(LSM6DS3_ACC, &lsm6ds3_init_flag_test);
+
+    //GSE_FUN();    
+    LSM6DS3_power(accel_hw, 0);  	
+    i2c_del_driver(&lsm6ds3_i2c_driver);
+    return 0;
+}
+#ifdef LSM6DS3_TILT_FUNC
+static int lsm6ds3_tilt_get_data(u16 *value, int *status)
+{
+	return 0;
+}
+static void lsm6ds3_eint_func(void)
+{
+	struct lsm6ds3_i2c_data *priv = obj_i2c_data;
+	//GSE_FUN();
+	if(!priv)
+	{
+		return;
+	}	
+	schedule_work(&priv->eint_work);
+}
+
+static int lsm6ds3_setup_eint(void)
+{
+#ifdef CUST_EINT_LSM6DS3_TYPE
+		mt_set_gpio_dir(GPIO_LSM6DS3_EINT_PIN, GPIO_DIR_IN);
+		mt_set_gpio_mode(GPIO_LSM6DS3_EINT_PIN, GPIO_LSM6DS3_EINT_PIN_M_EINT);
+		mt_set_gpio_pull_enable(GPIO_LSM6DS3_EINT_PIN, true);
+		mt_set_gpio_pull_select(GPIO_LSM6DS3_EINT_PIN, GPIO_PULL_UP);
+	
+		mt_eint_set_hw_debounce(CUST_EINT_LSM6DS3_NUM, CUST_EINT_LSM6DS3_DEBOUNCE_CN);
+		mt_eint_registration(CUST_EINT_LSM6DS3_NUM, CUST_EINT_LSM6DS3_TYPE, lsm6ds3_eint_func, 0);
+
+		mt_eint_mask(CUST_EINT_LSM6DS3_NUM);
+#endif
+	return 0;
+}
+static int lsm6ds3_tilt_local_init(void)
+{
+	int res = 0;
+
+	struct tilt_control_path tilt_ctl={0};
+	struct tilt_data_path tilt_data={0};
+
+	mutex_lock(&lsm6ds3_init_mutex);
+	set_bit(LSM6DS3_TILT, &lsm6ds3_init_flag_test);
+	
+	if((0==test_bit(LSM6DS3_ACC, &lsm6ds3_init_flag_test)) \
+		&& (0==test_bit(LSM6DS3_STEP_C, &lsm6ds3_init_flag_test)))
+	{
+		res = lsm6ds3_local_init_common();
+		if(res < 0)
+		{
+			goto lsm6ds3_tilt_local_init_failed;
+		}
+	}
+
+	if(lsm6ds3_acc_init_flag == -1)
+	{
+		mutex_unlock(&lsm6ds3_init_mutex);
+		GSE_ERR("%s init failed!\n", __FUNCTION__);
+		return -1;
+	}
+	else
+	{
+		res = lsm6ds3_setup_eint();
+		tilt_ctl.open_report_data= lsm6ds3_tilt_open_report_data;	
+		res = tilt_register_control_path(&tilt_ctl);
+
+		tilt_data.get_data = lsm6ds3_tilt_get_data;
+		res = tilt_register_data_path(&tilt_data);
+	}
+	mutex_unlock(&lsm6ds3_init_mutex);
+	return 0;
+	
+lsm6ds3_tilt_local_init_failed:
+	mutex_unlock(&lsm6ds3_init_mutex);
+	GSE_ERR("%s init failed!\n", __FUNCTION__);
+	return -1;
+}
+static int lsm6ds3_tilt_local_uninit(void)
+{
+	clear_bit(LSM6DS3_TILT, &lsm6ds3_init_flag_test);
+    return 0;
+}
+#endif
+
+#ifdef LSM6DS3_STEP_COUNTER
+static int lsm6ds3_step_c_local_init(void)
+{
+	int res = 0;
+
+	struct step_c_control_path step_ctl={0};
+	struct step_c_data_path step_data={0};	
+	
+	mutex_lock(&lsm6ds3_init_mutex);
+		
+	set_bit(LSM6DS3_STEP_C, &lsm6ds3_init_flag_test);
+	
+	if((0==test_bit(LSM6DS3_ACC, &lsm6ds3_init_flag_test)) \
+		&& (0 == test_bit(LSM6DS3_TILT, &lsm6ds3_init_flag_test)))
+	{
+		res = lsm6ds3_local_init_common();
+		if(res < 0)
+		{
+			goto lsm6ds3_step_c_local_init_failed;
+		}
+			
+	}
+
+	if(lsm6ds3_acc_init_flag == -1)
+	{
+		mutex_unlock(&lsm6ds3_init_mutex);
+		GSE_ERR("%s init failed!\n", __FUNCTION__);
+		return -1;
+	}
+	else
+	{
+		step_ctl.open_report_data= lsm6ds3_step_c_open_report_data;
+		step_ctl.enable_nodata = lsm6ds3_step_c_enable_nodata;
+		step_ctl.enable_step_detect  = lsm6ds3_step_c_enable_step_detect;
+		step_ctl.set_delay = lsm6ds3_step_c_set_delay;
+		step_ctl.is_report_input_direct = false;
+		step_ctl.is_support_batch = false;		
+#ifdef LSM6DS3_SIGNIFICANT_MOTION
+		step_ctl.enable_significant = lsm6ds3_step_c_enable_significant;
+#endif
+
+		res = step_c_register_control_path(&step_ctl);
+		if(res)
+		{
+			 GSE_ERR("register step counter control path err\n");
+			goto lsm6ds3_step_c_local_init_failed;
+		}
+	
+		step_data.get_data = lsm6ds3_step_c_get_data;
+		step_data.get_data_step_d = lsm6ds3_step_c_get_data_step_d;
+		step_data.get_data_significant = lsm6ds3_step_c_get_data_significant;
+		
+		step_data.vender_div = 1;
+		res = step_c_register_data_path(&step_data);
+		if(res)
+		{
+			GSE_ERR("register step counter data path err= %d\n", res);
+			goto lsm6ds3_step_c_local_init_failed;
+		}
+	}
+	mutex_unlock(&lsm6ds3_init_mutex);
+	return 0;
+	
+lsm6ds3_step_c_local_init_failed:
+	mutex_unlock(&lsm6ds3_init_mutex);
+	GSE_ERR("%s init failed!\n", __FUNCTION__);
+	return res;
+
+}
+static int lsm6ds3_step_c_local_uninit(void)
+{
+	clear_bit(LSM6DS3_STEP_C, &lsm6ds3_init_flag_test);
+    return 0;
+}
+#endif
+#else
+static int lsm6ds3_probe(struct platform_device *pdev) 
+{
+	struct acc_hw *accel_hw = get_cust_acc_hw();
+	GSE_FUN();
+
+	LSM6DS3_power(accel_hw, 1);
+	
+	if(i2c_add_driver(&lsm6ds3_i2c_driver))
+	{
+		GSE_ERR("add driver error\n");
+		return -1;
+	}
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int lsm6ds3_remove(struct platform_device *pdev)
+{
+	struct acc_hw *accel_hw = get_cust_acc_hw();
+
+    //GSE_FUN();    
+    LSM6DS3_power(accel_hw, 0);  	
+    i2c_del_driver(&lsm6ds3_i2c_driver);
+    return 0;
+}
+
+/*----------------------------------------------------------------------------*/
+#ifdef CONFIG_OF
+static const struct of_device_id gsensor_of_match[] = {
+	{ .compatible = "mediatek,gsensor", },
+	{},
+};
+#endif
+
+static struct platform_driver lsm6ds3_driver = {
+	.probe      = lsm6ds3_probe,
+	.remove     = lsm6ds3_remove,    
+	.driver     = {
+			.name  = "gsensor",
+		//	.owner	= THIS_MODULE,
+	#ifdef CONFIG_OF
+			.of_match_table = gsensor_of_match,
+	#endif
+	}
+};
+
+#endif
+/*----------------------------------------------------------------------------*/
+static int __init lsm6ds3_init(void)
+{
+	//GSE_FUN();
+	struct acc_hw *hw = get_cust_acc_hw();
+    GSE_LOG("%s: i2c_number=%d\n", __func__,hw->i2c_num);
+    i2c_register_board_info(hw->i2c_num, &i2c_lsm6ds3, 1);
+	
+#ifdef LSM6DS3_NEW_ARCH
+	acc_driver_add(&lsm6ds3_init_info);	
+#ifdef LSM6DS3_STEP_COUNTER //step counter
+	step_c_driver_add(&lsm6ds3_step_c_init_info); //step counter
+#endif
+#ifdef LSM6DS3_TILT_FUNC
+	tilt_driver_add(&lsm6ds3_tilt_init_info);
+#endif
+
+#else
+	if(platform_driver_register(&lsm6ds3_driver))
+	{
+		GSE_ERR("failed to register driver");
+		return -ENODEV;
+	}
+#endif
+    
+	return 0;    
+}
+/*----------------------------------------------------------------------------*/
+static void __exit lsm6ds3_exit(void)
+{
+	GSE_FUN();
+#ifndef LSM6DS3_NEW_ARCH	
+	platform_driver_unregister(&lsm6ds3_driver);
+#endif
+
+}
+/*----------------------------------------------------------------------------*/
+module_init(lsm6ds3_init);
+module_exit(lsm6ds3_exit);
+/*----------------------------------------------------------------------------*/
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("LSM6DS3 Accelerometer");
+MODULE_AUTHOR("xj.wang@mediatek.com, darren.han@st.com");
+
+
+
+
+
+
+/*----------------------------------------------------------------- LSM6DS3 ------------------------------------------------------------------*/
diff --git a/drivers/misc/mediatek/accelerometer/lsm6ds3/lsm6ds3.h b/drivers/misc/mediatek/accelerometer/lsm6ds3/lsm6ds3.h
new file mode 100644
index 000000000..b30d2d114
--- /dev/null
+++ b/drivers/misc/mediatek/accelerometer/lsm6ds3/lsm6ds3.h
@@ -0,0 +1,350 @@
+/* lsm6ds3.h
+ *
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ */
+#ifndef L3M6DS3_H
+#define L3M6DS3_H
+	 
+#include <linux/ioctl.h>
+	 
+#define LSM6DS3_I2C_SLAVE_ADDR		0xD0
+#define LSM6DS3_FIXED_DEVID			0x69
+
+
+/* LSM6DS3 Register Map  (Please refer to LSM6DS3 Specifications) */
+#define LSM6DS3_FUNC_CFG_ACCESS  0x00
+#define LSM6DS3_RAM_ACCESS  	0X01
+#define LSM6DS3_SENSOR_SYNC_TIME_FRAME 0X04
+
+/*FIFO control register*/
+#define LSM6DS3_FIFO_CTRL1 0x06
+#define LSM6DS3_FIFO_CTRL2 0x07
+#define LSM6DS3_FIFO_CTRL3 0x08
+#define LSM6DS3_FIFO_CTRL4 0x09
+#define LSM6DS3_FIFO_CTRL5 0x0a
+
+/*Orientation configuration*/
+#define LSM6DS3_ORIENT_CFG_G 0x0B
+
+/*Interrupt control*/
+#define LSM6DS3_INT1_CTRL 0x0D
+#define LSM6DS3_INT2_CTRL 0x0E
+
+#define LSM6DS3_WHO_AM_I 0x0F
+
+/*Acc and Gyro control registers*/
+#define LSM6DS3_CTRL1_XL 0x10
+#define LSM6DS3_CTRL2_G 0x11
+#define LSM6DS3_CTRL3_C	0x12
+#define LSM6DS3_CTRL4_C	0x13
+#define LSM6DS3_CTRL5_C	0x14
+#define LSM6DS3_CTRL6_C	0x15
+#define LSM6DS3_CTRL7_G	0x16
+#define LSM6DS3_CTRL8_XL	0x17
+#define LSM6DS3_CTRL9_XL	0x18
+#define LSM6DS3_CTRL10_C	0x19
+
+#define LSM6DS3_MASTER_CONFIG 0x1A
+#define LSM6DS3_WAKE_UP_SRC 0x1B
+#define LSM6DS3_TAP_SRC 0x1C
+#define LSM6DS3_D6D_SRC	0x1D
+#define LSM6DS3_STATUS_REG 0x1E
+
+/*Output register*/
+#define LSM6DS3_OUT_TEMP_L 0x20
+#define LSM6DS3_OUT_TEMP_H 0x21
+#define LSM6DS3_OUTX_L_G   0x22
+#define LSM6DS3_OUTX_H_G   0x23
+#define LSM6DS3_OUTY_L_G   0x24
+#define LSM6DS3_OUTY_H_G   0x25
+#define LSM6DS3_OUTZ_L_G   0x26
+#define LSM6DS3_OUTZ_H_G   0x27
+
+#define LSM6DS3_OUTX_L_XL   0x28
+#define LSM6DS3_OUTX_H_XL   0x29
+#define LSM6DS3_OUTY_L_XL   0x2A
+#define LSM6DS3_OUTY_H_XL   0x2B
+#define LSM6DS3_OUTZ_L_XL   0x2C
+#define LSM6DS3_OUTZ_H_XL   0x2D
+
+/*Sensor Hub registers*/
+#define LSM6DS3_SENSORHUB1_REG 0x2E
+#define LSM6DS3_SENSORHUB2_REG 0x2F
+#define LSM6DS3_SENSORHUB3_REG 0x30
+#define LSM6DS3_SENSORHUB4_REG 0x31
+#define LSM6DS3_SENSORHUB5_REG 0x32
+#define LSM6DS3_SENSORHUB6_REG 0x33
+#define LSM6DS3_SENSORHUB7_REG 0x34
+#define LSM6DS3_SENSORHUB8_REG 0x35
+#define LSM6DS3_SENSORHUB9_REG 0x36
+#define LSM6DS3_SENSORHUB10_REG 0x37
+#define LSM6DS3_SENSORHUB11_REG 0x38
+#define LSM6DS3_SENSORHUB12_REG 0x39
+
+#define LSM6DS3_SENSORHUB13_REG 0x4D
+#define LSM6DS3_SENSORHUB14_REG 0x4E
+#define LSM6DS3_SENSORHUB15_REG 0x4F
+#define LSM6DS3_SENSORHUB16_REG 0x50
+#define LSM6DS3_SENSORHUB17_REG 0x51
+#define LSM6DS3_SENSORHUB18_REG 0x52
+
+/*FIFO status registers*/
+#define LSM6DS3_FIFO_STATUS1 0x3A
+#define LSM6DS3_FIFO_STATUS2 0x3B
+#define LSM6DS3_FIFO_STATUS3 0x3C
+#define LSM6DS3_FIFO_STATUS4 0x3D
+
+#define LSM6DS3_FIFO_DATA_OUT_L 0x3E
+#define LSM6DS3_FIFO_DATA_OUT_H 0x3F
+
+#define LSM6DS3_TIMESTAMP0_REG 0x40
+#define LSM6DS3_TIMESTAMP1_REG 0x41
+#define LSM6DS3_TIMESTAMP2_REG 0x42
+
+#define LSM6DS3_STEP_TIMESTAMP_L 0x49
+#define LSM6DS3_STEP_TIMESTAMP_H 0x4A
+
+#define LSM6DS3_STEP_COUNTER_L	0x4B
+#define LSM6DS3_STEP_COUNTER_H	0x4C
+
+#define LSM6DS3_FUNC_SRC 	0x53
+#define LSM6DS3_TAP_CFG	0x58
+#define LSM6DS3_TAP_THS_6D 0x59
+#define LSM6DS3_INT_DUR2	0x5A
+#define LSM6DS3_WAKE_UP_THS	0x5B
+#define LSM6DS3_WAKE_UP_DUR 0x5C
+
+#define LSM6DS3_FREE_FALL 0x5D
+#define LSM6DS3_MD1_CFG 0x5E
+#define LSM6DS3_MD2_CFG 0x5F
+
+/*Output Raw data*/
+#define LSM6DS3_OUT_MAG_RAW_X_L 0x66
+#define LSM6DS3_OUT_MAG_RAW_X_H 0x67
+#define LSM6DS3_OUT_MAG_RAW_Y_L 0x68
+#define LSM6DS3_OUT_MAG_RAW_Y_H 0x69
+#define LSM6DS3_OUT_MAG_RAW_Z_L 0x6A
+#define LSM6DS3_OUT_MAG_RAW_Z_H 0x6B
+
+/*Embedded function register*/
+#define LSM6DS3_SLV0_ADD 0x02
+#define LSM6DS3_SLV0_SUBADD 0x03
+#define LSM6DS3_SLAVE0_CONFIG 0x04
+
+#define LSM6DS3_SLV1_ADD 0x05
+#define LSM6DS3_SLV1_SUBADD 0x06
+#define LSM6DS3_SLAVE1_CONFIG 0x07
+
+#define LSM6DS3_SLV2_ADD 0x08
+#define LSM6DS3_SLV2_SUBADD 0x09
+#define LSM6DS3_SLAVE2_CONFIG 0x0a
+
+#define LSM6DS3_SLV3_ADD 0x0b
+#define LSM6DS3_SLV3_SUBADD 0x0c
+#define LSM6DS3_SLAVE3_CONFIG 0x0d
+
+#define LSM6DS3_DATAWRITE_SRC_MODE_SUB_SLV0 0x0e
+#define LSM6DS3_SM_THS 0x13
+#define LSM6DS3_STEP_COUNT_DELTA 0x15
+#define LSM6DS3_MAG_SI_XX 0x24
+#define LSM6DS3_MAG_SI_XY 0x25
+#define LSM6DS3_MAG_SI_XZ 0x26
+#define LSM6DS3_MAG_SI_YX 0x27
+#define LSM6DS3_MAG_SI_YY 0x28
+#define LSM6DS3_MAG_SI_YZ 0x29
+#define LSM6DS3_MAG_SI_ZX 0x2a
+#define LSM6DS3_MAG_SI_ZY 0x2b
+#define LSM6DS3_MAG_SI_ZZ 0x2c
+
+#define LSM6DS3_MAG_OFFX_L 0x2D
+#define LSM6DS3_MAG_OFFX_H 0x2E
+#define LSM6DS3_MAG_OFFY_L 0x2F
+#define LSM6DS3_MAG_OFFY_H 0x30
+#define LSM6DS3_MAG_OFFZ_L 0x31
+#define LSM6DS3_MAG_OFFZ_H 0x32
+
+
+
+
+/*LSM6DS3 Register Bit definitions*/
+#define LSM6DS3_ACC_RANGE_MASK 0x0C
+#define LSM6DS3_ACC_RANGE_2g		    0x00
+#define LSM6DS3_ACC_RANGE_4g		    0x08
+#define LSM6DS3_ACC_RANGE_8g		    0x0C
+#define LSM6DS3_ACC_RANGE_16g		    0x04
+
+#define LSM6DS3_ACC_ODR_MASK	0xF0
+#define LSM6DS3_ACC_ODR_POWER_DOWN		0x00
+#define LSM6DS3_ACC_ODR_13HZ		    0x10
+#define LSM6DS3_ACC_ODR_26HZ		    0x20
+#define LSM6DS3_ACC_ODR_52HZ		    0x30
+#define LSM6DS3_ACC_ODR_104HZ		    0x40
+#define LSM6DS3_ACC_ODR_208HZ		    0x50
+#define LSM6DS3_ACC_ODR_416HZ		    0x60
+#define LSM6DS3_ACC_ODR_833HZ		    0x70
+#define LSM6DS3_ACC_ODR_1660HZ		    0x80
+#define LSM6DS3_ACC_ODR_3330HZ		    0x90
+#define LSM6DS3_ACC_ODR_6660HZ		    0xA0
+
+#define LSM6DS3_GYRO_RANGE_MASK	0x0E
+#define LSM6DS3_GYRO_RANGE_125DPS	0x02
+#define LSM6DS3_GYRO_RANGE_245DPS	0x00
+#define LSM6DS3_GYRO_RANGE_500DPS	0x04
+#define LSM6DS3_GYRO_RANGE_1000DPS	0x08
+#define LSM6DS3_GYRO_RANGE_2000DPS	0x0c
+
+#define LSM6DS3_GYRO_ODR_MASK	0xf0
+#define LSM6DS3_GYRO_ODR_POWER_DOWN	0x00
+#define LSM6DS3_GYRO_ODR_13HZ		0x10
+#define LSM6DS3_GYRO_ODR_26HZ		0x20
+#define LSM6DS3_GYRO_ODR_52HZ		0x30
+#define LSM6DS3_GYRO_ODR_104HZ		0x40
+#define LSM6DS3_GYRO_ODR_208HZ		0x50
+#define LSM6DS3_GYRO_ODR_416HZ		0x60
+#define LSM6DS3_GYRO_ODR_833HZ		0x70
+#define LSM6DS3_GYRO_ODR_1660HZ		0x80
+
+#define AUTO_INCREMENT 0x80
+#define LSM6DS3_CTRL3_C_IFINC 0x04
+
+
+#define LSM6DS3_ACC_SENSITIVITY_2G		61	/*ug/LSB*/
+#define LSM6DS3_ACC_SENSITIVITY_4G		122	/*ug/LSB*/
+#define LSM6DS3_ACC_SENSITIVITY_8G		244	/*ug/LSB*/
+#define LSM6DS3_ACC_SENSITIVITY_16G		488	/*ug/LSB*/
+
+
+#define LSM6DS3_ACC_ENABLE_AXIS_MASK 0X38
+#define LSM6DS3_ACC_ENABLE_AXIS_X 0x08
+#define LSM6DS3_ACC_ENABLE_AXIS_Y 0x10
+#define LSM6DS3_ACC_ENABLE_AXIS_Z 0x20
+
+
+
+#define LSM6DS3_GYRO_SENSITIVITY_125DPS		4375	/*udps/LSB*/
+#define LSM6DS3_GYRO_SENSITIVITY_245DPS		8750	/*udps/LSB*/
+#define LSM6DS3_GYRO_SENSITIVITY_500DPS		17500	/*udps/LSB*/
+#define LSM6DS3_GYRO_SENSITIVITY_1000DPS	35000	/*udps/LSB*/
+#define LSM6DS3_GYRO_SENSITIVITY_2000DPS	70000	/*udps/LSB*/
+
+#define LSM6DS3_GYRO_ENABLE_AXIS_MASK 0x38
+#define LSM6DS3_GYRO_ENABLE_AXIS_X 0x08
+#define LSM6DS3_GYRO_ENABLE_AXIS_Y 0x10
+#define LSM6DS3_GYRO_ENABLE_AXIS_Z 0x20
+
+#define LSM6DS3_ACC_GYRO_FUNC_EN_MASK  	0x04
+#define LSM6DS3_PEDO_EN_MASK 0x40
+typedef enum {
+  	LSM6DS3_ACC_GYRO_PEDO_EN_DISABLED 		 =0x00,
+  	LSM6DS3_ACC_GYRO_PEDO_EN_ENABLED 		 =0x40,
+} LSM6DS3_ACC_GYRO_PEDO_EN_t;
+
+#define LSM6DS3_TILT_EN_MASK 0x20
+
+typedef enum {
+  	LSM6DS3_ACC_GYRO_TILT_EN_DISABLED 		 =0x00,
+  	LSM6DS3_ACC_GYRO_TILT_EN_ENABLED 		 =0x20,
+} LSM6DS3_ACC_GYRO_TILT_EN_t;
+
+typedef enum {
+  	LSM6DS3_ACC_GYRO_INT1 		 =0,
+  	LSM6DS3_ACC_GYRO_INT2 		 =1,
+} LSM6DS3_ACC_GYRO_ROUNT_INT_t;
+typedef enum {
+  	LSM6DS3_ACC_GYRO_FUNC_EN_DISABLED 		 =0x00,
+  	LSM6DS3_ACC_GYRO_FUNC_EN_ENABLED 		 =0x04,
+} LSM6DS3_ACC_GYRO_FUNC_EN_t;
+
+typedef enum {
+  	LSM6DS3_ACC_GYRO_INT_TILT_DISABLED 		 =0x00,
+  	LSM6DS3_ACC_GYRO_INT_TILT_ENABLED 		 =0x02,
+} LSM6DS3_ACC_GYRO_INT2_TILT_t;
+
+#define  	LSM6DS3_ACC_GYRO_INT_TILT_MASK  	0x02
+typedef enum {
+  	LSM6DS3_ACC_GYRO_TILT_DISABLED 		 =0x00,
+  	LSM6DS3_ACC_GYRO_TILT_ENABLED 		 =0x02,
+} LSM6DS3_ACC_GYRO_TILT_t;
+
+#define LSM6DS3_ACC_GYRO_TILT_MASK  	0x02
+
+typedef enum {
+  	LSM6DS3_ACC_GYRO_INT_SIGN_MOT_DISABLED 		 =0x00,
+  	LSM6DS3_ACC_GYRO_INT_SIGN_MOT_ENABLED 		 =0x40,
+} LSM6DS3_ACC_GYRO_INT_SIGN_MOT_t;
+
+#define  	LSM6DS3_ACC_GYRO_INT_SIGN_MOT_MASK  	0x40
+
+typedef enum {
+  	LSM6DS3_ACC_GYRO_SIGN_MOT_DISABLED 		 =0x00,
+  	LSM6DS3_ACC_GYRO_SIGN_MOT_ENABLED 		 =0x01,
+} LSM6DS3_ACC_GYRO_SIGN_MOT_t;
+
+#define  	LSM6DS3_ACC_GYRO_SIGN_MOT_MASK  	0x01
+
+typedef enum {
+  	LSM6DS3_ACC_GYRO_RAM_PAGE_DISABLED 		 =0x00,
+  	LSM6DS3_ACC_GYRO_RAM_PAGE_ENABLED 		 =0x80,
+} LSM6DS3_ACC_GYRO_RAM_PAGE_t;
+
+#define LSM6DS3_RAM_PAGE_MASK  	0x80
+#define LSM6DS3_CONFIG_PEDO_THS_MIN          0x0F
+
+typedef enum {
+  	LSM6DS3_ACC_GYRO_PEDO_RST_STEP_DISABLED 	 =0x00,
+  	LSM6DS3_ACC_GYRO_PEDO_RST_STEP_ENABLED 		 =0x02,
+} LSM6DS3_ACC_GYRO_PEDO_RST_STEP_t;
+
+#define LSM6DS3_PEDO_RST_STEP_MASK  0x02
+
+typedef enum {
+  	LSM6DS3_ACC_GYRO_INT_ACTIVE_HIGH 	 =0x00,
+  	LSM6DS3_ACC_GYRO_INT_ACTIVE_LOW		 =0x20,
+} LSM6DS3_ACC_GYRO_INT_ACTIVE_t;
+
+#define LSM6DS3_ACC_GYRO_INT_ACTIVE_MASK 0x20
+
+typedef enum {
+  	LSM6DS3_ACC_GYRO_INT_LATCH 	  =0x01,
+  	LSM6DS3_ACC_GYRO_INT_NO_LATCH	 =0x00,
+} LSM6DS3_ACC_GYRO_INT_LATCH_CTL_t;
+
+#define LSM6DS3_ACC_GYRO_INT_LATCH_CTL_MASK 0x01
+
+
+
+#define LSM6DS3_SIGNICANT_MOTION_INT_STATUS 0x40
+#define LSM6DS3_TILT_INT_STATUS 0x20
+#define LSM6DS3_STEP_DETECT_INT_STATUS 0x10
+
+
+
+#define LSM6DS3_SUCCESS		       0
+#define LSM6DS3_ERR_I2C		      -1
+#define LSM6DS3_ERR_STATUS			  -3
+#define LSM6DS3_ERR_SETUP_FAILURE	  -4
+#define LSM6DS3_ERR_GETGSENSORDATA  -5
+#define LSM6DS3_ERR_IDENTIFICATION	  -6
+
+#define LSM6DS3_BUFSIZE 60
+
+/*------------------------------------------------------------------*/
+
+// 1 rad = 180/PI degree, L3G4200D_OUT_MAGNIFY = 131,
+// 180*131/PI = 7506
+#define DEGREE_TO_RAD	180*1000000/PI//7506  // fenggy mask
+//#define DEGREE_TO_RAD 819	 
+#endif //L3M6DS3_H
+
diff --git a/drivers/misc/mediatek/accelerometer/mc3410-new/Makefile b/drivers/misc/mediatek/accelerometer/mc3410-new/Makefile
new file mode 100644
index 000000000..5aa68c8db
--- /dev/null
+++ b/drivers/misc/mediatek/accelerometer/mc3410-new/Makefile
@@ -0,0 +1,4 @@
+include $(srctree)/drivers/misc/mediatek/Makefile.custom
+
+obj-y	:=  mc3410.o
+
diff --git a/drivers/misc/mediatek/accelerometer/mc3410-new/mc3410.c b/drivers/misc/mediatek/accelerometer/mc3410-new/mc3410.c
new file mode 100644
index 000000000..02a1c3d7a
--- /dev/null
+++ b/drivers/misc/mediatek/accelerometer/mc3410-new/mc3410.c
@@ -0,0 +1,4179 @@
+/*
+* Copyright(C)2014 MediaTek Inc.
+* Modification based on code covered by the below mentioned copyright
+* and/or permission notice(S).
+*/
+
+/*****************************************************************************
+ *
+ * Copyright (c) 2014 mCube, Inc.  All rights reserved.
+ *
+ * This source is subject to the mCube Software License.
+ * This software is protected by Copyright and the information and source code
+ * contained herein is confidential. The software including the source code
+ * may not be copied and the information contained herein may not be used or
+ * disclosed except with the written permission of mCube Inc.
+ *
+ * All other rights reserved.
+ *
+ * This code and information are provided "as is" without warranty of any
+ * kind, either expressed or implied, including but not limited to the
+ * implied warranties of merchantability and/or fitness for a
+ * particular purpose.
+ *
+ * The following software/firmware and/or related documentation ("mCube Software")
+ * have been modified by mCube Inc. All revisions are subject to any receiver's
+ * applicable license agreements with mCube Inc.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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.
+ *
+ *
+ *****************************************************************************/
+
+/*****************************************************************************
+ *** HEADER FILES
+ *****************************************************************************/
+#include <linux/interrupt.h>
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include <linux/irq.h>
+#include <linux/miscdevice.h>
+#include <asm/uaccess.h>
+#include <linux/delay.h>
+#include <linux/input.h>
+#include <linux/workqueue.h>
+#include <linux/kobject.h>
+#include <linux/earlysuspend.h>
+#include <linux/platform_device.h>
+#include <asm/atomic.h>
+#include <mach/mt_pm_ldo.h>
+#include <linux/ioctl.h>
+#include <linux/wakelock.h>
+
+#include <cust_acc.h>
+#include <linux/hwmsensor.h>
+#include <linux/hwmsen_dev.h>
+#include <linux/sensors_io.h>
+#include <linux/hwmsen_helper.h>
+#include <accel.h>
+#include "mc3410.h"
+
+/*****************************************************************************
+ *** CONFIGURATION
+ *****************************************************************************/
+//#define _MC3XXX_SUPPORT_DOT_CALIBRATION_  /* Just for mcube's calibration APK*/
+#define _MC3XXX_SUPPORT_LPF_
+#define _MC3XXX_SUPPORT_CONCURRENCY_PROTECTION_
+//#define _MC3XXX_SUPPORT_APPLY_AVERAGE_AGORITHM_
+//#define _MC3XXX_SUPPORT_PERIODIC_DOC_
+//#define _MC3XXX_SUPPORT_VPROXIMITY_SENSOR_
+#define _MC3XXX_SUPPORT_LRF_
+//#define _MC3XXX_SUPPORT_POWER_SAVING_SHUTDOWN_POWER_
+#define C_MAX_FIR_LENGTH    (32)
+#define VIRTUAL_Z    0
+
+/*****************************************************************************
+ *** CONSTANT / DEFINITION
+ *****************************************************************************/
+/**************************
+ *** CONFIGURATION
+ **************************/
+#define MC3XXX_DEV_NAME                        "MC3XXX"
+#define MC3XXX_DEV_DRIVER_VERSION              "2.1.6"
+#define MC3XXX_DEV_DRIVER_VERSION_VIRTUAL_Z    "1.0.1"
+
+/**************************
+ *** COMMON
+ **************************/
+#define MC3XXX_AXIS_X      0
+#define MC3XXX_AXIS_Y      1
+#define MC3XXX_AXIS_Z      2
+#define MC3XXX_AXES_NUM    3
+#define MC3XXX_DATA_LEN    6
+#define MC3XXX_RESOLUTION_LOW     1
+#define MC3XXX_RESOLUTION_HIGH    2
+#define MC3XXX_LOW_REOLUTION_DATA_SIZE     3
+#define MC3XXX_HIGH_REOLUTION_DATA_SIZE    6
+#define MC3XXX_INIT_SUCC    (0)
+#define MC3XXX_INIT_FAIL    (-1)
+#define MC3XXX_REGMAP_LENGTH    (64)
+#define DEBUG_SWITCH        1
+#define C_I2C_FIFO_SIZE     8
+
+/**************************
+ *** DEBUG
+ **************************/
+    /*********************
+     *** G-Sensor
+     *********************/
+    #if DEBUG_SWITCH
+        #define GSE_TAG                  "[Gsensor] "
+        #define GSE_FUN(f)               printk(KERN_INFO GSE_TAG"%s\n", __FUNCTION__)
+        #define GSE_ERR(fmt, args...)    printk(KERN_ERR GSE_TAG"%s %d : "fmt, __FUNCTION__, __LINE__, ##args)
+        #define GSE_LOG(fmt, args...)    printk(KERN_NOTICE GSE_TAG fmt, ##args)
+    #else
+        #define GSE_TAG
+        #define GSE_FUN(f)               do {} while (0)
+        #define GSE_ERR(fmt, args...)    do {} while (0)
+        #define GSE_LOG(fmt, args...)    do {} while (0)
+    #endif
+
+    /*********************
+     *** vProximity Sensor
+     *********************/
+    #if 0
+        #define PS_TAG                  "[mCube/Psensor] "
+        #define PS_FUN(f)               printk(KERN_INFO PS_TAG"%s\n", __FUNCTION__)
+        #define PS_ERR(fmt, args...)    printk(KERN_ERR  PS_TAG"%s %d : "fmt, __FUNCTION__, __LINE__, ##args)
+        #define PS_LOG(fmt, args...)    printk(KERN_ERR PS_TAG fmt, ##args)
+    #else
+        #define PS_TAG
+        #define PS_FUN(f)               do {} while (0)
+        #define PS_ERR(fmt, args...)    do {} while (0)
+        #define PS_LOG(fmt, args...)    do {} while (0)
+    #endif
+
+/* Maintain  cust info here */
+struct acc_hw accel_cust;
+static struct acc_hw *hw = &accel_cust;
+
+/* For  driver get cust info */
+struct acc_hw *get_cust_acc(void) {
+    return &accel_cust;
+}
+/*****************************************************************************
+ *** DATA TYPE / STRUCTURE DEFINITION / ENUM
+ *****************************************************************************/
+typedef enum
+{
+    MCUBE_TRC_FILTER  = 0x01,
+    MCUBE_TRC_RAWDATA = 0x02,
+    MCUBE_TRC_IOCTL   = 0x04,
+    MCUBE_TRC_CALI    = 0X08,
+    MCUBE_TRC_INFO    = 0X10,
+    MCUBE_TRC_REGXYZ  = 0X20,
+}   MCUBE_TRC;
+
+struct scale_factor
+{
+    u8    whole;
+    u8    fraction;
+};
+
+struct data_resolution
+{
+    struct scale_factor    scalefactor;
+    int                    sensitivity;
+};
+
+struct data_filter
+{
+    s16    raw[C_MAX_FIR_LENGTH][MC3XXX_AXES_NUM];
+    int    sum[MC3XXX_AXES_NUM];
+    int    num;
+    int    idx;
+};
+
+struct mc3xxx_i2c_data
+{
+    //================================================
+    struct i2c_client          *client;
+    struct acc_hw              *hw;
+    struct hwmsen_convert       cvt;
+
+    //================================================
+    struct data_resolution     *reso;
+    atomic_t                    trace;
+    atomic_t                    suspend;
+    atomic_t                    selftest;
+    atomic_t                    filter;
+    s16                         cali_sw[MC3XXX_AXES_NUM + 1];
+
+    //================================================
+    s16                         offset[MC3XXX_AXES_NUM + 1];
+    s16                         data[MC3XXX_AXES_NUM + 1];
+
+    //================================================
+    #if defined(_MC3XXX_SUPPORT_LPF_)
+        atomic_t                firlen;
+        atomic_t                fir_en;
+        struct data_filter      fir;
+    #endif
+
+    //================================================
+    #if defined(USE_EARLY_SUSPEND)
+        struct early_suspend    early_drv;
+    #endif
+};
+
+#ifdef _MC3XXX_SUPPORT_LRF_
+    typedef struct
+    {
+        s16    nIsNewRound;
+        s16    nPreDiff;
+        s16    nPreValue;
+        s16    nMaxValue;
+        s16    nMinValue;
+        s16    nRepValue;
+        s16    nNewDataMonitorCount;
+    }   S_LRF_CB;
+#endif
+
+/*****************************************************************************
+ *** EXTERNAL FUNCTION
+ *****************************************************************************/
+//extern struct acc_hw*    mc3xxx_get_cust_acc_hw(void);
+
+/*****************************************************************************
+ *** STATIC FUNCTION
+ *****************************************************************************/
+static int mc3xxx_i2c_probe(struct i2c_client *client, const struct i2c_device_id *id);
+static int mc3xxx_i2c_remove(struct i2c_client *client);
+static int _mc3xxx_i2c_auto_probe(struct i2c_client *client);
+#ifndef USE_EARLY_SUSPEND
+    static int mc3xxx_suspend(struct i2c_client *client, pm_message_t msg);
+    static int mc3xxx_resume(struct i2c_client *client);
+#endif
+static int mc3xxx_local_init(void);
+static int mc3xxx_remove(void);
+static int MC3XXX_SetPowerMode(struct i2c_client *client, bool enable);
+static int MC3XXX_WriteCalibration(struct i2c_client *client, int dat[MC3XXX_AXES_NUM]);
+static void MC3XXX_SetGain(void);
+
+/*****************************************************************************
+ *** STATIC VARIBLE & CONTROL BLOCK DECLARATION
+ *****************************************************************************/
+static unsigned char    s_bResolution = 0x00;
+static unsigned char    s_bPCODE      = 0x00;
+static unsigned char    s_bPCODER     = 0x00;
+static unsigned char    s_bHWID       = 0x00;
+static unsigned char    s_bMPOL       = 0x00;
+static int    s_nInitFlag = MC3XXX_INIT_FAIL;
+static struct acc_init_info  mc3xxx_init_info =
+    {
+        .name   = MC3XXX_DEV_NAME,
+        .init   = mc3xxx_local_init,
+        .uninit = mc3xxx_remove,
+    };
+
+static const struct i2c_device_id                mc3xxx_i2c_id[]              = { {MC3XXX_DEV_NAME, 0}, {} };
+//static       struct i2c_board_info __initdata    mc3xxx_i2c_board_info        = { I2C_BOARD_INFO(MC3XXX_DEV_NAME, 0x4C) };
+static       unsigned short                      mc3xxx_i2c_auto_probe_addr[] = { 0x4C, 0x6C, 0x4E, 0x6D, 0x6E, 0x6F };
+static struct i2c_driver    mc3xxx_i2c_driver = {
+                                                    .driver = {
+                                                                  .name = MC3XXX_DEV_NAME,
+                                                              },
+                                                    .probe  = mc3xxx_i2c_probe,
+                                                    .remove = mc3xxx_i2c_remove,
+
+                                                    #if !defined(USE_EARLY_SUSPEND)
+                                                        .suspend = mc3xxx_suspend,
+                                                        .resume  = mc3xxx_resume,
+                                                    #endif
+
+                                                    .id_table = mc3xxx_i2c_id,
+                                                };
+
+static struct i2c_client        *mc3xxx_i2c_client = NULL;
+static struct mc3xxx_i2c_data   *mc3xxx_obj_i2c_data = NULL;
+static struct data_resolution    mc3xxx_offset_resolution = { {7, 8}, 256 };
+static bool    mc3xxx_sensor_power = false;
+static GSENSOR_VECTOR3D    gsensor_gain, gsensor_offset;
+static char    selftestRes[10] = {0};
+static struct file* fd_file = NULL;
+static mm_segment_t oldfs;
+static unsigned char offset_buf[6];
+static signed int    offset_data[3];
+static signed int    gain_data[3];
+static unsigned char    s_baOTP_OffsetData[6] = { 0 };
+static signed int    s_nIsRBM_Enabled = false;
+static DEFINE_MUTEX(MC3XXX_i2c_mutex);
+
+#ifdef _MC3XXX_SUPPORT_LRF_
+    static S_LRF_CB    s_taLRF_CB[MC3XXX_AXES_NUM];
+#endif
+
+#ifdef _MC3XXX_SUPPORT_CONCURRENCY_PROTECTION_
+    static struct semaphore    s_tSemaProtect;
+#endif
+
+#ifdef _MC3XXX_SUPPORT_DOT_CALIBRATION_
+    static int    s_nIsCaliLoaded = false;
+    static int    LPF_FirstRun = 1;
+#endif
+
+static int             LPF_SamplingRate = 5;
+static int             LPF_CutoffFrequency = 0x00000004;
+static unsigned int    iAReal0_X;
+static unsigned int    iAcc0Lpf0_X;
+static unsigned int    iAcc0Lpf1_X;
+static unsigned int    iAcc1Lpf0_X;
+static unsigned int    iAcc1Lpf1_X;
+
+static unsigned int    iAReal0_Y;
+static unsigned int    iAcc0Lpf0_Y;
+static unsigned int    iAcc0Lpf1_Y;
+static unsigned int    iAcc1Lpf0_Y;
+static unsigned int    iAcc1Lpf1_Y;
+
+static unsigned int    iAReal0_Z;
+static unsigned int    iAcc0Lpf0_Z;
+static unsigned int    iAcc0Lpf1_Z;
+static unsigned int    iAcc1Lpf0_Z;
+static unsigned int    iAcc1Lpf1_Z;
+
+static signed char    s_bAccuracyStatus = SENSOR_STATUS_ACCURACY_MEDIUM;
+
+#ifdef _MC3XXX_SUPPORT_PERIODIC_DOC_
+    static DECLARE_WAIT_QUEUE_HEAD(wq_mc3xxx_open_status);
+
+    static atomic_t    s_t_mc3xxx_open_status = ATOMIC_INIT(0);
+
+    static unsigned char    s_bIsPDOC_Enabled = false;
+#endif
+
+#ifdef _MC3XXX_SUPPORT_VPROXIMITY_SENSOR_
+    static int    P_STATUS;
+    static int    prev_P_STATUS = 0;
+#endif
+
+/*****************************************************************************
+ *** MACRO
+ *****************************************************************************/
+#ifdef _MC3XXX_SUPPORT_CONCURRENCY_PROTECTION_
+    #define MC3XXX_MUTEX_INIT()   sema_init(&s_tSemaProtect, 1)
+
+    #define MC3XXX_MUTEX_LOCK()                             \
+                if (down_interruptible(&s_tSemaProtect))    \
+                    return (-ERESTARTSYS)
+
+    #define MC3XXX_MUTEX_LOCK_RETURN_VOID()                 \
+                if (down_interruptible(&s_tSemaProtect))    \
+                    return
+
+    #define MC3XXX_MUTEX_UNLOCK()   up(&s_tSemaProtect)
+#else
+    #define MC3XXX_MUTEX_INIT()                do {} while (0)
+    #define MC3XXX_MUTEX_LOCK()                do {} while (0)
+    #define MC3XXX_MUTEX_LOCK_RETURN_VOID()    do {} while (0)
+    #define MC3XXX_MUTEX_UNLOCK()              do {} while (0)
+#endif
+
+#define MCUBE_RREMAP(nDataX, nDataY)                                                  \
+            if (MC3XXX_PCODE_3250 == s_bPCODE)                                        \
+            {                                                                         \
+                int    _nTemp = 0;                                                    \
+                                                                                      \
+                _nTemp = nDataX;                                                      \
+                nDataX = nDataY;                                                      \
+                nDataY = -_nTemp;                                                     \
+                GSE_LOG("[%s] 3250 read remap\n", __FUNCTION__);                      \
+            }                                                                         \
+            else                                                                      \
+            {                                                                         \
+                if (s_bMPOL & 0x01)    nDataX = -nDataX;                              \
+                if (s_bMPOL & 0x02)    nDataY = -nDataY;                              \
+            }
+
+#define MCUBE_WREMAP(nDataX, nDataY)                                                  \
+            if (MC3XXX_PCODE_3250 == s_bPCODE)                                        \
+            {                                                                         \
+                int    _nTemp = 0;                                                    \
+                                                                                      \
+                _nTemp = nDataX;                                                      \
+                nDataX = -nDataY;                                                     \
+                nDataY = _nTemp;                                                      \
+                GSE_LOG("[%s] 3250 write remap\n", __FUNCTION__);                     \
+            }                                                                         \
+            else                                                                      \
+            {                                                                         \
+                if (s_bMPOL & 0x01)    nDataX = -nDataX;                              \
+                if (s_bMPOL & 0x02)    nDataY = -nDataY;                              \
+                GSE_LOG("[%s] 35X0 remap [s_bMPOL: %d]\n", __FUNCTION__, s_bMPOL);    \
+            }
+
+#define IS_MCFM12()    ((0xC0 <= s_bHWID) && (s_bHWID <= 0xCF))
+#define IS_MCFM3X()    ((0x20 == s_bHWID) || ((0x22 <= s_bHWID) && (s_bHWID <= 0x2F)))
+
+/*****************************************************************************
+ *** TODO
+ *****************************************************************************/
+#define DATA_PATH              "/sdcard2/mcube-register-map.txt"
+
+#ifdef _MC3XXX_SUPPORT_DOT_CALIBRATION_
+    static char    file_path[MC3XXX_BUF_SIZE]        = "/data/data/com.mcube.acc/files/mcube-calib.txt";
+    static char    backup_file_path[MC3XXX_BUF_SIZE] = "/data/misc/sensors/mcube-calib.txt";
+#endif
+
+/*****************************************************************************
+ *** FUNCTION
+ *****************************************************************************/
+
+/**************I2C operate API*****************************/
+static int MC3XXX_i2c_read_block(struct i2c_client *client, u8 addr, u8 *data, u8 len)
+{
+    u8 beg = addr;
+    int err;
+    struct i2c_msg msgs[2]={{0},{0}};
+
+    mutex_lock(&MC3XXX_i2c_mutex);
+
+    msgs[0].addr = client->addr;
+    msgs[0].flags = 0;
+    msgs[0].len =1;
+    msgs[0].buf = &beg;
+
+    msgs[1].addr = client->addr;
+    msgs[1].flags = I2C_M_RD;
+    msgs[1].len =len;
+    msgs[1].buf = data;
+
+    if (!client)
+    {
+        mutex_unlock(&MC3XXX_i2c_mutex);
+        return -EINVAL;
+    }
+    else if (len > C_I2C_FIFO_SIZE)
+    {
+        GSE_ERR(" length %d exceeds %d\n", len, C_I2C_FIFO_SIZE);
+        mutex_unlock(&MC3XXX_i2c_mutex);
+        return -EINVAL;
+    }
+    err = i2c_transfer(client->adapter, msgs, sizeof(msgs)/sizeof(msgs[0]));
+    if (err != 2)
+    {
+        GSE_ERR("i2c_transfer error: (%d %p %d) %d\n",addr, data, len, err);
+        err = -EIO;
+    }
+    else
+    {
+        err = 0;
+    }
+    mutex_unlock(&MC3XXX_i2c_mutex);
+    return err;
+
+}
+
+static int MC3XXX_i2c_write_block(struct i2c_client *client, u8 addr, u8 *data, u8 len)
+{   /*because address also occupies one byte, the maximum length for write is 7 bytes*/
+    int err, idx, num;
+    char buf[C_I2C_FIFO_SIZE];
+    err =0;
+    mutex_lock(&MC3XXX_i2c_mutex);
+    if (!client)
+    {
+        mutex_unlock(&MC3XXX_i2c_mutex);
+        return -EINVAL;
+    }
+    else if (len >= C_I2C_FIFO_SIZE)
+    {
+        GSE_ERR(" length %d exceeds %d\n", len, C_I2C_FIFO_SIZE);
+        mutex_unlock(&MC3XXX_i2c_mutex);
+        return -EINVAL;
+    }
+
+    num = 0;
+    buf[num++] = addr;
+    for (idx = 0; idx < len; idx++)
+    {
+        buf[num++] = data[idx];
+    }
+
+    err = i2c_master_send(client, buf, num);
+    if (err < 0)
+    {
+        GSE_ERR("send command error!!\n");
+        mutex_unlock(&MC3XXX_i2c_mutex);
+        return -EFAULT;
+    }
+	else
+    {
+        err = 0;
+    }
+    mutex_unlock(&MC3XXX_i2c_mutex);
+    return err;
+}
+
+
+/*****************************************
+ *** GetLowPassFilter
+ *****************************************/
+static unsigned int GetLowPassFilter(unsigned int X0,unsigned int Y1)
+{
+    unsigned int lTemp;
+
+    lTemp = Y1;
+    lTemp *= LPF_CutoffFrequency;    // 4HZ LPF RC=0.04
+    X0 *= LPF_SamplingRate;
+    lTemp += X0;
+    lTemp += LPF_CutoffFrequency;
+    lTemp /= (LPF_CutoffFrequency + LPF_SamplingRate);
+    Y1 = lTemp;
+
+    return Y1;
+}
+
+/*****************************************
+ *** openFile
+ *****************************************/
+static struct file *openFile(char *path,int flag,int mode)
+{
+    struct file *fp = NULL;
+
+    fp = filp_open(path, flag, mode);
+
+    if (IS_ERR(fp) || !fp->f_op)
+    {
+        GSE_LOG("Calibration File filp_open return NULL\n");
+        return NULL;
+    }
+    else
+    {
+        return fp;
+    }
+}
+
+/*****************************************
+ *** readFile
+ *****************************************/
+#ifdef _MC3XXX_SUPPORT_DOT_CALIBRATION_
+static int readFile(struct file *fp,char *buf,int readlen)
+{
+    if (fp->f_op && fp->f_op->read)
+        return fp->f_op->read(fp,buf,readlen, &fp->f_pos);
+    else
+        return -1;
+}
+#endif
+
+/*****************************************
+ *** writeFile
+ *****************************************/
+static int writeFile(struct file *fp,char *buf,int writelen)
+{
+    if (fp->f_op && fp->f_op->write)
+        return fp->f_op->write(fp,buf,writelen, &fp->f_pos);
+    else
+        return -1;
+}
+
+/*****************************************
+ *** closeFile
+ *****************************************/
+static int closeFile(struct file *fp)
+{
+    filp_close(fp,NULL);
+    return 0;
+}
+
+/*****************************************
+ *** initKernelEnv
+ *****************************************/
+static void initKernelEnv(void)
+{
+    oldfs = get_fs();
+    set_fs(KERNEL_DS);
+    printk(KERN_INFO "initKernelEnv\n");
+}
+
+/*****************************************
+ *** mcube_write_log_data
+ *****************************************/
+static int mcube_write_log_data(struct i2c_client *client, u8 data[0x3f])
+{
+    #define _WRT_LOG_DATA_BUFFER_SIZE    (66 * 50)
+
+    s16 rbm_data[3] = {0}, raw_data[3] = {0};
+    int err = 0;
+    char *_pszBuffer = NULL;
+    int n = 0, i = 0;
+
+    initKernelEnv();
+    fd_file = openFile(DATA_PATH ,O_RDWR | O_CREAT,0);
+    if (fd_file == NULL)
+    {
+        GSE_LOG("mcube_write_log_data fail to open\n");
+    }
+    else
+    {
+        rbm_data[MC3XXX_AXIS_X] = (s16)((data[0x0d]) | (data[0x0e] << 8));
+        rbm_data[MC3XXX_AXIS_Y] = (s16)((data[0x0f]) | (data[0x10] << 8));
+        rbm_data[MC3XXX_AXIS_Z] = (s16)((data[0x11]) | (data[0x12] << 8));
+
+        raw_data[MC3XXX_AXIS_X] = (rbm_data[MC3XXX_AXIS_X] + offset_data[0]/2)*gsensor_gain.x/gain_data[0];
+        raw_data[MC3XXX_AXIS_Y] = (rbm_data[MC3XXX_AXIS_Y] + offset_data[1]/2)*gsensor_gain.y/gain_data[1];
+        raw_data[MC3XXX_AXIS_Z] = (rbm_data[MC3XXX_AXIS_Z] + offset_data[2]/2)*gsensor_gain.z/gain_data[2];
+
+        _pszBuffer = kzalloc(_WRT_LOG_DATA_BUFFER_SIZE, GFP_KERNEL);
+
+        if (NULL == _pszBuffer)
+        {
+            GSE_ERR("fail to allocate memory for buffer\n");
+            return -1;
+        }
+
+        memset(_pszBuffer, 0, _WRT_LOG_DATA_BUFFER_SIZE);
+
+        n += sprintf(_pszBuffer+n, "G-sensor RAW X = %d  Y = %d  Z = %d\n", raw_data[0] ,raw_data[1] ,raw_data[2]);
+        n += sprintf(_pszBuffer+n, "G-sensor RBM X = %d  Y = %d  Z = %d\n", rbm_data[0] ,rbm_data[1] ,rbm_data[2]);
+
+        for (i = 0; i < 64; i++)
+        {
+            n += sprintf(_pszBuffer+n, "mCube register map Register[%x] = 0x%x\n",i,data[i]);
+        }
+
+        msleep(50);
+
+        if ((err = writeFile(fd_file,_pszBuffer,n)) > 0)
+            GSE_LOG("buf:%s\n",_pszBuffer);
+        else
+            GSE_LOG("write file error %d\n",err);
+
+        kfree(_pszBuffer);
+        set_fs(oldfs);
+        closeFile(fd_file);
+    }
+
+    return 0;
+}
+
+#ifdef _MC3XXX_SUPPORT_DOT_CALIBRATION_
+/*****************************************
+ *** mcube_read_cali_file
+ *****************************************/
+static int mcube_read_cali_file(struct i2c_client *client)
+{
+    int cali_data[3] = {0},cali_data1[3] = {0};
+    int err = 0;
+    char buf[64] = {0};
+
+    GSE_LOG("[%s]\n",__func__);
+
+    initKernelEnv();
+    fd_file = openFile(file_path,O_RDONLY,0);
+
+    if (fd_file == NULL)
+    {
+        GSE_LOG("%s:fail to open calibration file: %s\n", __func__, file_path);
+        fd_file = openFile(backup_file_path,O_RDONLY,0);
+
+        if(fd_file == NULL)
+        {
+            GSE_LOG("%s:fail to open calibration file: %s\n", __func__, backup_file_path);
+            cali_data[0] = 0;
+            cali_data[1] = 0;
+            cali_data[2] = 0;
+            return (-1);
+        }
+        else
+        {
+            GSE_LOG("Open backup calibration file successfully: %s\n", backup_file_path);
+        }
+    }
+    else
+    {
+        GSE_LOG("Open calibration file successfully: %s\n", file_path);
+    }
+
+    memset(buf, 0, sizeof(buf));
+
+    if ((err = readFile(fd_file, buf, sizeof(buf))) > 0)
+        GSE_LOG("cali_file: buf:%s\n",buf);
+    else
+        GSE_LOG("read file error %d\n",err);
+
+    set_fs(oldfs);
+    closeFile(fd_file);
+
+    sscanf(buf, "%d %d %d",&cali_data[MC3XXX_AXIS_X], &cali_data[MC3XXX_AXIS_Y], &cali_data[MC3XXX_AXIS_Z]);
+    GSE_LOG("cali_data: %d %d %d\n", cali_data[MC3XXX_AXIS_X], cali_data[MC3XXX_AXIS_Y], cali_data[MC3XXX_AXIS_Z]);
+
+    cali_data1[MC3XXX_AXIS_X] = cali_data[MC3XXX_AXIS_X] * gsensor_gain.x / GRAVITY_EARTH_1000;
+    cali_data1[MC3XXX_AXIS_Y] = cali_data[MC3XXX_AXIS_Y] * gsensor_gain.y / GRAVITY_EARTH_1000;
+    cali_data1[MC3XXX_AXIS_Z] = cali_data[MC3XXX_AXIS_Z] * gsensor_gain.z / GRAVITY_EARTH_1000;
+
+    GSE_LOG("cali_data1: %d %d %d\n", cali_data1[MC3XXX_AXIS_X], cali_data1[MC3XXX_AXIS_Y], cali_data1[MC3XXX_AXIS_Z]);
+
+    MC3XXX_WriteCalibration(client, cali_data1);
+
+    return 0;
+}
+
+/*****************************************
+ *** mcube_load_cali
+ *****************************************/
+static void    mcube_load_cali(struct i2c_client *pt_i2c_client)
+{
+    if (false == s_nIsCaliLoaded)
+    {
+        GSE_LOG("[%s] loading cali file...\n", __FUNCTION__);
+
+        if (MC3XXX_RETCODE_SUCCESS == mcube_read_cali_file(pt_i2c_client))
+            s_nIsCaliLoaded = true;
+    }
+}
+
+#endif // _MC3XXX_SUPPORT_DOT_CALIBRATION_
+
+/*****************************************
+ *** mcube_psensor_ioctl
+ *****************************************/
+#ifdef _MC3XXX_SUPPORT_VPROXIMITY_SENSOR_
+static long mcube_psensor_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
+{
+    int err=0;
+    void __user *data;
+    int pSensor=0;
+
+    if(_IOC_DIR(cmd) & _IOC_READ)
+    {
+        err = !access_ok(VERIFY_WRITE, (void __user *)arg, _IOC_SIZE(cmd));
+    }
+    else if(_IOC_DIR(cmd) & _IOC_WRITE)
+    {
+        err = !access_ok(VERIFY_READ, (void __user *)arg, _IOC_SIZE(cmd));
+    }
+
+    if(err)
+    {
+        PS_ERR("access error: %08X, (%2d, %2d)\n", cmd, _IOC_DIR(cmd), _IOC_SIZE(cmd));
+        return -EFAULT;
+    }
+
+    switch(cmd)
+    {
+        case PSENSOR_IOCTL_SET_POSTURE:
+            data = (void __user*)arg;
+            if(data == NULL)
+            {
+                err = -EINVAL;
+                break;
+            }
+
+            if(copy_from_user(&pSensor, data, sizeof(int)))
+            {
+                err = -EFAULT;
+                break;
+            }
+
+            P_STATUS = pSensor;
+            PS_LOG("IOCTL Get P_STATUS = %d",pSensor);
+            break;
+        default:
+
+            PS_ERR("unknown IOCTL: 0x%08x\n", cmd);
+            //err = -ENOIOCTLCMD;
+            break;
+    }
+    return err;
+}
+
+/*****************************************
+ *** STATIC STRUCTURE:: fops
+ *****************************************/
+static struct file_operations mcube_psensor_fops = {
+    .owner = THIS_MODULE,
+    .unlocked_ioctl = mcube_psensor_ioctl,
+};
+
+/*****************************************
+ *** STATIC STRUCTURE:: misc-device
+ *****************************************/
+static struct miscdevice mcube_psensor_device = {
+    .minor = MISC_DYNAMIC_MINOR,
+    .name = "psensor",
+    .fops  = &mcube_psensor_fops,
+};
+
+/*****************************************
+ *** psensor_ps_operate
+ *****************************************/
+static int psensor_ps_operate(void* self, uint32_t command, void* buff_in, int size_in,
+        void* buff_out /*sensor_data*/, int size_out, int* actualout)
+{
+    int err = 0;
+    int value;
+    int val=-1;
+    hwm_sensor_data* sensor_data;
+    //struct mcube_psensor_priv *obj = (struct mcube_psensor_priv *)self;
+
+    //PS_FUN(f);
+    switch (command)
+    {
+        case SENSOR_DELAY:
+            if((buff_in == NULL) || (size_in < sizeof(int)))
+            {
+                PS_ERR("Set delay parameter error!\n");
+                err = -EINVAL;
+            }
+            // Do nothing
+            break;
+
+        case SENSOR_ENABLE:
+            value = *(int *)buff_in;
+            if(value)
+            {
+                if((buff_in == NULL) || (size_in < sizeof(int)))
+                {
+                    PS_ERR("Enable sensor parameter error!\n");
+                    err = -EINVAL;
+                }
+
+                else
+                {
+                    value = *(int *)buff_in;
+                    //obj->enable = 1;
+                    PS_ERR("enable ps \n");
+                }
+            }
+            else
+            {
+                //obj->enable = 0;
+                PS_ERR("disable ps \n");
+            }
+            break;
+        case SENSOR_GET_DATA:
+            PS_LOG("fwq get ps data !!!!!!\n");
+            if((buff_out == NULL) || (size_out< sizeof(hwm_sensor_data)))
+            {
+                PS_ERR("get sensor data parameter error!\n");
+                err = -EINVAL;
+            }
+            else
+            {
+                sensor_data = (hwm_sensor_data *)buff_out;
+                PS_LOG("mcube sensor data P_STATUS=%d\n",P_STATUS);
+
+                if(prev_P_STATUS != P_STATUS )
+                {
+                    if(P_STATUS == 0)
+                    {
+                        PS_LOG("TURN ON LCD\n");
+                        val=1;
+                        PS_LOG("Set val = 1, Proximity sensor far away\n");
+                    }
+                    else if (P_STATUS == 1)
+                    {
+                        PS_LOG("TURN OFF LCD\n");
+                        val=0;
+                        PS_LOG("Set val = 0, Proximity sensor close\n");
+                    }
+                    PS_LOG("mcube sensor data prev_P_STATUS=%d, P_STATUS=%d\n",prev_P_STATUS,P_STATUS);
+                    prev_P_STATUS = P_STATUS;
+                }
+                else
+                {
+                    PS_LOG("P_STATUS %5d=>%5d\n",prev_P_STATUS,P_STATUS);
+                    prev_P_STATUS = P_STATUS;
+                    val= (P_STATUS==0)? (1):(0);
+                }
+                sensor_data->values[0] = val;
+                sensor_data->value_divide = 1;
+                sensor_data->status = SENSOR_STATUS_ACCURACY_MEDIUM;
+                prev_P_STATUS = P_STATUS;
+            }
+            break;
+        default:
+            PS_ERR("proxmy sensor operate function has no this command %d!\n", command);
+            err = -1;
+            break;
+    }
+
+    return err;
+}
+#endif  // end of _MC3XXX_SUPPORT_VPROXIMITY_SENSOR_
+
+/*****************************************
+ *** MC3XXX_power
+ *****************************************/
+static void MC3XXX_power(struct acc_hw *hw, unsigned int on)
+{
+#ifdef __USE_LINUX_REGULATOR_FRAMEWORK__
+#else
+    static unsigned int power_on = 0;
+
+    if(hw->power_id != MT65XX_POWER_NONE)        // have externel LDO
+    {
+        GSE_LOG("power %s\n", on ? "on" : "off");
+        if(power_on == on)    // power status not change
+        {
+            GSE_LOG("ignore power control: %d\n", on);
+        }
+        else if(on)    // power on
+        {
+            if(!hwPowerOn(hw->power_id, hw->power_vol, MC3XXX_DEV_NAME))
+            {
+                GSE_ERR("power on fails!!\n");
+            }
+        }
+        else    // power off
+        {
+            if (!hwPowerDown(hw->power_id, "MC3XXX"))
+            {
+                GSE_ERR("power off fail!!\n");
+            }
+        }
+    }
+    power_on = on;
+#endif
+}
+
+#ifdef _MC3XXX_SUPPORT_DOT_CALIBRATION_
+/*****************************************
+ *** MC3XXX_rbm
+ *****************************************/
+static void MC3XXX_rbm(struct i2c_client *client, int enable)
+{
+    u8    _baDataBuf[3] = { 0 };
+
+    _baDataBuf[0] = 0x43;
+    MC3XXX_i2c_write_block(client, 0x07, _baDataBuf, 0x01);
+
+    MC3XXX_i2c_read_block(client, 0x04, _baDataBuf, 0x01);
+
+    //GSE_LOG("[%s] REG(0x04): 0x%X, enable: %d\n", __FUNCTION__, _baDataBuf[0], enable);
+
+    if (0x00 == (_baDataBuf[0] & 0x40))
+    {
+        _baDataBuf[0] = 0x6D;
+        MC3XXX_i2c_write_block(client, 0x1B, _baDataBuf, 0x01);
+
+        _baDataBuf[0] = 0x43;
+        MC3XXX_i2c_write_block(client, 0x1B, _baDataBuf, 0x01);
+    }
+
+    //MC3XXX_i2c_read_block(client, 0x04, _baDataBuf, 0x01);
+    //GSE_LOG("BEGIN - REG(0x04): 0x%X\n", _baDataBuf[0]);
+
+    if (1 == enable)
+    {
+        _baDataBuf[0] = 0x00;
+        MC3XXX_i2c_write_block(client, 0x3B, _baDataBuf, 0x01);
+
+        _baDataBuf[0] = 0x02;
+        MC3XXX_i2c_write_block(client, 0x14, _baDataBuf, 0x01);
+
+        if (MC3XXX_RESOLUTION_LOW == s_bResolution)
+        {
+            gsensor_gain.x = gsensor_gain.y = gsensor_gain.z = 1024;
+        }
+
+        s_nIsRBM_Enabled = 1;
+        LPF_FirstRun = 1;
+
+        GSE_LOG("set rbm!!\n");
+    }
+    else if (0 == enable)
+    {
+        _baDataBuf[0] = 0x00;
+        MC3XXX_i2c_write_block(client, 0x14, _baDataBuf, 0x01);
+
+        _baDataBuf[0] = s_bPCODER;
+        MC3XXX_i2c_write_block(client, 0x3B, _baDataBuf, 0x01);
+
+        MC3XXX_SetGain();
+
+        s_nIsRBM_Enabled = 0;
+
+        GSE_LOG("clear rbm!!\n");
+    }
+
+    MC3XXX_i2c_read_block(client, 0x04, _baDataBuf, 0x01);
+
+    //GSE_LOG("RBM CONTROL DONE - REG(0x04): 0x%X\n", _baDataBuf[0]);
+
+    if (_baDataBuf[0] & 0x40)
+    {
+        _baDataBuf[0] = 0x6D;
+        MC3XXX_i2c_write_block(client, 0x1B, _baDataBuf, 0x01);
+
+        _baDataBuf[0] = 0x43;
+        MC3XXX_i2c_write_block(client, 0x1B, _baDataBuf, 0x01);
+    }
+
+    //MC3XXX_i2c_read_block(client, 0x04, _baDataBuf, 0x01);
+    //GSE_LOG("END - REG(0x04): 0x%X\n", _baDataBuf[0]);
+
+    _baDataBuf[0] = 0x41;
+    MC3XXX_i2c_write_block(client, 0x07, _baDataBuf, 0x01);
+
+    msleep(220);
+}
+
+/*****************************************
+ *** MC3XXX_ReadData_RBM
+ *****************************************/
+static int MC3XXX_ReadData_RBM(struct i2c_client *client, int data[MC3XXX_AXES_NUM])
+{
+    u8 addr = 0x0d;
+    u8 rbm_buf[MC3XXX_DATA_LEN] = {0};
+    int err = 0;
+
+    if(NULL == client)
+    {
+        err = -EINVAL;
+        return err;
+    }
+
+/********************************************/
+
+    if((err = MC3XXX_i2c_read_block(client, addr, rbm_buf, 0x06)))
+    {
+        GSE_ERR("error: %d\n", err);
+        return err;
+    }
+
+    data[MC3XXX_AXIS_X] = (s16)((rbm_buf[0]) | (rbm_buf[1] << 8));
+    data[MC3XXX_AXIS_Y] = (s16)((rbm_buf[2]) | (rbm_buf[3] << 8));
+    data[MC3XXX_AXIS_Z] = (s16)((rbm_buf[4]) | (rbm_buf[5] << 8));
+
+    GSE_LOG("rbm_buf<<<<<[%02x %02x %02x %02x %02x %02x]\n",rbm_buf[0], rbm_buf[2], rbm_buf[2], rbm_buf[3], rbm_buf[4], rbm_buf[5]);
+    GSE_LOG("RBM<<<<<[%04x %04x %04x]\n", data[MC3XXX_AXIS_X], data[MC3XXX_AXIS_Y], data[MC3XXX_AXIS_Z]);
+    GSE_LOG("RBM<<<<<[%04d %04d %04d]\n", data[MC3XXX_AXIS_X], data[MC3XXX_AXIS_Y], data[MC3XXX_AXIS_Z]);
+
+
+/********************************************/
+    MCUBE_RREMAP(data[0], data[1]);
+
+    return err;
+}
+#endif // _MC3XXX_SUPPORT_DOT_CALIBRATION_
+
+/*****************************************
+ *** MC3XXX_ValidateSensorIC
+ *****************************************/
+static int MC3XXX_ValidateSensorIC(unsigned char *pbPCode, unsigned char *pbHwID)
+{
+    GSE_LOG("[%s] *pbPCode: 0x%02X, *pbHwID: 0x%02X\n", __FUNCTION__, *pbPCode, *pbHwID);
+
+    if (   (0x01 == *pbHwID)
+        || (0x03 == *pbHwID)
+        || ((0x04 <= *pbHwID) && (*pbHwID <= 0x0F)))
+    {
+        if ((MC3XXX_PCODE_3210 == *pbPCode) || (MC3XXX_PCODE_3230 == *pbPCode) || (MC3XXX_PCODE_3250 == *pbPCode))
+            return (MC3XXX_RETCODE_SUCCESS);
+    }
+    else if (   (0x02 == *pbHwID)
+             || (0x21 == *pbHwID)
+             || ((0x10 <= *pbHwID) && (*pbHwID <= 0x1F)))
+    {
+        if (   (MC3XXX_PCODE_3210 == *pbPCode) || (MC3XXX_PCODE_3230  == *pbPCode)
+            || (MC3XXX_PCODE_3250 == *pbPCode)
+            || (MC3XXX_PCODE_3410 == *pbPCode) || (MC3XXX_PCODE_3410N == *pbPCode)
+            || (MC3XXX_PCODE_3430 == *pbPCode) || (MC3XXX_PCODE_3430N == *pbPCode))
+        {
+            return (MC3XXX_RETCODE_SUCCESS);
+        }
+    }
+    else if ((0xC0 <= *pbHwID) && (*pbHwID <= 0xCF))
+    {
+        *pbPCode = (*pbPCode & 0x71);
+
+        if ((MC3XXX_PCODE_3510 == *pbPCode) || (MC3XXX_PCODE_3530 == *pbPCode))
+            return (MC3XXX_RETCODE_SUCCESS);
+    }
+    else if ((0x20 == *pbHwID) || ((0x22 <= *pbHwID) && (*pbHwID <= 0x2F)))
+    {
+        *pbPCode = (*pbPCode & 0xF1);
+
+        if (   (MC3XXX_PCODE_3210      == *pbPCode) || (MC3XXX_PCODE_3216      == *pbPCode) || (MC3XXX_PCODE_3236      == *pbPCode)
+            || (MC3XXX_PCODE_RESERVE_1 == *pbPCode) || (MC3XXX_PCODE_RESERVE_2 == *pbPCode) || (MC3XXX_PCODE_RESERVE_3 == *pbPCode)
+            || (MC3XXX_PCODE_RESERVE_4 == *pbPCode) || (MC3XXX_PCODE_RESERVE_5 == *pbPCode) || (MC3XXX_PCODE_RESERVE_6 == *pbPCode)
+            || (MC3XXX_PCODE_RESERVE_7 == *pbPCode) || (MC3XXX_PCODE_RESERVE_8 == *pbPCode) || (MC3XXX_PCODE_RESERVE_9 == *pbPCode))
+        {
+            return (MC3XXX_RETCODE_SUCCESS);
+        }
+    }
+
+    return (MC3XXX_RETCODE_ERROR_IDENTIFICATION);
+}
+
+/*****************************************
+ *** MC3XXX_Read_Chip_ID
+ *****************************************/
+static int MC3XXX_Read_Chip_ID(struct i2c_client *client, char *buf)
+{
+    u8     _bChipID[4] = { 0 };
+
+    GSE_LOG("[%s]\n", __func__);
+
+    if (!buf || !client)
+        return EINVAL;
+
+    if (MC3XXX_i2c_read_block(client, 0x3C, _bChipID, 4))
+    {
+        GSE_ERR("[%s] i2c read fail\n", __func__);
+        _bChipID[0] = 0;
+        _bChipID[1] = 0;
+        _bChipID[2] = 0;
+        _bChipID[3] = 0;
+    }
+
+    GSE_LOG("[%s] %02X-%02X-%02X-%02X\n", __func__, _bChipID[3], _bChipID[2], _bChipID[1], _bChipID[0]);
+
+    return sprintf(buf, "%02X-%02X-%02X-%02X\n", _bChipID[3], _bChipID[2], _bChipID[1], _bChipID[0]);
+}
+
+/*****************************************
+ *** MC3XXX_Read_Reg_Map
+ *****************************************/
+static int MC3XXX_Read_Reg_Map(struct i2c_client *p_i2c_client, u8 *pbUserBuf)
+{
+    u8     _baData[MC3XXX_REGMAP_LENGTH] = { 0 };
+    int    _nIndex = 0;
+
+    GSE_LOG("[%s]\n", __func__);
+
+    if(NULL == p_i2c_client)
+        return (-EINVAL);
+
+    for(_nIndex = 0; _nIndex < MC3XXX_REGMAP_LENGTH; _nIndex++)
+    {
+        MC3XXX_i2c_read_block(p_i2c_client, _nIndex, &_baData[_nIndex], 1);
+
+        if (NULL != pbUserBuf)
+            pbUserBuf[_nIndex] = _baData[_nIndex];
+
+        printk(KERN_INFO "[Gsensor] REG[0x%02X] = 0x%02X\n", _nIndex, _baData[_nIndex]);
+    }
+
+    mcube_write_log_data(p_i2c_client, _baData);
+
+    return (0);
+}
+
+/*****************************************
+ *** MC3XXX_SaveDefaultOffset
+ *****************************************/
+static void MC3XXX_SaveDefaultOffset(struct i2c_client *p_i2c_client)
+{
+    GSE_LOG("[%s]\n", __func__);
+
+    MC3XXX_i2c_read_block(p_i2c_client, 0x21, &s_baOTP_OffsetData[0], 3);
+    MC3XXX_i2c_read_block(p_i2c_client, 0x24, &s_baOTP_OffsetData[3], 3);
+
+    GSE_LOG("s_baOTP_OffsetData: 0x%02X - 0x%02X - 0x%02X - 0x%02X - 0x%02X - 0x%02X\n",
+            s_baOTP_OffsetData[0], s_baOTP_OffsetData[1], s_baOTP_OffsetData[2],
+            s_baOTP_OffsetData[3], s_baOTP_OffsetData[4], s_baOTP_OffsetData[5]);
+}
+
+/*****************************************
+ *** MC3XXX_LPF
+ *****************************************/
+#ifdef _MC3XXX_SUPPORT_LPF_
+static void MC3XXX_LPF(struct mc3xxx_i2c_data *priv, s16 data[MC3XXX_AXES_NUM])
+{
+    if(atomic_read(&priv->filter))
+    {
+        if(atomic_read(&priv->fir_en) && !atomic_read(&priv->suspend))
+        {
+            int idx, firlen = atomic_read(&priv->firlen);
+            if(priv->fir.num < firlen)
+            {
+                priv->fir.raw[priv->fir.num][MC3XXX_AXIS_X] = data[MC3XXX_AXIS_X];
+                priv->fir.raw[priv->fir.num][MC3XXX_AXIS_Y] = data[MC3XXX_AXIS_Y];
+                priv->fir.raw[priv->fir.num][MC3XXX_AXIS_Z] = data[MC3XXX_AXIS_Z];
+                priv->fir.sum[MC3XXX_AXIS_X] += data[MC3XXX_AXIS_X];
+                priv->fir.sum[MC3XXX_AXIS_Y] += data[MC3XXX_AXIS_Y];
+                priv->fir.sum[MC3XXX_AXIS_Z] += data[MC3XXX_AXIS_Z];
+                if(atomic_read(&priv->trace) & MCUBE_TRC_FILTER)
+                {
+                    GSE_LOG("add [%2d] [%5d %5d %5d] => [%5d %5d %5d]\n", priv->fir.num,
+                    priv->fir.raw[priv->fir.num][MC3XXX_AXIS_X], priv->fir.raw[priv->fir.num][MC3XXX_AXIS_Y], priv->fir.raw[priv->fir.num][MC3XXX_AXIS_Z],
+                    priv->fir.sum[MC3XXX_AXIS_X], priv->fir.sum[MC3XXX_AXIS_Y], priv->fir.sum[MC3XXX_AXIS_Z]);
+                }
+                priv->fir.num++;
+                priv->fir.idx++;
+            }
+            else
+            {
+                idx = priv->fir.idx % firlen;
+                priv->fir.sum[MC3XXX_AXIS_X] -= priv->fir.raw[idx][MC3XXX_AXIS_X];
+                priv->fir.sum[MC3XXX_AXIS_Y] -= priv->fir.raw[idx][MC3XXX_AXIS_Y];
+                priv->fir.sum[MC3XXX_AXIS_Z] -= priv->fir.raw[idx][MC3XXX_AXIS_Z];
+                priv->fir.raw[idx][MC3XXX_AXIS_X] = data[MC3XXX_AXIS_X];
+                priv->fir.raw[idx][MC3XXX_AXIS_Y] = data[MC3XXX_AXIS_Y];
+                priv->fir.raw[idx][MC3XXX_AXIS_Z] = data[MC3XXX_AXIS_Z];
+                priv->fir.sum[MC3XXX_AXIS_X] += data[MC3XXX_AXIS_X];
+                priv->fir.sum[MC3XXX_AXIS_Y] += data[MC3XXX_AXIS_Y];
+                priv->fir.sum[MC3XXX_AXIS_Z] += data[MC3XXX_AXIS_Z];
+                priv->fir.idx++;
+                data[MC3XXX_AXIS_X] = priv->fir.sum[MC3XXX_AXIS_X]/firlen;
+                data[MC3XXX_AXIS_Y] = priv->fir.sum[MC3XXX_AXIS_Y]/firlen;
+                data[MC3XXX_AXIS_Z] = priv->fir.sum[MC3XXX_AXIS_Z]/firlen;
+                if(atomic_read(&priv->trace) & MCUBE_TRC_FILTER)
+                {
+                    GSE_LOG("add [%2d] [%5d %5d %5d] => [%5d %5d %5d] : [%5d %5d %5d]\n", idx,
+                    priv->fir.raw[idx][MC3XXX_AXIS_X], priv->fir.raw[idx][MC3XXX_AXIS_Y], priv->fir.raw[idx][MC3XXX_AXIS_Z],
+                    priv->fir.sum[MC3XXX_AXIS_X], priv->fir.sum[MC3XXX_AXIS_Y], priv->fir.sum[MC3XXX_AXIS_Z],
+                    data[MC3XXX_AXIS_X], data[MC3XXX_AXIS_Y], data[MC3XXX_AXIS_Z]);
+                }
+            }
+        }
+    }
+}
+#endif    // END OF #ifdef _MC3XXX_SUPPORT_LPF_
+
+#ifdef _MC3XXX_SUPPORT_LRF_
+/*****************************************
+ *** _MC3XXX_LowResFilter
+ *****************************************/
+static void _MC3XXX_LowResFilter(s16 nAxis, s16 naData[MC3XXX_AXES_NUM])
+{
+    #define _LRF_DIFF_COUNT_POS                  2
+    #define _LRF_DIFF_COUNT_NEG                  (-_LRF_DIFF_COUNT_POS)
+    #define _LRF_DIFF_BOUNDARY_POS               (_LRF_DIFF_COUNT_POS + 1)
+    #define _LRF_DIFF_BOUNDARY_NEG               (_LRF_DIFF_COUNT_NEG - 1)
+    #define _LRF_DIFF_DATA_UNCHANGE_MAX_COUNT    11
+
+    signed int    _nCurrDiff = 0;
+    signed int    _nSumDiff  = 0;
+    s16           _nCurrData = naData[nAxis];
+
+    _nCurrDiff = (_nCurrData - s_taLRF_CB[nAxis].nRepValue);
+
+    if ((_LRF_DIFF_COUNT_NEG < _nCurrDiff) && (_nCurrDiff < _LRF_DIFF_COUNT_POS))
+    {
+        if (s_taLRF_CB[nAxis].nIsNewRound)
+        {
+            s_taLRF_CB[nAxis].nMaxValue = _nCurrData;
+            s_taLRF_CB[nAxis].nMinValue = _nCurrData;
+
+            s_taLRF_CB[nAxis].nIsNewRound = 0;
+            s_taLRF_CB[nAxis].nNewDataMonitorCount = 0;
+        }
+        else
+        {
+            if (_nCurrData > s_taLRF_CB[nAxis].nMaxValue)
+                s_taLRF_CB[nAxis].nMaxValue = _nCurrData;
+            else if (_nCurrData < s_taLRF_CB[nAxis].nMinValue)
+                s_taLRF_CB[nAxis].nMinValue = _nCurrData;
+
+            if (s_taLRF_CB[nAxis].nMinValue != s_taLRF_CB[nAxis].nMaxValue)
+            {
+                if (_nCurrData == s_taLRF_CB[nAxis].nPreValue)
+                    s_taLRF_CB[nAxis].nNewDataMonitorCount++;
+                else
+                    s_taLRF_CB[nAxis].nNewDataMonitorCount = 0;
+            }
+        }
+
+        if (1 != (s_taLRF_CB[nAxis].nMaxValue - s_taLRF_CB[nAxis].nMinValue))
+            s_taLRF_CB[nAxis].nRepValue = ((s_taLRF_CB[nAxis].nMaxValue + s_taLRF_CB[nAxis].nMinValue) / 2);
+
+        _nSumDiff = (_nCurrDiff + s_taLRF_CB[nAxis].nPreDiff);
+
+        if (_nCurrDiff)
+            s_taLRF_CB[nAxis].nPreDiff = _nCurrDiff;
+
+        if ((_LRF_DIFF_BOUNDARY_NEG < _nSumDiff) && (_nSumDiff < _LRF_DIFF_BOUNDARY_POS))
+        {
+            if (_LRF_DIFF_DATA_UNCHANGE_MAX_COUNT > s_taLRF_CB[nAxis].nNewDataMonitorCount)
+            {
+                naData[nAxis] = s_taLRF_CB[nAxis].nRepValue;
+                goto _LRF_RETURN;
+            }
+        }
+    }
+
+    s_taLRF_CB[nAxis].nRepValue   = _nCurrData;
+    s_taLRF_CB[nAxis].nPreDiff    = 0;
+    s_taLRF_CB[nAxis].nIsNewRound = 1;
+
+_LRF_RETURN:
+
+    GSE_LOG(">>>>> [_MC3XXX_LowResFilter][%d] _nCurrDiff: %4d       _nSumDiff: %4d         _nCurrData:    %4d         Rep:    %4d\n", nAxis, _nCurrDiff, _nSumDiff, _nCurrData, s_taLRF_CB[nAxis].nRepValue);
+
+    s_taLRF_CB[nAxis].nPreValue = _nCurrData;
+
+    #undef _LRF_DIFF_COUNT_POS
+    #undef _LRF_DIFF_COUNT_NEG
+    #undef _LRF_DIFF_BOUNDARY_POS
+    #undef _LRF_DIFF_BOUNDARY_NEG
+    #undef _LRF_DIFF_DATA_UNCHANGE_MAX_COUNT
+}
+#endif    // END OF #ifdef _MC3XXX_SUPPORT_LRF_
+
+/*****************************************
+ *** _MC3XXX_ReadData_RBM2RAW
+ *****************************************/
+static void    _MC3XXX_ReadData_RBM2RAW(s16 waData[MC3XXX_AXES_NUM])
+{
+    struct mc3xxx_i2c_data   *_pt_i2c_obj = mc3xxx_obj_i2c_data;
+	
+    waData[MC3XXX_AXIS_X] = (waData[MC3XXX_AXIS_X] + offset_data[MC3XXX_AXIS_X] / 2) * 1024 / gain_data[MC3XXX_AXIS_X] + 8096;
+    waData[MC3XXX_AXIS_Y] = (waData[MC3XXX_AXIS_Y] + offset_data[MC3XXX_AXIS_Y] / 2) * 1024 / gain_data[MC3XXX_AXIS_Y] + 8096;
+    waData[MC3XXX_AXIS_Z] = (waData[MC3XXX_AXIS_Z] + offset_data[MC3XXX_AXIS_Z] / 2) * 1024 / gain_data[MC3XXX_AXIS_Z] + 8096;
+    if(atomic_read(&_pt_i2c_obj->trace) & MCUBE_TRC_INFO) {
+        GSE_LOG("RBM->RAW <<<<<[%08d %08d %08d]\n", waData[MC3XXX_AXIS_X], waData[MC3XXX_AXIS_Y], waData[MC3XXX_AXIS_Z]);
+    }
+    iAReal0_X             = (0x0010 * waData[MC3XXX_AXIS_X]);
+    iAcc1Lpf0_X           = GetLowPassFilter(iAReal0_X,iAcc1Lpf1_X);
+    iAcc0Lpf0_X           = GetLowPassFilter(iAcc1Lpf0_X,iAcc0Lpf1_X);
+    waData[MC3XXX_AXIS_X] = (iAcc0Lpf0_X / 0x0010);
+
+    iAReal0_Y             = (0x0010 * waData[MC3XXX_AXIS_Y]);
+    iAcc1Lpf0_Y           = GetLowPassFilter(iAReal0_Y,iAcc1Lpf1_Y);
+    iAcc0Lpf0_Y           = GetLowPassFilter(iAcc1Lpf0_Y,iAcc0Lpf1_Y);
+    waData[MC3XXX_AXIS_Y] = (iAcc0Lpf0_Y / 0x0010);
+
+    iAReal0_Z             = (0x0010 * waData[MC3XXX_AXIS_Z]);
+    iAcc1Lpf0_Z           = GetLowPassFilter(iAReal0_Z,iAcc1Lpf1_Z);
+    iAcc0Lpf0_Z           = GetLowPassFilter(iAcc1Lpf0_Z,iAcc0Lpf1_Z);
+    waData[MC3XXX_AXIS_Z] = (iAcc0Lpf0_Z / 0x0010);
+    if(atomic_read(&_pt_i2c_obj->trace) & MCUBE_TRC_INFO) {
+        GSE_LOG("RBM->RAW->LPF <<<<<[%08d %08d %08d]\n", waData[MC3XXX_AXIS_X], waData[MC3XXX_AXIS_Y], waData[MC3XXX_AXIS_Z]);
+    }
+    waData[MC3XXX_AXIS_X] = (waData[MC3XXX_AXIS_X] - 8096) * gsensor_gain.x / 1024;
+    waData[MC3XXX_AXIS_Y] = (waData[MC3XXX_AXIS_Y] - 8096) * gsensor_gain.y / 1024;
+    waData[MC3XXX_AXIS_Z] = (waData[MC3XXX_AXIS_Z] - 8096) * gsensor_gain.z / 1024;
+    if(atomic_read(&_pt_i2c_obj->trace) & MCUBE_TRC_INFO) {
+        GSE_LOG("RBM->RAW->LPF->RAW <<<<<[%08d %08d %08d]\n", waData[MC3XXX_AXIS_X], waData[MC3XXX_AXIS_Y], waData[MC3XXX_AXIS_Z]);
+    }
+    iAcc0Lpf1_X=iAcc0Lpf0_X;
+    iAcc1Lpf1_X=iAcc1Lpf0_X;
+    iAcc0Lpf1_Y=iAcc0Lpf0_Y;
+    iAcc1Lpf1_Y=iAcc1Lpf0_Y;
+    iAcc0Lpf1_Z=iAcc0Lpf0_Z;
+    iAcc1Lpf1_Z=iAcc1Lpf0_Z;
+}
+
+/*****************************************
+ *** MC3XXX_ReadData
+ *****************************************/
+static int    MC3XXX_ReadData(struct i2c_client *pt_i2c_client, s16 waData[MC3XXX_AXES_NUM])
+{
+    u8    _baData[MC3XXX_DATA_LEN] = { 0 };
+    struct mc3xxx_i2c_data   *_pt_i2c_obj = ((struct mc3xxx_i2c_data*) i2c_get_clientdata(pt_i2c_client));
+	
+	if(atomic_read(&_pt_i2c_obj->trace) & MCUBE_TRC_INFO) {
+        GSE_LOG("[%s] s_nIsRBM_Enabled: %d\n", __FUNCTION__, s_nIsRBM_Enabled);
+	}
+    if (NULL == pt_i2c_client)
+    {
+        GSE_ERR("ERR: Null Pointer\n");
+
+        return (MC3XXX_RETCODE_ERROR_NULL_POINTER);
+    }
+
+    if (!s_nIsRBM_Enabled)
+    {
+        if (MC3XXX_RESOLUTION_LOW == s_bResolution)
+        {
+            if (MC3XXX_i2c_read_block(pt_i2c_client, MC3XXX_REG_XOUT, _baData, MC3XXX_LOW_REOLUTION_DATA_SIZE))
+            {
+                GSE_ERR("ERR: fail to read data via I2C!\n");
+
+                return (MC3XXX_RETCODE_ERROR_I2C);
+            }
+
+            waData[MC3XXX_AXIS_X] = ((s8) _baData[0]);
+            waData[MC3XXX_AXIS_Y] = ((s8) _baData[1]);
+            waData[MC3XXX_AXIS_Z] = ((s8) _baData[2]);
+            if(atomic_read(&_pt_i2c_obj->trace) & MCUBE_TRC_INFO) {
+                GSE_LOG("[%s][low] X: %d, Y: %d, Z: %d\n", __FUNCTION__, waData[MC3XXX_AXIS_X], waData[MC3XXX_AXIS_Y], waData[MC3XXX_AXIS_Z]);
+            }
+
+            #ifdef _MC3XXX_SUPPORT_LRF_
+                _MC3XXX_LowResFilter(MC3XXX_AXIS_X, waData);
+                _MC3XXX_LowResFilter(MC3XXX_AXIS_Y, waData);
+                _MC3XXX_LowResFilter(MC3XXX_AXIS_Z, waData);
+            #endif
+        }
+        else if (MC3XXX_RESOLUTION_HIGH == s_bResolution)
+        {
+            if (MC3XXX_i2c_read_block(pt_i2c_client, MC3XXX_REG_XOUT_EX_L, _baData, MC3XXX_HIGH_REOLUTION_DATA_SIZE))
+            {
+                GSE_ERR("ERR: fail to read data via I2C!\n");
+
+                return (MC3XXX_RETCODE_ERROR_I2C);
+            }
+
+            waData[MC3XXX_AXIS_X] = ((signed short) ((_baData[0]) | (_baData[1]<<8)));
+            waData[MC3XXX_AXIS_Y] = ((signed short) ((_baData[2]) | (_baData[3]<<8)));
+            waData[MC3XXX_AXIS_Z] = ((signed short) ((_baData[4]) | (_baData[5]<<8)));
+                if(atomic_read(&_pt_i2c_obj->trace) & MCUBE_TRC_INFO) {
+                    GSE_LOG("[%s][high] X: %d, Y: %d, Z: %d\n", __FUNCTION__, waData[MC3XXX_AXIS_X], waData[MC3XXX_AXIS_Y], waData[MC3XXX_AXIS_Z]);
+                }
+			}
+        if(atomic_read(&_pt_i2c_obj->trace) & MCUBE_TRC_INFO) {
+            GSE_LOG("RAW<<<<<[%04d %04d %04d]\n", waData[MC3XXX_AXIS_X], waData[MC3XXX_AXIS_Y], waData[MC3XXX_AXIS_Z]);
+        }
+        #ifdef _MC3XXX_SUPPORT_LPF_
+        {
+            struct mc3xxx_i2c_data   *_ptPrivData = i2c_get_clientdata(pt_i2c_client);
+
+            MC3XXX_LPF(_ptPrivData, waData);
+			if(atomic_read(&_pt_i2c_obj->trace) & MCUBE_TRC_INFO) {
+                GSE_LOG("LPF<<<<<[%04d %04d %04d]\n", waData[MC3XXX_AXIS_X], waData[MC3XXX_AXIS_Y], waData[MC3XXX_AXIS_Z]);
+			}
+		}
+        #endif
+    }
+    else
+    {
+        if (MC3XXX_i2c_read_block(pt_i2c_client, MC3XXX_REG_XOUT_EX_L, _baData, MC3XXX_HIGH_REOLUTION_DATA_SIZE))
+        {
+            GSE_ERR("ERR: fail to read data via I2C!\n");
+
+            return (MC3XXX_RETCODE_ERROR_I2C);
+        }
+
+        waData[MC3XXX_AXIS_X] = ((s16)((_baData[0]) | (_baData[1] << 8)));
+        waData[MC3XXX_AXIS_Y] = ((s16)((_baData[2]) | (_baData[3] << 8)));
+        waData[MC3XXX_AXIS_Z] = ((s16)((_baData[4]) | (_baData[5] << 8)));
+        if(atomic_read(&_pt_i2c_obj->trace) & MCUBE_TRC_INFO) {
+             GSE_LOG("RBM<<<<<[%08d %08d %08d]\n", waData[MC3XXX_AXIS_X], waData[MC3XXX_AXIS_Y], waData[MC3XXX_AXIS_Z]);
+        }
+        _MC3XXX_ReadData_RBM2RAW(waData);
+    }
+
+    MCUBE_RREMAP(waData[MC3XXX_AXIS_X], waData[MC3XXX_AXIS_Y]);
+
+    return (MC3XXX_RETCODE_SUCCESS);
+}
+
+/*****************************************
+ *** MC3XXX_ReadOffset
+ *****************************************/
+static int MC3XXX_ReadOffset(struct i2c_client *client, s16 ofs[MC3XXX_AXES_NUM])
+{
+    int err=0;
+    u8 off_data[6]={0};
+
+
+    if (MC3XXX_RESOLUTION_HIGH == s_bResolution)
+    {
+        if ((err = MC3XXX_i2c_read_block(client, MC3XXX_REG_XOUT_EX_L, off_data, MC3XXX_DATA_LEN)))
+        {
+            GSE_ERR("error: %d\n", err);
+            return err;
+        }
+        ofs[MC3XXX_AXIS_X] = ((s16)(off_data[0]))|((s16)(off_data[1])<<8);
+        ofs[MC3XXX_AXIS_Y] = ((s16)(off_data[2]))|((s16)(off_data[3])<<8);
+        ofs[MC3XXX_AXIS_Z] = ((s16)(off_data[4]))|((s16)(off_data[5])<<8);
+    }
+    else if (MC3XXX_RESOLUTION_LOW == s_bResolution)
+    {
+        if ((err = MC3XXX_i2c_read_block(client, 0, off_data, 3)))
+        {
+            GSE_ERR("error: %d\n", err);
+            return err;
+        }
+        ofs[MC3XXX_AXIS_X] = (s8)off_data[0];
+        ofs[MC3XXX_AXIS_Y] = (s8)off_data[1];
+        ofs[MC3XXX_AXIS_Z] = (s8)off_data[2];
+    }
+
+    GSE_LOG("MC3XXX_ReadOffset %d %d %d \n",ofs[MC3XXX_AXIS_X] ,ofs[MC3XXX_AXIS_Y],ofs[MC3XXX_AXIS_Z]);
+
+    MCUBE_RREMAP(ofs[0], ofs[1]);
+
+    return err;
+}
+
+/*****************************************
+ *** MC3XXX_ResetCalibration
+ *****************************************/
+static int MC3XXX_ResetCalibration(struct i2c_client *client)
+{
+    struct mc3xxx_i2c_data *obj = i2c_get_clientdata(client);
+    u8 buf[MC3XXX_AXES_NUM] = {0x00, 0x00, 0x00};
+    s16 tmp=0;
+    int err=0;
+
+    u8  bMsbFilter       = 0x3F;
+    s16 wSignBitMask     = 0x2000;
+    s16 wSignPaddingBits = 0xC000;
+
+    buf[0] = 0x43;
+
+    if((err = MC3XXX_i2c_write_block(client, 0x07, buf, 1)))
+    {
+        GSE_ERR("error 0x07: %d\n", err);
+    }
+
+
+    if((err = MC3XXX_i2c_write_block(client, 0x21, offset_buf, 6))) // add by liang for writing offset register as OTP value
+    {
+        GSE_ERR("error: %d\n", err);
+    }
+
+    buf[0] = 0x41;
+
+    if((err = MC3XXX_i2c_write_block(client, 0x07, buf, 1)))
+    {
+        GSE_ERR("error: %d\n", err);
+    }
+
+    msleep(20);
+
+    if (IS_MCFM12() || IS_MCFM3X())
+    {
+        bMsbFilter       = 0x7F;
+        wSignBitMask     = 0x4000;
+        wSignPaddingBits = 0x8000;
+    }
+
+    tmp = ((offset_buf[1] & bMsbFilter) << 8) + offset_buf[0];  // add by Liang for set offset_buf as OTP value
+    if (tmp & wSignBitMask)
+        tmp |= wSignPaddingBits;
+    offset_data[0] = tmp;
+
+    tmp = ((offset_buf[3] & bMsbFilter) << 8) + offset_buf[2];  // add by Liang for set offset_buf as OTP value
+        if (tmp & wSignBitMask)
+            tmp |= wSignPaddingBits;
+    offset_data[1] = tmp;
+
+    tmp = ((offset_buf[5] & bMsbFilter) << 8) + offset_buf[4];  // add by Liang for set offset_buf as OTP value
+    if (tmp & wSignBitMask)
+        tmp |= wSignPaddingBits;
+    offset_data[2] = tmp;
+
+    memset(obj->cali_sw, 0x00, sizeof(obj->cali_sw));
+
+    return err;
+}
+
+/*****************************************
+ *** MC3XXX_ReadCalibration
+ *****************************************/
+static int MC3XXX_ReadCalibration(struct i2c_client *client, int dat[MC3XXX_AXES_NUM])
+{
+    struct mc3xxx_i2c_data *obj = i2c_get_clientdata(client);
+    int err =0;
+
+    if ((err = MC3XXX_ReadOffset(client, obj->offset))) {
+        GSE_ERR("read offset fail, %d\n", err);
+        return err;
+    }
+
+    dat[MC3XXX_AXIS_X] = obj->offset[MC3XXX_AXIS_X];
+    dat[MC3XXX_AXIS_Y] = obj->offset[MC3XXX_AXIS_Y];
+    dat[MC3XXX_AXIS_Z] = obj->offset[MC3XXX_AXIS_Z];
+
+    GSE_LOG("MC3XXX_ReadCalibration %d %d %d \n",dat[obj->cvt.map[MC3XXX_AXIS_X]] ,dat[obj->cvt.map[MC3XXX_AXIS_Y]],dat[obj->cvt.map[MC3XXX_AXIS_Z]]);
+
+    return 0;
+}
+
+/*****************************************
+ *** MC3XXX_WriteCalibration
+ *****************************************/
+static int MC3XXX_WriteCalibration(struct i2c_client *client, int dat[MC3XXX_AXES_NUM])
+{
+    struct mc3xxx_i2c_data *obj = i2c_get_clientdata(client);
+    int err = 0;
+    u8 buf[9] ={ 0};
+    s16 tmp = 0, x_gain = 0, y_gain = 0, z_gain = 0;
+    s32 x_off = 0, y_off = 0, z_off = 0;
+    int cali[MC3XXX_AXES_NUM] = {0};
+
+    u8  bMsbFilter       = 0x3F;
+    s16 wSignBitMask     = 0x2000;
+    s16 wSignPaddingBits = 0xC000;
+    s32 dwRangePosLimit  = 0x1FFF;
+    s32 dwRangeNegLimit  = -0x2000;
+
+    GSE_LOG("UPDATE dat: (%+3d %+3d %+3d)\n", dat[MC3XXX_AXIS_X], dat[MC3XXX_AXIS_Y], dat[MC3XXX_AXIS_Z]);
+
+    cali[MC3XXX_AXIS_X] = obj->cvt.sign[MC3XXX_AXIS_X]*(dat[obj->cvt.map[MC3XXX_AXIS_X]]);
+    cali[MC3XXX_AXIS_Y] = obj->cvt.sign[MC3XXX_AXIS_Y]*(dat[obj->cvt.map[MC3XXX_AXIS_Y]]);
+    cali[MC3XXX_AXIS_Z] = obj->cvt.sign[MC3XXX_AXIS_Z]*(dat[obj->cvt.map[MC3XXX_AXIS_Z]]);
+
+    MCUBE_WREMAP(cali[MC3XXX_AXIS_X], cali[MC3XXX_AXIS_Y]);
+
+    GSE_LOG("UPDATE dat: (%+3d %+3d %+3d)\n", cali[MC3XXX_AXIS_X], cali[MC3XXX_AXIS_Y], cali[MC3XXX_AXIS_Z]);
+
+    // read registers 0x21~0x29
+    if ((err = MC3XXX_i2c_read_block(client, 0x21, buf, 3)))
+    {
+        GSE_ERR("error: %d\n", err);
+        return err;
+    }
+    if ((err = MC3XXX_i2c_read_block(client, 0x24, &buf[3], 3)))
+    {
+        GSE_ERR("error: %d\n", err);
+        return err;
+    }
+    if ((err = MC3XXX_i2c_read_block(client, 0x27, &buf[6], 3)))
+    {
+        GSE_ERR("error: %d\n", err);
+        return err;
+    }
+
+    if (IS_MCFM12() || IS_MCFM3X())
+    {
+        bMsbFilter       = 0x7F;
+        wSignBitMask     = 0x4000;
+        wSignPaddingBits = 0x8000;
+        dwRangePosLimit  = 0x3FFF;
+        dwRangeNegLimit  = -0x4000;
+    }
+
+    // get x,y,z offset
+    tmp = ((buf[1] & bMsbFilter) << 8) + buf[0];
+    if (tmp & wSignBitMask)
+        tmp |= wSignPaddingBits;
+    x_off = tmp;
+
+    tmp = ((buf[3] & bMsbFilter) << 8) + buf[2];
+    if (tmp & wSignBitMask)
+        tmp |= wSignPaddingBits;
+    y_off = tmp;
+
+    tmp = ((buf[5] & bMsbFilter) << 8) + buf[4];
+    if (tmp & wSignBitMask)
+        tmp |= wSignPaddingBits;
+    z_off = tmp;
+
+    // get x,y,z gain
+    x_gain = ((buf[1] >> 7) << 8) + buf[6];
+    y_gain = ((buf[3] >> 7) << 8) + buf[7];
+    z_gain = ((buf[5] >> 7) << 8) + buf[8];
+
+    // prepare new offset
+    x_off = x_off + 16 * cali[MC3XXX_AXIS_X] * 256 * 128 / 3 / gsensor_gain.x / (40 + x_gain);
+    y_off = y_off + 16 * cali[MC3XXX_AXIS_Y] * 256 * 128 / 3 / gsensor_gain.y / (40 + y_gain);
+    z_off = z_off + 16 * cali[MC3XXX_AXIS_Z] * 256 * 128 / 3 / gsensor_gain.z / (40 + z_gain);
+
+    //add for over range
+    if( x_off > dwRangePosLimit)
+    {
+        x_off = dwRangePosLimit;
+    }
+    else if( x_off < dwRangeNegLimit)
+    {
+        x_off = dwRangeNegLimit;
+    }
+
+    if( y_off > dwRangePosLimit)
+    {
+        y_off = dwRangePosLimit;
+    }
+    else if( y_off < dwRangeNegLimit)
+    {
+        y_off = dwRangeNegLimit;
+    }
+
+    if( z_off > dwRangePosLimit)
+    {
+        z_off = dwRangePosLimit;
+    }
+    else if( z_off < dwRangeNegLimit)
+    {
+        z_off = dwRangeNegLimit;
+    }
+
+    //storege the cerrunt offset data with DOT format
+    offset_data[0] = x_off;
+    offset_data[1] = y_off;
+    offset_data[2] = z_off;
+
+    //storege the cerrunt Gain data with GOT format
+    gain_data[0] = 256*8*128/3/(40+x_gain);
+    gain_data[1] = 256*8*128/3/(40+y_gain);
+    gain_data[2] = 256*8*128/3/(40+z_gain);
+
+    buf[0]=0x43;
+    MC3XXX_i2c_write_block(client, 0x07, buf, 1);
+
+    buf[0] = x_off & 0xff;
+    buf[1] = ((x_off >> 8) & bMsbFilter) | (x_gain & 0x0100 ? 0x80 : 0);
+    buf[2] = y_off & 0xff;
+    buf[3] = ((y_off >> 8) & bMsbFilter) | (y_gain & 0x0100 ? 0x80 : 0);
+    buf[4] = z_off & 0xff;
+    buf[5] = ((z_off >> 8) & bMsbFilter) | (z_gain & 0x0100 ? 0x80 : 0);
+
+    MC3XXX_i2c_write_block(client, 0x21, buf, 6);
+
+    buf[0]=0x41;
+    MC3XXX_i2c_write_block(client, 0x07, buf, 1);
+
+    msleep(50);
+
+    return err;
+}
+
+/*****************************************
+ *** MC3XXX_SetPowerMode
+ *****************************************/
+static int MC3XXX_SetPowerMode(struct i2c_client *client, bool enable)
+{
+    u8 databuf[2] = {0};
+    int res = 0;
+    u8 addr = MC3XXX_REG_MODE_FEATURE;
+    struct mc3xxx_i2c_data *obj = i2c_get_clientdata(client);
+
+    if(enable == mc3xxx_sensor_power)
+        GSE_LOG("Sensor power status should not be set again!!!\n");
+
+    if(MC3XXX_i2c_read_block(client, addr, databuf,1))
+    {
+        GSE_ERR("read power ctl register err!\n");
+        return MC3XXX_RETCODE_ERROR_I2C;
+    }
+
+    GSE_LOG("set power read MC3XXX_REG_MODE_FEATURE =%x\n", databuf[0]);
+
+    if(enable)
+    {
+        databuf[0] = 0x41;
+        res = MC3XXX_i2c_write_block(client, MC3XXX_REG_MODE_FEATURE, databuf, 1);
+        #ifdef _MC3XXX_SUPPORT_DOT_CALIBRATION_
+            mcube_load_cali(client);
+        #endif
+    }
+    else
+    {
+        databuf[0] = 0x43;
+        res = MC3XXX_i2c_write_block(client, MC3XXX_REG_MODE_FEATURE, databuf, 1);
+    }
+
+    if(res < 0)
+    {
+        GSE_LOG("fwq set power mode failed!\n");
+        return MC3XXX_RETCODE_ERROR_I2C;
+    }
+    else if(atomic_read(&obj->trace) & MCUBE_TRC_INFO)
+    {
+        GSE_LOG("fwq set power mode ok %d!\n", databuf[1]);
+    }
+
+    mc3xxx_sensor_power = enable;
+
+    return MC3XXX_RETCODE_SUCCESS;
+}
+
+/*****************************************
+ *** MC3XXX_SetResolution
+ *****************************************/
+static void MC3XXX_SetResolution(void)
+{
+    GSE_LOG("[%s]\n", __FUNCTION__);
+
+    switch (s_bPCODE)
+    {
+    case MC3XXX_PCODE_3230:
+    case MC3XXX_PCODE_3430:
+    case MC3XXX_PCODE_3430N:
+    case MC3XXX_PCODE_3530:
+    case MC3XXX_PCODE_3236:
+         s_bResolution = MC3XXX_RESOLUTION_LOW;
+         break;
+
+    case MC3XXX_PCODE_3210:
+    case MC3XXX_PCODE_3250:
+    case MC3XXX_PCODE_3410:
+    case MC3XXX_PCODE_3410N:
+    case MC3XXX_PCODE_3510:
+    case MC3XXX_PCODE_3216:
+         s_bResolution = MC3XXX_RESOLUTION_HIGH;
+         break;
+
+    // === RESERVED ==================================BGN===
+    // === (move to normal section once it is confirmed) ===
+    case MC3XXX_PCODE_RESERVE_10:
+         GSE_ERR("RESERVED ONLINE!\n");
+         // TODO: should have a default configuration...
+         break;
+
+    case MC3XXX_PCODE_RESERVE_1:
+    case MC3XXX_PCODE_RESERVE_3:
+    case MC3XXX_PCODE_RESERVE_4:
+    case MC3XXX_PCODE_RESERVE_5:
+    case MC3XXX_PCODE_RESERVE_6:
+    case MC3XXX_PCODE_RESERVE_8:
+    case MC3XXX_PCODE_RESERVE_9:
+         GSE_ERR("RESERVED ONLINE!\n");
+         s_bResolution = MC3XXX_RESOLUTION_LOW;
+         break;
+
+    case MC3XXX_PCODE_RESERVE_2:
+    case MC3XXX_PCODE_RESERVE_7:
+         GSE_ERR("RESERVED ONLINE!\n");
+         s_bResolution = MC3XXX_RESOLUTION_HIGH;
+         break;
+    // === RESERVED ==================================END===
+
+    default:
+         GSE_ERR("ERR: no resolution assigned!\n");
+         break;
+    }
+
+    GSE_LOG("[%s] s_bResolution: %d\n", __FUNCTION__, s_bResolution);
+}
+
+/*****************************************
+ *** MC3XXX_SetSampleRate
+ *****************************************/
+static void MC3XXX_SetSampleRate(struct i2c_client *pt_i2c_client)
+{
+    unsigned char    _baDataBuf[2] = { 0 };
+
+    GSE_LOG("[%s]\n", __FUNCTION__);
+
+    _baDataBuf[0] = MC3XXX_REG_SAMPLE_RATE;
+    _baDataBuf[1] = 0x00;
+
+    if (IS_MCFM12() || IS_MCFM3X())
+    {
+        unsigned char    _baData2Buf[2] = { 0 };
+
+        _baData2Buf[0] = 0x2A;
+        MC3XXX_i2c_read_block(pt_i2c_client, 0x2A, _baData2Buf, 1);
+
+        GSE_LOG("[%s] REG(0x2A) = 0x%02X\n", __FUNCTION__, _baData2Buf[0]);
+
+        _baData2Buf[0] = (_baData2Buf[0] & 0xC0);
+
+        switch (_baData2Buf[0])
+        {
+        case 0x00:    _baDataBuf[0] = 0x00;                                                    break;
+        case 0x40:    _baDataBuf[0] = 0x08;                                                    break;
+        case 0x80:    _baDataBuf[0] = 0x09;                                                    break;
+        case 0xC0:    _baDataBuf[0] = 0x0A;                                                    break;
+
+        default:      GSE_ERR("[%s] no chance to get here... check code!\n", __FUNCTION__);    break;
+        }
+    }
+    else 
+        _baDataBuf[0] = 0x00;
+	
+    MC3XXX_i2c_write_block(pt_i2c_client, MC3XXX_REG_SAMPLE_RATE, _baDataBuf, 1);
+}
+
+/*****************************************
+ *** MC3XXX_ConfigRegRange
+ *****************************************/
+static void MC3XXX_ConfigRegRange(struct i2c_client *pt_i2c_client)
+{
+    unsigned char    _baDataBuf[2] = { 0 };
+    int res = 0;
+
+    //_baDataBuf[0] = 0x3F;
+    /* Modify low pass filter bandwith to 512hz, for solving sensor data don't change issue */
+	_baDataBuf[0] = 0x0F;
+
+    if (MC3XXX_RESOLUTION_LOW == s_bResolution)
+        _baDataBuf[0] = 0x32;
+
+    if (IS_MCFM12() || IS_MCFM3X())
+    {
+        if (MC3XXX_RESOLUTION_LOW == s_bResolution)
+            _baDataBuf[0] = 0x02;
+        else
+            _baDataBuf[0] = 0x25;
+    }
+    res = MC3XXX_i2c_write_block(pt_i2c_client, MC3XXX_REG_RANGE_CONTROL, _baDataBuf, 1);
+    if (res < 0)
+        GSE_ERR("MC3XXX_ConfigRegRange fail \n");
+
+    GSE_LOG("[%s] set 0x%X\n", __FUNCTION__, _baDataBuf[1]);
+}
+
+/*****************************************
+ *** MC3XXX_SetGain
+ *****************************************/
+static void MC3XXX_SetGain(void)
+{
+    gsensor_gain.x = gsensor_gain.y = gsensor_gain.z = 1024;
+
+    if (MC3XXX_RESOLUTION_LOW == s_bResolution)
+    {
+        gsensor_gain.x = gsensor_gain.y = gsensor_gain.z = 86;
+
+        if (IS_MCFM12() || IS_MCFM3X())
+        {
+            gsensor_gain.x = gsensor_gain.y = gsensor_gain.z = 64;
+        }
+    }
+
+    GSE_LOG("[%s] gain: %d / %d / %d\n", __FUNCTION__, gsensor_gain.x, gsensor_gain.y, gsensor_gain.z);
+}
+
+/*****************************************
+ *** MC3XXX_Init
+ *****************************************/
+static int MC3XXX_Init(struct i2c_client *client, int reset_cali)
+{
+    unsigned char    _baDataBuf[2] = { 0 };
+
+    GSE_LOG("[%s]\n", __FUNCTION__);
+
+    #ifdef _MC3XXX_SUPPORT_POWER_SAVING_SHUTDOWN_POWER_
+        if (MC3XXX_RETCODE_SUCCESS != _mc3xxx_i2c_auto_probe(client))
+        {
+            //GSE_ERR("ERR: fail to probe mCube sensor!\n");
+            return (MC3XXX_RETCODE_ERROR_I2C);
+        }
+
+        //GSE_LOG("[%s] confirmed i2c addr: 0x%X\n", __FUNCTION__, client->addr);
+    #endif
+
+    _baDataBuf[0] = 0x43;
+    MC3XXX_i2c_write_block(client, MC3XXX_REG_MODE_FEATURE, _baDataBuf, 1);
+
+    MC3XXX_SetResolution();
+    MC3XXX_SetSampleRate(client);
+    MC3XXX_ConfigRegRange(client);
+    MC3XXX_SetGain();
+
+    _baDataBuf[0] = 0x00;
+    MC3XXX_i2c_write_block(client, MC3XXX_REG_TAP_DETECTION_ENABLE, _baDataBuf, 1);
+
+    _baDataBuf[0] = 0x00;
+    MC3XXX_i2c_write_block(client, MC3XXX_REG_INTERRUPT_ENABLE, _baDataBuf, 1);
+
+    _baDataBuf[0] = 0;
+    MC3XXX_i2c_read_block(client, 0x2A, _baDataBuf, 1);
+    s_bMPOL = (_baDataBuf[0] & 0x03);
+
+    #ifdef _MC3XXX_SUPPORT_DOT_CALIBRATION_
+        MC3XXX_rbm(client,0);
+    #endif
+
+    #ifdef _MC3XXX_SUPPORT_LPF_
+    {
+        struct mc3xxx_i2c_data   *_pt_i2c_data = i2c_get_clientdata(client);
+
+        memset(&_pt_i2c_data->fir, 0x00, sizeof(_pt_i2c_data->fir));
+    }
+    #endif
+
+    #ifdef _MC3XXX_SUPPORT_LRF_
+        memset(&s_taLRF_CB, 0, sizeof(s_taLRF_CB));
+    #endif
+
+    #ifdef _MC3XXX_SUPPORT_PERIODIC_DOC_
+        init_waitqueue_head(&wq_mc3xxx_open_status);
+    #endif
+
+    GSE_LOG("[%s] init ok.\n", __FUNCTION__);
+
+    return (MC3XXX_RETCODE_SUCCESS);
+}
+
+/*****************************************
+ *** MC3XXX_ReadChipInfo
+ *****************************************/
+static int MC3XXX_ReadChipInfo(struct i2c_client *client, char *buf, int bufsize)
+{
+    if((NULL == buf)||(bufsize<=30))
+    {
+        return -1;
+    }
+
+    if(NULL == client)
+    {
+        *buf = 0;
+        return -2;
+    }
+
+    sprintf(buf, "MC3XXX Chip");
+    return 0;
+}
+
+/*****************************************
+ *** MC3XXX_ReadSensorData
+ *****************************************/
+static int MC3XXX_ReadSensorData(struct i2c_client *pt_i2c_client, char *pbBuf, int nBufSize)
+{
+    int                       _naAccelData[MC3XXX_AXES_NUM] = { 0 };
+    struct mc3xxx_i2c_data   *_pt_i2c_obj = ((struct mc3xxx_i2c_data*) i2c_get_clientdata(pt_i2c_client));
+
+    if ((NULL == pt_i2c_client) || (NULL == pbBuf))
+    {
+        GSE_ERR("ERR: Null Pointer \n");
+
+        return (MC3XXX_RETCODE_ERROR_NULL_POINTER);
+    }
+
+    if (false == mc3xxx_sensor_power)
+    {
+        if (MC3XXX_RETCODE_SUCCESS != MC3XXX_SetPowerMode(pt_i2c_client, true))
+            GSE_ERR("ERR: fail to set power mode!\n");
+    }
+
+    #ifdef _MC3XXX_SUPPORT_DOT_CALIBRATION_
+        mcube_load_cali(pt_i2c_client);
+
+        if ((s_nIsRBM_Enabled) && (1 == LPF_FirstRun))
+        {
+            int    _nLoopIndex = 0;
+
+            LPF_FirstRun = 0;
+
+            for (_nLoopIndex = 0; _nLoopIndex < (LPF_SamplingRate + LPF_CutoffFrequency); _nLoopIndex++)
+                MC3XXX_ReadData(pt_i2c_client, _pt_i2c_obj->data);
+        }
+    #endif
+
+    if (MC3XXX_RETCODE_SUCCESS != MC3XXX_ReadData(pt_i2c_client, _pt_i2c_obj->data))
+    {
+        GSE_ERR("ERR: fail to read data!\n");
+
+        return (MC3XXX_RETCODE_ERROR_I2C);
+    }
+
+    //output format: mg
+    if(atomic_read(&_pt_i2c_obj->trace) & MCUBE_TRC_INFO) {
+        GSE_LOG("[%s] raw data: %d, %d, %d\n", __FUNCTION__, _pt_i2c_obj->data[MC3XXX_AXIS_X], _pt_i2c_obj->data[MC3XXX_AXIS_Y], _pt_i2c_obj->data[MC3XXX_AXIS_Z]);
+    }
+	_naAccelData[(_pt_i2c_obj->cvt.map[MC3XXX_AXIS_X])] = (_pt_i2c_obj->cvt.sign[MC3XXX_AXIS_X] * _pt_i2c_obj->data[MC3XXX_AXIS_X]);
+    _naAccelData[(_pt_i2c_obj->cvt.map[MC3XXX_AXIS_Y])] = (_pt_i2c_obj->cvt.sign[MC3XXX_AXIS_Y] * _pt_i2c_obj->data[MC3XXX_AXIS_Y]);
+    _naAccelData[(_pt_i2c_obj->cvt.map[MC3XXX_AXIS_Z])] = (_pt_i2c_obj->cvt.sign[MC3XXX_AXIS_Z] * _pt_i2c_obj->data[MC3XXX_AXIS_Z]);
+
+	if(atomic_read(&_pt_i2c_obj->trace) & MCUBE_TRC_INFO) {
+        GSE_LOG("[%s] map data: %d, %d, %d!\n", __FUNCTION__, _naAccelData[MC3XXX_AXIS_X], _naAccelData[MC3XXX_AXIS_Y], _naAccelData[MC3XXX_AXIS_Z]);
+	}
+	_naAccelData[MC3XXX_AXIS_X] = (_naAccelData[MC3XXX_AXIS_X] * GRAVITY_EARTH_1000 / gsensor_gain.x);
+    _naAccelData[MC3XXX_AXIS_Y] = (_naAccelData[MC3XXX_AXIS_Y] * GRAVITY_EARTH_1000 / gsensor_gain.y);
+    _naAccelData[MC3XXX_AXIS_Z] = (_naAccelData[MC3XXX_AXIS_Z] * GRAVITY_EARTH_1000 / gsensor_gain.z);
+
+	if(atomic_read(&_pt_i2c_obj->trace) & MCUBE_TRC_INFO) {
+	    GSE_LOG("[%s] accel data: %d, %d, %d!\n", __FUNCTION__, _naAccelData[MC3XXX_AXIS_X], _naAccelData[MC3XXX_AXIS_Y], _naAccelData[MC3XXX_AXIS_Z]);
+	}
+    sprintf(pbBuf, "%04x %04x %04x", _naAccelData[MC3XXX_AXIS_X], _naAccelData[MC3XXX_AXIS_Y], _naAccelData[MC3XXX_AXIS_Z]);
+
+    return (MC3XXX_RETCODE_SUCCESS);
+}
+
+/*****************************************
+ *** _MC3XXX_ReadAverageData
+ *****************************************/
+#ifdef _MC3XXX_SUPPORT_APPLY_AVERAGE_AGORITHM_
+static int _MC3XXX_ReadAverageData(struct i2c_client *client, char *buf)
+{
+    struct mc3xxx_i2c_data *obj = (struct mc3xxx_i2c_data*)i2c_get_clientdata(client);
+    int acc[MC3XXX_AXES_NUM]={0};
+    s16 sensor_data[3]={0};
+    s16 sensor_data_max[3]={0};
+    s16 sensor_data_mini[3]={0};
+    s32 sensor_data_sum[3]={0};
+
+    int i = 0,j=0;
+
+    MC3XXX_ReadData(client, sensor_data);
+    GSE_LOG("MC3XXX_ReadRawData MC3XXX_ReadData: %d, %d, %d!\n", sensor_data[MC3XXX_AXIS_X], sensor_data[MC3XXX_AXIS_Y], sensor_data[MC3XXX_AXIS_Z]);
+    sensor_data_max[MC3XXX_AXIS_X]=sensor_data[MC3XXX_AXIS_X];
+    sensor_data_max[MC3XXX_AXIS_Y]=sensor_data[MC3XXX_AXIS_Y];
+    sensor_data_max[MC3XXX_AXIS_Z]=sensor_data[MC3XXX_AXIS_Z];
+
+    sensor_data_mini[MC3XXX_AXIS_X]=sensor_data[MC3XXX_AXIS_X];
+    sensor_data_mini[MC3XXX_AXIS_Y]=sensor_data[MC3XXX_AXIS_Y];
+    sensor_data_mini[MC3XXX_AXIS_Z]=sensor_data[MC3XXX_AXIS_Z];
+
+    sensor_data_sum[MC3XXX_AXIS_X]+=sensor_data[MC3XXX_AXIS_X];
+    sensor_data_sum[MC3XXX_AXIS_Y]+=sensor_data[MC3XXX_AXIS_Y];
+    sensor_data_sum[MC3XXX_AXIS_Z]+=sensor_data[MC3XXX_AXIS_Z];
+
+    for(i=0; i<11 ; i++)
+    {
+        MC3XXX_ReadData(client, sensor_data);
+        GSE_LOG("MC3XXX_ReadRawData MC3XXX_ReadData: %d, %d, %d!\n", sensor_data[MC3XXX_AXIS_X], sensor_data[MC3XXX_AXIS_Y], sensor_data[MC3XXX_AXIS_Z]);
+
+        sensor_data_sum[MC3XXX_AXIS_X]+=sensor_data[MC3XXX_AXIS_X];
+        sensor_data_sum[MC3XXX_AXIS_Y]+=sensor_data[MC3XXX_AXIS_Y];
+        sensor_data_sum[MC3XXX_AXIS_Z]+=sensor_data[MC3XXX_AXIS_Z];
+        for(j=0; j<3 ; j++)
+        {
+            if(sensor_data[j]>sensor_data_max[j])
+            {
+                sensor_data_max[j]=sensor_data[j];
+            }
+            if(sensor_data[j]<sensor_data_mini[j])
+            {
+                sensor_data_mini[j]=sensor_data[j];
+            }
+        }
+    }
+    GSE_LOG("MC3XXX_ReadRawData sensor_data_max: %d, %d, %d!\n", sensor_data_max[MC3XXX_AXIS_X], sensor_data_max[MC3XXX_AXIS_Y], sensor_data_max[MC3XXX_AXIS_Z]);
+    GSE_LOG("MC3XXX_ReadRawData sensor_data_mini: %d, %d, %d!\n", sensor_data_mini[MC3XXX_AXIS_X], sensor_data_mini[MC3XXX_AXIS_Y], sensor_data_mini[MC3XXX_AXIS_Z]);
+    sensor_data[MC3XXX_AXIS_X] = (s16)((sensor_data_sum[MC3XXX_AXIS_X]-sensor_data_max[MC3XXX_AXIS_X]-sensor_data_mini[MC3XXX_AXIS_X])/10);
+    sensor_data[MC3XXX_AXIS_Y] = (s16)((sensor_data_sum[MC3XXX_AXIS_Y]-sensor_data_max[MC3XXX_AXIS_Y]-sensor_data_mini[MC3XXX_AXIS_Y])/10);
+    sensor_data[MC3XXX_AXIS_Z] = (s16)((sensor_data_sum[MC3XXX_AXIS_Z]-sensor_data_max[MC3XXX_AXIS_Z]-sensor_data_mini[MC3XXX_AXIS_Z])/10);
+    GSE_LOG("MC3XXX_ReadRawData sensor_data: %d, %d, %d!\n", sensor_data[MC3XXX_AXIS_X], sensor_data[MC3XXX_AXIS_Y], sensor_data[MC3XXX_AXIS_Z]);
+
+    acc[(obj->cvt.map[MC3XXX_AXIS_X])] = obj->cvt.sign[MC3XXX_AXIS_X] * sensor_data[MC3XXX_AXIS_X];
+    acc[(obj->cvt.map[MC3XXX_AXIS_Y])] = obj->cvt.sign[MC3XXX_AXIS_Y] * sensor_data[MC3XXX_AXIS_Y];
+    acc[(obj->cvt.map[MC3XXX_AXIS_Z])] = obj->cvt.sign[MC3XXX_AXIS_Z] * sensor_data[MC3XXX_AXIS_Z];
+
+    GSE_LOG("MC3XXX_ReadRawData mapdata: %d, %d, %d!\n", acc[MC3XXX_AXIS_X], acc[MC3XXX_AXIS_Y], acc[MC3XXX_AXIS_Z]);
+
+    acc[MC3XXX_AXIS_X] = (acc[MC3XXX_AXIS_X]*GRAVITY_EARTH_1000/gsensor_gain.x);
+    acc[MC3XXX_AXIS_Y] = (acc[MC3XXX_AXIS_Y]*GRAVITY_EARTH_1000/gsensor_gain.y);
+    acc[MC3XXX_AXIS_Z] = (acc[MC3XXX_AXIS_Z]*GRAVITY_EARTH_1000/gsensor_gain.z);
+
+    GSE_LOG("MC3XXX_ReadRawData mapdata1: %d, %d, %d!\n", acc[MC3XXX_AXIS_X], acc[MC3XXX_AXIS_Y], acc[MC3XXX_AXIS_Z]);
+
+    sprintf(buf, "%04x %04x %04x", acc[MC3XXX_AXIS_X], acc[MC3XXX_AXIS_Y], acc[MC3XXX_AXIS_Z]);
+
+    return 0;
+}
+#endif    // END OF #ifdef _MC3XXX_SUPPORT_APPLY_AVERAGE_AGORITHM_
+
+/*****************************************
+ *** MC3XXX_ReadRawData
+ *****************************************/
+static int MC3XXX_ReadRawData(struct i2c_client *client, char *buf)
+{
+    int res = 0;
+
+    if (!buf || !client)
+    {
+        return EINVAL;
+    }
+
+    if(mc3xxx_sensor_power == false)
+    {
+        res = MC3XXX_SetPowerMode(client, true);
+        if(res)
+        {
+            GSE_ERR("Power on mc3xxx error %d!\n", res);
+        }
+    }
+
+    #ifdef _MC3XXX_SUPPORT_APPLY_AVERAGE_AGORITHM_
+        return (_MC3XXX_ReadAverageData(client, buf));
+    #else
+    {
+        s16 sensor_data[3]={0};
+
+        if((res = MC3XXX_ReadData(client, sensor_data)))
+        {
+            GSE_ERR("I2C error: ret value=%d", res);
+            return EIO;
+        }
+        else
+        {
+            sprintf(buf, "%04x %04x %04x", sensor_data[MC3XXX_AXIS_X],
+            sensor_data[MC3XXX_AXIS_Y], sensor_data[MC3XXX_AXIS_Z]);
+        }
+    }
+    #endif
+
+    return 0;
+}
+
+#ifdef _MC3XXX_SUPPORT_DOT_CALIBRATION_
+/*****************************************
+ *** MC3XXX_ReadRBMData
+ *****************************************/
+static int MC3XXX_ReadRBMData(struct i2c_client *client, char *buf)
+{
+    int res = 0;
+    int data[3] = { 0 };
+
+    if (!buf || !client)
+    {
+        return EINVAL;
+    }
+
+    if(mc3xxx_sensor_power == false)
+    {
+        res = MC3XXX_SetPowerMode(client, true);
+        if(res)
+        {
+            GSE_ERR("Power on mc3xxx error %d!\n", res);
+        }
+    }
+
+    if((res = MC3XXX_ReadData_RBM(client, data)))
+    {
+        GSE_ERR("I2C error: ret value=%d", res);
+        return EIO;
+    }
+    else
+    {
+        sprintf(buf, "%04x %04x %04x", data[MC3XXX_AXIS_X],
+            data[MC3XXX_AXIS_Y], data[MC3XXX_AXIS_Z]);
+
+    }
+
+    return res;
+}
+#endif // _MC3XXX_SUPPORT_DOT_CALIBRATION_
+
+/*****************************************
+ *** MC3XXX_JudgeTestResult
+ *****************************************/
+static int MC3XXX_JudgeTestResult(struct i2c_client *client)
+{
+    int    res                  = 0;
+    int    self_result          = 0;
+    s16    acc[MC3XXX_AXES_NUM] = { 0 };
+
+    if((res = MC3XXX_ReadData(client, acc)))
+    {
+        GSE_ERR("I2C error: ret value=%d", res);
+        return EIO;
+    }
+    else
+    {
+        acc[MC3XXX_AXIS_X] = acc[MC3XXX_AXIS_X] * 1000 / gsensor_gain.x;
+        acc[MC3XXX_AXIS_Y] = acc[MC3XXX_AXIS_Y] * 1000 / gsensor_gain.y;
+        acc[MC3XXX_AXIS_Z] = acc[MC3XXX_AXIS_Z] * 1000 / gsensor_gain.z;
+
+        self_result = (  (acc[MC3XXX_AXIS_X] * acc[MC3XXX_AXIS_X])
+                       + (acc[MC3XXX_AXIS_Y] * acc[MC3XXX_AXIS_Y])
+                       + (acc[MC3XXX_AXIS_Z] * acc[MC3XXX_AXIS_Z]));
+
+        if ( (self_result > 475923) && (self_result < 2185360) )    //between 0.7g and 1.5g
+        {
+            GSE_ERR("MC3XXX_JudgeTestResult successful\n");
+            return MC3XXX_RETCODE_SUCCESS;
+        }
+        else
+        {
+            GSE_ERR("MC3XXX_JudgeTestResult failt\n");
+            return -EINVAL;
+        }
+    }
+}
+
+/*****************************************
+ *** MC3XXX_SelfCheck
+ *****************************************/
+static void MC3XXX_SelfCheck(struct i2c_client *client, u8 *pUserBuf)
+{
+    u8    _bRData1 = 0;
+    u8    _bRData2 = 0;
+    u8    _bRData3 = 0;
+    u8    _baDataBuf[2] = { 0 };
+
+    MC3XXX_i2c_read_block(client, 0x20, &_bRData1, 1);
+    MC3XXX_i2c_read_block(client, 0x3B, &_bRData2, 1);
+
+    _baDataBuf[0] = 0x43;
+    MC3XXX_i2c_write_block(client, 0x07, _baDataBuf, 1);
+
+    mdelay(10);
+
+    for ( ; ; )
+    {
+        _baDataBuf[0] = 0x6D;
+        MC3XXX_i2c_write_block(client, 0x1B, _baDataBuf, 1);
+
+        _baDataBuf[0] = 0x43;
+        MC3XXX_i2c_write_block(client, 0x1B, _baDataBuf, 1);
+
+        _bRData3 = 0x00;
+        MC3XXX_i2c_read_block(client, 0x04, &_bRData3, 1);
+
+        if (_bRData3 & 0x40)
+            break;
+    }
+
+    _baDataBuf[0] = (_bRData2 & 0xFE);
+    MC3XXX_i2c_write_block(client, 0x3B, _baDataBuf, 1);
+
+    _baDataBuf[0] = 0x03;
+    MC3XXX_i2c_write_block(client, 0x20, _baDataBuf, 1);
+
+    _baDataBuf[0] = 0x40;
+    MC3XXX_i2c_write_block(client, 0x14, _baDataBuf, 1);
+
+    mdelay(10);
+
+    _baDataBuf[0] = pUserBuf[0];
+    MC3XXX_i2c_write_block(client, 0x00, _baDataBuf, 1);
+
+    _baDataBuf[0] = 0x41;
+    MC3XXX_i2c_write_block(client, 0x07, _baDataBuf, 1);
+
+    mdelay(10);
+
+    _baDataBuf[0] = 0x43;
+    MC3XXX_i2c_write_block(client, 0x07, _baDataBuf, 1);
+
+    mdelay(10);
+
+    MC3XXX_Read_Reg_Map(client, pUserBuf);
+
+    mdelay(10);
+
+    _baDataBuf[0] = 0x00;
+    MC3XXX_i2c_write_block(client, 0x14, _baDataBuf, 1);
+
+    _baDataBuf[0] = _bRData1;
+    MC3XXX_i2c_write_block(client, 0x20, _baDataBuf, 1);
+
+    _baDataBuf[0] = _bRData2;
+    MC3XXX_i2c_write_block(client, 0x3B, _baDataBuf, 1);
+
+    mdelay(10);
+
+    for ( ; ; )
+    {
+        _baDataBuf[0] = 0x6D;
+        MC3XXX_i2c_write_block(client, 0x1B, _baDataBuf, 1);
+
+        _baDataBuf[0] = 0x43;
+        MC3XXX_i2c_write_block(client, 0x1B, _baDataBuf, 1);
+
+        _bRData3 = 0xFF;
+        MC3XXX_i2c_read_block(client, 0x04, &_bRData3, 1);
+
+        if (!(_bRData3 & 0x40))
+            break;
+    }
+
+    mdelay(10);
+}
+
+/*****************************************
+ *** MC3XXX_GetOpenStatus
+ *****************************************/
+#ifdef _MC3XXX_SUPPORT_PERIODIC_DOC_
+static int    MC3XXX_GetOpenStatus(void)
+{
+    //GSE_LOG("[%s] %d\n", __FUNCTION__, atomic_read(&s_t_mc3xxx_open_status));
+
+    wait_event_interruptible(wq_mc3xxx_open_status, (atomic_read(&s_t_mc3xxx_open_status) != 0));
+
+    //GSE_LOG("[%s] pass wait_event_interruptible: %d\n", __FUNCTION__, atomic_read(&s_t_mc3xxx_open_status));
+
+    return (atomic_read(&s_t_mc3xxx_open_status));
+}
+#endif
+
+/*****************************************
+ *** show_chipinfo_value
+ *****************************************/
+static ssize_t show_chipinfo_value(struct device_driver *ddri, char *buf)
+{
+    struct i2c_client *client = mc3xxx_i2c_client;
+    char strbuf[MC3XXX_BUF_SIZE]={0};
+    GSE_LOG("fwq show_chipinfo_value \n");
+    if(NULL == client)
+    {
+        GSE_ERR("i2c client is null!!\n");
+        return 0;
+    }
+
+    MC3XXX_ReadChipInfo(client, strbuf, MC3XXX_BUF_SIZE);
+    return snprintf(buf, PAGE_SIZE, "%s\n", strbuf);
+}
+
+/*****************************************
+ *** show_sensordata_value
+ *****************************************/
+static ssize_t show_sensordata_value(struct device_driver *ddri, char *buf)
+{
+    struct i2c_client *client = mc3xxx_i2c_client;
+    char strbuf[MC3XXX_BUF_SIZE] = { 0 };
+
+    if(NULL == client)
+    {
+        GSE_ERR("i2c client is null!!\n");
+        return 0;
+    }
+
+    MC3XXX_MUTEX_LOCK();
+    MC3XXX_ReadSensorData(client, strbuf, MC3XXX_BUF_SIZE);
+    MC3XXX_MUTEX_UNLOCK();
+    return snprintf(buf, PAGE_SIZE, "%s\n", strbuf);
+}
+
+/*****************************************
+ *** show_cali_value
+ *****************************************/
+static ssize_t show_cali_value(struct device_driver *ddri, char *buf)
+{
+    struct i2c_client *client = mc3xxx_i2c_client;
+    struct mc3xxx_i2c_data *obj;
+    int err = 0;
+    int len = 0;
+    int mul = 0;
+    int tmp[MC3XXX_AXES_NUM] = { 0 };
+
+    GSE_LOG("fwq show_cali_value \n");
+
+    if(NULL == client)
+    {
+        GSE_ERR("i2c client is null!!\n");
+        return 0;
+    }
+
+    obj = (struct mc3xxx_i2c_data*) i2c_get_clientdata(client);
+
+
+    if((err = MC3XXX_ReadOffset(client, obj->offset)))
+    {
+        return -EINVAL;
+    }
+    else if((err = MC3XXX_ReadCalibration(client, tmp)))
+    {
+        return -EINVAL;
+    }
+    else
+    {
+// +++ 20130104 -- obj->reso is no longer used, replaced by gensor_gain to avoid system crash
+        //mul = obj->reso->sensitivity/mc3xxx_offset_resolution.sensitivity;
+        mul = gsensor_gain.x /mc3xxx_offset_resolution.sensitivity;
+        /*len += snprintf(buf+len, PAGE_SIZE-len, "[HW ][%d] (%+3d, %+3d, %+3d) : (0x%02X, 0x%02X, 0x%02X)\n", mul,
+            obj->offset[MC3XXX_AXIS_X], obj->offset[MC3XXX_AXIS_Y], obj->offset[MC3XXX_AXIS_Z],
+            obj->offset[MC3XXX_AXIS_X], obj->offset[MC3XXX_AXIS_Y], obj->offset[MC3XXX_AXIS_Z]);
+        */
+// +++ 20130104 -- obj->reso is no longer used, replaced by gensor_gain to avoid system crash
+        len += snprintf(buf+len, PAGE_SIZE-len, "[HW] (%+3d, %+3d, %+3d) : (0x%02X, 0x%02X, 0x%02X)\n",
+            obj->offset[MC3XXX_AXIS_X], obj->offset[MC3XXX_AXIS_Y], obj->offset[MC3XXX_AXIS_Z],
+            obj->offset[MC3XXX_AXIS_X], obj->offset[MC3XXX_AXIS_Y], obj->offset[MC3XXX_AXIS_Z]);
+
+        len += snprintf(buf+len, PAGE_SIZE-len, "[SW][%d] (%+3d, %+3d, %+3d)\n", 1,
+            obj->cali_sw[MC3XXX_AXIS_X], obj->cali_sw[MC3XXX_AXIS_Y], obj->cali_sw[MC3XXX_AXIS_Z]);
+
+        len += snprintf(buf+len, PAGE_SIZE-len, "[ALL]  (%+3d, %+3d, %+3d) : (%+3d, %+3d, %+3d)\n",
+            obj->offset[MC3XXX_AXIS_X]*mul + obj->cali_sw[MC3XXX_AXIS_X],
+            obj->offset[MC3XXX_AXIS_Y]*mul + obj->cali_sw[MC3XXX_AXIS_Y],
+            obj->offset[MC3XXX_AXIS_Z]*mul + obj->cali_sw[MC3XXX_AXIS_Z],
+            tmp[MC3XXX_AXIS_X], tmp[MC3XXX_AXIS_Y], tmp[MC3XXX_AXIS_Z]);
+
+        return len;
+    }
+}
+
+/*****************************************
+ *** store_cali_value
+ *****************************************/
+static ssize_t store_cali_value(struct device_driver *ddri, const char *buf, size_t count)
+{
+    struct i2c_client *client = mc3xxx_i2c_client;
+    int err=0;
+    int x=0;
+    int y=0;
+    int z=0;
+    int dat[MC3XXX_AXES_NUM]={0};
+
+    if(!strncmp(buf, "rst", 3))
+    {
+        MC3XXX_MUTEX_LOCK();
+        err = MC3XXX_ResetCalibration(client);
+        MC3XXX_MUTEX_UNLOCK();
+
+        if (err)
+            GSE_ERR("reset offset err = %d\n", err);
+    }
+    else if(3 == sscanf(buf, "0x%02X 0x%02X 0x%02X", &x, &y, &z))
+    {
+        dat[MC3XXX_AXIS_X] = x;
+        dat[MC3XXX_AXIS_Y] = y;
+        dat[MC3XXX_AXIS_Z] = z;
+
+        MC3XXX_MUTEX_LOCK();
+        err = MC3XXX_WriteCalibration(client, dat);
+        MC3XXX_MUTEX_UNLOCK();
+
+        if (err)
+            GSE_ERR("write calibration err = %d\n", err);
+    }
+    else
+    {
+        GSE_ERR("invalid format\n");
+    }
+
+    return count;
+}
+
+/*****************************************
+ *** show_selftest_value
+ *****************************************/
+static ssize_t show_selftest_value(struct device_driver *ddri, char *buf)
+{
+    struct i2c_client *client = mc3xxx_i2c_client;
+
+    if (NULL == client)
+    {
+        GSE_ERR("i2c client is null!!\n");
+        return 0;
+    }
+
+    return snprintf(buf, 8, "%s\n", selftestRes);
+}
+
+/*****************************************
+ *** store_selftest_value
+ *****************************************/
+static ssize_t store_selftest_value(struct device_driver *ddri, const char *buf, size_t count)
+{   /*write anything to this register will trigger the process*/
+    struct i2c_client *client = mc3xxx_i2c_client;
+    int num = 0;
+
+    if (1 != sscanf(buf, "%d", &num))
+    {
+        GSE_ERR("parse number fail\n");
+        return count;
+    }
+    else if(0 == num)
+    {
+        GSE_ERR("invalid data count\n");
+        return count;
+    }
+
+    GSE_LOG("NORMAL:\n");
+    MC3XXX_MUTEX_LOCK();
+    MC3XXX_SetPowerMode(client, true);
+    MC3XXX_MUTEX_UNLOCK();
+    GSE_LOG("SELFTEST:\n");
+
+    if (!MC3XXX_JudgeTestResult(client))
+    {
+        GSE_LOG("SELFTEST : PASS\n");
+        strcpy(selftestRes,"y");
+    }
+    else
+    {
+        GSE_LOG("SELFTEST : FAIL\n");
+        strcpy(selftestRes,"n");
+    }
+
+    return count;
+}
+
+/*****************************************
+ *** show_firlen_value
+ *****************************************/
+static ssize_t show_firlen_value(struct device_driver *ddri, char *buf)
+{
+    #ifdef _MC3XXX_SUPPORT_LPF_
+        struct i2c_client *client = mc3xxx_i2c_client;
+        struct mc3xxx_i2c_data *obj = i2c_get_clientdata(client);
+        GSE_LOG("fwq show_firlen_value \n");
+        if(atomic_read(&obj->firlen))
+        {
+            int idx = 0, len = atomic_read(&obj->firlen);
+            GSE_LOG("len = %2d, idx = %2d\n", obj->fir.num, obj->fir.idx);
+
+            for(idx = 0; idx < len; idx++)
+            {
+                GSE_LOG("[%5d %5d %5d]\n", obj->fir.raw[idx][MC3XXX_AXIS_X], obj->fir.raw[idx][MC3XXX_AXIS_Y], obj->fir.raw[idx][MC3XXX_AXIS_Z]);
+            }
+
+            GSE_LOG("sum = [%5d %5d %5d]\n", obj->fir.sum[MC3XXX_AXIS_X], obj->fir.sum[MC3XXX_AXIS_Y], obj->fir.sum[MC3XXX_AXIS_Z]);
+            GSE_LOG("avg = [%5d %5d %5d]\n", obj->fir.sum[MC3XXX_AXIS_X]/len, obj->fir.sum[MC3XXX_AXIS_Y]/len, obj->fir.sum[MC3XXX_AXIS_Z]/len);
+        }
+        return snprintf(buf, PAGE_SIZE, "%d\n", atomic_read(&obj->firlen));
+    #else
+        GSE_LOG("fwq show_firlen_value \n");
+        return snprintf(buf, PAGE_SIZE, "not support\n");
+    #endif
+}
+
+/*****************************************
+ *** store_firlen_value
+ *****************************************/
+static ssize_t store_firlen_value(struct device_driver *ddri, const char *buf, size_t count)
+{
+    #ifdef _MC3XXX_SUPPORT_LPF_
+    struct i2c_client *client = mc3xxx_i2c_client;
+    struct mc3xxx_i2c_data *obj = i2c_get_clientdata(client);
+    int firlen = 0;
+
+    GSE_LOG("fwq store_firlen_value \n");
+
+    if(1 != sscanf(buf, "%d", &firlen))
+    {
+    GSE_ERR("invallid format\n");
+    }
+    else if(firlen > C_MAX_FIR_LENGTH)
+    {
+    GSE_ERR("exceeds maximum filter length\n");
+    }
+    else
+    {
+        atomic_set(&obj->firlen, firlen);
+    if(0 == firlen)
+    {
+        atomic_set(&obj->fir_en, 0);
+    }
+    else
+    {
+        memset(&obj->fir, 0x00, sizeof(obj->fir));
+        atomic_set(&obj->fir_en, 1);
+    }
+    }
+    #endif
+    return count;
+}
+
+/*****************************************
+ *** show_trace_value
+ *****************************************/
+static ssize_t show_trace_value(struct device_driver *ddri, char *buf)
+{
+    ssize_t res = 0;
+    struct mc3xxx_i2c_data *obj = mc3xxx_obj_i2c_data;
+
+    GSE_LOG("fwq show_trace_value \n");
+
+    if (obj == NULL)
+    {
+        GSE_ERR("i2c_data obj is null!!\n");
+        return 0;
+    }
+
+    res = snprintf(buf, PAGE_SIZE, "0x%04X\n", atomic_read(&obj->trace));
+    return res;
+}
+
+/*****************************************
+ *** store_trace_value
+ *****************************************/
+static ssize_t store_trace_value(struct device_driver *ddri, const char *buf, size_t count)
+{
+    struct mc3xxx_i2c_data *obj = mc3xxx_obj_i2c_data;
+    int trace = 0;
+
+    GSE_LOG("fwq store_trace_value \n");
+
+    if (obj == NULL)
+    {
+        GSE_ERR("i2c_data obj is null!!\n");
+        return 0;
+    }
+
+    if(1 == sscanf(buf, "0x%x", &trace))
+    {
+        atomic_set(&obj->trace, trace);
+    }
+    else
+    {
+        GSE_ERR("invalid content: '%s', length = %zu\n", buf, count);
+    }
+
+    return count;
+}
+
+/*****************************************
+ *** show_status_value
+ *****************************************/
+static ssize_t show_status_value(struct device_driver *ddri, char *buf)
+{
+    ssize_t len = 0;
+    struct mc3xxx_i2c_data *obj = mc3xxx_obj_i2c_data;
+
+    GSE_LOG("fwq show_status_value \n");
+
+    if (obj == NULL)
+    {
+        GSE_ERR("i2c_data obj is null!!\n");
+        return 0;
+    }
+
+    if(obj->hw)
+    {
+        len += snprintf(buf+len, PAGE_SIZE-len, "CUST: %d %d (%d %d)\n",
+                obj->hw->i2c_num, obj->hw->direction, obj->hw->power_id, obj->hw->power_vol);
+    }
+    else
+    {
+        len += snprintf(buf+len, PAGE_SIZE-len, "CUST: NULL\n");
+    }
+    return len;
+}
+
+/*****************************************
+ *** show_power_status
+ *****************************************/
+static ssize_t show_power_status(struct device_driver *ddri, char *buf)
+{
+    ssize_t res = 0;
+    u8 uData = 0;
+    struct mc3xxx_i2c_data *obj = mc3xxx_obj_i2c_data;
+
+    if (obj == NULL)
+    {
+        GSE_ERR("i2c_data obj is null!!\n");
+        return 0;
+    }
+    MC3XXX_i2c_read_block(obj->client, MC3XXX_REG_MODE_FEATURE, &uData, 1);
+
+    res = snprintf(buf, PAGE_SIZE, "0x%04X\n", uData);
+    return res;
+}
+
+/*****************************************
+ *** show_version_value
+ *****************************************/
+static ssize_t show_version_value(struct device_driver *ddri, char *buf)
+{
+    if ( 1 == VIRTUAL_Z)
+        return snprintf(buf, PAGE_SIZE, "%s\n", MC3XXX_DEV_DRIVER_VERSION_VIRTUAL_Z);
+    else
+        return snprintf(buf, PAGE_SIZE, "%s\n", MC3XXX_DEV_DRIVER_VERSION);
+}
+
+/*****************************************
+ *** show_chip_id
+ *****************************************/
+static ssize_t show_chip_id(struct device_driver *ddri, char *buf)
+{
+    struct mc3xxx_i2c_data   *_pt_i2c_data = mc3xxx_obj_i2c_data;
+
+    return MC3XXX_Read_Chip_ID(_pt_i2c_data->client, buf);
+}
+
+/*****************************************
+ *** show_virtual_z
+ *****************************************/
+static ssize_t show_virtual_z(struct device_driver *ddri, char *buf)
+{
+    return snprintf(buf, PAGE_SIZE, "%s\n", VIRTUAL_Z == 1 ? "Virtual Z Support": "No Virtual Z Support");
+}
+
+/*****************************************
+ *** show_regiter_map
+ *****************************************/
+static ssize_t show_regiter_map(struct device_driver *ddri, char *buf)
+{
+    u8         _bIndex       = 0;
+    u8         _baRegMap[64] = { 0 };
+    ssize_t    _tLength      = 0;
+
+    struct i2c_client *client = mc3xxx_i2c_client;
+
+    if ((0xA5 == buf[0]) && (0x7B == buf[1]) && (0x40 == buf[2]))
+    {
+        MC3XXX_MUTEX_LOCK();
+        MC3XXX_Read_Reg_Map(client, buf);
+        MC3XXX_MUTEX_UNLOCK();
+
+        buf[0x21] = s_baOTP_OffsetData[0];
+        buf[0x22] = s_baOTP_OffsetData[1];
+        buf[0x23] = s_baOTP_OffsetData[2];
+        buf[0x24] = s_baOTP_OffsetData[3];
+        buf[0x25] = s_baOTP_OffsetData[4];
+        buf[0x26] = s_baOTP_OffsetData[5];
+
+        _tLength = 64;
+    }
+    else
+    {
+        MC3XXX_MUTEX_LOCK();
+        MC3XXX_Read_Reg_Map(client, _baRegMap);
+        MC3XXX_MUTEX_UNLOCK();
+
+        for (_bIndex = 0; _bIndex < 64; _bIndex++)
+            _tLength += snprintf((buf + _tLength), (PAGE_SIZE - _tLength), "Reg[0x%02X]: 0x%02X\n", _bIndex, _baRegMap[_bIndex]);
+    }
+
+    return (_tLength);
+}
+
+/*****************************************
+ *** store_regiter_map
+ *****************************************/
+static ssize_t store_regiter_map(struct device_driver *ddri, const char *buf, size_t count)
+{
+    // reserved
+    //GSE_LOG("[%s] buf[0]: 0x%02X\n", __FUNCTION__, buf[0]);
+
+    return count;
+}
+
+/*****************************************
+ *** show_chip_orientation
+ *****************************************/
+static ssize_t show_chip_orientation(struct device_driver *ptDevDrv, char *pbBuf)
+{
+    ssize_t          _tLength = 0;
+    struct acc_hw   *_ptAccelHw = get_cust_acc();
+
+    GSE_LOG("[%s] default direction: %d\n", __FUNCTION__, _ptAccelHw->direction);
+
+    _tLength = snprintf(pbBuf, PAGE_SIZE, "default direction = %d\n", _ptAccelHw->direction);
+
+    return (_tLength);
+}
+
+/*****************************************
+ *** store_chip_orientation
+ *****************************************/
+static ssize_t store_chip_orientation(struct device_driver *ptDevDrv, const char *pbBuf, size_t tCount)
+{
+    int                       _nDirection = 0;
+    struct mc3xxx_i2c_data   *_pt_i2c_obj = mc3xxx_obj_i2c_data;
+
+    if (NULL == _pt_i2c_obj)
+        return (0);
+
+    if (1 == sscanf(pbBuf, "%d", &_nDirection))
+    {
+        if (hwmsen_get_convert(_nDirection, &_pt_i2c_obj->cvt))
+            GSE_ERR("ERR: fail to set direction\n");
+    }
+
+    GSE_LOG("[%s] set direction: %d\n", __FUNCTION__, _nDirection);
+
+    return (tCount);
+}
+
+/*****************************************
+ *** show_accuracy_status
+ *****************************************/
+static ssize_t show_accuracy_status(struct device_driver *ddri, char *buf)
+{
+    return snprintf(buf, PAGE_SIZE, "%d\n", s_bAccuracyStatus);
+}
+
+/*****************************************
+ *** store_accuracy_status
+ *****************************************/
+static ssize_t store_accuracy_status(struct device_driver *ddri, const char *buf, size_t count)
+{
+    int    _nAccuracyStatus = 0;
+
+    if (1 != sscanf(buf, "%d", &_nAccuracyStatus))
+    {
+        GSE_ERR("incorrect argument\n");
+        return count;
+    }
+
+    if (SENSOR_STATUS_ACCURACY_HIGH < _nAccuracyStatus)
+    {
+        GSE_ERR("illegal accuracy status\n");
+        return count;
+    }
+
+    s_bAccuracyStatus = ((int8_t) _nAccuracyStatus);
+
+    return count;
+}
+
+/*****************************************
+ *** show_selfcheck_value
+ *****************************************/
+static ssize_t show_selfcheck_value(struct device_driver *ptDevDriver, char *pbBuf)
+{
+    struct i2c_client   *_pt_i2c_client = mc3xxx_i2c_client;
+
+    //GSE_LOG("[%s] 0x%02X\n", __FUNCTION__, pbBuf[0]);
+
+    MC3XXX_MUTEX_LOCK();
+    MC3XXX_SelfCheck(_pt_i2c_client, pbBuf);
+    MC3XXX_Init(_pt_i2c_client, 0);
+    MC3XXX_MUTEX_UNLOCK();
+
+    return (64);
+}
+
+/*****************************************
+ *** store_selfcheck_value
+ *****************************************/
+static ssize_t store_selfcheck_value(struct device_driver *ddri, const char *buf, size_t count)
+{
+    // reserved
+    //GSE_LOG("[%s] buf[0]: 0x%02X\n", __FUNCTION__, buf[0]);
+
+    return count;
+}
+
+/*****************************************
+ *** show_chip_validate_value
+ *****************************************/
+static ssize_t show_chip_validate_value(struct device_driver *ptDevDriver, char *pbBuf)
+{
+    unsigned char    _bChipValidation = 0;
+
+    _bChipValidation = MC3XXX_ValidateSensorIC(&s_bPCODE, &s_bHWID);
+
+    return snprintf(pbBuf, PAGE_SIZE, "%d\n", _bChipValidation);
+}
+
+/*****************************************
+ *** show_pdoc_enable_value
+ *****************************************/
+static ssize_t show_pdoc_enable_value(struct device_driver *ptDevDriver, char *pbBuf)
+{
+    #ifdef _MC3XXX_SUPPORT_PERIODIC_DOC_
+        return snprintf(pbBuf, PAGE_SIZE, "%d\n", s_bIsPDOC_Enabled);
+    #else
+        unsigned char    _bIsPDOC_Enabled = false;
+
+        return snprintf(pbBuf, PAGE_SIZE, "%d\n", _bIsPDOC_Enabled);
+    #endif
+}
+
+/*****************************************
+ *** DRIVER ATTRIBUTE LIST TABLE
+ *****************************************/
+static DRIVER_ATTR(chipinfo   ,           S_IRUGO, show_chipinfo_value,   NULL                  );
+static DRIVER_ATTR(sensordata ,           S_IRUGO, show_sensordata_value, NULL                  );
+static DRIVER_ATTR(cali       , S_IWUSR | S_IRUGO, show_cali_value,       store_cali_value      );
+static DRIVER_ATTR(selftest   , S_IWUSR | S_IRUGO, show_selftest_value,   store_selftest_value  );
+static DRIVER_ATTR(firlen     , S_IWUSR | S_IRUGO, show_firlen_value,     store_firlen_value    );
+static DRIVER_ATTR(trace      , S_IWUSR | S_IRUGO, show_trace_value,      store_trace_value     );
+static DRIVER_ATTR(status     ,           S_IRUGO, show_status_value,     NULL                  );
+static DRIVER_ATTR(power      ,           S_IRUGO, show_power_status,     NULL                  );
+static DRIVER_ATTR(version    ,           S_IRUGO, show_version_value,    NULL                  );
+static DRIVER_ATTR(chipid     ,           S_IRUGO, show_chip_id,          NULL                  );
+static DRIVER_ATTR(virtualz   ,           S_IRUGO, show_virtual_z,        NULL                  );
+static DRIVER_ATTR(regmap     , S_IWUSR | S_IRUGO, show_regiter_map,      store_regiter_map     );
+static DRIVER_ATTR(orientation, S_IWUSR | S_IRUGO, show_chip_orientation, store_chip_orientation);
+static DRIVER_ATTR(accuracy   , S_IWUSR | S_IRUGO, show_accuracy_status , store_accuracy_status );
+static DRIVER_ATTR(selfcheck  , S_IWUSR | S_IRUGO, show_selfcheck_value , store_selfcheck_value );
+static DRIVER_ATTR(validate   ,           S_IRUGO, show_chip_validate_value, NULL                  );
+static DRIVER_ATTR(pdoc       ,           S_IRUGO, show_pdoc_enable_value  , NULL                  );
+
+static struct driver_attribute   *mc3xxx_attr_list[] = {
+                                                           &driver_attr_chipinfo,
+                                                           &driver_attr_sensordata,
+                                                           &driver_attr_cali,
+                                                           &driver_attr_selftest,
+                                                           &driver_attr_firlen,
+                                                           &driver_attr_trace,
+                                                           &driver_attr_status,
+                                                           &driver_attr_power,
+                                                           &driver_attr_version,
+                                                           &driver_attr_chipid,
+                                                           &driver_attr_virtualz,
+                                                           &driver_attr_regmap,
+                                                           &driver_attr_orientation,
+                                                           &driver_attr_accuracy,
+                                                           &driver_attr_selfcheck,
+                                                           &driver_attr_validate,
+                                                           &driver_attr_pdoc,
+                                                       };
+
+/*****************************************
+ *** mc3xxx_create_attr
+ *****************************************/
+static int mc3xxx_create_attr(struct device_driver *driver)
+{
+    int idx, err = 0;
+    int num = (int)(sizeof(mc3xxx_attr_list)/sizeof(mc3xxx_attr_list[0]));
+    if (driver == NULL)
+    {
+        return -EINVAL;
+    }
+
+    for(idx = 0; idx < num; idx++)
+    {
+        if((err = driver_create_file(driver, mc3xxx_attr_list[idx])))
+        {
+            GSE_ERR("driver_create_file (%s) = %d\n", mc3xxx_attr_list[idx]->attr.name, err);
+            break;
+        }
+    }
+    return err;
+}
+
+/*****************************************
+ *** mc3xxx_delete_attr
+ *****************************************/
+static int mc3xxx_delete_attr(struct device_driver *driver)
+{
+    int idx ,err = 0;
+    int num = (int)(sizeof(mc3xxx_attr_list)/sizeof(mc3xxx_attr_list[0]));
+
+    if(driver == NULL)
+    {
+        return -EINVAL;
+    }
+
+
+    for(idx = 0; idx < num; idx++)
+    {
+        driver_remove_file(driver, mc3xxx_attr_list[idx]);
+    }
+
+
+    return err;
+}
+
+/*****************************************
+ *** gsensor_operate
+ *****************************************/
+static int gsensor_operate(void* self, uint32_t command, void* buff_in, int size_in,
+        void* buff_out, int size_out, int* actualout)
+{
+    int err = 0;
+    int value = 0;
+    struct mc3xxx_i2c_data *priv = (struct mc3xxx_i2c_data*)self;
+    hwm_sensor_data* gsensor_data = NULL;
+    char buff[MC3XXX_BUF_SIZE] = { 0 };
+
+    GSE_FUN(f);
+    switch (command)
+    {
+        case SENSOR_DELAY:
+            GSE_LOG("fwq set delay\n");
+            if((buff_in == NULL) || (size_in < sizeof(int)))
+            {
+                GSE_ERR("Set delay parameter error!\n");
+                err = -EINVAL;
+            }
+            else
+            {
+                value = *(int *)buff_in;
+                if(value >= 50)
+                {
+                    atomic_set(&priv->filter, 0);
+                }
+                else
+                {
+                        #if defined(_MC3XXX_SUPPORT_LPF_)
+                     priv->fir.num = 0;
+                    priv->fir.idx = 0;
+                    priv->fir.sum[MC3XXX_AXIS_X] = 0;
+                    priv->fir.sum[MC3XXX_AXIS_Y] = 0;
+                    priv->fir.sum[MC3XXX_AXIS_Z] = 0;
+                    atomic_set(&priv->filter, 1);
+                                    #endif
+                }
+            }
+            break;
+
+        case SENSOR_ENABLE:
+            if((buff_in == NULL) || (size_in < sizeof(int)))
+            {
+                GSE_ERR("Enable sensor parameter error!\n");
+                err = -EINVAL;
+            }
+            else
+            {
+                value = *(int *)buff_in;
+
+                GSE_LOG("fwq sensor enable gsensor: %d\n", value);
+
+                if(((value == 0) && (mc3xxx_sensor_power == false)) ||((value == 1) && (mc3xxx_sensor_power == true)))
+                {
+                    GSE_LOG("Gsensor device have updated!\n");
+                }
+                else
+                {
+                    MC3XXX_MUTEX_LOCK();
+                    err = MC3XXX_SetPowerMode( priv->client, !mc3xxx_sensor_power);
+                    MC3XXX_MUTEX_UNLOCK();
+                }
+
+                #ifdef _MC3XXX_SUPPORT_PERIODIC_DOC_
+                    if (0 == value)
+                        atomic_set(&s_t_mc3xxx_open_status, 0);
+                    else
+                        atomic_set(&s_t_mc3xxx_open_status, 1);
+
+                    wake_up(&wq_mc3xxx_open_status);
+                #endif
+            }
+            break;
+
+        case SENSOR_GET_DATA:
+            GSE_LOG("fwq sensor operate get data\n");
+            if((buff_out == NULL) || (size_out< sizeof(hwm_sensor_data)))
+            {
+                GSE_ERR("get sensor data parameter error!\n");
+                err = -EINVAL;
+            }
+            else
+            {
+                gsensor_data = (hwm_sensor_data *)buff_out;
+                MC3XXX_MUTEX_LOCK();
+                MC3XXX_ReadSensorData(priv->client, buff, MC3XXX_BUF_SIZE);
+                MC3XXX_MUTEX_UNLOCK();
+                sscanf(buff, "%x %x %x", &gsensor_data->values[0],
+                    &gsensor_data->values[1], &gsensor_data->values[2]);
+                gsensor_data->status = s_bAccuracyStatus;
+                gsensor_data->value_divide = 1000;
+                GSE_LOG("MC3XXX_ReadSensorData: X :%d,Y: %d, Z: %d\n",gsensor_data->values[0],gsensor_data->values[1],gsensor_data->values[2]);
+            }
+            break;
+        default:
+            GSE_ERR("gsensor operate function no this parameter %d!\n", command);
+            err = -1;
+            break;
+    }
+
+    return err;
+}
+
+/*****************************************
+ *** mc3xxx_open
+ *****************************************/
+static int mc3xxx_open(struct inode *inode, struct file *file)
+{
+    file->private_data = mc3xxx_i2c_client;
+
+    if(file->private_data == NULL)
+    {
+        GSE_ERR("null pointer!!\n");
+        return -EINVAL;
+    }
+    return nonseekable_open(inode, file);
+}
+
+/*****************************************
+ *** mc3xxx_release
+ *****************************************/
+static int mc3xxx_release(struct inode *inode, struct file *file)
+{
+    file->private_data = NULL;
+    return 0;
+}
+
+/*****************************************
+ *** mc3xxx_ioctl
+ *****************************************/
+static long mc3xxx_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
+{
+    struct i2c_client *client = (struct i2c_client*)file->private_data;
+    struct mc3xxx_i2c_data *obj = (struct mc3xxx_i2c_data*)i2c_get_clientdata(client);
+    char strbuf[MC3XXX_BUF_SIZE] = {0};
+    void __user *data = NULL;
+    SENSOR_DATA sensor_data = {0};
+    long err = 0;
+    int cali[3] = {0};
+
+        #ifdef _MC3XXX_SUPPORT_DOT_CALIBRATION_
+            int prod = -1;
+            int tempZ = 0;
+            unsigned char _bTempPCode = 0x00;
+        #endif
+
+    //GSE_FUN(f);
+    if(_IOC_DIR(cmd) & _IOC_READ)
+    {
+        err = !access_ok(VERIFY_WRITE, (void __user *)arg, _IOC_SIZE(cmd));
+    }
+    else if(_IOC_DIR(cmd) & _IOC_WRITE)
+    {
+        err = !access_ok(VERIFY_READ, (void __user *)arg, _IOC_SIZE(cmd));
+    }
+
+    if(err)
+    {
+        GSE_ERR("access error: %08X, (%2d, %2d)\n", cmd, _IOC_DIR(cmd), _IOC_SIZE(cmd));
+        return -EFAULT;
+    }
+
+    switch(cmd)
+    {
+        case GSENSOR_IOCTL_INIT:
+            GSE_LOG("fwq GSENSOR_IOCTL_INIT\n");
+            MC3XXX_MUTEX_LOCK();
+            MC3XXX_Init(client, 0);
+            MC3XXX_MUTEX_UNLOCK();
+            break;
+
+        case GSENSOR_IOCTL_READ_CHIPINFO:
+            GSE_LOG("fwq GSENSOR_IOCTL_READ_CHIPINFO\n");
+            data = (void __user *) arg;
+            if(data == NULL)
+            {
+                err = -EINVAL;
+                break;
+            }
+
+            MC3XXX_ReadChipInfo(client, strbuf, MC3XXX_BUF_SIZE);
+            if(copy_to_user(data, strbuf, strlen(strbuf)+1))
+            {
+                err = -EFAULT;
+                break;
+            }
+            break;
+
+        case GSENSOR_IOCTL_READ_SENSORDATA:
+            data = (void __user *) arg;
+            if(data == NULL)
+            {
+                err = -EINVAL;
+                break;
+            }
+            MC3XXX_MUTEX_LOCK();
+            #ifdef _MC3XXX_SUPPORT_APPLY_AVERAGE_AGORITHM_
+                MC3XXX_ReadRawData(client, strbuf);
+            #else
+                MC3XXX_ReadSensorData(client, strbuf, MC3XXX_BUF_SIZE);
+            #endif
+            MC3XXX_MUTEX_UNLOCK();
+            if(copy_to_user(data, strbuf, strlen(strbuf)+1))
+            {
+                err = -EFAULT;
+                break;
+            }
+            break;
+
+        case GSENSOR_IOCTL_READ_GAIN:
+            GSE_LOG("fwq GSENSOR_IOCTL_READ_GAIN\n");
+            data = (void __user *) arg;
+            if(data == NULL)
+            {
+                err = -EINVAL;
+                break;
+            }
+
+            if(copy_to_user(data, &gsensor_gain, sizeof(GSENSOR_VECTOR3D)))
+            {
+                err = -EFAULT;
+                break;
+            }
+            break;
+
+        case GSENSOR_IOCTL_READ_OFFSET:
+            GSE_LOG("fwq GSENSOR_IOCTL_READ_OFFSET\n");
+            data = (void __user *) arg;
+            if(data == NULL)
+            {
+                err = -EINVAL;
+                break;
+            }
+
+            if(copy_to_user(data, &gsensor_offset, sizeof(GSENSOR_VECTOR3D)))
+            {
+                err = -EFAULT;
+                break;
+            }
+            break;
+
+        case GSENSOR_IOCTL_READ_RAW_DATA:
+            GSE_LOG("fwq GSENSOR_IOCTL_READ_RAW_DATA\n");
+            data = (void __user *) arg;
+            if(data == NULL)
+            {
+                err = -EINVAL;
+                break;
+            }
+            MC3XXX_MUTEX_LOCK();
+            MC3XXX_ReadRawData(client, strbuf);
+            MC3XXX_MUTEX_UNLOCK();
+            if(copy_to_user(data, strbuf, strlen(strbuf)+1))
+            {
+                err = -EFAULT;
+                break;
+            }
+            break;
+
+        case GSENSOR_IOCTL_SET_CALI:
+            GSE_LOG("fwq GSENSOR_IOCTL_SET_CALI!!\n");
+            data = (void __user*)arg;
+            if(data == NULL)
+            {
+                err = -EINVAL;
+                break;
+            }
+            if(copy_from_user(&sensor_data, data, sizeof(sensor_data)))
+            {
+                err = -EFAULT;
+                break;
+            }
+            if(atomic_read(&obj->suspend))
+            {
+                GSE_ERR("Perform calibration in suspend state!!\n");
+                err = -EINVAL;
+            }
+            else
+            {
+                obj->cali_sw[MC3XXX_AXIS_X] += sensor_data.x;
+                obj->cali_sw[MC3XXX_AXIS_Y] += sensor_data.y;
+                obj->cali_sw[MC3XXX_AXIS_Z] += sensor_data.z;
+
+                cali[MC3XXX_AXIS_X] = sensor_data.x * gsensor_gain.x / GRAVITY_EARTH_1000;
+                cali[MC3XXX_AXIS_Y] = sensor_data.y * gsensor_gain.y / GRAVITY_EARTH_1000;
+                cali[MC3XXX_AXIS_Z] = sensor_data.z * gsensor_gain.z / GRAVITY_EARTH_1000;
+
+                MC3XXX_MUTEX_LOCK();
+                err = MC3XXX_WriteCalibration(client, cali);
+                MC3XXX_MUTEX_UNLOCK();
+            }
+            break;
+
+        case GSENSOR_IOCTL_CLR_CALI:
+            GSE_LOG("fwq GSENSOR_IOCTL_CLR_CALI!!\n");
+            MC3XXX_MUTEX_LOCK();
+            err = MC3XXX_ResetCalibration(client);
+            MC3XXX_MUTEX_UNLOCK();
+            break;
+
+        case GSENSOR_IOCTL_GET_CALI:
+            GSE_LOG("fwq mc3xxx GSENSOR_IOCTL_GET_CALI\n");
+            data = (void __user*)arg;
+            if(data == NULL)
+            {
+                err = -EINVAL;
+                break;
+            }
+//            if((err = MC3XXX_ReadCalibration(client, cali)))
+//            {
+//                break;
+//            }
+
+            sensor_data.x = obj->cali_sw[MC3XXX_AXIS_X];
+            sensor_data.y = obj->cali_sw[MC3XXX_AXIS_Y];
+            sensor_data.z = obj->cali_sw[MC3XXX_AXIS_Z];
+            if(copy_to_user(data, &sensor_data, sizeof(sensor_data)))
+            {
+                err = -EFAULT;
+                break;
+            }
+            break;
+
+        //add in Sensors_io.h
+        // ==============================================================================
+        #ifdef _MC3XXX_SUPPORT_DOT_CALIBRATION_
+
+        case GSENSOR_MCUBE_IOCTL_SET_CALI:
+            GSE_LOG("fwq GSENSOR_MCUBE_IOCTL_SET_CALI!!\n");
+            data = (void __user*)arg;
+            if(data == NULL)
+            {
+                err = -EINVAL;
+                break;
+            }
+            if(copy_from_user(&sensor_data, data, sizeof(sensor_data)))
+            {
+                err = -EFAULT;
+                break;
+            }
+            if(atomic_read(&obj->suspend))
+            {
+                GSE_ERR("Perform calibration in suspend state!!\n");
+                err = -EINVAL;
+            }
+            else
+            {
+                cali[MC3XXX_AXIS_X] = sensor_data.x * gsensor_gain.x / GRAVITY_EARTH_1000;
+                cali[MC3XXX_AXIS_Y] = sensor_data.y * gsensor_gain.y / GRAVITY_EARTH_1000;
+                cali[MC3XXX_AXIS_Z] = sensor_data.z * gsensor_gain.z / GRAVITY_EARTH_1000;
+
+                MC3XXX_MUTEX_LOCK();
+                err = MC3XXX_WriteCalibration(client, cali);
+                MC3XXX_MUTEX_UNLOCK();
+            }
+            break;
+
+        case GSENSOR_IOCTL_SET_CALI_MODE:
+            GSE_LOG("fwq mc3xxx GSENSOR_IOCTL_SET_CALI_MODE\n");
+            break;
+
+        case GSENSOR_MCUBE_IOCTL_READ_RBM_DATA:
+            GSE_LOG("fwq GSENSOR_MCUBE_IOCTL_READ_RBM_DATA\n");
+            data = (void __user *) arg;
+            if(data == NULL)
+            {
+                err = -EINVAL;
+                break;
+            }
+            MC3XXX_MUTEX_LOCK();
+            MC3XXX_ReadRBMData(client, strbuf);
+            MC3XXX_MUTEX_UNLOCK();
+            if(copy_to_user(data, strbuf, strlen(strbuf)+1))
+            {
+                err = -EFAULT;
+                break;
+            }
+            break;
+
+        case GSENSOR_MCUBE_IOCTL_SET_RBM_MODE:
+            GSE_LOG("fwq GSENSOR_MCUBE_IOCTL_SET_RBM_MODE\n");
+            MC3XXX_MUTEX_LOCK();
+            MC3XXX_rbm(client,1);
+            MC3XXX_MUTEX_UNLOCK();
+            break;
+
+        case GSENSOR_MCUBE_IOCTL_CLEAR_RBM_MODE:
+            GSE_LOG("fwq GSENSOR_MCUBE_IOCTL_CLEAR_RBM_MODE\n");
+            MC3XXX_MUTEX_LOCK();
+            MC3XXX_rbm(client,0);
+            MC3XXX_MUTEX_UNLOCK();
+            break;
+
+        case GSENSOR_MCUBE_IOCTL_REGISTER_MAP:
+            GSE_LOG("fwq GSENSOR_MCUBE_IOCTL_REGISTER_MAP\n");
+
+            MC3XXX_Read_Reg_Map(client, NULL);
+
+            break;
+
+        case GSENSOR_MCUBE_IOCTL_READ_PRODUCT_ID:
+            GSE_LOG("fwq GSENSOR_MCUBE_IOCTL_READ_PRODUCT_ID\n");
+            data = (void __user *) arg;
+            if(data == NULL)
+            {
+                err = -EINVAL;
+                break;
+            }
+
+            if (MC3XXX_RETCODE_SUCCESS != (prod = MC3XXX_ValidateSensorIC(&s_bPCODE, &s_bHWID)))
+                GSE_LOG("Not mCube accelerometers!\n");
+
+            if(copy_to_user(data, &prod, sizeof(prod)))
+            {
+                err = -EFAULT;
+                break;
+            }
+            break;
+
+        case GSENSOR_MCUBE_IOCTL_READ_FILEPATH:
+            GSE_LOG("fwq GSENSOR_MCUBE_IOCTL_READ_FILEPATH\n");
+            data = (void __user *) arg;
+            if(data == NULL)
+            {
+                err = -EINVAL;
+                break;
+            }
+
+            if(copy_to_user(data, file_path, (strlen(file_path)+1)))
+            {
+                err = -EFAULT;
+                break;
+            }
+            break;
+
+        case GSENSOR_MCUBE_IOCTL_VIRTUAL_Z:
+            GSE_LOG("fwq GSENSOR_MCUBE_IOCTL_VIRTUAL_Z\n");
+            data = (void __user *) arg;
+            tempZ = VIRTUAL_Z;
+            if(data == NULL)
+            {
+                err = -EINVAL;
+                break;
+            }
+            if(copy_to_user(data, &tempZ, sizeof(tempZ)))
+            {
+                err = -EFAULT;
+                break;
+            }
+            break;
+
+        case GSENSOR_MCUBE_IOCTL_READ_PCODE:
+            GSE_LOG("fwq GSENSOR_MCUBE_IOCTL_READ_PCODE\n");
+            data = (void __user *) arg;
+            if(data == NULL)
+            {
+                err = -EINVAL;
+                break;
+            }
+
+            _bTempPCode = s_bPCODE;
+            GSE_LOG("mCube PCode = %2x!\n", _bTempPCode);
+            if(copy_to_user(data, &_bTempPCode, sizeof(_bTempPCode)))
+            {
+                err = -EFAULT;
+                break;
+            }
+            break;
+
+        #endif    // END of _MC3XXX_SUPPORT_DOT_CALIBRATION_
+
+        // ==============================================================================
+        #ifdef _MC3XXX_SUPPORT_PERIODIC_DOC_
+
+        case GSENSOR_MCUBE_IOCTL_GET_OFLAG:
+            {
+                int            _nSensorsOpenStatus = 0;
+                void __user   *_pArg = ((void __user *) arg);
+
+                GSE_LOG("[%s] GSENSOR_MCUBE_IOCTL_GET_OFLAG\n", __func__);
+
+                _nSensorsOpenStatus = MC3XXX_GetOpenStatus();
+
+                if(copy_to_user(_pArg, &_nSensorsOpenStatus, sizeof(_nSensorsOpenStatus)))
+                    return (-EFAULT);
+            }
+            break;
+
+        #endif    // END of _MC3XXX_SUPPORT_PERIODIC_DOC_
+
+        default:
+            GSE_ERR("unknown IOCTL: 0x%08x\n", cmd);
+            err = -ENOIOCTLCMD;
+            break;
+
+    }
+
+    return err;
+}
+
+#ifdef CONFIG_COMPAT
+static long mc3xxx_compat_ioctl(struct file *file, unsigned int cmd,
+       unsigned long arg)
+{
+    long err = 0;
+
+	void __user *arg32 = compat_ptr(arg);
+	
+	if (!file->f_op || !file->f_op->unlocked_ioctl)
+		return -ENOTTY;
+	
+    switch (cmd)
+    {
+        case COMPAT_GSENSOR_IOCTL_READ_SENSORDATA:
+            if (arg32 == NULL)
+            {
+                err = -EINVAL;
+                break;    
+            }
+		
+		    err = file->f_op->unlocked_ioctl(file, GSENSOR_IOCTL_READ_SENSORDATA, (unsigned long)arg32);
+		    if (err){
+		        GSE_ERR("GSENSOR_IOCTL_READ_SENSORDATA unlocked_ioctl failed.");
+		        return err;
+		    }
+        break;
+        case COMPAT_GSENSOR_IOCTL_SET_CALI:
+            if (arg32 == NULL)
+            {
+                err = -EINVAL;
+                break;    
+            }
+		
+		    err = file->f_op->unlocked_ioctl(file, GSENSOR_IOCTL_SET_CALI, (unsigned long)arg32);
+		    if (err){
+		        GSE_ERR("GSENSOR_IOCTL_SET_CALI unlocked_ioctl failed.");
+		        return err;
+		    }
+        break;
+        case COMPAT_GSENSOR_IOCTL_GET_CALI:
+            if (arg32 == NULL)
+            {
+                err = -EINVAL;
+                break;    
+            }
+		
+		    err = file->f_op->unlocked_ioctl(file, GSENSOR_IOCTL_GET_CALI, (unsigned long)arg32);
+		    if (err){
+		        GSE_ERR("GSENSOR_IOCTL_GET_CALI unlocked_ioctl failed.");
+		        return err;
+		    }
+        break;
+        case COMPAT_GSENSOR_IOCTL_CLR_CALI:
+            if (arg32 == NULL)
+            {
+                err = -EINVAL;
+                break;    
+            }
+		
+		    err = file->f_op->unlocked_ioctl(file, GSENSOR_IOCTL_CLR_CALI, (unsigned long)arg32);
+		    if (err){
+		        GSE_ERR("GSENSOR_IOCTL_CLR_CALI unlocked_ioctl failed.");
+		        return err;
+		    }
+        break;
+
+        default:
+            GSE_ERR("unknown IOCTL: 0x%08x\n", cmd);
+            err = -ENOIOCTLCMD;
+        break;
+
+    }
+
+    return err;
+}
+#endif
+/*****************************************
+ *** MC3XXX_reset
+ *****************************************/
+static void MC3XXX_reset(struct i2c_client *client)
+{
+    unsigned char    _baBuf[2] = { 0 };
+
+    _baBuf[0] = 0x43;
+
+    MC3XXX_i2c_write_block(client, MC3XXX_REG_MODE_FEATURE, _baBuf, 0x01);
+
+    MC3XXX_i2c_read_block(client, 0x04, _baBuf, 0x01);
+
+    if (0x00 == (_baBuf[0] & 0x40))
+    {
+        _baBuf[0] = 0x6D;
+        MC3XXX_i2c_write_block(client, 0x1B, _baBuf, 0x01);
+
+        _baBuf[0] = 0x43;
+        MC3XXX_i2c_write_block(client, 0x1B, _baBuf, 0x01);
+    }
+
+    _baBuf[0] = 0x43;
+    MC3XXX_i2c_write_block(client, 0x07, _baBuf, 1);
+
+    _baBuf[0] = 0x80;
+    MC3XXX_i2c_write_block(client, 0x1C, _baBuf, 1);
+
+    _baBuf[0] = 0x80;
+    MC3XXX_i2c_write_block(client, 0x17, _baBuf, 1);
+
+    msleep(5);
+
+    _baBuf[0] = 0x00;
+    MC3XXX_i2c_write_block(client, 0x1C, _baBuf, 1);
+
+    _baBuf[0] = 0x00;
+    MC3XXX_i2c_write_block(client, 0x17, _baBuf, 1);
+
+    msleep(5);
+
+    MC3XXX_i2c_read_block(client, 0x21, offset_buf, 6);
+
+    MC3XXX_i2c_read_block(client, 0x04, _baBuf, 0x01);
+
+    if (_baBuf[0] & 0x40)
+    {
+        _baBuf[0] = 0x6D;
+        MC3XXX_i2c_write_block(client, 0x1B, _baBuf, 0x01);
+
+        _baBuf[0] = 0x43;
+        MC3XXX_i2c_write_block(client, 0x1B, _baBuf, 0x01);
+    }
+
+    _baBuf[0] = 0x41;
+
+    MC3XXX_i2c_write_block(client, MC3XXX_REG_MODE_FEATURE, _baBuf, 0x01);
+}
+
+/*****************************************
+ *** STRUCT:: mc3xxx_fops
+ *****************************************/
+static struct file_operations    mc3xxx_fops = {
+                                                   .owner          = THIS_MODULE,
+                                                   .open           = mc3xxx_open,
+                                                   .release        = mc3xxx_release,
+                                                   .unlocked_ioctl = mc3xxx_ioctl,
+                                                    #ifdef CONFIG_COMPAT
+													.compat_ioctl = mc3xxx_compat_ioctl,
+													#endif
+                                               };
+
+/*****************************************
+ *** STRUCT:: mc3xxx_device
+ *****************************************/
+static struct miscdevice    mc3xxx_device = {
+                                                .minor = MISC_DYNAMIC_MINOR,
+                                                .name  = "gsensor",
+                                                .fops  = &mc3xxx_fops,
+                                            };
+
+#ifndef USE_EARLY_SUSPEND
+/*****************************************
+ *** mc3xxx_suspend
+ *****************************************/
+static int mc3xxx_suspend(struct i2c_client *client, pm_message_t msg)
+{
+    struct mc3xxx_i2c_data *obj = i2c_get_clientdata(client);
+    int err = 0;
+    GSE_FUN();
+
+    if(msg.event == PM_EVENT_SUSPEND)
+    {
+        if(obj == NULL)
+        {
+            GSE_ERR("null pointer!!\n");
+            return -EINVAL;
+        }
+
+        atomic_set(&obj->suspend, 1);
+
+        MC3XXX_MUTEX_LOCK();
+        err = MC3XXX_SetPowerMode(client, false);
+        MC3XXX_MUTEX_UNLOCK();
+
+        if (err)
+        {
+            GSE_ERR("write power control fail!!\n");
+            return err;
+        }
+
+        MC3XXX_power(obj->hw, 0);
+    }
+    return err;
+}
+
+/*****************************************
+ *** mc3xxx_resume
+ *****************************************/
+static int mc3xxx_resume(struct i2c_client *client)
+{
+    struct mc3xxx_i2c_data *obj = i2c_get_clientdata(client);
+    int err;
+    GSE_FUN();
+
+    if(obj == NULL)
+    {
+        GSE_ERR("null pointer!!\n");
+        return -EINVAL;
+    }
+
+    MC3XXX_power(obj->hw, 1);
+
+    MC3XXX_MUTEX_LOCK();
+    err = MC3XXX_Init(client, 0);
+
+    if(err)
+    {
+        MC3XXX_MUTEX_UNLOCK();
+        GSE_ERR("initialize client fail!!\n");
+        return err;
+    }
+
+    err = MC3XXX_SetPowerMode(client, true);
+    MC3XXX_MUTEX_UNLOCK();
+
+    if (err)
+    {
+        GSE_ERR("write power control fail!!\n");
+        return err;
+    }
+
+    atomic_set(&obj->suspend, 0);
+
+    return 0;
+}
+#else
+/*****************************************
+ *** mc3xxx_early_suspend
+ *****************************************/
+static void mc3xxx_early_suspend(struct early_suspend *h)
+{
+    struct mc3xxx_i2c_data *obj = container_of(h, struct mc3xxx_i2c_data, early_drv);
+    int err;
+    GSE_FUN();
+
+    if(obj == NULL)
+    {
+        GSE_ERR("null pointer!!\n");
+        return;
+    }
+    atomic_set(&obj->suspend, 1);
+    /*
+    if(err = hwmsen_write_byte(obj->client, MC3XXX_REG_POWER_CTL, 0x00))
+    {
+        GSE_ERR("write power control fail!!\n");
+        return;
+    }
+    */
+    MC3XXX_MUTEX_LOCK_RETURN_VOID();
+    err = MC3XXX_SetPowerMode(obj->client, false);
+    MC3XXX_MUTEX_UNLOCK();
+
+    if (err)
+    {
+        GSE_ERR("write power control fail!!\n");
+        return;
+    }
+
+    MC3XXX_power(obj->hw, 0);
+}
+
+/*****************************************
+ *** mc3xxx_late_resume
+ *****************************************/
+static void mc3xxx_late_resume(struct early_suspend *h)
+{
+    struct mc3xxx_i2c_data *obj = container_of(h, struct mc3xxx_i2c_data, early_drv);
+    int err;
+    GSE_FUN();
+
+    if(obj == NULL)
+    {
+        GSE_ERR("null pointer!!\n");
+        return;
+    }
+
+    MC3XXX_power(obj->hw, 1);
+
+    MC3XXX_MUTEX_LOCK_RETURN_VOID();
+    err = MC3XXX_Init(obj->client, 0);
+
+    if(err)
+    {
+        MC3XXX_MUTEX_UNLOCK();
+        GSE_ERR("initialize client fail!!\n");
+        return;
+    }
+
+    err = MC3XXX_SetPowerMode(obj->client, true);
+    MC3XXX_MUTEX_UNLOCK();
+
+    if (err)
+    {
+        GSE_ERR("write power control fail!!\n");
+        return;
+    }
+
+
+    atomic_set(&obj->suspend, 0);
+}
+#endif
+
+/*****************************************
+ *** _mc3xxx_i2c_auto_probe
+ *****************************************/
+static int _mc3xxx_i2c_auto_probe(struct i2c_client *client)
+{
+    #define _MC3XXX_I2C_PROBE_ADDR_COUNT_    (sizeof(mc3xxx_i2c_auto_probe_addr) / sizeof(mc3xxx_i2c_auto_probe_addr[0]))
+
+    unsigned char    _baData1Buf[2] = { 0 };
+    unsigned char    _baData2Buf[2] = { 0 };
+
+    int              _nCount = 0;
+    int              _naCheckCount[_MC3XXX_I2C_PROBE_ADDR_COUNT_] = { 0 };
+
+    //GSE_FUN();
+
+    memset(_naCheckCount, 0, sizeof(_naCheckCount));
+
+_I2C_AUTO_PROBE_RECHECK_:
+    s_bPCODE  = 0x00;
+    s_bPCODER = 0x00;
+    s_bHWID   = 0x00;
+
+    for (_nCount = 0; _nCount < _MC3XXX_I2C_PROBE_ADDR_COUNT_; _nCount++)
+    {
+        client->addr = mc3xxx_i2c_auto_probe_addr[_nCount];
+
+        //GSE_LOG("[%s][%d] probing addr: 0x%X\n", __FUNCTION__, _nCount, client->addr);
+
+        _baData1Buf[0] = 0;
+        if (0 > MC3XXX_i2c_read_block(client, 0x3B, _baData1Buf, 1))
+        {
+            continue;
+        }
+
+        _naCheckCount[_nCount]++;
+
+        //GSE_LOG("[%s][%d] addr: 0x%X ok to read REG(0x3B): 0x%X\n", __FUNCTION__, _nCount, client->addr, _baData1Buf[0]);
+
+        if (0x00 == _baData1Buf[0])
+        {
+            if (1 == _naCheckCount[_nCount])
+            {
+                MC3XXX_reset(client);
+                msleep(3);
+                goto _I2C_AUTO_PROBE_RECHECK_;
+            }
+            else
+            {
+                continue;
+            }
+        }
+
+        _baData2Buf[0] = 0;
+        MC3XXX_i2c_read_block(client, 0x18, _baData2Buf, 1);
+
+        s_bPCODER = _baData1Buf[0];
+
+        if (MC3XXX_RETCODE_SUCCESS == MC3XXX_ValidateSensorIC(&_baData1Buf[0], &_baData2Buf[0]))
+        {
+            s_bPCODE = _baData1Buf[0];
+            s_bHWID  = _baData2Buf[0];
+
+            MC3XXX_SaveDefaultOffset(client);
+
+            //GSE_LOG("[%s] addr: 0x%X confirmed ok to use. s_bPCODE: 0x%02X, s_bHWID: 0x%02X\n", __FUNCTION__, client->addr, s_bPCODE, s_bHWID);
+
+            return (MC3XXX_RETCODE_SUCCESS);
+        }
+    }
+
+    return (MC3XXX_RETCODE_ERROR_I2C);
+
+    #undef _MC3XXX_I2C_PROBE_ADDR_COUNT_
+}
+
+
+// if use  this typ of enable , Gsensor should report inputEvent(x, y, z ,stats, div) to HAL
+static int mc3xxx_open_report_data(int open)
+{
+    //should queuq work to report event if  is_report_input_direct=true
+    return 0;
+}
+
+// if use  this typ of enable , Gsensor only enabled but not report inputEvent to HAL
+
+static int mc3xxx_enable_nodata(int en)
+{
+    int res =0;
+    int retry = 0;
+    bool power=false;
+
+    if(1==en)
+    {
+        power=true;
+    }
+    if(0==en)
+    {
+        power =false;
+    }
+
+    for(retry = 0; retry < 3; retry++){
+        res = MC3XXX_SetPowerMode(mc3xxx_obj_i2c_data->client, power);
+        if(res == 0)
+        {
+            GSE_LOG("MC3XXX_SetPowerMode done\n");
+            break;
+        }
+        GSE_LOG("MC3XXX_SetPowerMode fail\n");
+    }
+
+
+    if(res != 0)
+    {
+        GSE_LOG("MC3XXX_SetPowerMode fail!\n");
+        return -1;
+    }
+    GSE_LOG("mc3xxx_enable_nodata OK!\n");
+    return 0;
+}
+
+static int mc3xxx_set_delay(u64 ns)
+{
+    int value =0;
+    value = (int)ns/1000/1000;
+
+    GSE_LOG("mc3xxx_set_delay (%d), chip only use 1024HZ \n",value);
+    return 0;
+}
+
+static int mc3xxx_get_data(int* x ,int* y,int* z, int* status)
+{
+    char buff[MC3XXX_BUF_SIZE];
+    MC3XXX_ReadSensorData(mc3xxx_obj_i2c_data->client, buff, MC3XXX_BUF_SIZE);
+
+    sscanf(buff, "%x %x %x", x, y, z);
+    *status = SENSOR_STATUS_ACCURACY_MEDIUM;
+
+    return 0;
+}
+
+
+/*****************************************
+ *** mc3xxx_i2c_probe
+ *****************************************/
+static int mc3xxx_i2c_probe(struct i2c_client *client, const struct i2c_device_id *id)
+{
+    struct i2c_client *new_client;
+    struct mc3xxx_i2c_data *obj;
+    struct acc_control_path ctl={0};
+    struct acc_data_path data={0};
+    int err = 0;
+    GSE_FUN();
+
+    if (MC3XXX_RETCODE_SUCCESS != _mc3xxx_i2c_auto_probe(client))
+    {
+        //GSE_ERR("ERR: fail to probe mCube sensor!\n");
+        goto exit;
+    }
+
+    if(!(obj = kzalloc(sizeof(*obj), GFP_KERNEL)))
+    {
+        err = -ENOMEM;
+        goto exit;
+    }
+
+    obj->hw = hw;
+
+    if((err = hwmsen_get_convert(obj->hw->direction, &obj->cvt)))
+    {
+        GSE_ERR("invalid direction: %d\n", obj->hw->direction);
+        goto exit_kfree;
+    }
+
+    mc3xxx_obj_i2c_data = obj;
+    obj->client = client;
+    new_client = obj->client;
+    i2c_set_clientdata(new_client,obj);
+
+    atomic_set(&obj->trace, 0);
+    atomic_set(&obj->suspend, 0);
+
+    #ifdef _MC3XXX_SUPPORT_LPF_
+        if(obj->hw->firlen > C_MAX_FIR_LENGTH)
+        {
+            atomic_set(&obj->firlen, C_MAX_FIR_LENGTH);
+        }
+        else
+        {
+            atomic_set(&obj->firlen, obj->hw->firlen);
+        }
+
+        if(atomic_read(&obj->firlen) > 0)
+        {
+            atomic_set(&obj->fir_en, 1);
+        }
+    #endif
+
+    mc3xxx_i2c_client = new_client;
+
+    MC3XXX_reset(new_client);
+
+    if (MC3XXX_RETCODE_SUCCESS != _mc3xxx_i2c_auto_probe(client))
+    {
+        //GSE_ERR("ERR: fail to probe mCube sensor!\n");
+        goto exit_init_failed;
+    }
+
+    //GSE_LOG("[%s] 2nd confirmed i2c addr: 0x%X\n", __FUNCTION__, client->addr);
+
+    MC3XXX_i2c_read_block(client, 0x21, offset_buf, 6);
+
+    if((err = MC3XXX_Init(new_client, 1)))
+    {
+        goto exit_init_failed;
+    }
+
+    MC3XXX_MUTEX_INIT();
+
+    if((err = misc_register(&mc3xxx_device)))
+    {
+        GSE_ERR("mc3xxx_device register failed\n");
+        goto exit_misc_device_register_failed;
+    }
+    ctl.is_use_common_factory = false;
+        if((err = mc3xxx_create_attr(&(mc3xxx_init_info.platform_diver_addr->driver))))
+        {
+            GSE_ERR("create attribute err = %d\n", err);
+            goto exit_create_attr_failed;
+        }
+
+    ctl.open_report_data= mc3xxx_open_report_data;
+    ctl.enable_nodata = mc3xxx_enable_nodata;
+    ctl.set_delay  = mc3xxx_set_delay;
+    ctl.is_report_input_direct = false;
+    ctl.is_support_batch = obj->hw->is_batch_supported;
+
+    err = acc_register_control_path(&ctl);
+    if(err)
+    {
+         GSE_ERR("register acc control path err\n");
+        goto exit_kfree;
+    }
+
+    data.get_data = mc3xxx_get_data;
+    data.vender_div = 1000;
+    err = acc_register_data_path(&data);
+    if(err)
+    {
+        GSE_ERR("register acc data path err= %d\n", err);
+        goto exit_kfree;
+    }
+
+    #ifdef USE_EARLY_SUSPEND
+        obj->early_drv.level    = EARLY_SUSPEND_LEVEL_DISABLE_FB - 1,
+        obj->early_drv.suspend  = mc3xxx_early_suspend,
+        obj->early_drv.resume   = mc3xxx_late_resume,
+        register_early_suspend(&obj->early_drv);
+    #endif
+
+    GSE_LOG("%s: OK\n", __func__);
+    s_nInitFlag = MC3XXX_INIT_SUCC;
+    return 0;
+
+exit_create_attr_failed:
+    misc_deregister(&mc3xxx_device);
+exit_misc_device_register_failed:
+exit_init_failed:
+exit_kfree:
+    kfree(obj);
+    obj = NULL;
+exit:
+    GSE_ERR("%s: err = %d\n", __func__, err);
+    s_nInitFlag = MC3XXX_INIT_FAIL;
+
+    return err;
+}
+
+/*****************************************
+ *** mc3xxx_i2c_remove
+ *****************************************/
+static int mc3xxx_i2c_remove(struct i2c_client *client)
+{
+    int err = 0;
+
+    if((err = mc3xxx_delete_attr(&(mc3xxx_init_info.platform_diver_addr->driver))))
+        {
+            GSE_ERR("mc3xxx_delete_attr fail: %d\n", err);
+        }
+
+    if((err = misc_deregister(&mc3xxx_device)))
+    {
+        GSE_ERR("misc_deregister fail: %d\n", err);
+    }
+
+    #ifdef _MC3XXX_SUPPORT_VPROXIMITY_SENSOR_
+        misc_deregister(&mcube_psensor_device);
+    #endif
+
+    mc3xxx_i2c_client = NULL;
+    i2c_unregister_device(client);
+    kfree(i2c_get_clientdata(client));
+
+    return 0;
+}
+
+
+/*****************************************
+ *** mc3xxx_remove
+ *****************************************/
+static int mc3xxx_remove(void)
+{
+
+    GSE_FUN();
+
+    MC3XXX_power(hw, 0);
+    i2c_del_driver(&mc3xxx_i2c_driver);
+
+    return 0;
+}
+
+/*****************************************
+ *** mc3xxx_local_init
+ *****************************************/
+static int  mc3xxx_local_init(void)
+{
+
+    GSE_FUN();
+    MC3XXX_power(hw, 1);
+
+    if(i2c_add_driver(&mc3xxx_i2c_driver))
+    {
+        GSE_ERR("add driver error\n");
+        return -1;
+    }
+
+    if(MC3XXX_INIT_FAIL == s_nInitFlag)
+    {
+        return -1;
+    }
+
+    return 0;
+}
+
+/*****************************************
+ *** mc3xxx_init
+ *****************************************/
+static int __init mc3xxx_init(void)
+{
+    const char *name = "mediatek,MC3410";
+    hw =	get_accel_dts_func(name, hw);
+	if (!hw)
+		hw = get_cust_acc_hw();
+	struct i2c_board_info   mc3xxx_i2c_board_info  = { I2C_BOARD_INFO(MC3XXX_DEV_NAME, hw->i2c_addr[0]) };
+    GSE_LOG("%s: i2c_number:%d, i2c_addr:0x%02x\n", __func__,hw->i2c_num,hw->i2c_addr[0]);
+    i2c_register_board_info(hw->i2c_num, &mc3xxx_i2c_board_info, 1);
+    acc_driver_add(&mc3xxx_init_info);
+    return 0;
+}
+
+/*****************************************
+ *** mc3xxx_exit
+ *****************************************/
+static void __exit mc3xxx_exit(void)
+{
+    GSE_FUN();
+}
+
+/*----------------------------------------------------------------------------*/
+module_init(mc3xxx_init);
+module_exit(mc3xxx_exit);
+/*----------------------------------------------------------------------------*/
+MODULE_DESCRIPTION("mc3XXX G-Sensor Driver");
+MODULE_AUTHOR("mCube-inc");
+MODULE_LICENSE("GPL");
+MODULE_VERSION(MC3XXX_DEV_DRIVER_VERSION);
+
diff --git a/drivers/misc/mediatek/accelerometer/mc3410-new/mc3410.h b/drivers/misc/mediatek/accelerometer/mc3410-new/mc3410.h
new file mode 100644
index 000000000..3ac9248d2
--- /dev/null
+++ b/drivers/misc/mediatek/accelerometer/mc3410-new/mc3410.h
@@ -0,0 +1,117 @@
+/*****************************************************************************
+ *
+ * Copyright (c) 2014 mCube, Inc.  All rights reserved.
+ *
+ * This source is subject to the mCube Software License.
+ * This software is protected by Copyright and the information and source code
+ * contained herein is confidential. The software including the source code
+ * may not be copied and the information contained herein may not be used or
+ * disclosed except with the written permission of mCube Inc.
+ *
+ * All other rights reserved.
+ *
+ * This code and information are provided "as is" without warranty of any
+ * kind, either expressed or implied, including but not limited to the
+ * implied warranties of merchantability and/or fitness for a
+ * particular purpose.
+ *
+ * The following software/firmware and/or related documentation ("mCube Software")
+ * have been modified by mCube Inc. All revisions are subject to any receiver's
+ * applicable license agreements with mCube Inc.
+ *
+ * Accelerometer Sensor Driver
+ * 
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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 _MC3XXX_H_
+    #define _MC3XXX_H_ 
+	 
+#include <linux/ioctl.h>
+
+/***********************************************
+ *** REGISTER MAP
+ ***********************************************/
+#define MC3XXX_REG_XOUT                    0x00
+#define MC3XXX_REG_YOUT                    0x01
+#define MC3XXX_REG_ZOUT                    0x02
+#define MC3XXX_REG_TILT_STATUS             0x03
+#define MC3XXX_REG_SAMPLE_RATE_STATUS      0x04
+#define MC3XXX_REG_SLEEP_COUNT             0x05
+#define MC3XXX_REG_INTERRUPT_ENABLE        0x06
+#define MC3XXX_REG_MODE_FEATURE            0x07
+#define MC3XXX_REG_SAMPLE_RATE             0x08
+#define MC3XXX_REG_TAP_DETECTION_ENABLE    0x09
+#define MC3XXX_REG_TAP_DWELL_REJECT        0x0A
+#define MC3XXX_REG_DROP_CONTROL            0x0B
+#define MC3XXX_REG_SHAKE_DEBOUNCE          0x0C
+#define MC3XXX_REG_XOUT_EX_L               0x0D
+#define MC3XXX_REG_XOUT_EX_H               0x0E
+#define MC3XXX_REG_YOUT_EX_L               0x0F
+#define MC3XXX_REG_YOUT_EX_H               0x10
+#define MC3XXX_REG_ZOUT_EX_L               0x11
+#define MC3XXX_REG_ZOUT_EX_H               0x12
+#define MC3XXX_REG_RANGE_CONTROL           0x20
+#define MC3XXX_REG_SHAKE_THRESHOLD         0x2B
+#define MC3XXX_REG_UD_Z_TH                 0x2C
+#define MC3XXX_REG_UD_X_TH                 0x2D
+#define MC3XXX_REG_RL_Z_TH                 0x2E
+#define MC3XXX_REG_RL_Y_TH                 0x2F
+#define MC3XXX_REG_FB_Z_TH                 0x30
+#define MC3XXX_REG_DROP_THRESHOLD          0x31
+#define MC3XXX_REG_TAP_THRESHOLD           0x32
+#define MC3XXX_REG_PRODUCT_CODE            0x3B
+
+/***********************************************
+ *** RETURN CODE
+ ***********************************************/
+#define MC3XXX_RETCODE_SUCCESS                 (0)
+#define MC3XXX_RETCODE_ERROR_I2C               (-1)
+#define MC3XXX_RETCODE_ERROR_NULL_POINTER      (-2)
+#define MC3XXX_RETCODE_ERROR_STATUS            (-3)
+#define MC3XXX_RETCODE_ERROR_SETUP             (-4)
+#define MC3XXX_RETCODE_ERROR_GET_DATA          (-5)
+#define MC3XXX_RETCODE_ERROR_IDENTIFICATION    (-6)
+
+/***********************************************
+ *** CONFIGURATION
+ ***********************************************/
+#define MC3XXX_BUF_SIZE    256
+
+/***********************************************
+ *** PRODUCT ID
+ ***********************************************/
+#define MC3XXX_PCODE_3210     0x90
+#define MC3XXX_PCODE_3230     0x19
+#define MC3XXX_PCODE_3250     0x88
+#define MC3XXX_PCODE_3410     0xA8
+#define MC3XXX_PCODE_3410N    0xB8
+#define MC3XXX_PCODE_3430     0x29
+#define MC3XXX_PCODE_3430N    0x39
+#define MC3XXX_PCODE_3510     0x40
+#define MC3XXX_PCODE_3530     0x30
+#define MC3XXX_PCODE_3216     0x10
+#define MC3XXX_PCODE_3236     0x60
+
+#define MC3XXX_PCODE_RESERVE_1    0x20
+#define MC3XXX_PCODE_RESERVE_2    0x11
+#define MC3XXX_PCODE_RESERVE_3    0x21
+#define MC3XXX_PCODE_RESERVE_4    0x61
+#define MC3XXX_PCODE_RESERVE_5    0xA0
+#define MC3XXX_PCODE_RESERVE_6    0xE0
+#define MC3XXX_PCODE_RESERVE_7    0x91
+#define MC3XXX_PCODE_RESERVE_8    0xA1
+#define MC3XXX_PCODE_RESERVE_9    0xE1
+
+#define MC3XXX_PCODE_RESERVE_10    0x99
+
+#endif    // END OF _MC3XXX_H_
+
diff --git a/drivers/misc/mediatek/accelerometer/mc3xxx_auto/Makefile b/drivers/misc/mediatek/accelerometer/mc3xxx_auto/Makefile
new file mode 100755
index 000000000..356818502
--- /dev/null
+++ b/drivers/misc/mediatek/accelerometer/mc3xxx_auto/Makefile
@@ -0,0 +1,4 @@
+include $(srctree)/drivers/misc/mediatek/Makefile.custom
+
+obj-y	:=  mc3xxx.o
+
diff --git a/drivers/misc/mediatek/accelerometer/mc3xxx_auto/mc3xxx.c b/drivers/misc/mediatek/accelerometer/mc3xxx_auto/mc3xxx.c
new file mode 100644
index 000000000..a5fe14d53
--- /dev/null
+++ b/drivers/misc/mediatek/accelerometer/mc3xxx_auto/mc3xxx.c
@@ -0,0 +1,4197 @@
+/*****************************************************************************
+ *
+ * Copyright (c) 2014 mCube, Inc.  All rights reserved.
+ *
+ * This source is subject to the mCube Software License.
+ * This software is protected by Copyright and the information and source code
+ * contained herein is confidential. The software including the source code
+ * may not be copied and the information contained herein may not be used or
+ * disclosed except with the written permission of mCube Inc.
+ *
+ * All other rights reserved.
+ *
+ * This code and information are provided "as is" without warranty of any
+ * kind, either expressed or implied, including but not limited to the
+ * implied warranties of merchantability and/or fitness for a
+ * particular purpose.
+ *
+ * The following software/firmware and/or related documentation ("mCube Software")
+ * have been modified by mCube Inc. All revisions are subject to any receiver's
+ * applicable license agreements with mCube Inc.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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.
+ *
+ *
+ *****************************************************************************/
+
+/*****************************************************************************
+ *** HEADER FILES
+ *****************************************************************************/
+#include <linux/interrupt.h>
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include <linux/irq.h>
+#include <linux/miscdevice.h>
+#include <asm/uaccess.h>
+#include <linux/delay.h>
+#include <linux/input.h>
+#include <linux/workqueue.h>
+#include <linux/kobject.h>
+#include <linux/earlysuspend.h>
+#include <linux/platform_device.h>
+#include <asm/atomic.h>
+
+#include <mach/mt_typedefs.h>
+#include <mach/mt_gpio.h>
+#include <mach/mt_pm_ldo.h>
+
+#include <cust_acc.h>
+#include <linux/hwmsensor.h>
+#include <linux/hwmsen_dev.h>
+#include <linux/sensors_io.h>
+#include <linux/hwmsen_helper.h>
+
+#include <linux/ioctl.h>
+#include <linux/wakelock.h>
+
+
+#define _MC3XXX_SUPPORT_MTK_NEW_ARCH__
+
+#ifdef _MC3XXX_SUPPORT_MTK_NEW_ARCH__
+#ifdef CUSTOM_KERNEL_SENSORHUB
+#include <SCP_sensorHub.h>
+#endif
+#include <accel.h>
+#endif
+
+#include "mc3xxx.h"
+
+/*****************************************************************************
+ *** CONFIGURATION
+ *****************************************************************************/
+#define _MC3XXX_SUPPORT_AUTO_DETECT_
+//#define _MC3XXX_SUPPORT_DOT_CALIBRATION_
+#define _MC3XXX_SUPPORT_LPF_
+#define _MC3XXX_SUPPORT_CONCURRENCY_PROTECTION_
+//#define _MC3XXX_SUPPORT_APPLY_AVERAGE_AGORITHM_
+//#define _MC3XXX_SUPPORT_PERIODIC_DOC_
+//#define _MC3XXX_SUPPORT_VPROXIMITY_SENSOR_
+#define _MC3XXX_SUPPORT_LRF_
+//#define _MC3XXX_SUPPORT_POWER_SAVING_SHUTDOWN_POWER_
+
+#define C_MAX_FIR_LENGTH    (32)
+
+#define VIRTUAL_Z    0
+
+/*****************************************************************************
+ *** CONSTANT / DEFINITION
+ *****************************************************************************/
+/**************************
+ *** CONFIGURATION
+ **************************/
+#define MC3XXX_DEV_NAME                        "MC3XXX"
+#define MC3XXX_DEV_DRIVER_VERSION              "2.1.6"
+#define MC3XXX_DEV_DRIVER_VERSION_VIRTUAL_Z    "1.0.1"
+
+/**************************
+ *** COMMON
+ **************************/
+#define MC3XXX_AXIS_X      0
+#define MC3XXX_AXIS_Y      1
+#define MC3XXX_AXIS_Z      2
+#define MC3XXX_AXES_NUM    3
+#define MC3XXX_DATA_LEN    6
+
+#define MC3XXX_RESOLUTION_LOW     1
+#define MC3XXX_RESOLUTION_HIGH    2
+
+#define MC3XXX_LOW_REOLUTION_DATA_SIZE     3
+#define MC3XXX_HIGH_REOLUTION_DATA_SIZE    6
+
+#define MC3XXX_INIT_SUCC    (0)
+#define MC3XXX_INIT_FAIL    (-1)
+
+#define MC3XXX_REGMAP_LENGTH    (64)
+
+/**************************
+ *** DEBUG
+ **************************/
+    /*********************
+     *** G-Sensor
+     *********************/
+    #if 0
+        #define GSE_TAG                  "[Gsensor] "
+        #define GSE_FUN(f)               printk(KERN_INFO GSE_TAG"%s\n", __FUNCTION__)
+        #define GSE_ERR(fmt, args...)    printk(KERN_ERR GSE_TAG"%s %d : "fmt, __FUNCTION__, __LINE__, ##args)
+        #define GSE_LOG(fmt, args...)    printk(KERN_INFO GSE_TAG fmt, ##args)
+    #else
+        #define GSE_TAG
+        #define GSE_FUN(f)               do {} while (0)
+        #define GSE_ERR(fmt, args...)    do {} while (0)
+        #define GSE_LOG(fmt, args...)    do {} while (0)
+    #endif
+    
+    /*********************
+     *** vProximity Sensor
+     *********************/
+    #if 0
+        #define PS_TAG                  "[mCube/Psensor] "
+        #define PS_FUN(f)               printk(KERN_INFO PS_TAG"%s\n", __FUNCTION__)
+        #define PS_ERR(fmt, args...)    printk(KERN_ERR  PS_TAG"%s %d : "fmt, __FUNCTION__, __LINE__, ##args)
+        #define PS_LOG(fmt, args...)    printk(KERN_ERR PS_TAG fmt, ##args)
+    #else
+        #define PS_TAG
+        #define PS_FUN(f)               do {} while (0)
+        #define PS_ERR(fmt, args...)    do {} while (0)
+        #define PS_LOG(fmt, args...)    do {} while (0)
+    #endif       
+
+/*****************************************************************************
+ *** DATA TYPE / STRUCTURE DEFINITION / ENUM
+ *****************************************************************************/
+typedef enum
+{
+    MCUBE_TRC_FILTER  = 0x01,
+    MCUBE_TRC_RAWDATA = 0x02,
+    MCUBE_TRC_IOCTL   = 0x04,
+    MCUBE_TRC_CALI    = 0X08,
+    MCUBE_TRC_INFO    = 0X10,
+    MCUBE_TRC_REGXYZ  = 0X20,
+}   MCUBE_TRC;
+
+struct scale_factor
+{
+    u8    whole;
+    u8    fraction;
+};
+
+struct data_resolution
+{
+    struct scale_factor    scalefactor;
+    int                    sensitivity;
+};
+
+struct data_filter
+{
+    s16    raw[C_MAX_FIR_LENGTH][MC3XXX_AXES_NUM];
+    int    sum[MC3XXX_AXES_NUM];
+    int    num;
+    int    idx;
+};
+
+struct mc3xxx_i2c_data
+{
+    //================================================
+    struct i2c_client          *client;
+    struct acc_hw              *hw;
+    struct hwmsen_convert       cvt;
+
+    //================================================
+    struct data_resolution     *reso;
+    atomic_t                    trace;
+    atomic_t                    suspend;
+    atomic_t                    selftest;
+    atomic_t                    filter;
+    s16                         cali_sw[MC3XXX_AXES_NUM + 1];
+    
+    //================================================
+    s16                         offset[MC3XXX_AXES_NUM + 1];
+    s16                         data[MC3XXX_AXES_NUM + 1];
+    
+    //================================================
+    #if defined(_MC3XXX_SUPPORT_LPF_)
+        atomic_t                firlen;
+        atomic_t                fir_en;
+        struct data_filter      fir;
+    #endif 
+
+    //================================================
+    #if defined(CONFIG_HAS_EARLYSUSPEND)
+        struct early_suspend    early_drv;
+    #endif     
+};
+
+#ifdef _MC3XXX_SUPPORT_LRF_
+    typedef struct
+    {
+        s16    nIsNewRound;
+        s16    nPreDiff;
+        s16    nPreValue;
+        s16    nMaxValue;
+        s16    nMinValue;
+        s16    nRepValue;
+        s16    nNewDataMonitorCount;
+    }   S_LRF_CB;
+#endif
+
+/*****************************************************************************
+ *** EXTERNAL FUNCTION
+ *****************************************************************************/
+extern struct acc_hw*    mc3xxx_get_cust_acc_hw(void);
+
+/*****************************************************************************
+ *** STATIC FUNCTION
+ *****************************************************************************/
+static int mc3xxx_i2c_probe(struct i2c_client *client, const struct i2c_device_id *id);
+static int mc3xxx_i2c_remove(struct i2c_client *client);
+
+static int _mc3xxx_i2c_auto_probe(struct i2c_client *client);
+
+#ifndef CONFIG_HAS_EARLYSUSPEND
+    static int mc3xxx_suspend(struct i2c_client *client, pm_message_t msg);
+    static int mc3xxx_resume(struct i2c_client *client);
+#endif
+
+#ifdef _MC3XXX_SUPPORT_AUTO_DETECT_
+    static int mc3xxx_local_init(void);
+    static int mc3xxx_remove(void);
+#endif
+
+static int MC3XXX_SetPowerMode(struct i2c_client *client, bool enable);
+static int MC3XXX_WriteCalibration(struct i2c_client *client, int dat[MC3XXX_AXES_NUM]);
+
+static void MC3XXX_SetGain(void);
+
+/*****************************************************************************
+ *** STATIC VARIBLE & CONTROL BLOCK DECLARATION
+ *****************************************************************************/
+static unsigned char    s_bResolution = 0x00;
+static unsigned char    s_bPCODE      = 0x00;
+static unsigned char    s_bPCODER     = 0x00;
+static unsigned char    s_bHWID       = 0x00;
+static unsigned char    s_bMPOL       = 0x00;
+
+static int    s_nInitFlag = MC3XXX_INIT_FAIL;
+
+#ifdef _MC3XXX_SUPPORT_AUTO_DETECT_
+    static struct sensor_init_info  mc3xxx_init_info =
+    {
+        .name   = MC3XXX_DEV_NAME,
+        .init   = mc3xxx_local_init,
+        .uninit = mc3xxx_remove,
+    };
+#else
+    static struct platform_driver    mc3xxx_gsensor_driver;
+#endif
+
+
+static const struct i2c_device_id                mc3xxx_i2c_id[]              = { {MC3XXX_DEV_NAME, 0}, {} };
+static       struct i2c_board_info __initdata    mc3xxx_i2c_board_info        = { I2C_BOARD_INFO(MC3XXX_DEV_NAME, 0x4C) };
+static       unsigned short                      mc3xxx_i2c_auto_probe_addr[] = { 0x4C, 0x6C, 0x4E, 0x6D, 0x6E, 0x6F };
+
+static struct i2c_driver    mc3xxx_i2c_driver = {
+	.driver = {
+	              .name = MC3XXX_DEV_NAME,
+	          },
+	.probe  = mc3xxx_i2c_probe,
+	.remove = mc3xxx_i2c_remove,
+
+	#if !defined(CONFIG_HAS_EARLYSUSPEND)    
+	    .suspend = mc3xxx_suspend,
+	    .resume  = mc3xxx_resume,
+	#endif
+
+	.id_table = mc3xxx_i2c_id,
+	};
+
+static struct i2c_client        *mc3xxx_i2c_client = NULL;
+static struct mc3xxx_i2c_data   *mc3xxx_obj_i2c_data = NULL;
+
+static struct data_resolution    mc3xxx_offset_resolution = { {7, 8}, 256 };
+
+static bool    mc3xxx_sensor_power = false;
+
+static GSENSOR_VECTOR3D    gsensor_gain, gsensor_offset;
+
+static char    selftestRes[10] = {0};
+
+static struct file* fd_file = NULL;
+static mm_segment_t oldfs;
+static unsigned char offset_buf[6]; 
+static signed int    offset_data[3];
+static signed int    gain_data[3];
+static unsigned char    s_baOTP_OffsetData[6] = { 0 };
+static signed int    s_nIsRBM_Enabled = false;
+
+#ifdef _MC3XXX_SUPPORT_LRF_
+    static S_LRF_CB    s_taLRF_CB[MC3XXX_AXES_NUM];
+#endif
+
+#ifdef _MC3XXX_SUPPORT_CONCURRENCY_PROTECTION_
+    static struct semaphore    s_tSemaProtect;
+#endif
+
+#if 1
+//#ifdef _MC3XXX_SUPPORT_DOT_CALIBRATION_
+    static int    s_nIsCaliLoaded = false;
+    static int    LPF_FirstRun = 1;
+#endif
+
+static int             LPF_SamplingRate = 5;
+static int             LPF_CutoffFrequency = 0x00000004;
+static unsigned int    iAReal0_X;
+static unsigned int    iAcc0Lpf0_X;
+static unsigned int    iAcc0Lpf1_X;
+static unsigned int    iAcc1Lpf0_X;
+static unsigned int    iAcc1Lpf1_X;
+
+static unsigned int    iAReal0_Y;
+static unsigned int    iAcc0Lpf0_Y;
+static unsigned int    iAcc0Lpf1_Y;
+static unsigned int    iAcc1Lpf0_Y;
+static unsigned int    iAcc1Lpf1_Y;
+
+static unsigned int    iAReal0_Z;
+static unsigned int    iAcc0Lpf0_Z;
+static unsigned int    iAcc0Lpf1_Z;
+static unsigned int    iAcc1Lpf0_Z;
+static unsigned int    iAcc1Lpf1_Z;
+
+static signed char    s_bAccuracyStatus = SENSOR_STATUS_ACCURACY_MEDIUM;
+
+#ifdef _MC3XXX_SUPPORT_PERIODIC_DOC_
+    static DECLARE_WAIT_QUEUE_HEAD(wq_mc3xxx_open_status);
+
+    static atomic_t    s_t_mc3xxx_open_status = ATOMIC_INIT(0);
+
+    static unsigned char    s_bIsPDOC_Enabled = false;
+#endif
+
+#ifdef _MC3XXX_SUPPORT_VPROXIMITY_SENSOR_
+    static int    P_STATUS;
+    static int    prev_P_STATUS = 0;
+#endif
+
+/*****************************************************************************
+ *** MACRO
+ *****************************************************************************/
+#ifdef _MC3XXX_SUPPORT_CONCURRENCY_PROTECTION_
+    #define MC3XXX_MUTEX_INIT()   sema_init(&s_tSemaProtect, 1)
+
+    #define MC3XXX_MUTEX_LOCK()                             \
+                if (down_interruptible(&s_tSemaProtect))    \
+                    return (-ERESTARTSYS)
+
+    #define MC3XXX_MUTEX_LOCK_RETURN_VOID()                 \
+                if (down_interruptible(&s_tSemaProtect))    \
+                    return
+
+    #define MC3XXX_MUTEX_UNLOCK()   up(&s_tSemaProtect)
+#else
+    #define MC3XXX_MUTEX_INIT()                do {} while (0)
+    #define MC3XXX_MUTEX_LOCK()                do {} while (0)
+    #define MC3XXX_MUTEX_LOCK_RETURN_VOID()    do {} while (0)
+    #define MC3XXX_MUTEX_UNLOCK()              do {} while (0)
+#endif
+
+#define MCUBE_RREMAP(nDataX, nDataY)                                                  \
+            if (MC3XXX_PCODE_3250 == s_bPCODE)                                        \
+            {                                                                         \
+                int    _nTemp = 0;                                                    \
+                                                                                      \
+                _nTemp = nDataX;                                                      \
+                nDataX = nDataY;                                                      \
+                nDataY = -_nTemp;                                                     \
+                GSE_LOG("[%s] 3250 read remap\n", __FUNCTION__);                      \
+            }                                                                         \
+            else                                                                      \
+            {                                                                         \
+                if (s_bMPOL & 0x01)    nDataX = -nDataX;                              \
+                if (s_bMPOL & 0x02)    nDataY = -nDataY;                              \
+                GSE_LOG("[%s] 35X0 remap [s_bMPOL: %d]\n", __FUNCTION__, s_bMPOL);    \
+            }
+
+#define MCUBE_WREMAP(nDataX, nDataY)                                                  \
+            if (MC3XXX_PCODE_3250 == s_bPCODE)                                        \
+            {                                                                         \
+                int    _nTemp = 0;                                                    \
+                                                                                      \
+                _nTemp = nDataX;                                                      \
+                nDataX = -nDataY;                                                     \
+                nDataY = _nTemp;                                                      \
+                GSE_LOG("[%s] 3250 write remap\n", __FUNCTION__);                     \
+            }                                                                         \
+            else                                                                      \
+            {                                                                         \
+                if (s_bMPOL & 0x01)    nDataX = -nDataX;                              \
+                if (s_bMPOL & 0x02)    nDataY = -nDataY;                              \
+                GSE_LOG("[%s] 35X0 remap [s_bMPOL: %d]\n", __FUNCTION__, s_bMPOL);    \
+            }
+
+#define IS_MCFM12()    ((0xC0 <= s_bHWID) && (s_bHWID <= 0xCF))
+#define IS_MCFM3X()    ((0x20 == s_bHWID) || ((0x22 <= s_bHWID) && (s_bHWID <= 0x2F)))
+
+/*****************************************************************************
+ *** TODO
+ *****************************************************************************/
+#define DATA_PATH              "/sdcard2/mcube-register-map.txt"
+
+#ifdef _MC3XXX_SUPPORT_DOT_CALIBRATION_
+    static char    file_path[MC3XXX_BUF_SIZE]        = "/data/data/com.mcube.acc/files/mcube-calib.txt";
+    static char    backup_file_path[MC3XXX_BUF_SIZE] = "/data/misc/sensors/mcube-calib.txt";
+#endif
+
+/*****************************************************************************
+ *** FUNCTION
+ *****************************************************************************/
+
+/*****************************************
+ *** GetLowPassFilter
+ *****************************************/
+static unsigned int GetLowPassFilter(unsigned int X0,unsigned int Y1)
+{
+    unsigned int lTemp;
+    
+    lTemp = Y1;
+    lTemp *= LPF_CutoffFrequency;    // 4HZ LPF RC=0.04
+    X0 *= LPF_SamplingRate;		
+    lTemp += X0;
+    lTemp += LPF_CutoffFrequency;
+    lTemp /= (LPF_CutoffFrequency + LPF_SamplingRate);		
+    Y1 = lTemp;
+    
+    return Y1;
+}
+
+/*****************************************
+ *** openFile
+ *****************************************/
+static struct file *openFile(char *path,int flag,int mode) 
+{ 
+    struct file *fp = NULL; 
+    
+    fp = filp_open(path, flag, mode); 
+
+    if (IS_ERR(fp) || !fp->f_op) 
+    {
+        GSE_LOG("Calibration File filp_open return NULL\n");
+        return NULL; 
+    }
+    else 
+    {
+        return fp; 
+    }
+} 
+ 
+/*****************************************
+ *** readFile
+ *****************************************/
+#ifdef _MC3XXX_SUPPORT_DOT_CALIBRATION_
+static int readFile(struct file *fp,char *buf,int readlen) 
+{ 
+    if (fp->f_op && fp->f_op->read) 
+        return fp->f_op->read(fp,buf,readlen, &fp->f_pos); 
+    else 
+        return -1; 
+} 
+#endif
+
+/*****************************************
+ *** writeFile
+ *****************************************/
+static int writeFile(struct file *fp,char *buf,int writelen) 
+{ 
+    if (fp->f_op && fp->f_op->write) 
+        return fp->f_op->write(fp,buf,writelen, &fp->f_pos); 
+    else 
+        return -1; 
+}
+ 
+/*****************************************
+ *** closeFile
+ *****************************************/
+static int closeFile(struct file *fp) 
+{ 
+    filp_close(fp,NULL); 
+    return 0; 
+} 
+
+/*****************************************
+ *** initKernelEnv
+ *****************************************/
+static void initKernelEnv(void) 
+{ 
+    oldfs = get_fs(); 
+    set_fs(KERNEL_DS);
+    printk(KERN_INFO "initKernelEnv\n");
+} 
+
+/*****************************************
+ *** mcube_write_log_data
+ *****************************************/
+static int mcube_write_log_data(struct i2c_client *client, u8 data[0x3f])
+{
+    #define _WRT_LOG_DATA_BUFFER_SIZE    (66 * 50)
+    
+    s16 rbm_data[3] = {0}, raw_data[3] = {0};
+    int err = 0;
+    char *_pszBuffer = NULL;
+    int n = 0, i = 0;
+
+    initKernelEnv();
+    fd_file = openFile(DATA_PATH ,O_RDWR | O_CREAT,0); 
+    if (fd_file == NULL) 
+    {
+        GSE_LOG("mcube_write_log_data fail to open\n");	
+    }
+    else
+    {
+        rbm_data[MC3XXX_AXIS_X] = (s16)((data[0x0d]) | (data[0x0e] << 8));
+        rbm_data[MC3XXX_AXIS_Y] = (s16)((data[0x0f]) | (data[0x10] << 8));
+        rbm_data[MC3XXX_AXIS_Z] = (s16)((data[0x11]) | (data[0x12] << 8));
+        
+        raw_data[MC3XXX_AXIS_X] = (rbm_data[MC3XXX_AXIS_X] + offset_data[0]/2)*gsensor_gain.x/gain_data[0];
+        raw_data[MC3XXX_AXIS_Y] = (rbm_data[MC3XXX_AXIS_Y] + offset_data[1]/2)*gsensor_gain.y/gain_data[1];
+        raw_data[MC3XXX_AXIS_Z] = (rbm_data[MC3XXX_AXIS_Z] + offset_data[2]/2)*gsensor_gain.z/gain_data[2];
+        
+        _pszBuffer = kzalloc(_WRT_LOG_DATA_BUFFER_SIZE, GFP_KERNEL);
+
+        if (NULL == _pszBuffer)
+        {
+            GSE_ERR("fail to allocate memory for buffer\n");
+            return -1;
+        }
+
+        memset(_pszBuffer, 0, _WRT_LOG_DATA_BUFFER_SIZE); 
+        
+        n += sprintf(_pszBuffer+n, "G-sensor RAW X = %d  Y = %d  Z = %d\n", raw_data[0] ,raw_data[1] ,raw_data[2]);
+        n += sprintf(_pszBuffer+n, "G-sensor RBM X = %d  Y = %d  Z = %d\n", rbm_data[0] ,rbm_data[1] ,rbm_data[2]);
+
+        for (i = 0; i < 64; i++)
+        {
+            n += sprintf(_pszBuffer+n, "mCube register map Register[%x] = 0x%x\n",i,data[i]);
+        }
+
+        msleep(50);		
+
+        if ((err = writeFile(fd_file,_pszBuffer,n)) > 0) 
+            GSE_LOG("buf:%s\n",_pszBuffer); 
+        else 
+            GSE_LOG("write file error %d\n",err); 
+        
+        kfree(_pszBuffer);
+        set_fs(oldfs); 
+        closeFile(fd_file); 
+    }
+
+    return 0;
+}
+
+#ifdef _MC3XXX_SUPPORT_DOT_CALIBRATION_
+/*****************************************
+ *** mcube_read_cali_file
+ *****************************************/
+static int mcube_read_cali_file(struct i2c_client *client)
+{
+    int cali_data[3] = {0},cali_data1[3] = {0};
+    int err = 0;
+    char buf[64] = {0};
+
+    GSE_LOG("[%s] sizeof(buf): %d\n",__func__, sizeof(buf));
+
+    initKernelEnv();
+    fd_file = openFile(file_path,O_RDONLY,0); 
+
+    if (fd_file == NULL) 
+    {
+        GSE_LOG("%s:fail to open calibration file: %s\n", __func__, file_path);
+        fd_file = openFile(backup_file_path,O_RDONLY,0); 
+
+        if(fd_file == NULL)	
+        {	
+            GSE_LOG("%s:fail to open calibration file: %s\n", __func__, backup_file_path);
+            cali_data[0] = 0;
+            cali_data[1] = 0;
+            cali_data[2] = 0;
+            return (-1);
+        }
+        else
+        {		
+            GSE_LOG("Open backup calibration file successfully: %s\n", backup_file_path);
+        }
+    }
+    else
+    {
+        GSE_LOG("Open calibration file successfully: %s\n", file_path);
+    }
+			
+    memset(buf, 0, sizeof(buf)); 
+
+    if ((err = readFile(fd_file, buf, sizeof(buf))) > 0) 
+        GSE_LOG("cali_file: buf:%s\n",buf); 
+    else 
+        GSE_LOG("read file error %d\n",err); 
+    
+    set_fs(oldfs); 
+    closeFile(fd_file); 
+
+    sscanf(buf, "%d %d %d",&cali_data[MC3XXX_AXIS_X], &cali_data[MC3XXX_AXIS_Y], &cali_data[MC3XXX_AXIS_Z]);
+    GSE_LOG("cali_data: %d %d %d\n", cali_data[MC3XXX_AXIS_X], cali_data[MC3XXX_AXIS_Y], cali_data[MC3XXX_AXIS_Z]); 	
+    
+    cali_data1[MC3XXX_AXIS_X] = cali_data[MC3XXX_AXIS_X] * gsensor_gain.x / GRAVITY_EARTH_1000;
+    cali_data1[MC3XXX_AXIS_Y] = cali_data[MC3XXX_AXIS_Y] * gsensor_gain.y / GRAVITY_EARTH_1000;
+    cali_data1[MC3XXX_AXIS_Z] = cali_data[MC3XXX_AXIS_Z] * gsensor_gain.z / GRAVITY_EARTH_1000;
+    
+    GSE_LOG("cali_data1: %d %d %d\n", cali_data1[MC3XXX_AXIS_X], cali_data1[MC3XXX_AXIS_Y], cali_data1[MC3XXX_AXIS_Z]); 	
+    
+    MC3XXX_WriteCalibration(client, cali_data1);
+    
+    return 0;
+}
+
+/*****************************************
+ *** mcube_load_cali
+ *****************************************/
+static void    mcube_load_cali(struct i2c_client *pt_i2c_client)
+{
+    if (false == s_nIsCaliLoaded)
+    {
+        GSE_LOG("[%s] loading cali file...\n", __FUNCTION__);
+        
+        if (MC3XXX_RETCODE_SUCCESS == mcube_read_cali_file(pt_i2c_client))
+            s_nIsCaliLoaded = true;
+    }
+}
+
+#endif // _MC3XXX_SUPPORT_DOT_CALIBRATION_
+
+/*****************************************
+ *** hwmsen_read_byte_sr
+ *****************************************/
+static int hwmsen_read_byte_sr(struct i2c_client *client, u8 addr, u8 *data)
+{
+    u8 buf = 0;
+    int ret = 0;
+    
+    client->addr = ((client->addr) & (I2C_MASK_FLAG | I2C_WR_FLAG |I2C_RS_FLAG));
+    buf = addr;
+    ret = i2c_master_send(client, (const char*)&buf, 1<<8 | 1);
+
+    if (ret < 0) {
+        HWM_ERR("send command error!!\n");
+        return -EFAULT;
+    }
+    
+    *data = buf;
+    client->addr = client->addr& I2C_MASK_FLAG;
+
+    return 0;
+}
+
+/*****************************************
+ *** mcube_psensor_ioctl
+ *****************************************/
+#ifdef _MC3XXX_SUPPORT_VPROXIMITY_SENSOR_
+static long mcube_psensor_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
+{ 
+    int err=0;
+    void __user *data;
+    int pSensor=0;
+    
+	if(_IOC_DIR(cmd) & _IOC_READ)
+	{
+		err = !access_ok(VERIFY_WRITE, (void __user *)arg, _IOC_SIZE(cmd));
+	}
+	else if(_IOC_DIR(cmd) & _IOC_WRITE)
+	{
+		err = !access_ok(VERIFY_READ, (void __user *)arg, _IOC_SIZE(cmd));
+	}
+
+	if(err)
+	{
+		PS_ERR("access error: %08X, (%2d, %2d)\n", cmd, _IOC_DIR(cmd), _IOC_SIZE(cmd));
+		return -EFAULT;
+	}
+
+	switch(cmd)
+	{
+		case PSENSOR_IOCTL_SET_POSTURE:
+			data = (void __user*)arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}
+			
+			if(copy_from_user(&pSensor, data, sizeof(int)))
+			{
+				err = -EFAULT;
+				break;	  
+			}
+			
+			P_STATUS = pSensor;
+			PS_LOG("IOCTL Get P_STATUS = %d",pSensor);		
+			break;
+		default:
+			
+			PS_ERR("unknown IOCTL: 0x%08x\n", cmd);
+			//err = -ENOIOCTLCMD;
+			break;	
+	}       
+	return err;
+}
+
+/*****************************************
+ *** STATIC STRUCTURE:: fops
+ *****************************************/
+static struct file_operations mcube_psensor_fops = {
+	.owner = THIS_MODULE,
+	.unlocked_ioctl = mcube_psensor_ioctl,
+};
+
+/*****************************************
+ *** STATIC STRUCTURE:: misc-device
+ *****************************************/
+static struct miscdevice mcube_psensor_device = {
+	.minor = MISC_DYNAMIC_MINOR,
+	.name = "psensor",
+	.fops  = &mcube_psensor_fops,
+};
+
+/*****************************************
+ *** psensor_ps_operate
+ *****************************************/
+static int psensor_ps_operate(void* self, uint32_t command, void* buff_in, int size_in,
+		void* buff_out /*sensor_data*/, int size_out, int* actualout)
+{
+	int err = 0;
+	int value;
+	int val=-1;
+	hwm_sensor_data* sensor_data;
+	//struct mcube_psensor_priv *obj = (struct mcube_psensor_priv *)self;
+	
+	//PS_FUN(f);
+	switch (command)
+	{
+		case SENSOR_DELAY:
+			if((buff_in == NULL) || (size_in < sizeof(int)))
+			{
+				PS_ERR("Set delay parameter error!\n");
+				err = -EINVAL;
+			}
+			// Do nothing
+			break;
+
+		case SENSOR_ENABLE:
+			value = *(int *)buff_in;
+			if(value)
+			{
+				if((buff_in == NULL) || (size_in < sizeof(int)))
+				{
+					PS_ERR("Enable sensor parameter error!\n");
+					err = -EINVAL;
+				}
+				
+				else
+				{				
+					value = *(int *)buff_in;
+					//obj->enable = 1;
+					PS_ERR("enable ps \n"); 
+				}
+			}
+			else
+			{
+				//obj->enable = 0;
+				PS_ERR("disable ps \n"); 
+			}
+			break;
+		case SENSOR_GET_DATA:
+			PS_LOG("fwq get ps data !!!!!!\n");
+			if((buff_out == NULL) || (size_out< sizeof(hwm_sensor_data)))
+			{
+				PS_ERR("get sensor data parameter error!\n");
+				err = -EINVAL;
+			}
+			else
+			{
+				sensor_data = (hwm_sensor_data *)buff_out;				
+				PS_LOG("mcube sensor data P_STATUS=%d\n",P_STATUS);
+				
+				if(prev_P_STATUS != P_STATUS )
+				{
+					if(P_STATUS == 0)
+					{
+						PS_LOG("TURN ON LCD\n");
+						val=1;
+						PS_LOG("Set val = 1, Proximity sensor far away\n");
+					}
+					else if (P_STATUS == 1)
+					{	
+						PS_LOG("TURN OFF LCD\n");
+						val=0;
+						PS_LOG("Set val = 0, Proximity sensor close\n");
+					}
+					PS_LOG("mcube sensor data prev_P_STATUS=%d, P_STATUS=%d\n",prev_P_STATUS,P_STATUS);
+					prev_P_STATUS = P_STATUS;	
+				}
+				else
+				{
+					PS_LOG("P_STATUS %5d=>%5d\n",prev_P_STATUS,P_STATUS);
+					prev_P_STATUS = P_STATUS;
+					val= (P_STATUS==0)? (1):(0);
+				}	
+				sensor_data->values[0] = val;	
+				sensor_data->value_divide = 1;
+				sensor_data->status = SENSOR_STATUS_ACCURACY_MEDIUM;
+				prev_P_STATUS = P_STATUS;	
+			}
+			break;
+		default:
+			PS_ERR("proxmy sensor operate function has no this command %d!\n", command);
+			err = -1;
+			break;
+	}
+	
+	return err;
+}
+#endif  // end of _MC3XXX_SUPPORT_VPROXIMITY_SENSOR_
+
+/*****************************************
+ *** MC3XXX_power
+ *****************************************/
+static void MC3XXX_power(struct acc_hw *hw, unsigned int on) 
+{
+	static unsigned int power_on = 0;
+
+	if(hw->power_id != MT65XX_POWER_NONE)		// have externel LDO
+	{        
+		GSE_LOG("power %s\n", on ? "on" : "off");
+		if(power_on == on)	// power status not change
+		{
+			GSE_LOG("ignore power control: %d\n", on);
+		}
+		else if(on)	// power on
+		{
+			if(!hwPowerOn(hw->power_id, hw->power_vol, MC3XXX_DEV_NAME))
+			{
+				GSE_ERR("power on fails!!\n");
+			}
+		}
+		else	// power off
+		{
+			if (!hwPowerDown(hw->power_id, MC3XXX_DEV_NAME))
+			{
+				GSE_ERR("power off fail!!\n");
+			}			  
+		}
+	}
+	power_on = on;    
+}
+
+#if 1
+//#ifdef _MC3XXX_SUPPORT_DOT_CALIBRATION_
+/*****************************************
+ *** MC3XXX_rbm
+ *****************************************/
+static void MC3XXX_rbm(struct i2c_client *client, int enable)
+{
+    u8    _baDataBuf[3] = { 0 };
+    
+    _baDataBuf[0] = 0x43; 
+    hwmsen_write_block(client, 0x07, _baDataBuf, 0x01);
+    
+    hwmsen_read_block(client, 0x04, _baDataBuf, 0x01);
+    
+    //GSE_LOG("[%s] REG(0x04): 0x%X, enable: %d\n", __FUNCTION__, _baDataBuf[0], enable);
+    
+    if (0x00 == (_baDataBuf[0] & 0x40))
+    {
+        _baDataBuf[0] = 0x6D; 
+        hwmsen_write_block(client, 0x1B, _baDataBuf, 0x01);
+
+        _baDataBuf[0] = 0x43; 
+        hwmsen_write_block(client, 0x1B, _baDataBuf, 0x01);
+    }
+    
+    //hwmsen_read_block(client, 0x04, _baDataBuf, 0x01);
+    //GSE_LOG("BEGIN - REG(0x04): 0x%X\n", _baDataBuf[0]);
+    
+    if (1 == enable)
+    {
+        _baDataBuf[0] = 0x00;
+        hwmsen_write_block(client, 0x3B, _baDataBuf, 0x01);
+        
+        _baDataBuf[0] = 0x02; 
+        hwmsen_write_block(client, 0x14, _baDataBuf, 0x01);
+
+        if (MC3XXX_RESOLUTION_LOW == s_bResolution)
+        {
+            gsensor_gain.x = gsensor_gain.y = gsensor_gain.z = 1024;
+        }
+
+        s_nIsRBM_Enabled = 1;
+        LPF_FirstRun = 1;
+        
+        GSE_LOG("set rbm!!\n");
+    }
+    else if (0 == enable)
+    {
+        _baDataBuf[0] = 0x00; 
+        hwmsen_write_block(client, 0x14, _baDataBuf, 0x01);
+        
+        _baDataBuf[0] = s_bPCODER; 
+        hwmsen_write_block(client, 0x3B, _baDataBuf, 0x01);
+        
+        MC3XXX_SetGain();
+
+        s_nIsRBM_Enabled = 0;
+        
+        GSE_LOG("clear rbm!!\n");
+    }
+    
+    hwmsen_read_block(client, 0x04, _baDataBuf, 0x01);
+    
+    //GSE_LOG("RBM CONTROL DONE - REG(0x04): 0x%X\n", _baDataBuf[0]);
+    
+    if (_baDataBuf[0] & 0x40)
+    {
+        _baDataBuf[0] = 0x6D; 
+        hwmsen_write_block(client, 0x1B, _baDataBuf, 0x01);
+        
+        _baDataBuf[0] = 0x43; 
+        hwmsen_write_block(client, 0x1B, _baDataBuf, 0x01);
+    }
+    
+    //hwmsen_read_block(client, 0x04, _baDataBuf, 0x01);
+    //GSE_LOG("END - REG(0x04): 0x%X\n", _baDataBuf[0]);
+    
+    _baDataBuf[0] = 0x41; 
+    hwmsen_write_block(client, 0x07, _baDataBuf, 0x01);
+    
+    msleep(220);
+}
+
+/*****************************************
+ *** MC3XXX_ReadData_RBM
+ *****************************************/
+static int MC3XXX_ReadData_RBM(struct i2c_client *client, int data[MC3XXX_AXES_NUM])
+{
+	u8 addr = 0x0d;
+	u8 rbm_buf[MC3XXX_DATA_LEN] = {0};
+	int err = 0;
+
+	if(NULL == client)
+	{
+		err = -EINVAL;
+		return err;
+	}
+	
+/********************************************/
+	
+	if((err = hwmsen_read_block(client, addr, rbm_buf, 0x06)))
+    {
+    	GSE_ERR("error: %d\n", err);
+    	return err;
+    }
+
+	data[MC3XXX_AXIS_X] = (s16)((rbm_buf[0]) | (rbm_buf[1] << 8));
+	data[MC3XXX_AXIS_Y] = (s16)((rbm_buf[2]) | (rbm_buf[3] << 8));
+	data[MC3XXX_AXIS_Z] = (s16)((rbm_buf[4]) | (rbm_buf[5] << 8));
+
+	GSE_LOG("rbm_buf<<<<<[%02x %02x %02x %02x %02x %02x]\n",rbm_buf[0], rbm_buf[2], rbm_buf[2], rbm_buf[3], rbm_buf[4], rbm_buf[5]);
+	GSE_LOG("RBM<<<<<[%04x %04x %04x]\n", data[MC3XXX_AXIS_X], data[MC3XXX_AXIS_Y], data[MC3XXX_AXIS_Z]);
+	GSE_LOG("RBM<<<<<[%04d %04d %04d]\n", data[MC3XXX_AXIS_X], data[MC3XXX_AXIS_Y], data[MC3XXX_AXIS_Z]);
+
+
+/********************************************/
+    MCUBE_RREMAP(data[0], data[1]);
+
+	return err;
+}
+#endif // _MC3XXX_SUPPORT_DOT_CALIBRATION_
+
+/*****************************************
+ *** MC3XXX_ValidateSensorIC
+ *****************************************/
+static int MC3XXX_ValidateSensorIC(unsigned char *pbPCode, unsigned char *pbHwID)
+{
+    GSE_LOG("[%s] *pbPCode: 0x%02X, *pbHwID: 0x%02X\n", __FUNCTION__, *pbPCode, *pbHwID);
+
+    if (   (0x01 == *pbHwID)
+        || (0x03 == *pbHwID)
+        || ((0x04 <= *pbHwID) && (*pbHwID <= 0x0F)))
+    {
+        if ((MC3XXX_PCODE_3210 == *pbPCode) || (MC3XXX_PCODE_3230 == *pbPCode) || (MC3XXX_PCODE_3250 == *pbPCode))
+            return (MC3XXX_RETCODE_SUCCESS);
+    }
+    else if (   (0x02 == *pbHwID)
+             || (0x21 == *pbHwID)
+             || ((0x10 <= *pbHwID) && (*pbHwID <= 0x1F)))
+    {
+        if (   (MC3XXX_PCODE_3210 == *pbPCode) || (MC3XXX_PCODE_3230  == *pbPCode)
+            || (MC3XXX_PCODE_3250 == *pbPCode)
+            || (MC3XXX_PCODE_3410 == *pbPCode) || (MC3XXX_PCODE_3410N == *pbPCode)
+            || (MC3XXX_PCODE_3430 == *pbPCode) || (MC3XXX_PCODE_3430N == *pbPCode))
+        {
+            return (MC3XXX_RETCODE_SUCCESS);
+        }
+    }
+    else if ((0xC0 <= *pbHwID) && (*pbHwID <= 0xCF))
+    {
+        *pbPCode = (*pbPCode & 0x71);
+
+        if ((MC3XXX_PCODE_3510 == *pbPCode) || (MC3XXX_PCODE_3530 == *pbPCode))
+            return (MC3XXX_RETCODE_SUCCESS);
+    }
+    else if ((0x20 == *pbHwID) || ((0x22 <= *pbHwID) && (*pbHwID <= 0x2F)))
+    {
+        *pbPCode = (*pbPCode & 0xF1);
+
+        if (   (MC3XXX_PCODE_3210      == *pbPCode) || (MC3XXX_PCODE_3216      == *pbPCode) || (MC3XXX_PCODE_3236      == *pbPCode)
+            || (MC3XXX_PCODE_RESERVE_1 == *pbPCode) || (MC3XXX_PCODE_RESERVE_2 == *pbPCode) || (MC3XXX_PCODE_RESERVE_3 == *pbPCode)
+            || (MC3XXX_PCODE_RESERVE_4 == *pbPCode) || (MC3XXX_PCODE_RESERVE_5 == *pbPCode) || (MC3XXX_PCODE_RESERVE_6 == *pbPCode)
+            || (MC3XXX_PCODE_RESERVE_7 == *pbPCode) || (MC3XXX_PCODE_RESERVE_8 == *pbPCode) || (MC3XXX_PCODE_RESERVE_9 == *pbPCode))
+        {
+            return (MC3XXX_RETCODE_SUCCESS);
+        }
+    }
+
+    return (MC3XXX_RETCODE_ERROR_IDENTIFICATION);
+}
+
+/*****************************************
+ *** MC3XXX_Read_Chip_ID
+ *****************************************/
+static int MC3XXX_Read_Chip_ID(struct i2c_client *client, char *buf)
+{
+    u8     _bChipID[4] = { 0 };
+
+    GSE_LOG("[%s]\n", __func__);
+
+    if (!buf || !client)
+        return EINVAL;
+
+    if (hwmsen_read_block(client, 0x3C, _bChipID, 4))
+    {
+        GSE_ERR("[%s] i2c read fail\n", __func__);
+        _bChipID[0] = 0;
+        _bChipID[1] = 0;
+        _bChipID[2] = 0;
+        _bChipID[3] = 0;
+    }
+
+    GSE_LOG("[%s] %02X-%02X-%02X-%02X\n", __func__, _bChipID[3], _bChipID[2], _bChipID[1], _bChipID[0]);
+
+    return sprintf(buf, "%02X-%02X-%02X-%02X\n", _bChipID[3], _bChipID[2], _bChipID[1], _bChipID[0]);
+}
+
+/*****************************************
+ *** MC3XXX_Read_Reg_Map
+ *****************************************/
+static int MC3XXX_Read_Reg_Map(struct i2c_client *p_i2c_client, u8 *pbUserBuf)
+{
+    u8     _baData[MC3XXX_REGMAP_LENGTH] = { 0 };
+    int    _nIndex = 0;
+
+    GSE_LOG("[%s]\n", __func__);
+
+    if(NULL == p_i2c_client)
+        return (-EINVAL);
+
+    for(_nIndex = 0; _nIndex < MC3XXX_REGMAP_LENGTH; _nIndex++)
+    {
+        hwmsen_read_block(p_i2c_client, _nIndex, &_baData[_nIndex], 1);
+
+        if (NULL != pbUserBuf)
+            pbUserBuf[_nIndex] = _baData[_nIndex];
+
+        printk(KERN_INFO "[Gsensor] REG[0x%02X] = 0x%02X\n", _nIndex, _baData[_nIndex]);
+    }
+
+    mcube_write_log_data(p_i2c_client, _baData);
+
+    return (0);
+}
+
+/*****************************************
+ *** MC3XXX_SaveDefaultOffset
+ *****************************************/
+static void MC3XXX_SaveDefaultOffset(struct i2c_client *p_i2c_client)
+{
+    GSE_LOG("[%s]\n", __func__);
+
+    hwmsen_read_block(p_i2c_client, 0x21, &s_baOTP_OffsetData[0], 3);
+    hwmsen_read_block(p_i2c_client, 0x24, &s_baOTP_OffsetData[3], 3);
+
+    GSE_LOG("s_baOTP_OffsetData: 0x%02X - 0x%02X - 0x%02X - 0x%02X - 0x%02X - 0x%02X\n",
+            s_baOTP_OffsetData[0], s_baOTP_OffsetData[1], s_baOTP_OffsetData[2],
+            s_baOTP_OffsetData[3], s_baOTP_OffsetData[4], s_baOTP_OffsetData[5]);
+}
+
+/*****************************************
+ *** MC3XXX_LPF
+ *****************************************/
+#ifdef _MC3XXX_SUPPORT_LPF_
+static void MC3XXX_LPF(struct mc3xxx_i2c_data *priv, s16 data[MC3XXX_AXES_NUM])
+{
+    if(atomic_read(&priv->filter))
+    {
+        if(atomic_read(&priv->fir_en) && !atomic_read(&priv->suspend))
+        {
+            int idx, firlen = atomic_read(&priv->firlen);   
+            if(priv->fir.num < firlen)
+            {                
+                priv->fir.raw[priv->fir.num][MC3XXX_AXIS_X] = data[MC3XXX_AXIS_X];
+                priv->fir.raw[priv->fir.num][MC3XXX_AXIS_Y] = data[MC3XXX_AXIS_Y];
+                priv->fir.raw[priv->fir.num][MC3XXX_AXIS_Z] = data[MC3XXX_AXIS_Z];
+                priv->fir.sum[MC3XXX_AXIS_X] += data[MC3XXX_AXIS_X];
+                priv->fir.sum[MC3XXX_AXIS_Y] += data[MC3XXX_AXIS_Y];
+                priv->fir.sum[MC3XXX_AXIS_Z] += data[MC3XXX_AXIS_Z];
+                if(atomic_read(&priv->trace) & MCUBE_TRC_FILTER)
+                {
+                    GSE_LOG("add [%2d] [%5d %5d %5d] => [%5d %5d %5d]\n", priv->fir.num,
+                    priv->fir.raw[priv->fir.num][MC3XXX_AXIS_X], priv->fir.raw[priv->fir.num][MC3XXX_AXIS_Y], priv->fir.raw[priv->fir.num][MC3XXX_AXIS_Z],
+                    priv->fir.sum[MC3XXX_AXIS_X], priv->fir.sum[MC3XXX_AXIS_Y], priv->fir.sum[MC3XXX_AXIS_Z]);
+                }
+                priv->fir.num++;
+                priv->fir.idx++;
+            }
+            else
+            {
+                idx = priv->fir.idx % firlen;
+                priv->fir.sum[MC3XXX_AXIS_X] -= priv->fir.raw[idx][MC3XXX_AXIS_X];
+                priv->fir.sum[MC3XXX_AXIS_Y] -= priv->fir.raw[idx][MC3XXX_AXIS_Y];
+                priv->fir.sum[MC3XXX_AXIS_Z] -= priv->fir.raw[idx][MC3XXX_AXIS_Z];
+                priv->fir.raw[idx][MC3XXX_AXIS_X] = data[MC3XXX_AXIS_X];
+                priv->fir.raw[idx][MC3XXX_AXIS_Y] = data[MC3XXX_AXIS_Y];
+                priv->fir.raw[idx][MC3XXX_AXIS_Z] = data[MC3XXX_AXIS_Z];
+                priv->fir.sum[MC3XXX_AXIS_X] += data[MC3XXX_AXIS_X];
+                priv->fir.sum[MC3XXX_AXIS_Y] += data[MC3XXX_AXIS_Y];
+                priv->fir.sum[MC3XXX_AXIS_Z] += data[MC3XXX_AXIS_Z];
+                priv->fir.idx++;
+                data[MC3XXX_AXIS_X] = priv->fir.sum[MC3XXX_AXIS_X]/firlen;
+                data[MC3XXX_AXIS_Y] = priv->fir.sum[MC3XXX_AXIS_Y]/firlen;
+                data[MC3XXX_AXIS_Z] = priv->fir.sum[MC3XXX_AXIS_Z]/firlen;
+                if(atomic_read(&priv->trace) & MCUBE_TRC_FILTER)
+                {
+                    GSE_LOG("add [%2d] [%5d %5d %5d] => [%5d %5d %5d] : [%5d %5d %5d]\n", idx,
+                    priv->fir.raw[idx][MC3XXX_AXIS_X], priv->fir.raw[idx][MC3XXX_AXIS_Y], priv->fir.raw[idx][MC3XXX_AXIS_Z],
+                    priv->fir.sum[MC3XXX_AXIS_X], priv->fir.sum[MC3XXX_AXIS_Y], priv->fir.sum[MC3XXX_AXIS_Z],
+                    data[MC3XXX_AXIS_X], data[MC3XXX_AXIS_Y], data[MC3XXX_AXIS_Z]);
+                }
+            }
+        }
+    }	
+}
+#endif    // END OF #ifdef _MC3XXX_SUPPORT_LPF_
+
+#ifdef _MC3XXX_SUPPORT_LRF_
+/*****************************************
+ *** _MC3XXX_LowResFilter
+ *****************************************/
+static void _MC3XXX_LowResFilter(s16 nAxis, s16 naData[MC3XXX_AXES_NUM])
+{
+    #define _LRF_DIFF_COUNT_POS                  2
+    #define _LRF_DIFF_COUNT_NEG                  (-_LRF_DIFF_COUNT_POS)
+    #define _LRF_DIFF_BOUNDARY_POS               (_LRF_DIFF_COUNT_POS + 1)
+    #define _LRF_DIFF_BOUNDARY_NEG               (_LRF_DIFF_COUNT_NEG - 1)
+    #define _LRF_DIFF_DATA_UNCHANGE_MAX_COUNT    11
+    
+    signed int    _nCurrDiff = 0;
+    signed int    _nSumDiff  = 0;
+    s16           _nCurrData = naData[nAxis];
+    
+    _nCurrDiff = (_nCurrData - s_taLRF_CB[nAxis].nRepValue);
+    
+    if ((_LRF_DIFF_COUNT_NEG < _nCurrDiff) && (_nCurrDiff < _LRF_DIFF_COUNT_POS))
+    {
+        if (s_taLRF_CB[nAxis].nIsNewRound)
+        {
+            s_taLRF_CB[nAxis].nMaxValue = _nCurrData;
+            s_taLRF_CB[nAxis].nMinValue = _nCurrData;
+            
+            s_taLRF_CB[nAxis].nIsNewRound = 0;
+            s_taLRF_CB[nAxis].nNewDataMonitorCount = 0;
+        }
+        else
+        {
+            if (_nCurrData > s_taLRF_CB[nAxis].nMaxValue)
+                s_taLRF_CB[nAxis].nMaxValue = _nCurrData;
+            else if (_nCurrData < s_taLRF_CB[nAxis].nMinValue)
+                s_taLRF_CB[nAxis].nMinValue = _nCurrData;
+            
+            if (s_taLRF_CB[nAxis].nMinValue != s_taLRF_CB[nAxis].nMaxValue)
+            {
+                if (_nCurrData == s_taLRF_CB[nAxis].nPreValue)
+                    s_taLRF_CB[nAxis].nNewDataMonitorCount++;
+                else
+                    s_taLRF_CB[nAxis].nNewDataMonitorCount = 0;
+            }
+        }
+        
+        if (1 != (s_taLRF_CB[nAxis].nMaxValue - s_taLRF_CB[nAxis].nMinValue))
+            s_taLRF_CB[nAxis].nRepValue = ((s_taLRF_CB[nAxis].nMaxValue + s_taLRF_CB[nAxis].nMinValue) / 2);
+        
+        _nSumDiff = (_nCurrDiff + s_taLRF_CB[nAxis].nPreDiff);
+        
+        if (_nCurrDiff)
+            s_taLRF_CB[nAxis].nPreDiff = _nCurrDiff;
+        
+        if ((_LRF_DIFF_BOUNDARY_NEG < _nSumDiff) && (_nSumDiff < _LRF_DIFF_BOUNDARY_POS))
+        {
+            if (_LRF_DIFF_DATA_UNCHANGE_MAX_COUNT > s_taLRF_CB[nAxis].nNewDataMonitorCount)
+            {
+                naData[nAxis] = s_taLRF_CB[nAxis].nRepValue;
+                goto _LRF_RETURN;
+            }
+        }
+    }
+    
+    s_taLRF_CB[nAxis].nRepValue   = _nCurrData;
+    s_taLRF_CB[nAxis].nPreDiff    = 0;
+    s_taLRF_CB[nAxis].nIsNewRound = 1;
+    
+_LRF_RETURN:
+    
+    GSE_LOG(">>>>> [_MC3XXX_LowResFilter][%d] _nCurrDiff: %4d       _nSumDiff: %4d         _nCurrData:    %4d         Rep:    %4d\n", nAxis, _nCurrDiff, _nSumDiff, _nCurrData, s_taLRF_CB[nAxis].nRepValue);
+    
+    s_taLRF_CB[nAxis].nPreValue = _nCurrData;
+    
+    #undef _LRF_DIFF_COUNT_POS
+    #undef _LRF_DIFF_COUNT_NEG
+    #undef _LRF_DIFF_BOUNDARY_POS
+    #undef _LRF_DIFF_BOUNDARY_NEG
+    #undef _LRF_DIFF_DATA_UNCHANGE_MAX_COUNT
+}
+#endif    // END OF #ifdef _MC3XXX_SUPPORT_LRF_
+
+/*****************************************
+ *** _MC3XXX_ReadData_RBM2RAW
+ *****************************************/
+static void    _MC3XXX_ReadData_RBM2RAW(s16 waData[MC3XXX_AXES_NUM])
+{
+    waData[MC3XXX_AXIS_X] = (waData[MC3XXX_AXIS_X] + offset_data[MC3XXX_AXIS_X] / 2) * 1024 / gain_data[MC3XXX_AXIS_X] + 8096;
+    waData[MC3XXX_AXIS_Y] = (waData[MC3XXX_AXIS_Y] + offset_data[MC3XXX_AXIS_Y] / 2) * 1024 / gain_data[MC3XXX_AXIS_Y] + 8096;
+    waData[MC3XXX_AXIS_Z] = (waData[MC3XXX_AXIS_Z] + offset_data[MC3XXX_AXIS_Z] / 2) * 1024 / gain_data[MC3XXX_AXIS_Z] + 8096;
+
+    GSE_LOG("RBM->RAW <<<<<[%08d %08d %08d]\n", waData[MC3XXX_AXIS_X], waData[MC3XXX_AXIS_Y], waData[MC3XXX_AXIS_Z]);
+
+    iAReal0_X             = (0x0010 * waData[MC3XXX_AXIS_X]);
+    iAcc1Lpf0_X           = GetLowPassFilter(iAReal0_X,iAcc1Lpf1_X);
+    iAcc0Lpf0_X           = GetLowPassFilter(iAcc1Lpf0_X,iAcc0Lpf1_X);
+    waData[MC3XXX_AXIS_X] = (iAcc0Lpf0_X / 0x0010);
+    
+    iAReal0_Y             = (0x0010 * waData[MC3XXX_AXIS_Y]);
+    iAcc1Lpf0_Y           = GetLowPassFilter(iAReal0_Y,iAcc1Lpf1_Y);
+    iAcc0Lpf0_Y           = GetLowPassFilter(iAcc1Lpf0_Y,iAcc0Lpf1_Y);
+    waData[MC3XXX_AXIS_Y] = (iAcc0Lpf0_Y / 0x0010);
+    
+    iAReal0_Z             = (0x0010 * waData[MC3XXX_AXIS_Z]);
+    iAcc1Lpf0_Z           = GetLowPassFilter(iAReal0_Z,iAcc1Lpf1_Z);
+    iAcc0Lpf0_Z           = GetLowPassFilter(iAcc1Lpf0_Z,iAcc0Lpf1_Z);
+    waData[MC3XXX_AXIS_Z] = (iAcc0Lpf0_Z / 0x0010);
+    
+    GSE_LOG("RBM->RAW->LPF <<<<<[%08d %08d %08d]\n", waData[MC3XXX_AXIS_X], waData[MC3XXX_AXIS_Y], waData[MC3XXX_AXIS_Z]);	
+
+    waData[MC3XXX_AXIS_X] = (waData[MC3XXX_AXIS_X] - 8096) * gsensor_gain.x / 1024;
+    waData[MC3XXX_AXIS_Y] = (waData[MC3XXX_AXIS_Y] - 8096) * gsensor_gain.y / 1024;
+    waData[MC3XXX_AXIS_Z] = (waData[MC3XXX_AXIS_Z] - 8096) * gsensor_gain.z / 1024;
+
+    GSE_LOG("RBM->RAW->LPF->RAW <<<<<[%08d %08d %08d]\n", waData[MC3XXX_AXIS_X], waData[MC3XXX_AXIS_Y], waData[MC3XXX_AXIS_Z]);
+
+    iAcc0Lpf1_X=iAcc0Lpf0_X;
+    iAcc1Lpf1_X=iAcc1Lpf0_X;
+    iAcc0Lpf1_Y=iAcc0Lpf0_Y;
+    iAcc1Lpf1_Y=iAcc1Lpf0_Y;
+    iAcc0Lpf1_Z=iAcc0Lpf0_Z;
+    iAcc1Lpf1_Z=iAcc1Lpf0_Z;
+}
+
+/*****************************************
+ *** MC3XXX_ReadData
+ *****************************************/
+static int    MC3XXX_ReadData(struct i2c_client *pt_i2c_client, s16 waData[MC3XXX_AXES_NUM])
+{
+    u8    _baData[MC3XXX_DATA_LEN] = { 0 };
+
+    GSE_LOG("[%s] s_nIsRBM_Enabled: %d\n", __FUNCTION__, s_nIsRBM_Enabled);
+
+    if (NULL == pt_i2c_client)
+    {
+        GSE_ERR("ERR: Null Pointer\n");	
+
+        return (MC3XXX_RETCODE_ERROR_NULL_POINTER);
+    }
+	
+    if (!s_nIsRBM_Enabled)
+    {
+        if (MC3XXX_RESOLUTION_LOW == s_bResolution)
+        {
+            if (hwmsen_read_block(pt_i2c_client, MC3XXX_REG_XOUT, _baData, MC3XXX_LOW_REOLUTION_DATA_SIZE))
+            {
+                GSE_ERR("ERR: fail to read data via I2C!\n");
+
+                return (MC3XXX_RETCODE_ERROR_I2C);
+            }
+    
+            waData[MC3XXX_AXIS_X] = ((s8) _baData[0]);
+            waData[MC3XXX_AXIS_Y] = ((s8) _baData[1]);
+            waData[MC3XXX_AXIS_Z] = ((s8) _baData[2]);	 
+    
+            GSE_LOG("[%s][low] X: %d, Y: %d, Z: %d\n", __FUNCTION__, waData[MC3XXX_AXIS_X], waData[MC3XXX_AXIS_Y], waData[MC3XXX_AXIS_Z]);
+
+
+            #ifdef _MC3XXX_SUPPORT_LRF_
+                _MC3XXX_LowResFilter(MC3XXX_AXIS_X, waData);
+                _MC3XXX_LowResFilter(MC3XXX_AXIS_Y, waData);
+                _MC3XXX_LowResFilter(MC3XXX_AXIS_Z, waData);
+            #endif
+        }
+        else if (MC3XXX_RESOLUTION_HIGH == s_bResolution)
+        {
+            if (hwmsen_read_block(pt_i2c_client, MC3XXX_REG_XOUT_EX_L, _baData, MC3XXX_HIGH_REOLUTION_DATA_SIZE))
+            {
+                GSE_ERR("ERR: fail to read data via I2C!\n");
+
+                return (MC3XXX_RETCODE_ERROR_I2C);
+            }
+        
+            waData[MC3XXX_AXIS_X] = ((signed short) ((_baData[0]) | (_baData[1]<<8)));
+            waData[MC3XXX_AXIS_Y] = ((signed short) ((_baData[2]) | (_baData[3]<<8)));
+            waData[MC3XXX_AXIS_Z] = ((signed short) ((_baData[4]) | (_baData[5]<<8)));
+        
+            GSE_LOG("[%s][high] X: %d, Y: %d, Z: %d\n", __FUNCTION__, waData[MC3XXX_AXIS_X], waData[MC3XXX_AXIS_Y], waData[MC3XXX_AXIS_Z]);
+        }
+
+        GSE_LOG("RAW<<<<<[%04d %04d %04d]\n", waData[MC3XXX_AXIS_X], waData[MC3XXX_AXIS_Y], waData[MC3XXX_AXIS_Z]);
+
+        #ifdef _MC3XXX_SUPPORT_LPF_
+        {
+            struct mc3xxx_i2c_data   *_ptPrivData = i2c_get_clientdata(pt_i2c_client);  
+
+            MC3XXX_LPF(_ptPrivData, waData);
+            GSE_LOG("LPF<<<<<[%04d %04d %04d]\n", waData[MC3XXX_AXIS_X], waData[MC3XXX_AXIS_Y], waData[MC3XXX_AXIS_Z]);
+        }
+        #endif
+    }
+    else
+    {
+        if (hwmsen_read_block(pt_i2c_client, MC3XXX_REG_XOUT_EX_L, _baData, MC3XXX_HIGH_REOLUTION_DATA_SIZE))
+        {
+            GSE_ERR("ERR: fail to read data via I2C!\n");
+
+            return (MC3XXX_RETCODE_ERROR_I2C);
+        }
+
+        waData[MC3XXX_AXIS_X] = ((s16)((_baData[0]) | (_baData[1] << 8)));
+        waData[MC3XXX_AXIS_Y] = ((s16)((_baData[2]) | (_baData[3] << 8)));
+        waData[MC3XXX_AXIS_Z] = ((s16)((_baData[4]) | (_baData[5] << 8)));
+
+        GSE_LOG("RBM<<<<<[%08d %08d %08d]\n", waData[MC3XXX_AXIS_X], waData[MC3XXX_AXIS_Y], waData[MC3XXX_AXIS_Z]);
+
+        _MC3XXX_ReadData_RBM2RAW(waData);
+    }
+
+    MCUBE_RREMAP(waData[MC3XXX_AXIS_X], waData[MC3XXX_AXIS_Y]);
+
+    return (MC3XXX_RETCODE_SUCCESS);
+}
+
+/*****************************************
+ *** MC3XXX_ReadOffset
+ *****************************************/
+static int MC3XXX_ReadOffset(struct i2c_client *client, s16 ofs[MC3XXX_AXES_NUM])
+{    
+	int err=0;
+	u8 off_data[6]={0};
+	
+
+	if (MC3XXX_RESOLUTION_HIGH == s_bResolution)
+	{
+		if ((err = hwmsen_read_block(client, MC3XXX_REG_XOUT_EX_L, off_data, MC3XXX_DATA_LEN))) 
+    	{
+    		GSE_ERR("error: %d\n", err);
+    		return err;
+    	}
+		ofs[MC3XXX_AXIS_X] = ((s16)(off_data[0]))|((s16)(off_data[1])<<8);
+		ofs[MC3XXX_AXIS_Y] = ((s16)(off_data[2]))|((s16)(off_data[3])<<8);
+		ofs[MC3XXX_AXIS_Z] = ((s16)(off_data[4]))|((s16)(off_data[5])<<8);
+	}
+	else if (MC3XXX_RESOLUTION_LOW == s_bResolution) 
+	{
+		if ((err = hwmsen_read_block(client, 0, off_data, 3))) 
+    	{
+    		GSE_ERR("error: %d\n", err);
+    		return err;
+    	}
+		ofs[MC3XXX_AXIS_X] = (s8)off_data[0];
+		ofs[MC3XXX_AXIS_Y] = (s8)off_data[1];
+		ofs[MC3XXX_AXIS_Z] = (s8)off_data[2];			
+	}
+
+	GSE_LOG("MC3XXX_ReadOffset %d %d %d \n",ofs[MC3XXX_AXIS_X] ,ofs[MC3XXX_AXIS_Y],ofs[MC3XXX_AXIS_Z]);
+
+    MCUBE_RREMAP(ofs[0], ofs[1]);
+
+    return err;  
+}
+
+/*****************************************
+ *** MC3XXX_ResetCalibration
+ *****************************************/
+static int MC3XXX_ResetCalibration(struct i2c_client *client)
+{
+	struct mc3xxx_i2c_data *obj = i2c_get_clientdata(client);
+	u8 buf[MC3XXX_AXES_NUM] = {0x00, 0x00, 0x00};
+	s16 tmp=0;
+	int err=0;
+
+    u8  bMsbFilter       = 0x3F;
+    s16 wSignBitMask     = 0x2000;
+    s16 wSignPaddingBits = 0xC000;
+
+	buf[0] = 0x43;
+	
+	if((err = hwmsen_write_block(client, 0x07, buf, 1)))
+	{
+		GSE_ERR("error 0x07: %d\n", err);
+	}
+
+
+	if((err = hwmsen_write_block(client, 0x21, offset_buf, 6))) // add by liang for writing offset register as OTP value 
+	{
+		GSE_ERR("error: %d\n", err);
+	}
+	
+	buf[0] = 0x41;
+
+	if((err = hwmsen_write_block(client, 0x07, buf, 1)))
+	{
+		GSE_ERR("error: %d\n", err);
+	}
+
+	msleep(20);
+
+    if (IS_MCFM12() || IS_MCFM3X())
+    {
+        bMsbFilter       = 0x7F;
+        wSignBitMask     = 0x4000;
+        wSignPaddingBits = 0x8000;
+    }
+
+	tmp = ((offset_buf[1] & bMsbFilter) << 8) + offset_buf[0];  // add by Liang for set offset_buf as OTP value 
+	if (tmp & wSignBitMask)
+		tmp |= wSignPaddingBits;
+	offset_data[0] = tmp;
+				
+	tmp = ((offset_buf[3] & bMsbFilter) << 8) + offset_buf[2];  // add by Liang for set offset_buf as OTP value 
+		if (tmp & wSignBitMask)
+			tmp |= wSignPaddingBits;
+	offset_data[1] = tmp;
+				
+	tmp = ((offset_buf[5] & bMsbFilter) << 8) + offset_buf[4];  // add by Liang for set offset_buf as OTP value 
+	if (tmp & wSignBitMask)
+		tmp |= wSignPaddingBits;
+	offset_data[2] = tmp;	
+
+	memset(obj->cali_sw, 0x00, sizeof(obj->cali_sw));
+
+	return err;  
+}
+
+/*****************************************
+ *** MC3XXX_ReadCalibration
+ *****************************************/
+static int MC3XXX_ReadCalibration(struct i2c_client *client, int dat[MC3XXX_AXES_NUM])
+{
+    struct mc3xxx_i2c_data *obj = i2c_get_clientdata(client);
+    int err =0;
+	
+    if ((err = MC3XXX_ReadOffset(client, obj->offset))) {
+        GSE_ERR("read offset fail, %d\n", err);
+        return err;
+    }    
+    
+    dat[MC3XXX_AXIS_X] = obj->offset[MC3XXX_AXIS_X];
+    dat[MC3XXX_AXIS_Y] = obj->offset[MC3XXX_AXIS_Y];
+    dat[MC3XXX_AXIS_Z] = obj->offset[MC3XXX_AXIS_Z];  
+
+	GSE_LOG("MC3XXX_ReadCalibration %d %d %d \n",dat[obj->cvt.map[MC3XXX_AXIS_X]] ,dat[obj->cvt.map[MC3XXX_AXIS_Y]],dat[obj->cvt.map[MC3XXX_AXIS_Z]]);
+                                      
+    return 0;
+}
+
+/*****************************************
+ *** MC3XXX_WriteCalibration
+ *****************************************/
+static int MC3XXX_WriteCalibration(struct i2c_client *client, int dat[MC3XXX_AXES_NUM])
+{
+    struct mc3xxx_i2c_data *obj = i2c_get_clientdata(client);
+    int err = 0;
+    u8 buf[9] ={ 0};
+    s16 tmp = 0, x_gain = 0, y_gain = 0, z_gain = 0;
+    s32 x_off = 0, y_off = 0, z_off = 0;
+    int cali[MC3XXX_AXES_NUM] = {0};
+    
+    u8  bMsbFilter       = 0x3F;
+    s16 wSignBitMask     = 0x2000;
+    s16 wSignPaddingBits = 0xC000;
+    s32 dwRangePosLimit  = 0x1FFF;
+    s32 dwRangeNegLimit  = -0x2000;
+    
+    GSE_LOG("UPDATE dat: (%+3d %+3d %+3d)\n", dat[MC3XXX_AXIS_X], dat[MC3XXX_AXIS_Y], dat[MC3XXX_AXIS_Z]);
+    
+    cali[MC3XXX_AXIS_X] = obj->cvt.sign[MC3XXX_AXIS_X]*(dat[obj->cvt.map[MC3XXX_AXIS_X]]);
+    cali[MC3XXX_AXIS_Y] = obj->cvt.sign[MC3XXX_AXIS_Y]*(dat[obj->cvt.map[MC3XXX_AXIS_Y]]);
+    cali[MC3XXX_AXIS_Z] = obj->cvt.sign[MC3XXX_AXIS_Z]*(dat[obj->cvt.map[MC3XXX_AXIS_Z]]);	
+    
+    MCUBE_WREMAP(cali[MC3XXX_AXIS_X], cali[MC3XXX_AXIS_Y]);
+    
+    GSE_LOG("UPDATE dat: (%+3d %+3d %+3d)\n", cali[MC3XXX_AXIS_X], cali[MC3XXX_AXIS_Y], cali[MC3XXX_AXIS_Z]);
+    
+    // read registers 0x21~0x29
+    if ((err = hwmsen_read_block(client, 0x21, buf, 3))) 
+    {
+        GSE_ERR("error: %d\n", err);
+        return err;
+    }
+    if ((err = hwmsen_read_block(client, 0x24, &buf[3], 3))) 
+    {
+        GSE_ERR("error: %d\n", err);
+        return err;
+    }
+    if ((err = hwmsen_read_block(client, 0x27, &buf[6], 3))) 
+    {
+        GSE_ERR("error: %d\n", err);
+        return err;
+    }
+    
+    if (IS_MCFM12() || IS_MCFM3X())
+    {
+        bMsbFilter       = 0x7F;
+        wSignBitMask     = 0x4000;
+        wSignPaddingBits = 0x8000;
+        dwRangePosLimit  = 0x3FFF;
+        dwRangeNegLimit  = -0x4000;
+    }
+    
+    // get x,y,z offset
+    tmp = ((buf[1] & bMsbFilter) << 8) + buf[0];
+    if (tmp & wSignBitMask)
+        tmp |= wSignPaddingBits;
+    x_off = tmp;
+    
+    tmp = ((buf[3] & bMsbFilter) << 8) + buf[2];
+    if (tmp & wSignBitMask)
+        tmp |= wSignPaddingBits;
+    y_off = tmp;
+    
+    tmp = ((buf[5] & bMsbFilter) << 8) + buf[4];
+    if (tmp & wSignBitMask)
+        tmp |= wSignPaddingBits;
+    z_off = tmp;
+    
+    // get x,y,z gain
+    x_gain = ((buf[1] >> 7) << 8) + buf[6];
+    y_gain = ((buf[3] >> 7) << 8) + buf[7];
+    z_gain = ((buf[5] >> 7) << 8) + buf[8];
+    
+    // prepare new offset
+    x_off = x_off + 16 * cali[MC3XXX_AXIS_X] * 256 * 128 / 3 / gsensor_gain.x / (40 + x_gain);
+    y_off = y_off + 16 * cali[MC3XXX_AXIS_Y] * 256 * 128 / 3 / gsensor_gain.y / (40 + y_gain);
+    z_off = z_off + 16 * cali[MC3XXX_AXIS_Z] * 256 * 128 / 3 / gsensor_gain.z / (40 + z_gain);
+    
+    //add for over range 
+    if( x_off > dwRangePosLimit) 
+    {
+        x_off = dwRangePosLimit;
+    }
+    else if( x_off < dwRangeNegLimit)
+    {
+        x_off = dwRangeNegLimit;
+    }
+    
+    if( y_off > dwRangePosLimit) 
+    {
+        y_off = dwRangePosLimit;
+    }
+    else if( y_off < dwRangeNegLimit)
+    {
+        y_off = dwRangeNegLimit;
+    }
+    
+    if( z_off > dwRangePosLimit) 
+    {
+        z_off = dwRangePosLimit;
+    }
+    else if( z_off < dwRangeNegLimit)
+    {
+        z_off = dwRangeNegLimit;
+    }
+    
+    //storege the cerrunt offset data with DOT format
+    offset_data[0] = x_off;
+    offset_data[1] = y_off;
+    offset_data[2] = z_off;
+    
+    //storege the cerrunt Gain data with GOT format
+    gain_data[0] = 256*8*128/3/(40+x_gain);
+    gain_data[1] = 256*8*128/3/(40+y_gain);
+    gain_data[2] = 256*8*128/3/(40+z_gain);
+    
+    buf[0]=0x43;
+    hwmsen_write_block(client, 0x07, buf, 1);
+    
+    buf[0] = x_off & 0xff;
+    buf[1] = ((x_off >> 8) & bMsbFilter) | (x_gain & 0x0100 ? 0x80 : 0);
+    buf[2] = y_off & 0xff;
+    buf[3] = ((y_off >> 8) & bMsbFilter) | (y_gain & 0x0100 ? 0x80 : 0);
+    buf[4] = z_off & 0xff;
+    buf[5] = ((z_off >> 8) & bMsbFilter) | (z_gain & 0x0100 ? 0x80 : 0);
+    
+    hwmsen_write_block(client, 0x21, buf, 6);
+    
+    buf[0]=0x41;
+    hwmsen_write_block(client, 0x07, buf, 1);		
+    
+    msleep(50);
+    
+    return err;
+}
+
+/*****************************************
+ *** MC3XXX_SetPowerMode
+ *****************************************/
+static int MC3XXX_SetPowerMode(struct i2c_client *client, bool enable)
+{
+    u8 databuf[2] = {0};    
+    int res = 0;
+    u8 addr = MC3XXX_REG_MODE_FEATURE;
+    struct mc3xxx_i2c_data *obj = i2c_get_clientdata(client);
+    
+    GSE_LOG("%s: %d\n", enable);   
+
+    if(enable == mc3xxx_sensor_power) {
+        GSE_LOG("Sensor power status should not be set again!!!\n");
+	    return MC3XXX_RETCODE_SUCCESS;    
+	}
+
+    
+    if(enable)
+    {
+        databuf[1] = 0x41;
+        databuf[0] = MC3XXX_REG_MODE_FEATURE;
+        res = i2c_master_send(client, databuf, 0x2);
+    
+        #ifdef _MC3XXX_SUPPORT_DOT_CALIBRATION_
+            mcube_load_cali(client);
+        #endif
+    }
+    else
+    {
+        databuf[1] = 0x43;
+        databuf[0] = MC3XXX_REG_MODE_FEATURE;
+        res = i2c_master_send(client, databuf, 0x2);
+    }
+    
+    if(res <= 0)
+    {
+        GSE_LOG("fwq set power mode failed!\n");
+        return MC3XXX_RETCODE_ERROR_I2C;
+    }
+    else if(atomic_read(&obj->trace) & MCUBE_TRC_INFO)
+    {
+        GSE_LOG("fwq set power mode ok %d!\n", databuf[1]);
+    }
+    
+    mc3xxx_sensor_power = enable;
+
+    return MC3XXX_RETCODE_SUCCESS;    
+}
+
+/*****************************************
+ *** MC3XXX_SetResolution
+ *****************************************/
+static void MC3XXX_SetResolution(void)
+{
+    GSE_LOG("[%s]\n", __FUNCTION__);
+
+    switch (s_bPCODE)
+    {
+    case MC3XXX_PCODE_3230:
+    case MC3XXX_PCODE_3430:
+    case MC3XXX_PCODE_3430N:
+    case MC3XXX_PCODE_3530:
+    case MC3XXX_PCODE_3236:
+         s_bResolution = MC3XXX_RESOLUTION_LOW;
+         break;
+
+    case MC3XXX_PCODE_3210:
+    case MC3XXX_PCODE_3250:
+    case MC3XXX_PCODE_3410:
+    case MC3XXX_PCODE_3410N:
+    case MC3XXX_PCODE_3510:
+    case MC3XXX_PCODE_3216:
+         s_bResolution = MC3XXX_RESOLUTION_HIGH;
+         break;
+
+    // === RESERVED ==================================BGN===
+    // === (move to normal section once it is confirmed) ===
+    case MC3XXX_PCODE_RESERVE_10:
+         GSE_ERR("RESERVED ONLINE!\n");
+         // TODO: should have a default configuration...
+         break;
+
+    case MC3XXX_PCODE_RESERVE_1:
+    case MC3XXX_PCODE_RESERVE_3:
+    case MC3XXX_PCODE_RESERVE_4:
+    case MC3XXX_PCODE_RESERVE_5:
+    case MC3XXX_PCODE_RESERVE_6:
+    case MC3XXX_PCODE_RESERVE_8:
+    case MC3XXX_PCODE_RESERVE_9:
+         GSE_ERR("RESERVED ONLINE!\n");
+         s_bResolution = MC3XXX_RESOLUTION_LOW;
+         break;
+
+    case MC3XXX_PCODE_RESERVE_2:
+    case MC3XXX_PCODE_RESERVE_7:
+         GSE_ERR("RESERVED ONLINE!\n");
+         s_bResolution = MC3XXX_RESOLUTION_HIGH;
+         break;
+    // === RESERVED ==================================END===
+
+    default:
+         GSE_ERR("ERR: no resolution assigned!\n");
+         break;
+    }
+
+    GSE_LOG("[%s] s_bResolution: %d\n", __FUNCTION__, s_bResolution);
+}
+
+/*****************************************
+ *** MC3XXX_SetSampleRate
+ *****************************************/
+static void MC3XXX_SetSampleRate(struct i2c_client *pt_i2c_client)
+{
+    unsigned char    _baDataBuf[2] = { 0 };
+
+    GSE_LOG("[%s]\n", __FUNCTION__);
+
+    _baDataBuf[0] = MC3XXX_REG_SAMPLE_RATE;
+    _baDataBuf[1] = 0x00;
+
+    if (IS_MCFM12() || IS_MCFM3X())
+    {
+        unsigned char    _baData2Buf[2] = { 0 };
+
+        _baData2Buf[0] = 0x2A;
+        i2c_master_send(pt_i2c_client, &(_baData2Buf[0]), 1);
+        i2c_master_recv(pt_i2c_client, &(_baData2Buf[0]), 1);
+
+        GSE_LOG("[%s] REG(0x2A) = 0x%02X\n", __FUNCTION__, _baData2Buf[0]);
+
+        _baData2Buf[0] = (_baData2Buf[0] & 0xC0);
+
+        switch (_baData2Buf[0])
+        {
+        case 0x00:    _baDataBuf[1] = 0x00;                                                    break;
+        case 0x40:    _baDataBuf[1] = 0x08;                                                    break;
+        case 0x80:    _baDataBuf[1] = 0x09;                                                    break;
+        case 0xC0:    _baDataBuf[1] = 0x0A;                                                    break;
+
+        default:      GSE_ERR("[%s] no chance to get here... check code!\n", __FUNCTION__);    break;
+        }
+    }
+
+    i2c_master_send(pt_i2c_client, _baDataBuf, 0x2);
+}
+
+/*****************************************
+ *** MC3XXX_ConfigRegRange
+ *****************************************/
+static void MC3XXX_ConfigRegRange(struct i2c_client *pt_i2c_client)
+{
+    unsigned char    _baDataBuf[2] = { 0 };
+
+    _baDataBuf[0] = MC3XXX_REG_RANGE_CONTROL;
+    _baDataBuf[1] = 0x3F;
+
+    if (MC3XXX_RESOLUTION_LOW == s_bResolution)
+        _baDataBuf[1] = 0x32;
+
+    if (IS_MCFM12() || IS_MCFM3X())
+    {
+        if (MC3XXX_RESOLUTION_LOW == s_bResolution)
+            _baDataBuf[1] = 0x02;
+        else
+            _baDataBuf[1] = 0x25;
+    }
+
+    i2c_master_send(pt_i2c_client, _baDataBuf, 0x2);
+
+    GSE_LOG("[%s] set 0x%X\n", __FUNCTION__, _baDataBuf[1]);
+}
+
+/*****************************************
+ *** MC3XXX_SetGain
+ *****************************************/
+static void MC3XXX_SetGain(void)
+{
+    gsensor_gain.x = gsensor_gain.y = gsensor_gain.z = 1024;
+
+    if (MC3XXX_RESOLUTION_LOW == s_bResolution)
+    {
+        gsensor_gain.x = gsensor_gain.y = gsensor_gain.z = 86;
+
+        if (IS_MCFM12() || IS_MCFM3X())
+        {
+            gsensor_gain.x = gsensor_gain.y = gsensor_gain.z = 64;
+        }
+    }
+    
+    GSE_LOG("[%s] gain: %d / %d / %d\n", __FUNCTION__, gsensor_gain.x, gsensor_gain.y, gsensor_gain.z);
+}
+
+/*****************************************
+ *** MC3XXX_Init
+ *****************************************/
+static int MC3XXX_Init(struct i2c_client *client, int reset_cali)
+{
+    unsigned char    _baDataBuf[2] = { 0 };
+
+    GSE_LOG("[%s]\n", __FUNCTION__);
+
+    #ifdef _MC3XXX_SUPPORT_POWER_SAVING_SHUTDOWN_POWER_
+        if (MC3XXX_RETCODE_SUCCESS != _mc3xxx_i2c_auto_probe(client))
+        {
+            //GSE_ERR("ERR: fail to probe mCube sensor!\n");
+            return (MC3XXX_RETCODE_ERROR_I2C);
+        }
+    
+        //GSE_LOG("[%s] confirmed i2c addr: 0x%X\n", __FUNCTION__, client->addr);
+    #endif
+
+    _baDataBuf[0] = MC3XXX_REG_MODE_FEATURE;
+    _baDataBuf[1] = 0x43;
+    i2c_master_send(client, _baDataBuf, 0x2);
+
+    MC3XXX_SetResolution();
+    MC3XXX_SetSampleRate(client);
+    MC3XXX_ConfigRegRange(client);
+    MC3XXX_SetGain();
+    
+    _baDataBuf[0] = MC3XXX_REG_TAP_DETECTION_ENABLE;
+    _baDataBuf[1] = 0x00;
+    i2c_master_send(client, _baDataBuf, 0x2);
+    
+    _baDataBuf[0] = MC3XXX_REG_INTERRUPT_ENABLE;
+    _baDataBuf[1] = 0x00;
+    i2c_master_send(client, _baDataBuf, 0x2);
+
+    _baDataBuf[0] = 0x2A;
+    i2c_master_send(client, &(_baDataBuf[0]), 1);
+    i2c_master_recv(client, &(_baDataBuf[0]), 1);
+    s_bMPOL = (_baDataBuf[0] & 0x03);
+
+    #if 1
+ //   #ifdef _MC3XXX_SUPPORT_DOT_CALIBRATION_
+        MC3XXX_rbm(client,0);
+    #endif
+
+    #ifdef _MC3XXX_SUPPORT_LPF_
+    {
+        struct mc3xxx_i2c_data   *_pt_i2c_data = i2c_get_clientdata(client);
+
+        memset(&_pt_i2c_data->fir, 0x00, sizeof(_pt_i2c_data->fir));
+    }
+    #endif
+
+    #ifdef _MC3XXX_SUPPORT_LRF_
+        memset(&s_taLRF_CB, 0, sizeof(s_taLRF_CB));
+    #endif
+
+    #ifdef _MC3XXX_SUPPORT_PERIODIC_DOC_
+        init_waitqueue_head(&wq_mc3xxx_open_status);
+    #endif
+
+    GSE_LOG("[%s] init ok.\n", __FUNCTION__);
+
+    return (MC3XXX_RETCODE_SUCCESS);
+}
+
+/*****************************************
+ *** MC3XXX_ReadChipInfo
+ *****************************************/
+static int MC3XXX_ReadChipInfo(struct i2c_client *client, char *buf, int bufsize)
+{
+	if((NULL == buf)||(bufsize<=30))
+	{
+		return -1;
+	}
+	
+	if(NULL == client)
+	{
+		*buf = 0;
+		return -2;
+	}
+
+	sprintf(buf, "MC3XXX Chip");
+	return 0;
+}
+
+/*****************************************
+ *** MC3XXX_ReadSensorData
+ *****************************************/
+static int MC3XXX_ReadSensorData(struct i2c_client *pt_i2c_client, char *pbBuf, int nBufSize)
+{
+    int                       _naAccelData[MC3XXX_AXES_NUM] = { 0 };
+    struct mc3xxx_i2c_data   *_pt_i2c_obj = ((struct mc3xxx_i2c_data*) i2c_get_clientdata(pt_i2c_client));
+
+    GSE_LOG("[%s]\n", __FUNCTION__);	
+
+    if ((NULL == pt_i2c_client) || (NULL == pbBuf))
+    {
+        GSE_ERR("ERR: Null Pointer, pt_i2c_client: 0x%X, pbBuf: 0x%X\n", (unsigned int) pt_i2c_client, (unsigned int) pbBuf);	
+
+        return (MC3XXX_RETCODE_ERROR_NULL_POINTER);
+    }
+
+    if (false == mc3xxx_sensor_power)
+    {
+        if (MC3XXX_RETCODE_SUCCESS != MC3XXX_SetPowerMode(pt_i2c_client, true))
+            GSE_ERR("ERR: fail to set power mode!\n");
+    }
+
+    #ifdef _MC3XXX_SUPPORT_DOT_CALIBRATION_
+        mcube_load_cali(pt_i2c_client);
+    
+        if ((s_nIsRBM_Enabled) && (1 == LPF_FirstRun))
+        {
+            int    _nLoopIndex = 0;
+            
+            LPF_FirstRun = 0;
+            
+            for (_nLoopIndex = 0; _nLoopIndex < (LPF_SamplingRate + LPF_CutoffFrequency); _nLoopIndex++)
+                MC3XXX_ReadData(pt_i2c_client, _pt_i2c_obj->data);
+        }
+    #endif
+
+    if (MC3XXX_RETCODE_SUCCESS != MC3XXX_ReadData(pt_i2c_client, _pt_i2c_obj->data))
+    {        
+        GSE_ERR("ERR: fail to read data!\n");
+
+        return (MC3XXX_RETCODE_ERROR_I2C);
+    }
+
+    //output format: mg
+    GSE_LOG("[%s] raw data: %d, %d, %d\n", __FUNCTION__, _pt_i2c_obj->data[MC3XXX_AXIS_X], _pt_i2c_obj->data[MC3XXX_AXIS_Y], _pt_i2c_obj->data[MC3XXX_AXIS_Z]);
+    _naAccelData[(_pt_i2c_obj->cvt.map[MC3XXX_AXIS_X])] = (_pt_i2c_obj->cvt.sign[MC3XXX_AXIS_X] * _pt_i2c_obj->data[MC3XXX_AXIS_X]);
+    _naAccelData[(_pt_i2c_obj->cvt.map[MC3XXX_AXIS_Y])] = (_pt_i2c_obj->cvt.sign[MC3XXX_AXIS_Y] * _pt_i2c_obj->data[MC3XXX_AXIS_Y]);
+    _naAccelData[(_pt_i2c_obj->cvt.map[MC3XXX_AXIS_Z])] = (_pt_i2c_obj->cvt.sign[MC3XXX_AXIS_Z] * _pt_i2c_obj->data[MC3XXX_AXIS_Z]);
+    
+    GSE_LOG("[%s] map data: %d, %d, %d!\n", __FUNCTION__, _naAccelData[MC3XXX_AXIS_X], _naAccelData[MC3XXX_AXIS_Y], _naAccelData[MC3XXX_AXIS_Z]);
+    
+    _naAccelData[MC3XXX_AXIS_X] = (_naAccelData[MC3XXX_AXIS_X] * GRAVITY_EARTH_1000 / gsensor_gain.x);
+    _naAccelData[MC3XXX_AXIS_Y] = (_naAccelData[MC3XXX_AXIS_Y] * GRAVITY_EARTH_1000 / gsensor_gain.y);
+    _naAccelData[MC3XXX_AXIS_Z] = (_naAccelData[MC3XXX_AXIS_Z] * GRAVITY_EARTH_1000 / gsensor_gain.z);	
+    GSE_LOG("[%s] accel data: %d, %d, %d!\n", __FUNCTION__, _naAccelData[MC3XXX_AXIS_X], _naAccelData[MC3XXX_AXIS_Y], _naAccelData[MC3XXX_AXIS_Z]);
+		
+    sprintf(pbBuf, "%04x %04x %04x", _naAccelData[MC3XXX_AXIS_X], _naAccelData[MC3XXX_AXIS_Y], _naAccelData[MC3XXX_AXIS_Z]);
+
+    return (MC3XXX_RETCODE_SUCCESS);
+}
+
+/*****************************************
+ *** _MC3XXX_ReadAverageData
+ *****************************************/
+#ifdef _MC3XXX_SUPPORT_APPLY_AVERAGE_AGORITHM_
+static int _MC3XXX_ReadAverageData(struct i2c_client *client, char *buf)
+{
+	struct mc3xxx_i2c_data *obj = (struct mc3xxx_i2c_data*)i2c_get_clientdata(client);
+	int acc[MC3XXX_AXES_NUM]={0};
+	s16 sensor_data[3]={0};
+	s16 sensor_data_max[3]={0};
+	s16 sensor_data_mini[3]={0};
+	s32 sensor_data_sum[3]={0};
+
+	int i = 0,j=0;
+
+	MC3XXX_ReadData(client, sensor_data);
+	GSE_LOG("MC3XXX_ReadRawData MC3XXX_ReadData: %d, %d, %d!\n", sensor_data[MC3XXX_AXIS_X], sensor_data[MC3XXX_AXIS_Y], sensor_data[MC3XXX_AXIS_Z]);
+	sensor_data_max[MC3XXX_AXIS_X]=sensor_data[MC3XXX_AXIS_X];
+	sensor_data_max[MC3XXX_AXIS_Y]=sensor_data[MC3XXX_AXIS_Y];
+	sensor_data_max[MC3XXX_AXIS_Z]=sensor_data[MC3XXX_AXIS_Z];
+	
+	sensor_data_mini[MC3XXX_AXIS_X]=sensor_data[MC3XXX_AXIS_X];
+	sensor_data_mini[MC3XXX_AXIS_Y]=sensor_data[MC3XXX_AXIS_Y];
+	sensor_data_mini[MC3XXX_AXIS_Z]=sensor_data[MC3XXX_AXIS_Z];
+	
+	sensor_data_sum[MC3XXX_AXIS_X]+=sensor_data[MC3XXX_AXIS_X];
+	sensor_data_sum[MC3XXX_AXIS_Y]+=sensor_data[MC3XXX_AXIS_Y];
+	sensor_data_sum[MC3XXX_AXIS_Z]+=sensor_data[MC3XXX_AXIS_Z];
+	
+	for(i=0; i<11 ; i++)
+	{
+		MC3XXX_ReadData(client, sensor_data);
+		GSE_LOG("MC3XXX_ReadRawData MC3XXX_ReadData: %d, %d, %d!\n", sensor_data[MC3XXX_AXIS_X], sensor_data[MC3XXX_AXIS_Y], sensor_data[MC3XXX_AXIS_Z]);
+		
+		sensor_data_sum[MC3XXX_AXIS_X]+=sensor_data[MC3XXX_AXIS_X];
+		sensor_data_sum[MC3XXX_AXIS_Y]+=sensor_data[MC3XXX_AXIS_Y];
+		sensor_data_sum[MC3XXX_AXIS_Z]+=sensor_data[MC3XXX_AXIS_Z];
+		for(j=0; j<3 ; j++)
+		{
+			if(sensor_data[j]>sensor_data_max[j])
+			{
+				sensor_data_max[j]=sensor_data[j];
+			}
+			if(sensor_data[j]<sensor_data_mini[j])
+			{
+				sensor_data_mini[j]=sensor_data[j];
+			}
+		}
+	}
+	GSE_LOG("MC3XXX_ReadRawData sensor_data_max: %d, %d, %d!\n", sensor_data_max[MC3XXX_AXIS_X], sensor_data_max[MC3XXX_AXIS_Y], sensor_data_max[MC3XXX_AXIS_Z]);
+	GSE_LOG("MC3XXX_ReadRawData sensor_data_mini: %d, %d, %d!\n", sensor_data_mini[MC3XXX_AXIS_X], sensor_data_mini[MC3XXX_AXIS_Y], sensor_data_mini[MC3XXX_AXIS_Z]);
+	sensor_data[MC3XXX_AXIS_X] = (s16)((sensor_data_sum[MC3XXX_AXIS_X]-sensor_data_max[MC3XXX_AXIS_X]-sensor_data_mini[MC3XXX_AXIS_X])/10);
+	sensor_data[MC3XXX_AXIS_Y] = (s16)((sensor_data_sum[MC3XXX_AXIS_Y]-sensor_data_max[MC3XXX_AXIS_Y]-sensor_data_mini[MC3XXX_AXIS_Y])/10);
+	sensor_data[MC3XXX_AXIS_Z] = (s16)((sensor_data_sum[MC3XXX_AXIS_Z]-sensor_data_max[MC3XXX_AXIS_Z]-sensor_data_mini[MC3XXX_AXIS_Z])/10);
+	GSE_LOG("MC3XXX_ReadRawData sensor_data: %d, %d, %d!\n", sensor_data[MC3XXX_AXIS_X], sensor_data[MC3XXX_AXIS_Y], sensor_data[MC3XXX_AXIS_Z]);
+	
+	acc[(obj->cvt.map[MC3XXX_AXIS_X])] = obj->cvt.sign[MC3XXX_AXIS_X] * sensor_data[MC3XXX_AXIS_X];
+	acc[(obj->cvt.map[MC3XXX_AXIS_Y])] = obj->cvt.sign[MC3XXX_AXIS_Y] * sensor_data[MC3XXX_AXIS_Y];
+	acc[(obj->cvt.map[MC3XXX_AXIS_Z])] = obj->cvt.sign[MC3XXX_AXIS_Z] * sensor_data[MC3XXX_AXIS_Z];
+	
+	GSE_LOG("MC3XXX_ReadRawData mapdata: %d, %d, %d!\n", acc[MC3XXX_AXIS_X], acc[MC3XXX_AXIS_Y], acc[MC3XXX_AXIS_Z]);
+	
+	acc[MC3XXX_AXIS_X] = (acc[MC3XXX_AXIS_X]*GRAVITY_EARTH_1000/gsensor_gain.x);
+	acc[MC3XXX_AXIS_Y] = (acc[MC3XXX_AXIS_Y]*GRAVITY_EARTH_1000/gsensor_gain.y);
+	acc[MC3XXX_AXIS_Z] = (acc[MC3XXX_AXIS_Z]*GRAVITY_EARTH_1000/gsensor_gain.z);	
+	
+	GSE_LOG("MC3XXX_ReadRawData mapdata1: %d, %d, %d!\n", acc[MC3XXX_AXIS_X], acc[MC3XXX_AXIS_Y], acc[MC3XXX_AXIS_Z]);
+	
+	sprintf(buf, "%04x %04x %04x", acc[MC3XXX_AXIS_X], acc[MC3XXX_AXIS_Y], acc[MC3XXX_AXIS_Z]);
+
+	return 0;
+}
+#endif    // END OF #ifdef _MC3XXX_SUPPORT_APPLY_AVERAGE_AGORITHM_
+
+/*****************************************
+ *** MC3XXX_ReadRawData
+ *****************************************/
+static int MC3XXX_ReadRawData(struct i2c_client *client, char *buf)
+{
+	int res = 0;
+
+	if (!buf || !client)
+	{
+		return EINVAL;
+	}
+
+	if(mc3xxx_sensor_power == false)
+	{
+		res = MC3XXX_SetPowerMode(client, true);
+		if(res)
+		{
+			GSE_ERR("Power on mc3xxx error %d!\n", res);
+		}
+	}
+
+	#ifdef _MC3XXX_SUPPORT_APPLY_AVERAGE_AGORITHM_
+		return (_MC3XXX_ReadAverageData(client, buf));
+	#else
+	{
+		s16 sensor_data[3]={0};
+	
+		if((res = MC3XXX_ReadData(client, sensor_data)))
+		{        
+			GSE_ERR("I2C error: ret value=%d", res);
+			return EIO;
+		}
+		else
+		{
+			sprintf(buf, "%04x %04x %04x", sensor_data[MC3XXX_AXIS_X], 
+			sensor_data[MC3XXX_AXIS_Y], sensor_data[MC3XXX_AXIS_Z]);
+		}
+	}
+	#endif
+	
+	return 0;
+}
+
+#ifdef _MC3XXX_SUPPORT_DOT_CALIBRATION_
+/*****************************************
+ *** MC3XXX_ReadRBMData
+ *****************************************/
+static int MC3XXX_ReadRBMData(struct i2c_client *client, char *buf)
+{
+	int res = 0;
+	int data[3] = { 0 };
+
+	if (!buf || !client)
+	{
+		return EINVAL;
+	}
+
+	if(mc3xxx_sensor_power == false)
+	{
+		res = MC3XXX_SetPowerMode(client, true);
+		if(res)
+		{
+			GSE_ERR("Power on mc3xxx error %d!\n", res);
+		}
+	}
+	
+	if((res = MC3XXX_ReadData_RBM(client, data)))
+	{        
+		GSE_ERR("I2C error: ret value=%d", res);
+		return EIO;
+	}
+	else
+	{
+		sprintf(buf, "%04x %04x %04x", data[MC3XXX_AXIS_X], 
+			data[MC3XXX_AXIS_Y], data[MC3XXX_AXIS_Z]);
+	
+	}
+	
+	return res;
+}
+#endif // _MC3XXX_SUPPORT_DOT_CALIBRATION_
+
+/*****************************************
+ *** MC3XXX_JudgeTestResult
+ *****************************************/
+static int MC3XXX_JudgeTestResult(struct i2c_client *client)
+{
+    int    res                  = 0;
+    int    self_result          = 0;
+    s16    acc[MC3XXX_AXES_NUM] = { 0 };
+	
+    if((res = MC3XXX_ReadData(client, acc)))
+    {        
+        GSE_ERR("I2C error: ret value=%d", res);
+        return EIO;
+    }
+    else
+    {			
+        acc[MC3XXX_AXIS_X] = acc[MC3XXX_AXIS_X] * 1000 / gsensor_gain.x;
+        acc[MC3XXX_AXIS_Y] = acc[MC3XXX_AXIS_Y] * 1000 / gsensor_gain.y;
+        acc[MC3XXX_AXIS_Z] = acc[MC3XXX_AXIS_Z] * 1000 / gsensor_gain.z;
+        
+        self_result = (  (acc[MC3XXX_AXIS_X] * acc[MC3XXX_AXIS_X])
+                       + (acc[MC3XXX_AXIS_Y] * acc[MC3XXX_AXIS_Y])
+                       + (acc[MC3XXX_AXIS_Z] * acc[MC3XXX_AXIS_Z]));
+        
+        if ( (self_result > 475923) && (self_result < 2185360) )    //between 0.7g and 1.5g 
+        {												 
+            GSE_ERR("MC3XXX_JudgeTestResult successful\n");
+            return MC3XXX_RETCODE_SUCCESS;
+        }
+        else
+        {
+            GSE_ERR("MC3XXX_JudgeTestResult failt\n");
+            return -EINVAL;
+        }
+    }
+}
+
+/*****************************************
+ *** MC3XXX_SelfCheck
+ *****************************************/
+static void MC3XXX_SelfCheck(struct i2c_client *client, u8 *pUserBuf)
+{
+    u8    _bRData1 = 0;
+    u8    _bRData2 = 0;
+    u8    _bRData3 = 0;
+    u8    _baDataBuf[2] = { 0 };
+
+    hwmsen_read_block(client, 0x20, &_bRData1, 1);
+    hwmsen_read_block(client, 0x3B, &_bRData2, 1);
+
+    _baDataBuf[0] = 0x43;
+    hwmsen_write_block(client, 0x07, _baDataBuf, 1);
+
+    mdelay(10);
+
+    for ( ; ; )
+    {
+        _baDataBuf[0] = 0x6D;
+        hwmsen_write_block(client, 0x1B, _baDataBuf, 1);
+        
+        _baDataBuf[0] = 0x43;
+        hwmsen_write_block(client, 0x1B, _baDataBuf, 1);
+
+        _bRData3 = 0x00;
+        hwmsen_read_block(client, 0x04, &_bRData3, 1);
+
+        if (_bRData3 & 0x40)
+            break;
+    }
+
+    _baDataBuf[0] = (_bRData2 & 0xFE);
+    hwmsen_write_block(client, 0x3B, _baDataBuf, 1);
+
+    _baDataBuf[0] = 0x03;
+    hwmsen_write_block(client, 0x20, _baDataBuf, 1);
+
+    _baDataBuf[0] = 0x40;
+    hwmsen_write_block(client, 0x14, _baDataBuf, 1);
+
+    mdelay(10);
+
+    _baDataBuf[0] = pUserBuf[0];
+    hwmsen_write_block(client, 0x00, _baDataBuf, 1);
+
+    _baDataBuf[0] = 0x41;
+    hwmsen_write_block(client, 0x07, _baDataBuf, 1);
+
+    mdelay(10);
+
+    _baDataBuf[0] = 0x43;
+    hwmsen_write_block(client, 0x07, _baDataBuf, 1);
+
+    mdelay(10);
+
+    MC3XXX_Read_Reg_Map(client, pUserBuf);
+
+    mdelay(10);
+
+    _baDataBuf[0] = 0x00;
+    hwmsen_write_block(client, 0x14, _baDataBuf, 1);
+
+    _baDataBuf[0] = _bRData1;
+    hwmsen_write_block(client, 0x20, _baDataBuf, 1);
+
+    _baDataBuf[0] = _bRData2;
+    hwmsen_write_block(client, 0x3B, _baDataBuf, 1);
+
+    mdelay(10);
+
+    for ( ; ; )
+    {
+        _baDataBuf[0] = 0x6D;
+        hwmsen_write_block(client, 0x1B, _baDataBuf, 1);
+        
+        _baDataBuf[0] = 0x43;
+        hwmsen_write_block(client, 0x1B, _baDataBuf, 1);
+
+        _bRData3 = 0xFF;
+        hwmsen_read_block(client, 0x04, &_bRData3, 1);
+        
+        if (!(_bRData3 & 0x40))
+            break;
+    }
+
+    mdelay(10);
+}
+
+/*****************************************
+ *** MC3XXX_GetOpenStatus
+ *****************************************/
+#ifdef _MC3XXX_SUPPORT_PERIODIC_DOC_
+static int    MC3XXX_GetOpenStatus(void)
+{
+    //GSE_LOG("[%s] %d\n", __FUNCTION__, atomic_read(&s_t_mc3xxx_open_status));
+
+    wait_event_interruptible(wq_mc3xxx_open_status, (atomic_read(&s_t_mc3xxx_open_status) != 0));
+
+    //GSE_LOG("[%s] pass wait_event_interruptible: %d\n", __FUNCTION__, atomic_read(&s_t_mc3xxx_open_status));
+    
+    return (atomic_read(&s_t_mc3xxx_open_status));
+}
+#endif
+
+/*****************************************
+ *** show_chipinfo_value
+ *****************************************/
+static ssize_t show_chipinfo_value(struct device_driver *ddri, char *buf)
+{
+	struct i2c_client *client = mc3xxx_i2c_client;
+	char strbuf[MC3XXX_BUF_SIZE]={0};
+    GSE_LOG("fwq show_chipinfo_value \n");
+	if(NULL == client)
+	{
+		GSE_ERR("i2c client is null!!\n");
+		return 0;
+	}
+	
+	MC3XXX_ReadChipInfo(client, strbuf, MC3XXX_BUF_SIZE);
+	return snprintf(buf, PAGE_SIZE, "%s\n", strbuf);        
+}
+
+/*****************************************
+ *** show_sensordata_value
+ *****************************************/
+static ssize_t show_sensordata_value(struct device_driver *ddri, char *buf)
+{
+	struct i2c_client *client = mc3xxx_i2c_client;
+	char strbuf[MC3XXX_BUF_SIZE] = { 0 };
+	
+	if(NULL == client)
+	{
+		GSE_ERR("i2c client is null!!\n");
+		return 0;
+	}
+
+    MC3XXX_MUTEX_LOCK();
+	MC3XXX_ReadSensorData(client, strbuf, MC3XXX_BUF_SIZE);
+    MC3XXX_MUTEX_UNLOCK();
+	return snprintf(buf, PAGE_SIZE, "%s\n", strbuf);            
+}
+
+/*****************************************
+ *** show_cali_value
+ *****************************************/
+static ssize_t show_cali_value(struct device_driver *ddri, char *buf)
+{
+	struct i2c_client *client = mc3xxx_i2c_client;
+	struct mc3xxx_i2c_data *obj;
+	int err = 0;
+	int len = 0;
+	int mul = 0;
+	int tmp[MC3XXX_AXES_NUM] = { 0 };
+
+    GSE_LOG("fwq show_cali_value \n");
+
+	if(NULL == client)
+	{
+		GSE_ERR("i2c client is null!!\n");
+		return 0;
+	}
+
+	obj = (struct mc3xxx_i2c_data*) i2c_get_clientdata(client);
+
+	
+	if((err = MC3XXX_ReadOffset(client, obj->offset)))
+	{
+		return -EINVAL;
+	}
+	else if((err = MC3XXX_ReadCalibration(client, tmp)))
+	{
+		return -EINVAL;
+	}
+	else
+	{    
+// +++ 20130104 -- obj->reso is no longer used, replaced by gensor_gain to avoid system crash		
+		//mul = obj->reso->sensitivity/mc3xxx_offset_resolution.sensitivity;
+		mul = gsensor_gain.x /mc3xxx_offset_resolution.sensitivity;
+		/*len += snprintf(buf+len, PAGE_SIZE-len, "[HW ][%d] (%+3d, %+3d, %+3d) : (0x%02X, 0x%02X, 0x%02X)\n", mul,                        
+			obj->offset[MC3XXX_AXIS_X], obj->offset[MC3XXX_AXIS_Y], obj->offset[MC3XXX_AXIS_Z],
+			obj->offset[MC3XXX_AXIS_X], obj->offset[MC3XXX_AXIS_Y], obj->offset[MC3XXX_AXIS_Z]);
+		*/	
+// +++ 20130104 -- obj->reso is no longer used, replaced by gensor_gain to avoid system crash
+		len += snprintf(buf+len, PAGE_SIZE-len, "[HW] (%+3d, %+3d, %+3d) : (0x%02X, 0x%02X, 0x%02X)\n",                        
+			obj->offset[MC3XXX_AXIS_X], obj->offset[MC3XXX_AXIS_Y], obj->offset[MC3XXX_AXIS_Z],
+			obj->offset[MC3XXX_AXIS_X], obj->offset[MC3XXX_AXIS_Y], obj->offset[MC3XXX_AXIS_Z]);
+			
+		len += snprintf(buf+len, PAGE_SIZE-len, "[SW][%d] (%+3d, %+3d, %+3d)\n", 1, 
+			obj->cali_sw[MC3XXX_AXIS_X], obj->cali_sw[MC3XXX_AXIS_Y], obj->cali_sw[MC3XXX_AXIS_Z]);
+
+		len += snprintf(buf+len, PAGE_SIZE-len, "[ALL]  (%+3d, %+3d, %+3d) : (%+3d, %+3d, %+3d)\n", 
+			obj->offset[MC3XXX_AXIS_X]*mul + obj->cali_sw[MC3XXX_AXIS_X],
+			obj->offset[MC3XXX_AXIS_Y]*mul + obj->cali_sw[MC3XXX_AXIS_Y],
+			obj->offset[MC3XXX_AXIS_Z]*mul + obj->cali_sw[MC3XXX_AXIS_Z],
+			tmp[MC3XXX_AXIS_X], tmp[MC3XXX_AXIS_Y], tmp[MC3XXX_AXIS_Z]);
+		
+		return len;
+    }
+}
+
+/*****************************************
+ *** store_cali_value
+ *****************************************/
+static ssize_t store_cali_value(struct device_driver *ddri, const char *buf, size_t count)
+{
+	struct i2c_client *client = mc3xxx_i2c_client;  
+	int err=0;
+	int x=0;
+	int y=0;
+	int z=0;
+	int dat[MC3XXX_AXES_NUM]={0};
+
+	if(!strncmp(buf, "rst", 3))
+	{
+        MC3XXX_MUTEX_LOCK();
+		err = MC3XXX_ResetCalibration(client);
+        MC3XXX_MUTEX_UNLOCK();
+
+		if (err)
+			GSE_ERR("reset offset err = %d\n", err);
+	}
+	else if(3 == sscanf(buf, "0x%02X 0x%02X 0x%02X", &x, &y, &z))
+	{
+		dat[MC3XXX_AXIS_X] = x;
+		dat[MC3XXX_AXIS_Y] = y;
+		dat[MC3XXX_AXIS_Z] = z;
+
+        MC3XXX_MUTEX_LOCK();
+		err = MC3XXX_WriteCalibration(client, dat);
+        MC3XXX_MUTEX_UNLOCK();
+
+		if (err)
+			GSE_ERR("write calibration err = %d\n", err);
+	}
+	else
+	{
+		GSE_ERR("invalid format\n");
+	}
+	
+	return count;
+}
+
+/*****************************************
+ *** show_selftest_value
+ *****************************************/
+static ssize_t show_selftest_value(struct device_driver *ddri, char *buf)
+{
+    struct i2c_client *client = mc3xxx_i2c_client;
+
+    if (NULL == client)
+    {
+        GSE_ERR("i2c client is null!!\n");
+        return 0;
+    }
+
+    return snprintf(buf, 8, "%s\n", selftestRes);
+}
+
+/*****************************************
+ *** store_selftest_value
+ *****************************************/
+static ssize_t store_selftest_value(struct device_driver *ddri, const char *buf, size_t count)
+{   /*write anything to this register will trigger the process*/
+    struct i2c_client *client = mc3xxx_i2c_client;  
+    int num = 0;
+    
+    if (1 != sscanf(buf, "%d", &num))
+    {
+        GSE_ERR("parse number fail\n");
+        return count;
+    }
+    else if(0 == num)
+    {
+        GSE_ERR("invalid data count\n");
+        return count;
+    }
+       
+    GSE_LOG("NORMAL:\n");
+    MC3XXX_MUTEX_LOCK();
+    MC3XXX_SetPowerMode(client, true); 
+    MC3XXX_MUTEX_UNLOCK();
+    GSE_LOG("SELFTEST:\n");    
+    
+    if (!MC3XXX_JudgeTestResult(client))
+    {
+        GSE_LOG("SELFTEST : PASS\n");
+        strcpy(selftestRes,"y");
+    }	
+    else
+    {
+        GSE_LOG("SELFTEST : FAIL\n");		
+        strcpy(selftestRes,"n");
+    }
+    
+    return count;
+}
+
+/*****************************************
+ *** show_firlen_value
+ *****************************************/
+static ssize_t show_firlen_value(struct device_driver *ddri, char *buf)
+{
+    #ifdef _MC3XXX_SUPPORT_LPF_
+        struct i2c_client *client = mc3xxx_i2c_client;
+        struct mc3xxx_i2c_data *obj = i2c_get_clientdata(client);
+        GSE_LOG("fwq show_firlen_value \n");
+        if(atomic_read(&obj->firlen))
+        {
+            int idx = 0, len = atomic_read(&obj->firlen);
+            GSE_LOG("len = %2d, idx = %2d\n", obj->fir.num, obj->fir.idx);
+            
+            for(idx = 0; idx < len; idx++)
+            {
+                GSE_LOG("[%5d %5d %5d]\n", obj->fir.raw[idx][MC3XXX_AXIS_X], obj->fir.raw[idx][MC3XXX_AXIS_Y], obj->fir.raw[idx][MC3XXX_AXIS_Z]);
+            }
+            
+            GSE_LOG("sum = [%5d %5d %5d]\n", obj->fir.sum[MC3XXX_AXIS_X], obj->fir.sum[MC3XXX_AXIS_Y], obj->fir.sum[MC3XXX_AXIS_Z]);
+            GSE_LOG("avg = [%5d %5d %5d]\n", obj->fir.sum[MC3XXX_AXIS_X]/len, obj->fir.sum[MC3XXX_AXIS_Y]/len, obj->fir.sum[MC3XXX_AXIS_Z]/len);
+        }
+        return snprintf(buf, PAGE_SIZE, "%d\n", atomic_read(&obj->firlen));
+    #else
+        GSE_LOG("fwq show_firlen_value \n");
+        return snprintf(buf, PAGE_SIZE, "not support\n");
+    #endif
+}
+
+/*****************************************
+ *** store_firlen_value
+ *****************************************/
+static ssize_t store_firlen_value(struct device_driver *ddri, const char *buf, size_t count)
+{
+    #ifdef _MC3XXX_SUPPORT_LPF_
+    struct i2c_client *client = mc3xxx_i2c_client;  
+    struct mc3xxx_i2c_data *obj = i2c_get_clientdata(client);
+    int firlen = 0;
+
+    GSE_LOG("fwq store_firlen_value \n");
+    
+    if(1 != sscanf(buf, "%d", &firlen))
+    {
+	GSE_ERR("invallid format\n");
+    }
+    else if(firlen > C_MAX_FIR_LENGTH)
+    {
+	GSE_ERR("exceeds maximum filter length\n");
+    }
+    else
+    { 
+        atomic_set(&obj->firlen, firlen);
+	if(0 == firlen)
+	{
+		atomic_set(&obj->fir_en, 0);
+	}
+	else
+	{
+		memset(&obj->fir, 0x00, sizeof(obj->fir));
+		atomic_set(&obj->fir_en, 1);
+	}
+    }
+    #endif    
+    return count;
+}
+
+/*****************************************
+ *** show_trace_value
+ *****************************************/
+static ssize_t show_trace_value(struct device_driver *ddri, char *buf)
+{
+	ssize_t res = 0;
+	struct mc3xxx_i2c_data *obj = mc3xxx_obj_i2c_data;
+
+    GSE_LOG("fwq show_trace_value \n");
+
+	if (obj == NULL)
+	{
+		GSE_ERR("i2c_data obj is null!!\n");
+		return 0;
+	}
+	
+	res = snprintf(buf, PAGE_SIZE, "0x%04X\n", atomic_read(&obj->trace));     
+	return res;    
+}
+
+/*****************************************
+ *** store_trace_value
+ *****************************************/
+static ssize_t store_trace_value(struct device_driver *ddri, const char *buf, size_t count)
+{
+	struct mc3xxx_i2c_data *obj = mc3xxx_obj_i2c_data;
+	int trace = 0;
+
+    GSE_LOG("fwq store_trace_value \n");
+
+	if (obj == NULL)
+	{
+		GSE_ERR("i2c_data obj is null!!\n");
+		return 0;
+	}
+	
+	if(1 == sscanf(buf, "0x%x", &trace))
+	{
+		atomic_set(&obj->trace, trace);
+	}	
+	else
+	{
+		GSE_ERR("invalid content: '%s', length = %d\n", buf, count);
+	}
+	
+	return count;    
+}
+
+/*****************************************
+ *** show_status_value
+ *****************************************/
+static ssize_t show_status_value(struct device_driver *ddri, char *buf)
+{
+	ssize_t len = 0;    
+	struct mc3xxx_i2c_data *obj = mc3xxx_obj_i2c_data;
+
+    GSE_LOG("fwq show_status_value \n");
+
+	if (obj == NULL)
+	{
+		GSE_ERR("i2c_data obj is null!!\n");
+		return 0;
+	}	
+	
+	if(obj->hw)
+	{
+		len += snprintf(buf+len, PAGE_SIZE-len, "CUST: %d %d (%d %d)\n", 
+	            obj->hw->i2c_num, obj->hw->direction, obj->hw->power_id, obj->hw->power_vol);   
+	}
+	else
+	{
+		len += snprintf(buf+len, PAGE_SIZE-len, "CUST: NULL\n");
+	}
+	return len;    
+}
+
+/*****************************************
+ *** show_power_status
+ *****************************************/
+static ssize_t show_power_status(struct device_driver *ddri, char *buf)
+{
+	ssize_t res = 0;
+	u8 uData = 0;
+	struct mc3xxx_i2c_data *obj = mc3xxx_obj_i2c_data;
+
+	if (obj == NULL)
+	{
+		GSE_ERR("i2c_data obj is null!!\n");
+		return 0;
+	}
+	hwmsen_read_byte(obj->client, MC3XXX_REG_MODE_FEATURE, &uData);
+	
+	res = snprintf(buf, PAGE_SIZE, "0x%04X\n", uData);     
+	return res;   
+}
+
+/*****************************************
+ *** show_version_value
+ *****************************************/
+static ssize_t show_version_value(struct device_driver *ddri, char *buf)
+{
+	if ( 1 == VIRTUAL_Z)	
+		return snprintf(buf, PAGE_SIZE, "%s\n", MC3XXX_DEV_DRIVER_VERSION_VIRTUAL_Z);
+	else
+		return snprintf(buf, PAGE_SIZE, "%s\n", MC3XXX_DEV_DRIVER_VERSION);	
+}
+
+/*****************************************
+ *** show_chip_id
+ *****************************************/
+static ssize_t show_chip_id(struct device_driver *ddri, char *buf)
+{
+	struct mc3xxx_i2c_data   *_pt_i2c_data = mc3xxx_obj_i2c_data;
+
+	return MC3XXX_Read_Chip_ID(_pt_i2c_data->client, buf);
+}
+
+/*****************************************
+ *** show_virtual_z
+ *****************************************/
+static ssize_t show_virtual_z(struct device_driver *ddri, char *buf)
+{
+	return snprintf(buf, PAGE_SIZE, "%s\n", VIRTUAL_Z == 1 ? "Virtual Z Support": "No Virtual Z Support");     
+}
+
+/*****************************************
+ *** show_regiter_map
+ *****************************************/
+static ssize_t show_regiter_map(struct device_driver *ddri, char *buf)
+{
+    u8         _bIndex       = 0;
+    u8         _baRegMap[64] = { 0 };
+    ssize_t    _tLength      = 0;
+
+    struct i2c_client *client = mc3xxx_i2c_client;
+
+    if ((0xA5 == buf[0]) && (0x7B == buf[1]) && (0x40 == buf[2]))
+    {
+        MC3XXX_MUTEX_LOCK();
+        MC3XXX_Read_Reg_Map(client, buf);
+        MC3XXX_MUTEX_UNLOCK();
+
+        buf[0x21] = s_baOTP_OffsetData[0];
+        buf[0x22] = s_baOTP_OffsetData[1];
+        buf[0x23] = s_baOTP_OffsetData[2];
+        buf[0x24] = s_baOTP_OffsetData[3];
+        buf[0x25] = s_baOTP_OffsetData[4];
+        buf[0x26] = s_baOTP_OffsetData[5];
+
+        _tLength = 64;
+    }
+    else
+    {
+        MC3XXX_MUTEX_LOCK();
+        MC3XXX_Read_Reg_Map(client, _baRegMap);
+        MC3XXX_MUTEX_UNLOCK();
+    
+        for (_bIndex = 0; _bIndex < 64; _bIndex++)
+            _tLength += snprintf((buf + _tLength), (PAGE_SIZE - _tLength), "Reg[0x%02X]: 0x%02X\n", _bIndex, _baRegMap[_bIndex]); 
+    }
+
+    return (_tLength);
+}
+
+/*****************************************
+ *** store_regiter_map
+ *****************************************/
+static ssize_t store_regiter_map(struct device_driver *ddri, const char *buf, size_t count)
+{
+    // reserved
+    //GSE_LOG("[%s] buf[0]: 0x%02X\n", __FUNCTION__, buf[0]);
+
+    return count;
+}
+
+/*****************************************
+ *** show_chip_orientation
+ *****************************************/
+static ssize_t show_chip_orientation(struct device_driver *ptDevDrv, char *pbBuf)
+{
+    ssize_t          _tLength = 0;
+    struct acc_hw   *_ptAccelHw = mc3xxx_get_cust_acc_hw();
+    
+    GSE_LOG("[%s] default direction: %d\n", __FUNCTION__, _ptAccelHw->direction);
+    
+    _tLength = snprintf(pbBuf, PAGE_SIZE, "default direction = %d\n", _ptAccelHw->direction);     
+    
+    return (_tLength);    
+}
+
+/*****************************************
+ *** store_chip_orientation
+ *****************************************/
+static ssize_t store_chip_orientation(struct device_driver *ptDevDrv, const char *pbBuf, size_t tCount)
+{
+    int                       _nDirection = 0;
+    struct mc3xxx_i2c_data   *_pt_i2c_obj = mc3xxx_obj_i2c_data;
+
+    if (NULL == _pt_i2c_obj)
+        return (0);
+
+    if (1 == sscanf(pbBuf, "%d", &_nDirection))
+    {
+        if (hwmsen_get_convert(_nDirection, &_pt_i2c_obj->cvt))
+            GSE_ERR("ERR: fail to set direction\n");
+    }
+
+    GSE_LOG("[%s] set direction: %d\n", __FUNCTION__, _nDirection);
+
+    return (tCount);
+}
+
+/*****************************************
+ *** show_accuracy_status
+ *****************************************/
+static ssize_t show_accuracy_status(struct device_driver *ddri, char *buf)
+{
+	return snprintf(buf, PAGE_SIZE, "%d\n", s_bAccuracyStatus);     
+}
+
+/*****************************************
+ *** store_accuracy_status
+ *****************************************/
+static ssize_t store_accuracy_status(struct device_driver *ddri, const char *buf, size_t count)
+{
+    int    _nAccuracyStatus = 0;
+    
+    if (1 != sscanf(buf, "%d", &_nAccuracyStatus))
+    {
+        GSE_ERR("incorrect argument\n");
+        return count;
+    }
+
+    if (SENSOR_STATUS_ACCURACY_HIGH < _nAccuracyStatus)
+    {
+        GSE_ERR("illegal accuracy status\n");
+        return count;
+    }
+
+    s_bAccuracyStatus = ((int8_t) _nAccuracyStatus);
+
+    return count;
+}
+
+/*****************************************
+ *** show_selfcheck_value
+ *****************************************/
+static ssize_t show_selfcheck_value(struct device_driver *ptDevDriver, char *pbBuf)
+{
+    struct i2c_client   *_pt_i2c_client = mc3xxx_i2c_client;
+
+    //GSE_LOG("[%s] 0x%02X\n", __FUNCTION__, pbBuf[0]);
+
+    MC3XXX_MUTEX_LOCK();
+    MC3XXX_SelfCheck(_pt_i2c_client, pbBuf);
+    MC3XXX_Init(_pt_i2c_client, 0);
+    MC3XXX_MUTEX_UNLOCK();
+
+	return (64);
+}
+
+/*****************************************
+ *** store_selfcheck_value
+ *****************************************/
+static ssize_t store_selfcheck_value(struct device_driver *ddri, const char *buf, size_t count)
+{
+    // reserved
+    //GSE_LOG("[%s] buf[0]: 0x%02X\n", __FUNCTION__, buf[0]);
+
+    return count;
+}
+
+/*****************************************
+ *** show_chip_validate_value
+ *****************************************/
+static ssize_t show_chip_validate_value(struct device_driver *ptDevDriver, char *pbBuf)
+{
+    unsigned char    _bChipValidation = 0;
+
+    _bChipValidation = MC3XXX_ValidateSensorIC(&s_bPCODE, &s_bHWID);
+
+    return snprintf(pbBuf, PAGE_SIZE, "%d\n", _bChipValidation);        
+}
+
+/*****************************************
+ *** show_pdoc_enable_value
+ *****************************************/
+static ssize_t show_pdoc_enable_value(struct device_driver *ptDevDriver, char *pbBuf)
+{
+    #ifdef _MC3XXX_SUPPORT_PERIODIC_DOC_
+        return snprintf(pbBuf, PAGE_SIZE, "%d\n", s_bIsPDOC_Enabled);        
+    #else
+        unsigned char    _bIsPDOC_Enabled = false;
+        
+        return snprintf(pbBuf, PAGE_SIZE, "%d\n", _bIsPDOC_Enabled);		 
+    #endif
+}
+
+/*****************************************
+ *** DRIVER ATTRIBUTE LIST TABLE
+ *****************************************/
+static DRIVER_ATTR(chipinfo   ,           S_IRUGO, show_chipinfo_value,   NULL                  );
+static DRIVER_ATTR(sensordata ,           S_IRUGO, show_sensordata_value, NULL                  );
+static DRIVER_ATTR(cali       , S_IWUSR | S_IRUGO, show_cali_value,       store_cali_value      );
+static DRIVER_ATTR(selftest   , S_IWUSR | S_IRUGO, show_selftest_value,   store_selftest_value  );
+static DRIVER_ATTR(firlen     , S_IWUSR | S_IRUGO, show_firlen_value,     store_firlen_value    );
+static DRIVER_ATTR(trace      , S_IWUSR | S_IRUGO, show_trace_value,      store_trace_value     );
+static DRIVER_ATTR(status     ,           S_IRUGO, show_status_value,     NULL                  );
+static DRIVER_ATTR(power      ,           S_IRUGO, show_power_status,     NULL                  );
+static DRIVER_ATTR(version    ,           S_IRUGO, show_version_value,    NULL                  );
+static DRIVER_ATTR(chipid     ,           S_IRUGO, show_chip_id,          NULL                  );
+static DRIVER_ATTR(virtualz   ,           S_IRUGO, show_virtual_z,        NULL                  );
+static DRIVER_ATTR(regmap     , S_IWUSR | S_IRUGO, show_regiter_map,      store_regiter_map     );
+static DRIVER_ATTR(orientation, S_IWUSR | S_IRUGO, show_chip_orientation, store_chip_orientation);
+static DRIVER_ATTR(accuracy   , S_IWUSR | S_IRUGO, show_accuracy_status , store_accuracy_status );
+static DRIVER_ATTR(selfcheck  , S_IWUSR | S_IRUGO, show_selfcheck_value , store_selfcheck_value );
+static DRIVER_ATTR(validate   ,           S_IRUGO, show_chip_validate_value, NULL                  );
+static DRIVER_ATTR(pdoc       ,           S_IRUGO, show_pdoc_enable_value  , NULL                  );
+
+static struct driver_attribute   *mc3xxx_attr_list[] = {
+                                                           &driver_attr_chipinfo,
+                                                           &driver_attr_sensordata,
+                                                           &driver_attr_cali,
+                                                           &driver_attr_selftest,
+                                                           &driver_attr_firlen,
+                                                           &driver_attr_trace,
+                                                           &driver_attr_status,
+                                                           &driver_attr_power, 
+                                                           &driver_attr_version, 
+                                                           &driver_attr_chipid,
+                                                           &driver_attr_virtualz,
+                                                           &driver_attr_regmap,
+                                                           &driver_attr_orientation,
+                                                           &driver_attr_accuracy,
+                                                           &driver_attr_selfcheck,
+                                                           &driver_attr_validate,
+                                                           &driver_attr_pdoc,
+                                                       };
+
+/*****************************************
+ *** mc3xxx_create_attr
+ *****************************************/
+static int mc3xxx_create_attr(struct device_driver *driver) 
+{
+	int idx, err = 0;
+	int num = (int)(sizeof(mc3xxx_attr_list)/sizeof(mc3xxx_attr_list[0]));
+	if (driver == NULL)
+	{
+		return -EINVAL;
+	}
+
+	for(idx = 0; idx < num; idx++)
+	{
+		if((err = driver_create_file(driver, mc3xxx_attr_list[idx])))
+		{            
+			GSE_ERR("driver_create_file (%s) = %d\n", mc3xxx_attr_list[idx]->attr.name, err);
+			break;
+		}
+	}    
+	return err;
+}
+
+/*****************************************
+ *** mc3xxx_delete_attr
+ *****************************************/
+static int mc3xxx_delete_attr(struct device_driver *driver)
+{
+	int idx ,err = 0;
+	int num = (int)(sizeof(mc3xxx_attr_list)/sizeof(mc3xxx_attr_list[0]));
+
+	if(driver == NULL)
+	{
+		return -EINVAL;
+	}
+	
+
+	for(idx = 0; idx < num; idx++)
+	{
+		driver_remove_file(driver, mc3xxx_attr_list[idx]);
+	}
+	
+
+	return err;
+}
+
+/*****************************************
+ *** gsensor_operate
+ *****************************************/
+static int gsensor_operate(void* self, uint32_t command, void* buff_in, int size_in,
+		void* buff_out, int size_out, int* actualout)
+{
+	int err = 0;
+	int value = 0;	
+	struct mc3xxx_i2c_data *priv = (struct mc3xxx_i2c_data*)self;
+	hwm_sensor_data* gsensor_data = NULL;
+	char buff[MC3XXX_BUF_SIZE] = { 0 };
+	
+	GSE_FUN(f);
+	switch (command)
+	{
+		case SENSOR_DELAY:
+			GSE_LOG("fwq set delay\n");
+			if((buff_in == NULL) || (size_in < sizeof(int)))
+			{
+				GSE_ERR("Set delay parameter error!\n");
+				err = -EINVAL;
+			}
+			else
+			{
+				value = *(int *)buff_in;
+				if(value >= 50)
+				{
+				    atomic_set(&priv->filter, 0);
+				}
+				else
+				{			
+	    			    #if defined(_MC3XXX_SUPPORT_LPF_)  
+	 				priv->fir.num = 0;
+					priv->fir.idx = 0;
+					priv->fir.sum[MC3XXX_AXIS_X] = 0;
+					priv->fir.sum[MC3XXX_AXIS_Y] = 0;
+					priv->fir.sum[MC3XXX_AXIS_Z] = 0;
+					atomic_set(&priv->filter, 1);
+                                    #endif					
+				}
+			}
+			break;
+
+		case SENSOR_ENABLE:
+			if((buff_in == NULL) || (size_in < sizeof(int)))
+			{
+				GSE_ERR("Enable sensor parameter error!\n");
+				err = -EINVAL;
+			}
+			else
+			{
+				value = *(int *)buff_in;
+
+                GSE_LOG("fwq sensor enable gsensor: %d\n", value);
+
+				if(((value == 0) && (mc3xxx_sensor_power == false)) ||((value == 1) && (mc3xxx_sensor_power == true)))
+				{
+					GSE_LOG("Gsensor device have updated!\n");
+				}
+				else
+				{
+                    MC3XXX_MUTEX_LOCK();
+					err = MC3XXX_SetPowerMode( priv->client, !mc3xxx_sensor_power);
+                    MC3XXX_MUTEX_UNLOCK();
+				}
+            
+                #ifdef _MC3XXX_SUPPORT_PERIODIC_DOC_
+                    if (0 == value)
+                        atomic_set(&s_t_mc3xxx_open_status, 0);
+                    else
+                        atomic_set(&s_t_mc3xxx_open_status, 1);
+                    
+                    wake_up(&wq_mc3xxx_open_status);
+                #endif
+			}
+			break;
+
+		case SENSOR_GET_DATA:
+			GSE_LOG("fwq sensor operate get data\n");
+			if((buff_out == NULL) || (size_out< sizeof(hwm_sensor_data)))
+			{
+				GSE_ERR("get sensor data parameter error!\n");
+				err = -EINVAL;
+			}
+			else
+			{
+				gsensor_data = (hwm_sensor_data *)buff_out;
+                MC3XXX_MUTEX_LOCK();
+				MC3XXX_ReadSensorData(priv->client, buff, MC3XXX_BUF_SIZE);
+                MC3XXX_MUTEX_UNLOCK();
+				sscanf(buff, "%x %x %x", &gsensor_data->values[0], 
+					&gsensor_data->values[1], &gsensor_data->values[2]);				
+                gsensor_data->status = s_bAccuracyStatus;
+				gsensor_data->value_divide = 1000;
+				GSE_LOG("MC3XXX_ReadSensorData: X :%d,Y: %d, Z: %d\n",gsensor_data->values[0],gsensor_data->values[1],gsensor_data->values[2]);
+			}
+			break;
+		default:
+			GSE_ERR("gsensor operate function no this parameter %d!\n", command);
+			err = -1;
+			break;
+	}
+	
+	return err;
+}
+
+/*****************************************
+ *** mc3xxx_open
+ *****************************************/
+static int mc3xxx_open(struct inode *inode, struct file *file)
+{
+	file->private_data = mc3xxx_i2c_client;
+
+	if(file->private_data == NULL)
+	{
+		GSE_ERR("null pointer!!\n");
+		return -EINVAL;
+	}
+	return nonseekable_open(inode, file);
+}
+
+/*****************************************
+ *** mc3xxx_release
+ *****************************************/
+static int mc3xxx_release(struct inode *inode, struct file *file)
+{
+	file->private_data = NULL;
+	return 0;
+}
+
+/*****************************************
+ *** mc3xxx_ioctl
+ *****************************************/
+static long mc3xxx_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
+{
+	struct i2c_client *client = (struct i2c_client*)file->private_data;
+	struct mc3xxx_i2c_data *obj = (struct mc3xxx_i2c_data*)i2c_get_clientdata(client);	
+	char strbuf[MC3XXX_BUF_SIZE] = {0};
+	void __user *data = NULL;
+	SENSOR_DATA sensor_data = {0};
+	long err = 0;
+	int cali[3] = {0};
+
+        #ifdef _MC3XXX_SUPPORT_DOT_CALIBRATION_
+            int prod = -1;
+            int tempZ = 0;
+            unsigned char _bTempPCode = 0x00;
+        #endif
+
+	//GSE_FUN(f);
+	if(_IOC_DIR(cmd) & _IOC_READ)
+	{
+		err = !access_ok(VERIFY_WRITE, (void __user *)arg, _IOC_SIZE(cmd));
+	}
+	else if(_IOC_DIR(cmd) & _IOC_WRITE)
+	{
+		err = !access_ok(VERIFY_READ, (void __user *)arg, _IOC_SIZE(cmd));
+	}
+
+	if(err)
+	{
+		GSE_ERR("access error: %08X, (%2d, %2d)\n", cmd, _IOC_DIR(cmd), _IOC_SIZE(cmd));
+		return -EFAULT;
+	}
+
+	switch(cmd)
+	{
+		case GSENSOR_IOCTL_INIT:
+			GSE_LOG("fwq GSENSOR_IOCTL_INIT\n");
+            MC3XXX_MUTEX_LOCK();
+			MC3XXX_Init(client, 0);			
+            MC3XXX_MUTEX_UNLOCK();
+			break;
+
+		case GSENSOR_IOCTL_READ_CHIPINFO:
+			GSE_LOG("fwq GSENSOR_IOCTL_READ_CHIPINFO\n");
+			data = (void __user *) arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}
+			
+			MC3XXX_ReadChipInfo(client, strbuf, MC3XXX_BUF_SIZE);
+			if(copy_to_user(data, strbuf, strlen(strbuf)+1))
+			{
+				err = -EFAULT;
+				break;
+			}				 
+			break;	  
+
+		case GSENSOR_IOCTL_READ_SENSORDATA:
+			data = (void __user *) arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}
+            MC3XXX_MUTEX_LOCK();
+            #ifdef _MC3XXX_SUPPORT_APPLY_AVERAGE_AGORITHM_
+                MC3XXX_ReadRawData(client, strbuf);
+            #else
+                MC3XXX_ReadSensorData(client, strbuf, MC3XXX_BUF_SIZE);
+            #endif
+            MC3XXX_MUTEX_UNLOCK();
+			if(copy_to_user(data, strbuf, strlen(strbuf)+1))
+			{
+				err = -EFAULT;
+				break;	  
+			}				 
+			break;
+
+		case GSENSOR_IOCTL_READ_GAIN:
+			GSE_LOG("fwq GSENSOR_IOCTL_READ_GAIN\n");
+			data = (void __user *) arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}			
+			
+			if(copy_to_user(data, &gsensor_gain, sizeof(GSENSOR_VECTOR3D)))
+			{
+				err = -EFAULT;
+				break;
+			}				 
+			break;
+
+		case GSENSOR_IOCTL_READ_OFFSET:
+			GSE_LOG("fwq GSENSOR_IOCTL_READ_OFFSET\n");
+			data = (void __user *) arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}
+			
+			if(copy_to_user(data, &gsensor_offset, sizeof(GSENSOR_VECTOR3D)))
+			{
+				err = -EFAULT;
+				break;
+			}				 
+			break;
+
+		case GSENSOR_IOCTL_READ_RAW_DATA:
+			GSE_LOG("fwq GSENSOR_IOCTL_READ_RAW_DATA\n");
+			data = (void __user *) arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}
+            MC3XXX_MUTEX_LOCK();
+			MC3XXX_ReadRawData(client, strbuf);
+            MC3XXX_MUTEX_UNLOCK();
+			if(copy_to_user(data, strbuf, strlen(strbuf)+1))
+			{
+				err = -EFAULT;
+				break;	  
+			}
+			break;	  
+
+		case GSENSOR_IOCTL_SET_CALI:
+			GSE_LOG("fwq GSENSOR_IOCTL_SET_CALI!!\n");
+			data = (void __user*)arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}
+			if(copy_from_user(&sensor_data, data, sizeof(sensor_data)))
+			{
+				err = -EFAULT;
+				break;	  
+			}
+			if(atomic_read(&obj->suspend))
+			{
+				GSE_ERR("Perform calibration in suspend state!!\n");
+				err = -EINVAL;
+			}
+			else
+			{
+				obj->cali_sw[MC3XXX_AXIS_X] += sensor_data.x;
+				obj->cali_sw[MC3XXX_AXIS_Y] += sensor_data.y;
+				obj->cali_sw[MC3XXX_AXIS_Z] += sensor_data.z;
+				
+				cali[MC3XXX_AXIS_X] = sensor_data.x * gsensor_gain.x / GRAVITY_EARTH_1000;
+				cali[MC3XXX_AXIS_Y] = sensor_data.y * gsensor_gain.y / GRAVITY_EARTH_1000;
+				cali[MC3XXX_AXIS_Z] = sensor_data.z * gsensor_gain.z / GRAVITY_EARTH_1000;	
+			  
+                MC3XXX_MUTEX_LOCK();
+				err = MC3XXX_WriteCalibration(client, cali);			 
+                MC3XXX_MUTEX_UNLOCK();
+			}
+			break;
+
+		case GSENSOR_IOCTL_CLR_CALI:
+			GSE_LOG("fwq GSENSOR_IOCTL_CLR_CALI!!\n");
+            MC3XXX_MUTEX_LOCK();
+			err = MC3XXX_ResetCalibration(client);
+            MC3XXX_MUTEX_UNLOCK();
+			break;
+
+		case GSENSOR_IOCTL_GET_CALI:
+			GSE_LOG("fwq mc3xxx GSENSOR_IOCTL_GET_CALI\n");
+			data = (void __user*)arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}
+//			if((err = MC3XXX_ReadCalibration(client, cali)))
+//			{
+//				break;
+//			}
+
+			sensor_data.x = obj->cali_sw[MC3XXX_AXIS_X];
+			sensor_data.y = obj->cali_sw[MC3XXX_AXIS_Y];
+			sensor_data.z = obj->cali_sw[MC3XXX_AXIS_Z];
+			if(copy_to_user(data, &sensor_data, sizeof(sensor_data)))
+			{
+				err = -EFAULT;
+				break;
+			}		
+			break;
+
+        //add in Sensors_io.h
+        // ==============================================================================
+        #ifdef _MC3XXX_SUPPORT_DOT_CALIBRATION_
+        
+		case GSENSOR_MCUBE_IOCTL_SET_CALI:
+			GSE_LOG("fwq GSENSOR_MCUBE_IOCTL_SET_CALI!!\n");
+			data = (void __user*)arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}
+			if(copy_from_user(&sensor_data, data, sizeof(sensor_data)))
+			{
+				err = -EFAULT;
+				break;	  
+			}
+			if(atomic_read(&obj->suspend))
+			{
+				GSE_ERR("Perform calibration in suspend state!!\n");
+				err = -EINVAL;
+			}
+			else
+			{
+				cali[MC3XXX_AXIS_X] = sensor_data.x * gsensor_gain.x / GRAVITY_EARTH_1000;
+				cali[MC3XXX_AXIS_Y] = sensor_data.y * gsensor_gain.y / GRAVITY_EARTH_1000;
+				cali[MC3XXX_AXIS_Z] = sensor_data.z * gsensor_gain.z / GRAVITY_EARTH_1000;	
+			  
+                MC3XXX_MUTEX_LOCK();
+				err = MC3XXX_WriteCalibration(client, cali);			 
+                MC3XXX_MUTEX_UNLOCK();
+			}
+			break;
+
+		case GSENSOR_IOCTL_SET_CALI_MODE:
+			GSE_LOG("fwq mc3xxx GSENSOR_IOCTL_SET_CALI_MODE\n");
+			break;
+
+		case GSENSOR_MCUBE_IOCTL_READ_RBM_DATA:
+			GSE_LOG("fwq GSENSOR_MCUBE_IOCTL_READ_RBM_DATA\n");
+			data = (void __user *) arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}
+            MC3XXX_MUTEX_LOCK();
+			MC3XXX_ReadRBMData(client, strbuf);
+            MC3XXX_MUTEX_UNLOCK();
+			if(copy_to_user(data, strbuf, strlen(strbuf)+1))
+			{
+				err = -EFAULT;
+				break;	  
+			}
+			break;
+
+		case GSENSOR_MCUBE_IOCTL_SET_RBM_MODE:
+			GSE_LOG("fwq GSENSOR_MCUBE_IOCTL_SET_RBM_MODE\n");
+            MC3XXX_MUTEX_LOCK();
+			MC3XXX_rbm(client,1);
+            MC3XXX_MUTEX_UNLOCK();
+			break;
+
+		case GSENSOR_MCUBE_IOCTL_CLEAR_RBM_MODE:
+			GSE_LOG("fwq GSENSOR_MCUBE_IOCTL_CLEAR_RBM_MODE\n");
+            MC3XXX_MUTEX_LOCK();
+			MC3XXX_rbm(client,0);
+            MC3XXX_MUTEX_UNLOCK();
+			break;
+
+		case GSENSOR_MCUBE_IOCTL_REGISTER_MAP:
+			GSE_LOG("fwq GSENSOR_MCUBE_IOCTL_REGISTER_MAP\n");
+
+			MC3XXX_Read_Reg_Map(client, NULL);
+
+			break;
+
+		case GSENSOR_MCUBE_IOCTL_READ_PRODUCT_ID:
+			GSE_LOG("fwq GSENSOR_MCUBE_IOCTL_READ_PRODUCT_ID\n");
+			data = (void __user *) arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}
+
+            if (MC3XXX_RETCODE_SUCCESS != (prod = MC3XXX_ValidateSensorIC(&s_bPCODE, &s_bHWID)))
+				GSE_LOG("Not mCube accelerometers!\n");
+
+			if(copy_to_user(data, &prod, sizeof(prod)))
+			{
+				err = -EFAULT;
+				break;
+			}				 
+			break;
+
+		case GSENSOR_MCUBE_IOCTL_READ_FILEPATH:
+			GSE_LOG("fwq GSENSOR_MCUBE_IOCTL_READ_FILEPATH\n");
+			data = (void __user *) arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}
+			
+			if(copy_to_user(data, file_path, (strlen(file_path)+1)))
+			{
+				err = -EFAULT;
+				break;
+			}				 
+			break;		
+
+		case GSENSOR_MCUBE_IOCTL_VIRTUAL_Z:
+			GSE_LOG("fwq GSENSOR_MCUBE_IOCTL_VIRTUAL_Z\n");
+			data = (void __user *) arg;
+			tempZ = VIRTUAL_Z;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}
+			if(copy_to_user(data, &tempZ, sizeof(tempZ)))
+			{
+				err = -EFAULT;
+				break;
+			}				 
+			break;
+
+		case GSENSOR_MCUBE_IOCTL_READ_PCODE:
+			GSE_LOG("fwq GSENSOR_MCUBE_IOCTL_READ_PCODE\n");
+			data = (void __user *) arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}
+
+			_bTempPCode = s_bPCODE;
+			GSE_LOG("mCube PCode = %2x!\n", _bTempPCode);
+			if(copy_to_user(data, &_bTempPCode, sizeof(_bTempPCode)))
+			{
+				err = -EFAULT;
+				break;
+			}				 
+			break;
+
+        #endif    // END of _MC3XXX_SUPPORT_DOT_CALIBRATION_
+
+        // ==============================================================================
+        #ifdef _MC3XXX_SUPPORT_PERIODIC_DOC_
+        
+        case GSENSOR_MCUBE_IOCTL_GET_OFLAG:
+            {
+                int            _nSensorsOpenStatus = 0;
+                void __user   *_pArg = ((void __user *) arg);
+                
+                GSE_LOG("[%s] GSENSOR_MCUBE_IOCTL_GET_OFLAG\n", __func__);
+                
+                _nSensorsOpenStatus = MC3XXX_GetOpenStatus();			
+                
+                if(copy_to_user(_pArg, &_nSensorsOpenStatus, sizeof(_nSensorsOpenStatus)))
+                    return (-EFAULT);
+            }
+            break; 	   
+
+        #endif    // END of _MC3XXX_SUPPORT_PERIODIC_DOC_
+			
+		default:
+			GSE_ERR("unknown IOCTL: 0x%08x\n", cmd);
+			err = -ENOIOCTLCMD;
+			break;
+			
+	}
+
+	return err;
+}
+
+/*****************************************
+ *** MC3XXX_reset
+ *****************************************/
+static void MC3XXX_reset(struct i2c_client *client)
+{
+    unsigned char    _baBuf[2] = { 0 };
+    
+    _baBuf[1] = 0x43;
+    _baBuf[0] = MC3XXX_REG_MODE_FEATURE;
+    i2c_master_send(client, _baBuf, 0x2);
+    
+    hwmsen_read_block(client, 0x04, _baBuf, 0x01);
+    
+    if (0x00 == (_baBuf[0] & 0x40))
+    {
+        _baBuf[0] = 0x6D; 
+        hwmsen_write_block(client, 0x1B, _baBuf, 0x01);
+        
+        _baBuf[0] = 0x43; 
+        hwmsen_write_block(client, 0x1B, _baBuf, 0x01);
+    }
+    
+    _baBuf[0] = 0x43;
+    hwmsen_write_block(client, 0x07, _baBuf, 1);	
+    
+    _baBuf[0] = 0x80;
+    hwmsen_write_block(client, 0x1C, _baBuf, 1);	
+
+    _baBuf[0] = 0x80;
+    hwmsen_write_block(client, 0x17, _baBuf, 1);	
+
+    msleep(5);
+    
+    _baBuf[0] = 0x00;
+    hwmsen_write_block(client, 0x1C, _baBuf, 1);	
+
+    _baBuf[0] = 0x00;
+    hwmsen_write_block(client, 0x17, _baBuf, 1);	
+
+    msleep(5);
+    
+    hwmsen_read_block(client, 0x21, offset_buf, 6);
+    
+    hwmsen_read_block(client, 0x04, _baBuf, 0x01);
+    
+    if (_baBuf[0] & 0x40)
+    {
+        _baBuf[0] = 0x6D; 
+        hwmsen_write_block(client, 0x1B, _baBuf, 0x01);
+        
+        _baBuf[0] = 0x43; 
+        hwmsen_write_block(client, 0x1B, _baBuf, 0x01);
+    }
+    
+    _baBuf[1] = 0x41;
+    _baBuf[0] = MC3XXX_REG_MODE_FEATURE;
+    i2c_master_send(client, _baBuf, 0x2);
+}
+
+/*****************************************
+ *** STRUCT:: mc3xxx_fops
+ *****************************************/
+static struct file_operations    mc3xxx_fops = {
+                                                   .owner          = THIS_MODULE,
+                                                   .open           = mc3xxx_open,
+                                                   .release        = mc3xxx_release,
+                                                   .unlocked_ioctl = mc3xxx_ioctl,
+                                               };
+
+/*****************************************
+ *** STRUCT:: mc3xxx_device
+ *****************************************/
+static struct miscdevice    mc3xxx_device = {
+                                                .minor = MISC_DYNAMIC_MINOR,
+                                                .name  = "gsensor",
+                                                .fops  = &mc3xxx_fops,
+                                            };
+
+#ifndef CONFIG_HAS_EARLYSUSPEND
+/*****************************************
+ *** mc3xxx_suspend
+ *****************************************/
+static int mc3xxx_suspend(struct i2c_client *client, pm_message_t msg) 
+{
+	struct mc3xxx_i2c_data *obj = i2c_get_clientdata(client);    
+	int err = 0;
+	GSE_FUN();    
+
+	if(msg.event == PM_EVENT_SUSPEND)
+	{   
+		if(obj == NULL)
+		{
+			GSE_ERR("null pointer!!\n");
+			return -EINVAL;
+		}
+		
+		atomic_set(&obj->suspend, 1);
+
+        MC3XXX_MUTEX_LOCK();
+        err = MC3XXX_SetPowerMode(client, false);
+        MC3XXX_MUTEX_UNLOCK();
+
+        if (err)
+        {
+            GSE_ERR("write power control fail!!\n");
+            return err;
+        }     
+
+		MC3XXX_power(obj->hw, 0);
+	}
+	return err;
+}
+
+/*****************************************
+ *** mc3xxx_resume
+ *****************************************/
+static int mc3xxx_resume(struct i2c_client *client)
+{
+	struct mc3xxx_i2c_data *obj = i2c_get_clientdata(client);        
+	int err;
+	GSE_FUN();
+
+	if(obj == NULL)
+	{
+		GSE_ERR("null pointer!!\n");
+		return -EINVAL;
+	}
+
+	MC3XXX_power(obj->hw, 1);
+
+    MC3XXX_MUTEX_LOCK();
+    err = MC3XXX_Init(client, 0);
+
+    if(err)
+    {
+        MC3XXX_MUTEX_UNLOCK();
+        GSE_ERR("initialize client fail!!\n");
+        return err;        
+    }
+
+    err = MC3XXX_SetPowerMode(client, true);
+    MC3XXX_MUTEX_UNLOCK();
+
+    if (err)
+    {
+        GSE_ERR("write power control fail!!\n");
+        return err;
+    }     
+
+	atomic_set(&obj->suspend, 0);
+
+	return 0;
+}
+#else
+/*****************************************
+ *** mc3xxx_early_suspend
+ *****************************************/
+static void mc3xxx_early_suspend(struct early_suspend *h) 
+{
+	struct mc3xxx_i2c_data *obj = container_of(h, struct mc3xxx_i2c_data, early_drv);   
+	int err;
+	GSE_FUN();    
+
+	if(obj == NULL)
+	{
+		GSE_ERR("null pointer!!\n");
+		return;
+	}
+	atomic_set(&obj->suspend, 1); 
+	/*
+	if(err = hwmsen_write_byte(obj->client, MC3XXX_REG_POWER_CTL, 0x00))
+	{
+		GSE_ERR("write power control fail!!\n");
+		return;
+	}  
+	*/
+    MC3XXX_MUTEX_LOCK_RETURN_VOID();
+    err = MC3XXX_SetPowerMode(obj->client, false);
+    MC3XXX_MUTEX_UNLOCK();
+
+    if (err)
+    {
+        GSE_ERR("write power control fail!!\n");
+        return;
+    }     
+
+	MC3XXX_power(obj->hw, 0);
+}
+
+/*****************************************
+ *** mc3xxx_late_resume
+ *****************************************/
+static void mc3xxx_late_resume(struct early_suspend *h)
+{
+	struct mc3xxx_i2c_data *obj = container_of(h, struct mc3xxx_i2c_data, early_drv);         
+	int err;
+	GSE_FUN();
+
+	if(obj == NULL)
+	{
+		GSE_ERR("null pointer!!\n");
+		return;
+	}
+
+	MC3XXX_power(obj->hw, 1);
+
+    MC3XXX_MUTEX_LOCK_RETURN_VOID();
+    err = MC3XXX_Init(obj->client, 0);
+
+    if(err)
+    {
+        MC3XXX_MUTEX_UNLOCK();
+        GSE_ERR("initialize client fail!!\n");
+        return;
+    }
+
+    err = MC3XXX_SetPowerMode(obj->client, true);
+    MC3XXX_MUTEX_UNLOCK();
+
+    if (err)
+    {
+        GSE_ERR("write power control fail!!\n");
+        return;
+    }     
+
+
+	atomic_set(&obj->suspend, 0);    
+}
+#endif
+
+#ifdef _MC3XXX_SUPPORT_MTK_NEW_ARCH__
+// if use  this typ of enable , Gsensor should report inputEvent(x, y, z ,stats, div) to HAL
+static int mc3xxx_open_report_data(int open)
+{
+	//should queuq work to report event if  is_report_input_direct=true
+	return 0;
+}
+
+// if use this typ of enable , Gsensor only enabled but not report inputEvent to HAL
+static int mc3xxx_enable_nodata(int en)
+{
+	struct i2c_client *client = mc3xxx_i2c_client;  
+	int res =0;
+	int retry = 0;
+	bool power=false;
+	
+	GSE_FUN();
+
+	if(1==en)
+	{
+		power=true;
+	}
+	if(0==en)
+	{
+		power =false;
+	}
+
+	for (retry = 0; retry < 3; retry++) {
+		res = MC3XXX_SetPowerMode(client, power);
+		if(res == 0)
+		{
+			GSE_LOG("MC3XXX_SetPowerMode done\n");
+			break;
+		}
+		GSE_LOG("MC3XXX_SetPowerMode fail\n");
+	}
+
+	if(res != 0)
+	{
+		GSE_LOG("MC3XXX_SetPowerMode fail!\n");
+		return -1;
+	}
+	GSE_LOG("MC3XXX_enable_nodata OK!\n");
+	return 0;
+}
+
+static int mc3xxx_set_delay(u64 ns)
+{
+	GSE_FUN();
+	return 0;
+}
+
+static int mc3xxx_get_data(int* x ,int* y,int* z, int* status)
+{
+	struct i2c_client *client = mc3xxx_i2c_client;  
+	char buff[MC3XXX_BUF_SIZE];
+	
+	GSE_FUN();
+	MC3XXX_ReadSensorData(client, buff, MC3XXX_BUF_SIZE);
+	
+	sscanf(buff, "%x %x %x", x, y, z);		
+	*status = SENSOR_STATUS_ACCURACY_MEDIUM;
+
+	return 0;
+}
+#endif //_MC3XXX_SUPPORT_MTK_NEW_ARCH__
+
+/*****************************************
+ *** _mc3xxx_i2c_auto_probe
+ *****************************************/
+static int _mc3xxx_i2c_auto_probe(struct i2c_client *client)
+{
+    #define _MC3XXX_I2C_PROBE_ADDR_COUNT_    (sizeof(mc3xxx_i2c_auto_probe_addr) / sizeof(mc3xxx_i2c_auto_probe_addr[0]))
+
+    unsigned char    _baData1Buf[2] = { 0 };
+    unsigned char    _baData2Buf[2] = { 0 };
+
+    int              _nCount = 0;
+    int              _naCheckCount[_MC3XXX_I2C_PROBE_ADDR_COUNT_] = { 0 };
+
+    //GSE_FUN();
+
+    memset(_naCheckCount, 0, sizeof(_naCheckCount));
+
+_I2C_AUTO_PROBE_RECHECK_:
+    s_bPCODE  = 0x00;
+    s_bPCODER = 0x00;
+    s_bHWID   = 0x00;
+
+    for (_nCount = 0; _nCount < _MC3XXX_I2C_PROBE_ADDR_COUNT_; _nCount++)
+    {
+        client->addr = mc3xxx_i2c_auto_probe_addr[_nCount];
+
+        //GSE_LOG("[%s][%d] probing addr: 0x%X\n", __FUNCTION__, _nCount, client->addr);
+
+        _baData1Buf[0] = 0x3B;
+        if (0 > i2c_master_send(client, &(_baData1Buf[0]), 1))
+        {
+            //GSE_ERR("ERR: addr: 0x%X fail to communicate-2!\n", client->addr);
+            continue;
+        }
+    
+        if (0 > i2c_master_recv(client, &(_baData1Buf[0]), 1))
+        {
+            //GSE_ERR("ERR: addr: 0x%X fail to communicate-3!\n", client->addr);
+            continue;
+        }
+
+        _naCheckCount[_nCount]++;
+
+        //GSE_LOG("[%s][%d] addr: 0x%X ok to read REG(0x3B): 0x%X\n", __FUNCTION__, _nCount, client->addr, _baData1Buf[0]);
+
+        if (0x00 == _baData1Buf[0])
+        {
+            if (1 == _naCheckCount[_nCount])
+            {
+                MC3XXX_reset(client);
+                msleep(3);
+                goto _I2C_AUTO_PROBE_RECHECK_;
+            }
+            else
+            {
+                continue;
+            }
+        }
+
+        _baData2Buf[0] = 0x18;
+        i2c_master_send(client, &(_baData2Buf[0]), 1);
+        i2c_master_recv(client, &(_baData2Buf[0]), 1);
+
+        s_bPCODER = _baData1Buf[0];
+
+        if (MC3XXX_RETCODE_SUCCESS == MC3XXX_ValidateSensorIC(&_baData1Buf[0], &_baData2Buf[0]))
+        {
+            s_bPCODE = _baData1Buf[0];
+            s_bHWID  = _baData2Buf[0];
+
+            MC3XXX_SaveDefaultOffset(client);
+
+            //GSE_LOG("[%s] addr: 0x%X confirmed ok to use. s_bPCODE: 0x%02X, s_bHWID: 0x%02X\n", __FUNCTION__, client->addr, s_bPCODE, s_bHWID);
+
+            return (MC3XXX_RETCODE_SUCCESS);
+        }
+    }
+
+    return (MC3XXX_RETCODE_ERROR_I2C);
+
+    #undef _MC3XXX_I2C_PROBE_ADDR_COUNT_
+}
+
+/*****************************************
+ *** mc3xxx_i2c_probe
+ *****************************************/
+static int mc3xxx_i2c_probe(struct i2c_client *client, const struct i2c_device_id *id)
+{
+	struct i2c_client *new_client;
+	struct mc3xxx_i2c_data *obj;
+	struct hwmsen_object sobj;
+	int err = 0;
+	
+#ifdef _MC3XXX_SUPPORT_MTK_NEW_ARCH__	
+	struct acc_control_path ctl={0};
+	struct acc_data_path data={0};	
+#endif
+
+	GSE_FUN();
+
+    if (MC3XXX_RETCODE_SUCCESS != _mc3xxx_i2c_auto_probe(client))
+    {
+        GSE_ERR("ERR: fail to probe mCube sensor!\n");
+        goto exit;
+    }
+
+	if(!(obj = kzalloc(sizeof(*obj), GFP_KERNEL)))
+	{
+		err = -ENOMEM;
+		goto exit;
+	}
+	
+	memset(obj, 0, sizeof(struct mc3xxx_i2c_data));
+
+	obj->hw = mc3xxx_get_cust_acc_hw();
+	
+	if((err = hwmsen_get_convert(obj->hw->direction, &obj->cvt)))
+	{
+		GSE_ERR("invalid direction: %d\n", obj->hw->direction);
+		goto exit_kfree;
+	}
+
+	mc3xxx_obj_i2c_data = obj;
+	obj->client = client;
+	new_client = obj->client;
+	i2c_set_clientdata(new_client,obj);
+	
+	atomic_set(&obj->trace, 0);
+	atomic_set(&obj->suspend, 0);
+	
+    #ifdef _MC3XXX_SUPPORT_LPF_
+        if(obj->hw->firlen > C_MAX_FIR_LENGTH)
+        {
+            atomic_set(&obj->firlen, C_MAX_FIR_LENGTH);
+        }	
+        else
+        {
+            atomic_set(&obj->firlen, obj->hw->firlen);
+        }
+        
+        if(atomic_read(&obj->firlen) > 0)
+        {
+            atomic_set(&obj->fir_en, 1);
+        }
+    #endif
+
+	mc3xxx_i2c_client = new_client;	
+	
+	MC3XXX_reset(new_client);
+
+    if (MC3XXX_RETCODE_SUCCESS != _mc3xxx_i2c_auto_probe(client))
+    {
+        GSE_ERR("ERR: fail to probe mCube sensor!\n");
+        goto exit_init_failed;
+    }
+
+    GSE_LOG("[%s] 2nd confirmed i2c addr: 0x%X\n", __FUNCTION__, client->addr);
+
+    hwmsen_read_block(client, 0x21, offset_buf, 6);
+
+	if((err = MC3XXX_Init(new_client, 1)))
+	{
+		goto exit_init_failed;
+	}
+	
+    MC3XXX_MUTEX_INIT();
+
+	if((err = misc_register(&mc3xxx_device)))
+	{
+		GSE_ERR("mc3xxx_device register failed\n");
+		goto exit_misc_device_register_failed;
+	}
+
+#if 1 
+    #ifdef _MC3XXX_SUPPORT_AUTO_DETECT_
+        if((err = mc3xxx_create_attr(&mc3xxx_init_info.platform_diver_addr->driver)))
+    #else
+        if((err = mc3xxx_create_attr(&mc3xxx_gsensor_driver.driver)))
+    #endif
+        {
+            GSE_ERR("create attribute err = %d\n", err);
+            goto exit_create_attr_failed;
+        }
+#endif
+
+#ifndef _MC3XXX_SUPPORT_MTK_NEW_ARCH__
+	sobj.self = obj;
+  	sobj.polling = 1;
+  	sobj.sensor_operate = gsensor_operate;
+
+	if((err = hwmsen_attach(ID_ACCELEROMETER, &sobj)))
+	{
+		GSE_ERR("attach fail = %d\n", err);
+		goto exit_hwmsen_attach_failed;
+	}
+#endif
+
+#ifdef _MC3XXX_SUPPORT_MTK_NEW_ARCH__		
+	ctl.open_report_data= mc3xxx_open_report_data;
+	ctl.enable_nodata = mc3xxx_enable_nodata;
+	ctl.set_delay  = mc3xxx_set_delay;
+	ctl.is_report_input_direct = false;
+	ctl.is_support_batch = obj->hw->is_batch_supported;
+
+	err = acc_register_control_path(&ctl);
+	if(err)
+	{
+	 	GSE_ERR("register acc control path err\n");
+		goto exit_kfree;
+	}
+
+	data.get_data = mc3xxx_get_data;
+	data.vender_div = 1000;
+	err = acc_register_data_path(&data);
+	if(err)
+	{
+		GSE_ERR("register acc data path err= %d\n", err);
+		goto exit_kfree;
+	}
+#endif	
+	
+    #ifdef _MC3XXX_SUPPORT_VPROXIMITY_SENSOR_
+    // p-sensor
+    {
+        struct hwmsen_object obj_ps;
+        
+        if((err = misc_register(&mcube_psensor_device)))
+        {
+            PS_ERR("mcube_psensor_device register failed\n");
+            goto exit_misc_device_psensor_register_failed;
+        }
+        
+        obj_ps.polling = 1;
+        obj_ps.sensor_operate = psensor_ps_operate;
+        
+        if((err = hwmsen_attach(ID_PROXIMITY, &obj_ps)))
+        {
+            PS_ERR("Proximity sensor attach fail = %d\n", err);
+            goto exit_hwmsen_attach_psensor_failed;
+        }
+    }
+    #endif
+
+    #ifdef CONFIG_HAS_EARLYSUSPEND
+        obj->early_drv.level    = EARLY_SUSPEND_LEVEL_DISABLE_FB - 1,
+        obj->early_drv.suspend  = mc3xxx_early_suspend,
+        obj->early_drv.resume   = mc3xxx_late_resume,    
+        register_early_suspend(&obj->early_drv);
+    #endif 
+
+	GSE_LOG("%s: OK\n", __func__);    
+    s_nInitFlag = MC3XXX_INIT_SUCC;
+	return 0;
+
+#ifdef _MC3XXX_SUPPORT_VPROXIMITY_SENSOR_
+exit_hwmsen_attach_psensor_failed:
+    misc_deregister(&mcube_psensor_device);
+exit_misc_device_psensor_register_failed:
+#endif    // END of _MC3XXX_SUPPORT_VPROXIMITY_SENSOR_
+
+exit_hwmsen_attach_failed:
+#ifndef _MC3XXX_SUPPORT_AUTO_DETECT_
+    mc3xxx_delete_attr(&mc3xxx_gsensor_driver.driver);
+#else
+    mc3xxx_delete_attr(&mc3xxx_init_info.platform_diver_addr->driver);
+#endif    // END of not _MC3XXX_SUPPORT_AUTO_DETECT_
+
+exit_create_attr_failed:
+	misc_deregister(&mc3xxx_device);
+
+exit_misc_device_register_failed:
+exit_init_failed:
+	//i2c_detach_client(new_client);
+
+exit_kfree:
+	kfree(obj);
+
+exit:
+	GSE_ERR("%s: err = %d\n", __func__, err);        
+	s_nInitFlag = MC3XXX_INIT_FAIL;
+
+	return err;
+}
+
+/*****************************************
+ *** mc3xxx_i2c_remove
+ *****************************************/
+static int mc3xxx_i2c_remove(struct i2c_client *client)
+{
+	int err = 0;	
+	
+    #ifdef _MC3XXX_SUPPORT_AUTO_DETECT_
+        if((err = mc3xxx_delete_attr(&mc3xxx_init_info.platform_diver_addr->driver)))
+    #else
+        if((err = mc3xxx_delete_attr(&mc3xxx_gsensor_driver.driver)))
+    #endif
+        {
+            GSE_ERR("mc3xxx_delete_attr fail: %d\n", err);
+        }
+	
+	if((err = misc_deregister(&mc3xxx_device)))
+	{
+		GSE_ERR("misc_deregister fail: %d\n", err);
+	}
+
+	if((err = hwmsen_detach(ID_ACCELEROMETER)))
+	{
+		GSE_ERR("hwmsen_detach fail: %d\n", err);
+	}
+
+    #ifdef _MC3XXX_SUPPORT_VPROXIMITY_SENSOR_
+        misc_deregister(&mcube_psensor_device);
+    #endif
+
+	mc3xxx_i2c_client = NULL;
+	i2c_unregister_device(client);
+	kfree(i2c_get_clientdata(client));
+
+	return 0;
+}
+
+/*****************************************
+ *** mc3xxx_probe
+ *****************************************/
+#ifndef _MC3XXX_SUPPORT_AUTO_DETECT_
+static int mc3xxx_probe(struct platform_device *pdev) 
+{
+	struct acc_hw *hw = mc3xxx_get_cust_acc_hw();
+	GSE_FUN();
+
+	MC3XXX_power(hw, 1);
+
+	if(i2c_add_driver(&mc3xxx_i2c_driver))
+	{
+		GSE_ERR("add driver error\n");
+
+		return -1;
+	}
+
+	return 0;
+}
+
+/*****************************************
+ *** mc3xxx_remove
+ *****************************************/
+static int mc3xxx_remove(struct platform_device *pdev)
+{
+    struct acc_hw *hw = mc3xxx_get_cust_acc_hw();
+
+    GSE_FUN();    
+    MC3XXX_power(hw, 0);    
+    i2c_del_driver(&mc3xxx_i2c_driver);
+
+    return 0;
+}
+
+/*****************************************
+ *** PLATFORM_DRIVER:: mc3xxx_gsensor_driver
+ *****************************************/
+static struct platform_driver    mc3xxx_gsensor_driver = {
+                                                             .probe  = mc3xxx_probe,
+                                                             .remove = mc3xxx_remove,    
+                                                             .driver = {
+                                                                           .name  = "gsensor",
+                                                                           .owner = THIS_MODULE,
+                                                                       }
+                                                         };
+#else    // else from _MC3XXX_SUPPORT_AUTO_DETECT_
+/*****************************************
+ *** mc3xxx_remove
+ *****************************************/
+static int mc3xxx_remove(void)
+{
+    struct acc_hw *hw = mc3xxx_get_cust_acc_hw();
+    
+    GSE_FUN();    
+
+    MC3XXX_power(hw, 0);    
+    i2c_del_driver(&mc3xxx_i2c_driver);
+
+    return 0;
+}
+
+/*****************************************
+ *** mc3xxx_local_init
+ *****************************************/
+static int  mc3xxx_local_init(void)
+{
+    struct acc_hw *hw = mc3xxx_get_cust_acc_hw();
+    
+    GSE_FUN();    
+    MC3XXX_power(hw, 1);    
+    
+    if(i2c_add_driver(&mc3xxx_i2c_driver))
+    {
+        GSE_ERR("add driver error\n");
+        return -1;
+    }
+
+    if(MC3XXX_INIT_FAIL == s_nInitFlag)
+    {
+        return -1;
+    }
+    
+    return 0;
+}
+#endif
+
+/*****************************************
+ *** mc3xxx_init
+ *****************************************/
+static int __init mc3xxx_init(void)
+{
+	struct acc_hw *hw = mc3xxx_get_cust_acc_hw();
+	i2c_register_board_info(hw->i2c_num, &mc3xxx_i2c_board_info, 1);
+	
+    #ifdef _MC3XXX_SUPPORT_AUTO_DETECT_
+	#ifdef _MC3XXX_SUPPORT_MTK_NEW_ARCH__
+		acc_driver_add(&mc3xxx_init_info);
+	#else
+        hwmsen_gsensor_add(&mc3xxx_init_info);
+	#endif
+    #else
+        if(platform_driver_register(&mc3xxx_gsensor_driver))
+        {
+            GSE_ERR("failed to register driver");
+            return -ENODEV;
+        }
+    #endif
+
+	return 0;    
+}
+
+/*****************************************
+ *** mc3xxx_exit
+ *****************************************/
+static void __exit mc3xxx_exit(void)
+{
+	GSE_FUN();
+
+    #ifndef _MC3XXX_SUPPORT_AUTO_DETECT_
+        platform_driver_unregister(&mc3xxx_gsensor_driver);
+    #endif
+}
+
+/*----------------------------------------------------------------------------*/
+module_init(mc3xxx_init);
+module_exit(mc3xxx_exit);
+/*----------------------------------------------------------------------------*/
+MODULE_DESCRIPTION("mc3XXX G-Sensor Driver");
+MODULE_AUTHOR("mCube-inc");
+MODULE_LICENSE("GPL");
+MODULE_VERSION(MC3XXX_DEV_DRIVER_VERSION);
+
diff --git a/drivers/misc/mediatek/accelerometer/mc3xxx_auto/mc3xxx.h b/drivers/misc/mediatek/accelerometer/mc3xxx_auto/mc3xxx.h
new file mode 100644
index 000000000..059a90827
--- /dev/null
+++ b/drivers/misc/mediatek/accelerometer/mc3xxx_auto/mc3xxx.h
@@ -0,0 +1,116 @@
+/*****************************************************************************
+ *
+ * Copyright (c) 2013 mCube, Inc.  All rights reserved.
+ *
+ * This source is subject to the mCube Software License.
+ * This software is protected by Copyright and the information and source code
+ * contained herein is confidential. The software including the source code
+ * may not be copied and the information contained herein may not be used or
+ * disclosed except with the written permission of mCube Inc.
+ *
+ * All other rights reserved.
+ *
+ * This code and information are provided "as is" without warranty of any
+ * kind, either expressed or implied, including but not limited to the
+ * implied warranties of merchantability and/or fitness for a
+ * particular purpose.
+ *
+ * The following software/firmware and/or related documentation ("mCube Software")
+ * have been modified by mCube Inc. All revisions are subject to any receiver's
+ * applicable license agreements with mCube Inc.
+ *
+ * Accelerometer Sensor Driver
+ * 
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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 _MC3XXX_H_
+    #define _MC3XXX_H_ 
+	 
+#include <linux/ioctl.h>
+
+/***********************************************
+ *** REGISTER MAP
+ ***********************************************/
+#define MC3XXX_REG_XOUT                    0x00
+#define MC3XXX_REG_YOUT                    0x01
+#define MC3XXX_REG_ZOUT                    0x02
+#define MC3XXX_REG_TILT_STATUS             0x03
+#define MC3XXX_REG_SAMPLE_RATE_STATUS      0x04
+#define MC3XXX_REG_SLEEP_COUNT             0x05
+#define MC3XXX_REG_INTERRUPT_ENABLE        0x06
+#define MC3XXX_REG_MODE_FEATURE            0x07
+#define MC3XXX_REG_SAMPLE_RATE             0x08
+#define MC3XXX_REG_TAP_DETECTION_ENABLE    0x09
+#define MC3XXX_REG_TAP_DWELL_REJECT        0x0A
+#define MC3XXX_REG_DROP_CONTROL            0x0B
+#define MC3XXX_REG_SHAKE_DEBOUNCE          0x0C
+#define MC3XXX_REG_XOUT_EX_L               0x0D
+#define MC3XXX_REG_XOUT_EX_H               0x0E
+#define MC3XXX_REG_YOUT_EX_L               0x0F
+#define MC3XXX_REG_YOUT_EX_H               0x10
+#define MC3XXX_REG_ZOUT_EX_L               0x11
+#define MC3XXX_REG_ZOUT_EX_H               0x12
+#define MC3XXX_REG_RANGE_CONTROL           0x20
+#define MC3XXX_REG_SHAKE_THRESHOLD         0x2B
+#define MC3XXX_REG_UD_Z_TH                 0x2C
+#define MC3XXX_REG_UD_X_TH                 0x2D
+#define MC3XXX_REG_RL_Z_TH                 0x2E
+#define MC3XXX_REG_RL_Y_TH                 0x2F
+#define MC3XXX_REG_FB_Z_TH                 0x30
+#define MC3XXX_REG_DROP_THRESHOLD          0x31
+#define MC3XXX_REG_TAP_THRESHOLD           0x32
+#define MC3XXX_REG_PRODUCT_CODE            0x3B
+
+/***********************************************
+ *** RETURN CODE
+ ***********************************************/
+#define MC3XXX_RETCODE_SUCCESS                 (0)
+#define MC3XXX_RETCODE_ERROR_I2C               (-1)
+#define MC3XXX_RETCODE_ERROR_NULL_POINTER      (-2)
+#define MC3XXX_RETCODE_ERROR_STATUS            (-3)
+#define MC3XXX_RETCODE_ERROR_SETUP             (-4)
+#define MC3XXX_RETCODE_ERROR_GET_DATA          (-5)
+#define MC3XXX_RETCODE_ERROR_IDENTIFICATION    (-6)
+
+/***********************************************
+ *** CONFIGURATION
+ ***********************************************/
+#define MC3XXX_BUF_SIZE    256
+
+/***********************************************
+ *** PRODUCT ID
+ ***********************************************/
+#define MC3XXX_PCODE_3210     0x90
+#define MC3XXX_PCODE_3230     0x19
+#define MC3XXX_PCODE_3250     0x88
+#define MC3XXX_PCODE_3410     0xA8
+#define MC3XXX_PCODE_3410N    0xB8
+#define MC3XXX_PCODE_3430     0x29
+#define MC3XXX_PCODE_3430N    0x39
+#define MC3XXX_PCODE_3510     0x40
+#define MC3XXX_PCODE_3530     0x30
+#define MC3XXX_PCODE_3216     0x10
+#define MC3XXX_PCODE_3236     0x60
+
+#define MC3XXX_PCODE_RESERVE_1    0x20
+#define MC3XXX_PCODE_RESERVE_2    0x11
+#define MC3XXX_PCODE_RESERVE_3    0x21
+#define MC3XXX_PCODE_RESERVE_4    0x61
+#define MC3XXX_PCODE_RESERVE_5    0xA0
+#define MC3XXX_PCODE_RESERVE_6    0xE0
+#define MC3XXX_PCODE_RESERVE_7    0x91
+#define MC3XXX_PCODE_RESERVE_8    0xA1
+#define MC3XXX_PCODE_RESERVE_9    0xE1
+
+#define MC3XXX_PCODE_RESERVE_10    0x99
+#endif    // END OF _MC3XXX_H_
+
diff --git a/drivers/misc/mediatek/accelerometer/mpu6050g-new/Makefile b/drivers/misc/mediatek/accelerometer/mpu6050g-new/Makefile
new file mode 100644
index 000000000..ab3418e05
--- /dev/null
+++ b/drivers/misc/mediatek/accelerometer/mpu6050g-new/Makefile
@@ -0,0 +1,4 @@
+include $(srctree)/drivers/misc/mediatek/Makefile.custom
+
+obj-y	:=  mpu6050.o
+
diff --git a/drivers/misc/mediatek/accelerometer/mpu6050g-new/mpu6050.c b/drivers/misc/mediatek/accelerometer/mpu6050g-new/mpu6050.c
new file mode 100644
index 000000000..59c8195d3
--- /dev/null
+++ b/drivers/misc/mediatek/accelerometer/mpu6050g-new/mpu6050.c
@@ -0,0 +1,2367 @@
+/* MPU6050 motion sensor driver
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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.
+ *
+ */
+
+#include <linux/interrupt.h>
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include <linux/irq.h>
+#include <linux/miscdevice.h>
+#include <asm/uaccess.h>
+#include <linux/delay.h>
+#include <linux/input.h>
+#include <linux/workqueue.h>
+#include <linux/kobject.h>
+#include <linux/earlysuspend.h>
+#include <linux/platform_device.h>
+#include <asm/atomic.h>
+
+#include <cust_acc.h>
+#include <linux/hwmsensor.h>
+#include <linux/hwmsen_dev.h>
+#include <linux/sensors_io.h>
+#include "mpu6050.h"
+#include <linux/hwmsen_helper.h>
+#include <linux/batch.h>
+
+#include <mach/mt_typedefs.h>
+#include <mach/mt_gpio.h>
+#include <mach/mt_pm_ldo.h>
+
+#include <accel.h>
+
+#define POWER_NONE_MACRO MT65XX_POWER_NONE
+static DEFINE_MUTEX(mpu6050_i2c_mutex);
+
+/*----------------------------------------------------------------------------*/
+#define DEBUG 1
+/*----------------------------------------------------------------------------*/
+#define CONFIG_MPU6050_LOWPASS   /*apply low pass filter on output*/       
+#define SW_CALIBRATION
+/*----------------------------------------------------------------------------*/
+#define MPU6050_AXIS_X          0
+#define MPU6050_AXIS_Y          1
+#define MPU6050_AXIS_Z          2
+#define MPU6050_AXES_NUM        3
+#define MPU6050_DATA_LEN        6
+#define MPU6050_DEV_NAME        "MPU6050G"   /* name must different with gyro mpu6050 */
+/*----------------------------------------------------------------------------*/
+static const struct i2c_device_id mpu6050_i2c_id[] = {{MPU6050_DEV_NAME,0},{}};
+static struct i2c_board_info __initdata i2c_mpu6050={ I2C_BOARD_INFO(MPU6050_DEV_NAME, (MPU6050_I2C_SLAVE_ADDR>>1))};
+
+/*----------------------------------------------------------------------------*/
+static int mpu6050_i2c_probe(struct i2c_client *client, const struct i2c_device_id *id); 
+static int mpu6050_i2c_remove(struct i2c_client *client);
+static int mpu6050_i2c_detect(struct i2c_client *client, struct i2c_board_info *info);
+#ifndef USE_EARLY_SUSPEND
+static int mpu6050_suspend(struct i2c_client *client, pm_message_t msg) ;
+static int mpu6050_resume(struct i2c_client *client);
+#endif
+
+static int mpu6050_local_init(void);
+static int mpu6050_remove(void);
+static int mpu6050_init_flag =-1; // 0<==>OK -1 <==> fail
+
+static struct acc_init_info mpu6050_init_info = {
+		.name = "mpu6050g",
+		.init = mpu6050_local_init,
+		.uninit = mpu6050_remove,
+};
+
+/*----------------------------------------------------------------------------*/
+typedef enum
+{
+    MPU6050_TRC_FILTER  = 0x01,
+    MPU6050_TRC_RAWDATA = 0x02,
+    MPU6050_TRC_IOCTL   = 0x04,
+    MPU6050_TRC_CALI    = 0X08,
+    MPU6050_TRC_INFO    = 0X10,
+} MPU6050_TRC;
+/*----------------------------------------------------------------------------*/
+struct scale_factor
+{
+    u8  whole;
+    u8  fraction;
+};
+/*----------------------------------------------------------------------------*/
+struct data_resolution
+{
+    struct scale_factor scalefactor;
+    int                 sensitivity;
+};
+/*----------------------------------------------------------------------------*/
+#define C_MAX_FIR_LENGTH (32)
+/*----------------------------------------------------------------------------*/
+struct data_filter
+{
+    s16 raw[C_MAX_FIR_LENGTH][MPU6050_AXES_NUM];
+    int sum[MPU6050_AXES_NUM];
+    int num;
+    int idx;
+};
+/*----------------------------------------------------------------------------*/
+struct mpu6050_i2c_data
+{
+    struct i2c_client *client;
+    struct acc_hw *hw;
+    struct hwmsen_convert   cvt;
+
+    /*misc*/
+    struct data_resolution *reso;
+    atomic_t                trace;
+    atomic_t                suspend;
+    atomic_t                selftest;
+    atomic_t                filter;
+    s16                     cali_sw[MPU6050_AXES_NUM+1];
+
+    /*data*/
+    s8                      offset[MPU6050_AXES_NUM+1];  /*+1: for 4-byte alignment*/
+    s16                     data[MPU6050_AXES_NUM+1];
+
+#if defined(CONFIG_MPU6050_LOWPASS)
+    atomic_t                firlen;
+    atomic_t                fir_en;
+    struct data_filter      fir;
+#endif 
+    /*early suspend*/
+#if defined(USE_EARLY_SUSPEND)
+    struct early_suspend    early_drv;
+#endif     
+    u8                      bandwidth;
+};
+/*----------------------------------------------------------------------------*/
+static struct i2c_driver mpu6050_i2c_driver = {
+    .driver = {
+        .name           = MPU6050_DEV_NAME,
+    },
+    .probe              = mpu6050_i2c_probe,
+    .remove             = mpu6050_i2c_remove,
+    .detect             = mpu6050_i2c_detect,
+#if !defined(USE_EARLY_SUSPEND)    
+    .suspend            = mpu6050_suspend,
+    .resume             = mpu6050_resume,
+#endif
+    .id_table = mpu6050_i2c_id,
+};
+
+/*----------------------------------------------------------------------------*/
+static struct i2c_client *mpu6050_i2c_client = NULL;
+static struct mpu6050_i2c_data *obj_i2c_data = NULL;
+static bool sensor_power = false;
+static GSENSOR_VECTOR3D gsensor_gain;
+static char selftestRes[8]= {0}; 
+
+
+/*----------------------------------------------------------------------------*/
+#define GSE_TAG                  "[Gsensor] "
+#define GSE_FUN(f)               		printk(KERN_ERR GSE_TAG"%s\n", __FUNCTION__)
+#define GSE_ERR(fmt, args...)    	printk(KERN_ERR GSE_TAG"%s %d : "fmt, __FUNCTION__, __LINE__, ##args)
+#define GSE_LOG(fmt, args...)    	printk(KERN_ERR GSE_TAG fmt, ##args)
+/*----------------------------------------------------------------------------*/
+static struct data_resolution mpu6050_data_resolution[] = {
+    /*8 combination by {FULL_RES,RANGE}*/
+    {{ 0,  6}, 16384},  /*+/-2g  in 16-bit resolution:  0.06 mg/LSB*/
+    {{ 0, 12}, 8192},   /*+/-4g  in 16-bit resolution:  0.12 mg/LSB*/
+    {{ 0, 24}, 4096},   /*+/-8g  in 16-bit resolution:  0.24 mg/LSB*/
+    {{ 0, 5}, 2048},   	/*+/-16g in 16-bit resolution:  0.49 mg/LSB*/
+};
+/*----------------------------------------------------------------------------*/
+static struct data_resolution mpu6050_offset_resolution = {{ 0, 5}, 2048};
+
+static unsigned int power_on = 0;
+
+extern int MPU6050_gyro_power(void);
+extern int MPU6050_gyro_mode(void);
+
+
+int MPU6050_gse_power( void)
+{
+    return(power_on);
+}
+EXPORT_SYMBOL(MPU6050_gse_power);
+
+int MPU6050_gse_mode(void)
+{
+    return sensor_power;
+}
+EXPORT_SYMBOL(MPU6050_gse_mode);
+
+
+/*----------------------------------------------------------------------------*/
+static int mpu_i2c_read_block(struct i2c_client *client, u8 addr, u8 *data, u8 len){
+        int err;
+        u8 beg = addr;
+        struct i2c_msg msgs[2] = {{0},{0}};
+
+		mutex_lock(&mpu6050_i2c_mutex);
+		msgs[0].addr = client->addr;
+		msgs[0].flags =0;
+		msgs[0].len =1;
+		msgs[0].buf =&beg;
+
+		msgs[1].addr = client->addr;
+		msgs[1].flags = I2C_M_RD;
+		msgs[1].len = len;
+		msgs[1].buf = data;	
+
+
+	if (!client) {
+		mutex_unlock(&mpu6050_i2c_mutex);
+		return -EINVAL;
+	}
+	else if (len > C_I2C_FIFO_SIZE) {
+		mutex_unlock(&mpu6050_i2c_mutex);
+		GSE_ERR(" length %d exceeds %d\n", len, C_I2C_FIFO_SIZE);
+		return -EINVAL;
+	}
+
+	err = i2c_transfer(client->adapter, msgs, sizeof(msgs)/sizeof(msgs[0]));
+	if (err != 2) {
+		GSE_ERR("i2c_transfer error: (%d %p %d) %d\n",
+			addr, data, len, err);
+		err = -EIO;
+	} else {
+		err = 0;
+	}
+	mutex_unlock(&mpu6050_i2c_mutex);
+	return err;
+
+}
+
+static int mpu_i2c_write_block(struct i2c_client *client, u8 addr, u8 *data, u8 len)
+{   /*because address also occupies one byte, the maximum length for write is 7 bytes*/
+    int err, idx, num;
+    char buf[C_I2C_FIFO_SIZE];
+    err =0;
+	mutex_lock(&mpu6050_i2c_mutex);
+    if (!client)
+    {
+        mutex_unlock(&mpu6050_i2c_mutex);
+        return -EINVAL;
+    }
+    else if (len >= C_I2C_FIFO_SIZE) 
+	{        
+	    mutex_unlock(&mpu6050_i2c_mutex);
+        GSE_ERR(" length %d exceeds %d\n", len, C_I2C_FIFO_SIZE);
+        return -EINVAL;
+    }    
+
+    num = 0;
+    buf[num++] = addr;
+    for (idx = 0; idx < len; idx++)
+    {
+        buf[num++] = data[idx];
+    }
+
+    err = i2c_master_send(client, buf, num);
+    if (err < 0)
+	{   mutex_unlock(&mpu6050_i2c_mutex);
+        GSE_ERR("send command error!!\n");
+        return -EFAULT;
+    } 
+	mutex_unlock(&mpu6050_i2c_mutex);
+    return err;
+}
+
+int MPU6050_hwmsen_read_block(u8 addr, u8 *buf, u8 len)
+{
+    if (NULL == mpu6050_i2c_client)
+    {
+        GSE_ERR("MPU6050_hwmsen_read_block null ptr!!\n");
+        return MPU6050_ERR_I2C;
+    }
+    return mpu_i2c_read_block(mpu6050_i2c_client, addr, buf, len);
+}
+EXPORT_SYMBOL(MPU6050_hwmsen_read_block);
+
+int MPU6050_hwmsen_write_block(u8 addr, u8 *buf, u8 len)
+{
+    if (NULL == mpu6050_i2c_client)
+    {
+        GSE_ERR("MPU6050_hwmsen_read_block null ptr!!\n");
+        return MPU6050_ERR_I2C;
+    }
+    return mpu_i2c_write_block(mpu6050_i2c_client, addr, buf, len);
+}
+EXPORT_SYMBOL(MPU6050_hwmsen_write_block);
+
+
+/*--------------------mpu6050 power control function----------------------------------*/
+static void MPU6050_power(struct acc_hw *hw, unsigned int on) 
+{
+
+
+    if (hw->power_id != POWER_NONE_MACRO)        // have externel LDO
+    {
+        GSE_LOG("power %s\n", on ? "on" : "off");
+        if (power_on == on)  // power status not change
+        {
+            GSE_LOG("ignore power control: %d\n", on);
+        }
+        else if (on) // power on
+        {
+            if (!hwPowerOn(hw->power_id, hw->power_vol, "MPU6050G"))
+            {
+                GSE_ERR("power on fails!!\n");
+            }
+        }
+        else    // power off
+        {
+            if (MPU6050_gyro_power() == false)
+            {
+                if (!hwPowerDown(hw->power_id, "MPU6050G"))
+                {
+                    GSE_ERR("power off fail!!\n");
+                }
+            }
+        }
+    }
+    power_on = on;    
+}
+
+/*----------------------------------------------------------------------------*/
+static int MPU6050_SetPowerMode(struct i2c_client *client, bool enable)
+{
+    u8 databuf[2];    
+    int res = 0;
+    //u8 addr = MPU6050_REG_POWER_CTL;
+    struct mpu6050_i2c_data *obj = i2c_get_clientdata(client);
+
+
+    if (enable == sensor_power)
+    {
+        GSE_LOG("Sensor power status is newest!\n");
+        return MPU6050_SUCCESS;
+    }
+  
+    res = mpu_i2c_read_block(client, MPU6050_REG_POWER_CTL, databuf, 0x1);
+    if (res < 0)
+    {
+        return MPU6050_ERR_I2C;
+    }
+
+    databuf[0] &= ~MPU6050_SLEEP;
+
+    if (enable == FALSE)
+    {
+        if (MPU6050_gyro_mode() == false)
+        {
+            databuf[0] |= MPU6050_SLEEP;
+        }
+    }
+    else
+    {
+        // do nothing
+    }
+
+    res = mpu_i2c_write_block(client, MPU6050_REG_POWER_CTL, databuf, 0x1);
+    if (res < 0)
+    {
+        GSE_LOG("set power mode failed!\n");
+        return MPU6050_ERR_I2C;
+    }
+    else if (atomic_read(&obj->trace) & MPU6050_TRC_INFO)
+    {
+        GSE_LOG("set power mode ok %d!\n", databuf[0]);
+    }
+
+    sensor_power = enable;
+    return MPU6050_SUCCESS;    
+}
+/*----------------------------------------------------------------------------*/
+static int MPU6050_SetDataResolution(struct mpu6050_i2c_data *obj)
+{
+    int err;
+    u8  dat, reso;
+
+    if ((err = mpu_i2c_read_block(obj->client, MPU6050_REG_DATA_FORMAT, &dat, 1)))
+    {
+        GSE_ERR("write data format fail!!\n");
+        return err;
+    }
+
+    /*the data_reso is combined by 3 bits: {FULL_RES, DATA_RANGE}*/
+    reso = 0x00;
+    reso = (dat & MPU6050_RANGE_16G) >> 3; 
+
+    if (reso < sizeof(mpu6050_data_resolution)/sizeof(mpu6050_data_resolution[0]))
+    {
+        obj->reso = &mpu6050_data_resolution[reso];
+        return 0;
+    }
+    else
+    {
+        return -EINVAL;
+    }
+}
+/*----------------------------------------------------------------------------*/
+static int MPU6050_ReadData(struct i2c_client *client, s16 data[MPU6050_AXES_NUM])
+{
+    struct mpu6050_i2c_data *priv = i2c_get_clientdata(client);        
+    u8 buf[MPU6050_DATA_LEN] = {0};
+    int err = 0;
+
+
+    if (NULL == client)
+    {
+        return -EINVAL;
+    }
+
+    {
+        /* write then burst read */
+        mpu_i2c_read_block(client, MPU6050_REG_DATAX0, buf, MPU6050_DATA_LEN);
+
+        data[MPU6050_AXIS_X] = (s16)((buf[MPU6050_AXIS_X*2] << 8) |
+                                     (buf[MPU6050_AXIS_X*2+1] ));
+        data[MPU6050_AXIS_Y] = (s16)((buf[MPU6050_AXIS_Y*2] << 8) |
+                                     (buf[MPU6050_AXIS_Y*2+1] ));
+        data[MPU6050_AXIS_Z] = (s16)((buf[MPU6050_AXIS_Z*2] << 8) |
+                                     (buf[MPU6050_AXIS_Z*2+1] ));
+
+        if (atomic_read(&priv->trace) & MPU6050_TRC_RAWDATA)
+        {
+            GSE_LOG("[%08X %08X %08X] => [%5d %5d %5d]\n", data[MPU6050_AXIS_X], data[MPU6050_AXIS_Y], data[MPU6050_AXIS_Z],
+                    data[MPU6050_AXIS_X], data[MPU6050_AXIS_Y], data[MPU6050_AXIS_Z]);
+        }
+#ifdef CONFIG_MPU6050_LOWPASS
+        if (atomic_read(&priv->filter))
+        {
+            if (atomic_read(&priv->fir_en) && !atomic_read(&priv->suspend))
+            {
+                int idx, firlen = atomic_read(&priv->firlen);   
+                if (priv->fir.num < firlen)
+                {
+                    priv->fir.raw[priv->fir.num][MPU6050_AXIS_X] = data[MPU6050_AXIS_X];
+                    priv->fir.raw[priv->fir.num][MPU6050_AXIS_Y] = data[MPU6050_AXIS_Y];
+                    priv->fir.raw[priv->fir.num][MPU6050_AXIS_Z] = data[MPU6050_AXIS_Z];
+                    priv->fir.sum[MPU6050_AXIS_X] += data[MPU6050_AXIS_X];
+                    priv->fir.sum[MPU6050_AXIS_Y] += data[MPU6050_AXIS_Y];
+                    priv->fir.sum[MPU6050_AXIS_Z] += data[MPU6050_AXIS_Z];
+                    if (atomic_read(&priv->trace) & MPU6050_TRC_FILTER)
+                    {
+                        GSE_LOG("add [%2d] [%5d %5d %5d] => [%5d %5d %5d]\n", priv->fir.num,
+                                priv->fir.raw[priv->fir.num][MPU6050_AXIS_X], priv->fir.raw[priv->fir.num][MPU6050_AXIS_Y], priv->fir.raw[priv->fir.num][MPU6050_AXIS_Z],
+                                priv->fir.sum[MPU6050_AXIS_X], priv->fir.sum[MPU6050_AXIS_Y], priv->fir.sum[MPU6050_AXIS_Z]);
+                    }
+                    priv->fir.num++;
+                    priv->fir.idx++;
+                }
+                else
+                {
+                    idx = priv->fir.idx % firlen;
+                    priv->fir.sum[MPU6050_AXIS_X] -= priv->fir.raw[idx][MPU6050_AXIS_X];
+                    priv->fir.sum[MPU6050_AXIS_Y] -= priv->fir.raw[idx][MPU6050_AXIS_Y];
+                    priv->fir.sum[MPU6050_AXIS_Z] -= priv->fir.raw[idx][MPU6050_AXIS_Z];
+                    priv->fir.raw[idx][MPU6050_AXIS_X] = data[MPU6050_AXIS_X];
+                    priv->fir.raw[idx][MPU6050_AXIS_Y] = data[MPU6050_AXIS_Y];
+                    priv->fir.raw[idx][MPU6050_AXIS_Z] = data[MPU6050_AXIS_Z];
+                    priv->fir.sum[MPU6050_AXIS_X] += data[MPU6050_AXIS_X];
+                    priv->fir.sum[MPU6050_AXIS_Y] += data[MPU6050_AXIS_Y];
+                    priv->fir.sum[MPU6050_AXIS_Z] += data[MPU6050_AXIS_Z];
+                    priv->fir.idx++;
+                    data[MPU6050_AXIS_X] = priv->fir.sum[MPU6050_AXIS_X]/firlen;
+                    data[MPU6050_AXIS_Y] = priv->fir.sum[MPU6050_AXIS_Y]/firlen;
+                    data[MPU6050_AXIS_Z] = priv->fir.sum[MPU6050_AXIS_Z]/firlen;
+                    if (atomic_read(&priv->trace) & MPU6050_TRC_FILTER)
+                    {
+                        GSE_LOG("add [%2d] [%5d %5d %5d] => [%5d %5d %5d] : [%5d %5d %5d]\n", idx,
+                                priv->fir.raw[idx][MPU6050_AXIS_X], priv->fir.raw[idx][MPU6050_AXIS_Y], priv->fir.raw[idx][MPU6050_AXIS_Z],
+                                priv->fir.sum[MPU6050_AXIS_X], priv->fir.sum[MPU6050_AXIS_Y], priv->fir.sum[MPU6050_AXIS_Z],
+                                data[MPU6050_AXIS_X], data[MPU6050_AXIS_Y], data[MPU6050_AXIS_Z]);
+                    }
+                }
+            }
+        }
+#endif         
+    }
+    return err;
+}
+/*----------------------------------------------------------------------------*/
+static int MPU6050_ReadOffset(struct i2c_client *client, s8 ofs[MPU6050_AXES_NUM])
+{    
+    int err = 0;
+#ifdef SW_CALIBRATION
+    ofs[0]=ofs[1]=ofs[2]=0x0;
+#else
+    if ((err = mpu_i2c_read_block(client, MPU6050_REG_OFSX, ofs, MPU6050_AXES_NUM)))
+    {
+        GSE_ERR("error: %d\n", err);
+    }
+#endif
+    //GSE_LOG("offesx=%x, y=%x, z=%x",ofs[0],ofs[1],ofs[2]);
+
+    return err;    
+}
+/*----------------------------------------------------------------------------*/
+static int MPU6050_ResetCalibration(struct i2c_client *client)
+{
+    struct mpu6050_i2c_data *obj = i2c_get_clientdata(client);
+#ifndef SW_CALIBRATION
+    s8 ofs[MPU6050_AXES_NUM] = {0x00, 0x00, 0x00};
+#endif
+    int err = 0;
+#ifdef SW_CALIBRATION
+    /* do not thing */
+#else
+
+    if ((err = hwmsen_write_block(client, MPU6050_REG_OFSX, ofs, MPU6050_AXES_NUM)))
+    {
+        GSE_ERR("error: %d\n", err);
+    }
+#endif
+
+    memset(obj->cali_sw, 0x00, sizeof(obj->cali_sw));
+    memset(obj->offset, 0x00, sizeof(obj->offset));
+
+    return err;    
+}
+/*----------------------------------------------------------------------------*/
+static int MPU6050_ReadCalibration(struct i2c_client *client, int dat[MPU6050_AXES_NUM])
+{
+    struct mpu6050_i2c_data *obj = i2c_get_clientdata(client);
+#ifdef SW_CALIBRATION
+    int mul;
+#else
+    int err;
+#endif
+#ifdef SW_CALIBRATION
+    mul = 0;//only SW Calibration, disable HW Calibration
+#else
+
+    if ((err = MPU6050_ReadOffset(client, obj->offset)))
+    {
+        GSE_ERR("read offset fail, %d\n", err);
+        return err;
+    }
+    mul = obj->reso->sensitivity/mpu6050_offset_resolution.sensitivity;
+#endif
+
+    dat[obj->cvt.map[MPU6050_AXIS_X]] = obj->cvt.sign[MPU6050_AXIS_X]*(obj->offset[MPU6050_AXIS_X]*mul + obj->cali_sw[MPU6050_AXIS_X]);
+    dat[obj->cvt.map[MPU6050_AXIS_Y]] = obj->cvt.sign[MPU6050_AXIS_Y]*(obj->offset[MPU6050_AXIS_Y]*mul + obj->cali_sw[MPU6050_AXIS_Y]);
+    dat[obj->cvt.map[MPU6050_AXIS_Z]] = obj->cvt.sign[MPU6050_AXIS_Z]*(obj->offset[MPU6050_AXIS_Z]*mul + obj->cali_sw[MPU6050_AXIS_Z]);                        
+
+    return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int MPU6050_ReadCalibrationEx(struct i2c_client *client, int act[MPU6050_AXES_NUM], int raw[MPU6050_AXES_NUM])
+{  
+    /*raw: the raw calibration data; act: the actual calibration data*/
+    struct mpu6050_i2c_data *obj = i2c_get_clientdata(client);
+#ifdef SW_CALIBRATION
+    int mul;
+#else
+    int err;
+#endif
+#ifdef SW_CALIBRATION
+    mul = 0;//only SW Calibration, disable HW Calibration
+#else
+
+    if ((err = MPU6050_ReadOffset(client, obj->offset)))
+    {
+        GSE_ERR("read offset fail, %d\n", err);
+        return err;
+    }
+    mul = obj->reso->sensitivity/mpu6050_offset_resolution.sensitivity;
+#endif
+
+    raw[MPU6050_AXIS_X] = obj->offset[MPU6050_AXIS_X]*mul + obj->cali_sw[MPU6050_AXIS_X];
+    raw[MPU6050_AXIS_Y] = obj->offset[MPU6050_AXIS_Y]*mul + obj->cali_sw[MPU6050_AXIS_Y];
+    raw[MPU6050_AXIS_Z] = obj->offset[MPU6050_AXIS_Z]*mul + obj->cali_sw[MPU6050_AXIS_Z];
+
+    act[obj->cvt.map[MPU6050_AXIS_X]] = obj->cvt.sign[MPU6050_AXIS_X]*raw[MPU6050_AXIS_X];
+    act[obj->cvt.map[MPU6050_AXIS_Y]] = obj->cvt.sign[MPU6050_AXIS_Y]*raw[MPU6050_AXIS_Y];
+    act[obj->cvt.map[MPU6050_AXIS_Z]] = obj->cvt.sign[MPU6050_AXIS_Z]*raw[MPU6050_AXIS_Z];                        
+
+    return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int MPU6050_WriteCalibration(struct i2c_client *client, int dat[MPU6050_AXES_NUM])
+{
+    struct mpu6050_i2c_data *obj = i2c_get_clientdata(client);
+    int err;
+    int cali[MPU6050_AXES_NUM], raw[MPU6050_AXES_NUM];
+#ifndef SW_CALIBRATION
+    int lsb = mpu6050_offset_resolution.sensitivity;
+    int divisor = obj->reso->sensitivity/lsb;
+#endif
+    if ((err = MPU6050_ReadCalibrationEx(client, cali, raw)))    /*offset will be updated in obj->offset*/
+    {
+        GSE_ERR("read offset fail, %d\n", err);
+        return err;
+    }
+
+    GSE_LOG("OLDOFF: (%+3d %+3d %+3d): (%+3d %+3d %+3d) / (%+3d %+3d %+3d)\n", 
+            raw[MPU6050_AXIS_X], raw[MPU6050_AXIS_Y], raw[MPU6050_AXIS_Z],
+            obj->offset[MPU6050_AXIS_X], obj->offset[MPU6050_AXIS_Y], obj->offset[MPU6050_AXIS_Z],
+            obj->cali_sw[MPU6050_AXIS_X], obj->cali_sw[MPU6050_AXIS_Y], obj->cali_sw[MPU6050_AXIS_Z]);
+
+    /*calculate the real offset expected by caller*/
+    cali[MPU6050_AXIS_X] += dat[MPU6050_AXIS_X];
+    cali[MPU6050_AXIS_Y] += dat[MPU6050_AXIS_Y];
+    cali[MPU6050_AXIS_Z] += dat[MPU6050_AXIS_Z];
+
+    GSE_LOG("UPDATE: (%+3d %+3d %+3d)\n", 
+            dat[MPU6050_AXIS_X], dat[MPU6050_AXIS_Y], dat[MPU6050_AXIS_Z]);
+#ifdef SW_CALIBRATION
+    obj->cali_sw[MPU6050_AXIS_X] = obj->cvt.sign[MPU6050_AXIS_X]*(cali[obj->cvt.map[MPU6050_AXIS_X]]);
+    obj->cali_sw[MPU6050_AXIS_Y] = obj->cvt.sign[MPU6050_AXIS_Y]*(cali[obj->cvt.map[MPU6050_AXIS_Y]]);
+    obj->cali_sw[MPU6050_AXIS_Z] = obj->cvt.sign[MPU6050_AXIS_Z]*(cali[obj->cvt.map[MPU6050_AXIS_Z]]);  
+#else
+
+    obj->offset[MPU6050_AXIS_X] = (s8)(obj->cvt.sign[MPU6050_AXIS_X]*(cali[obj->cvt.map[MPU6050_AXIS_X]])/(divisor));
+    obj->offset[MPU6050_AXIS_Y] = (s8)(obj->cvt.sign[MPU6050_AXIS_Y]*(cali[obj->cvt.map[MPU6050_AXIS_Y]])/(divisor));
+    obj->offset[MPU6050_AXIS_Z] = (s8)(obj->cvt.sign[MPU6050_AXIS_Z]*(cali[obj->cvt.map[MPU6050_AXIS_Z]])/(divisor));
+
+    /*convert software calibration using standard calibration*/
+    obj->cali_sw[MPU6050_AXIS_X] = obj->cvt.sign[MPU6050_AXIS_X]*(cali[obj->cvt.map[MPU6050_AXIS_X]])%(divisor);
+    obj->cali_sw[MPU6050_AXIS_Y] = obj->cvt.sign[MPU6050_AXIS_Y]*(cali[obj->cvt.map[MPU6050_AXIS_Y]])%(divisor);
+    obj->cali_sw[MPU6050_AXIS_Z] = obj->cvt.sign[MPU6050_AXIS_Z]*(cali[obj->cvt.map[MPU6050_AXIS_Z]])%(divisor);
+
+    GSE_LOG("NEWOFF: (%+3d %+3d %+3d): (%+3d %+3d %+3d) / (%+3d %+3d %+3d)\n", 
+            obj->offset[MPU6050_AXIS_X]*divisor + obj->cali_sw[MPU6050_AXIS_X], 
+            obj->offset[MPU6050_AXIS_Y]*divisor + obj->cali_sw[MPU6050_AXIS_Y], 
+            obj->offset[MPU6050_AXIS_Z]*divisor + obj->cali_sw[MPU6050_AXIS_Z], 
+            obj->offset[MPU6050_AXIS_X], obj->offset[MPU6050_AXIS_Y], obj->offset[MPU6050_AXIS_Z],
+            obj->cali_sw[MPU6050_AXIS_X], obj->cali_sw[MPU6050_AXIS_Y], obj->cali_sw[MPU6050_AXIS_Z]);
+
+    if ((err = hwmsen_write_block(obj->client, MPU6050_REG_OFSX, obj->offset, MPU6050_AXES_NUM)))
+    {
+        GSE_ERR("write offset fail: %d\n", err);
+        return err;
+    }
+#endif
+
+    return err;
+}
+/*----------------------------------------------------------------------------*/
+static int MPU6050_CheckDeviceID(struct i2c_client *client)
+{
+    u8 databuf[10];    
+    int res = 0;
+
+    memset(databuf, 0, sizeof(u8)*10);    
+
+    res = mpu_i2c_read_block(client, MPU6050_REG_DEVID, databuf, 0x1);
+    if (res < 0)
+    {
+        goto exit_MPU6050_CheckDeviceID;
+    }
+
+    GSE_LOG("MPU6050_CheckDeviceID 0x%x\n", databuf[0]);
+
+    exit_MPU6050_CheckDeviceID:
+    if (res < 0)
+    {
+        return MPU6050_ERR_I2C;
+    }
+    return MPU6050_SUCCESS;
+}
+
+/*----------------------------------------------------------------------------*/
+static int MPU6050_SetDataFormat(struct i2c_client *client, u8 dataformat)
+{
+    struct mpu6050_i2c_data *obj = i2c_get_clientdata(client);
+    u8 databuf[2];    
+    int res = 0;
+
+    memset(databuf, 0, sizeof(u8)*2);    
+    res = mpu_i2c_read_block(client, MPU6050_REG_DATA_FORMAT, databuf, 0x1);
+    if (res < 0)
+    {
+        return MPU6050_ERR_I2C;
+    }
+
+    /* write */    
+    databuf[0] = databuf[0] | dataformat;
+    res = mpu_i2c_write_block(client, MPU6050_REG_DATA_FORMAT, databuf, 0x1);
+
+    if (res < 0)
+    {
+        return MPU6050_ERR_I2C;
+    }
+    return MPU6050_SetDataResolution(obj);    
+}
+/*----------------------------------------------------------------------------*/
+static int MPU6050_SetBWRate(struct i2c_client *client, u8 bwrate)
+{
+    struct mpu6050_i2c_data *obj = i2c_get_clientdata(client);
+    u8 databuf[10];    
+    int res = 0;
+
+    if( (obj->bandwidth != bwrate) || (atomic_read(&obj->suspend)) )
+    {    
+        memset(databuf, 0, sizeof(u8)*10);    
+    
+        /* read */
+        res = mpu_i2c_read_block(client, MPU6050_REG_BW_RATE, databuf, 0x1);
+        if (res < 0)
+        {
+            return MPU6050_ERR_I2C;
+        }
+    
+        /* write */
+        databuf[0] = databuf[0] | bwrate;
+        res = mpu_i2c_write_block(client, MPU6050_REG_BW_RATE, databuf, 0x1);
+   
+        if (res < 0)
+        {
+            return MPU6050_ERR_I2C;
+        }
+
+    	obj->bandwidth = bwrate;
+    }
+
+    return MPU6050_SUCCESS;    
+}
+
+/*----------------------------------------------------------------------------*/
+static int MPU6050_Dev_Reset(struct i2c_client *client)
+{
+    u8 databuf[10];    
+    int res = 0;
+
+    memset(databuf, 0, sizeof(u8)*10);    
+
+    /* read */
+    res = mpu_i2c_read_block(client, MPU6050_REG_POWER_CTL, databuf, 0x1);
+    if (res < 0)
+    {
+        return MPU6050_ERR_I2C;
+    }
+
+    /* write */
+    databuf[0] = databuf[0] | MPU6050_DEV_RESET;
+    res = mpu_i2c_write_block(client, MPU6050_REG_POWER_CTL, databuf, 0x1);
+
+    if (res < 0)
+    {
+        return MPU6050_ERR_I2C;
+    }
+
+    do
+    {
+        res = mpu_i2c_read_block(client, MPU6050_REG_POWER_CTL, databuf, 0x1);
+		if (res < 0)
+    	{
+        	return MPU6050_ERR_I2C;
+    	}
+        GSE_LOG("[Gsensor] check reset bit");
+    }while((databuf[0]&MPU6050_DEV_RESET) != 0);
+
+    msleep(50);
+    return MPU6050_SUCCESS;    
+}
+
+
+/*----------------------------------------------------------------------------*/
+static int MPU6050_Reset(struct i2c_client *client)
+{
+    u8 databuf[10];    
+    int res = 0;
+
+    /* write */
+    databuf[0] = 0x7; /* reset gyro, g-sensor, temperature */
+    res = mpu_i2c_write_block(client, MPU6050_REG_RESET, databuf, 0x1);
+
+    if (res < 0)
+    {
+        return MPU6050_ERR_I2C;
+    }
+
+    msleep(20);
+    return MPU6050_SUCCESS;    
+}
+
+
+/*----------------------------------------------------------------------------*/
+static int MPU6050_SetIntEnable(struct i2c_client *client, u8 intenable)
+{
+    u8 databuf[2];    
+    int res = 0;
+
+    memset(databuf, 0, sizeof(u8)*2);    
+    databuf[0] = intenable;
+    res = mpu_i2c_write_block(client, MPU6050_REG_INT_ENABLE, databuf, 0x1);
+
+    if (res < 0)
+    {
+        return MPU6050_ERR_I2C;
+    }
+
+    return MPU6050_SUCCESS;    
+}
+/*----------------------------------------------------------------------------*/
+static int mpu6050_gpio_config(void)
+{
+//because we donot use EINT to support low power
+// config to GPIO input mode + PD 
+
+//set to GPIO_GSE_1_EINT_PIN
+	/*
+	mt_set_gpio_mode(GPIO_GSE_1_EINT_PIN, GPIO_GSE_1_EINT_PIN_M_GPIO);
+	mt_set_gpio_dir(GPIO_GSE_1_EINT_PIN, GPIO_DIR_IN);
+	mt_set_gpio_pull_enable(GPIO_GSE_1_EINT_PIN, GPIO_PULL_ENABLE);
+	mt_set_gpio_pull_select(GPIO_GSE_1_EINT_PIN, GPIO_PULL_DOWN);
+	 */
+//set to GPIO_GSE_2_EINT_PIN
+	/*
+	mt_set_gpio_mode(GPIO_GSE_2_EINT_PIN, GPIO_GSE_2_EINT_PIN_M_GPIO);
+	mt_set_gpio_dir(GPIO_GSE_2_EINT_PIN, GPIO_DIR_IN);
+	mt_set_gpio_pull_enable(GPIO_GSE_2_EINT_PIN, GPIO_PULL_ENABLE);
+	 mt_set_gpio_pull_select(GPIO_GSE_2_EINT_PIN, GPIO_PULL_DOWN);
+	 */
+    return 0;
+}
+
+static int mpu6050_init_client(struct i2c_client *client, int reset_cali)
+{
+    struct mpu6050_i2c_data *obj = i2c_get_clientdata(client);
+    int res = 0;
+
+    mpu6050_gpio_config();
+
+    res = MPU6050_SetPowerMode(client, true);
+    if (res != MPU6050_SUCCESS)
+    {
+        GSE_ERR("set power error\n");
+        return res;
+    }
+    res = MPU6050_CheckDeviceID(client); 
+    if (res != MPU6050_SUCCESS)
+    {
+        GSE_ERR("Check ID error\n");
+        return res;
+    }
+
+    res = MPU6050_SetBWRate(client, MPU6050_BW_184HZ);
+    if (res != MPU6050_SUCCESS ) //0x2C->BW=100Hz
+    {
+        GSE_ERR("set power error\n");
+        return res;
+    }
+
+    res = MPU6050_SetDataFormat(client, MPU6050_RANGE_16G);
+    if (res != MPU6050_SUCCESS) //0x2C->BW=100Hz
+    {
+        GSE_ERR("set data format error\n");
+        return res;
+    }
+
+    gsensor_gain.x = gsensor_gain.y = gsensor_gain.z = obj->reso->sensitivity;
+
+    res = MPU6050_SetIntEnable(client, 0x00);//disable INT
+    if (res != MPU6050_SUCCESS)
+    {
+        GSE_ERR("mpu6050_SetIntEnable error\n");
+        return res;
+    }
+
+    if (0 != reset_cali)
+    {
+        /*reset calibration only in power on*/
+        res = MPU6050_ResetCalibration(client);
+        if (res != MPU6050_SUCCESS)
+        {
+            return res;
+        }
+    }
+
+#ifdef CONFIG_MPU6050_LOWPASS
+    memset(&obj->fir, 0x00, sizeof(obj->fir));  
+#endif
+    return MPU6050_SUCCESS;
+}
+/*----------------------------------------------------------------------------*/
+static int MPU6050_ReadAllReg(struct i2c_client *client, char *buf, int bufsize)
+{   
+    u8 total_len= 0x5C; //(0x75-0x19);
+
+    u8 addr = 0x19;
+    u8 buff[total_len+1];
+    int err = 0;
+    int i;
+
+
+    if (sensor_power == FALSE)
+    {
+        err = MPU6050_SetPowerMode(client, true);
+        if (err)
+        {
+            GSE_ERR("Power on mpu6050 error %d!\n", err);
+        }
+        msleep(50);
+    }
+
+    mpu_i2c_read_block(client, addr, buff, total_len);
+
+    for ( i=0; i<=total_len; i++)
+    {
+        GSE_LOG("MPU6050 reg=0x%x, data=0x%x \n",(addr+i), buff[i]);
+    }
+
+    return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int MPU6050_ReadChipInfo(struct i2c_client *client, char *buf, int bufsize)
+{
+    u8 databuf[10];    
+
+    memset(databuf, 0, sizeof(u8)*10);
+
+    if ((NULL == buf)||(bufsize<=30))
+    {
+        return -1;
+    }
+
+    if (NULL == client)
+    {
+        *buf = 0;
+        return -2;
+    }
+
+    sprintf(buf, "MPU6050 Chip");
+    return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int MPU6050_ReadSensorData(struct i2c_client *client, char *buf, int bufsize)
+{
+    struct mpu6050_i2c_data *obj = obj_i2c_data; //(struct mpu6050_i2c_data*)i2c_get_clientdata(client);
+    int acc[MPU6050_AXES_NUM];
+    int res = 0;
+    client = obj->client;
+
+    if (atomic_read(&obj->suspend))
+    {
+        return -3;
+    }
+
+    if (NULL == buf)
+    {
+        return -1;
+    }
+    if (NULL == client)
+    {
+        *buf = 0;
+        return -2;
+    }
+
+    if (sensor_power == FALSE)
+    {
+        res = MPU6050_SetPowerMode(client, true);
+        if (res)
+        {
+            GSE_ERR("Power on mpu6050 error %d!\n", res);
+        }
+        msleep(50);
+    }
+
+    if ((res = MPU6050_ReadData(client, obj->data)))
+    {
+        GSE_ERR("I2C error: ret value=%d", res);
+        return -3;
+    }
+    else
+    {
+        obj->data[MPU6050_AXIS_X] += obj->cali_sw[MPU6050_AXIS_X];
+        obj->data[MPU6050_AXIS_Y] += obj->cali_sw[MPU6050_AXIS_Y];
+        obj->data[MPU6050_AXIS_Z] += obj->cali_sw[MPU6050_AXIS_Z];
+
+        /*remap coordinate*/
+        acc[obj->cvt.map[MPU6050_AXIS_X]] = obj->cvt.sign[MPU6050_AXIS_X]*obj->data[MPU6050_AXIS_X];
+        acc[obj->cvt.map[MPU6050_AXIS_Y]] = obj->cvt.sign[MPU6050_AXIS_Y]*obj->data[MPU6050_AXIS_Y];
+        acc[obj->cvt.map[MPU6050_AXIS_Z]] = obj->cvt.sign[MPU6050_AXIS_Z]*obj->data[MPU6050_AXIS_Z];
+		
+        //Out put the mg
+        acc[MPU6050_AXIS_X] = acc[MPU6050_AXIS_X] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+        acc[MPU6050_AXIS_Y] = acc[MPU6050_AXIS_Y] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+        acc[MPU6050_AXIS_Z] = acc[MPU6050_AXIS_Z] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;        
+
+        sprintf(buf, "%04x %04x %04x", acc[MPU6050_AXIS_X], acc[MPU6050_AXIS_Y], acc[MPU6050_AXIS_Z]);
+        if (atomic_read(&obj->trace) & MPU6050_TRC_IOCTL)
+        {
+            GSE_LOG("gsensor data: %s!\n", buf);
+        }
+    }
+
+    return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int MPU6050_ReadRawData(struct i2c_client *client, char *buf)
+{
+    struct mpu6050_i2c_data *obj = (struct mpu6050_i2c_data*)i2c_get_clientdata(client);
+    int res = 0;
+
+    if (!buf || !client)
+    {
+        return EINVAL;
+    }
+
+
+    if (atomic_read(&obj->suspend))
+    {
+        return EIO;
+    }
+
+    if ((res = MPU6050_ReadData(client, obj->data)))
+    {
+        GSE_ERR("I2C error: ret value=%d", res);
+        return EIO;
+    }
+    else
+    {
+        sprintf(buf, "%04x %04x %04x", obj->data[MPU6050_AXIS_X], 
+                obj->data[MPU6050_AXIS_Y], obj->data[MPU6050_AXIS_Z]);
+
+    }
+
+    return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int MPU6050_InitSelfTest(struct i2c_client *client)
+{
+    int res = 0;
+    u8  data;
+
+    res = MPU6050_SetBWRate(client, MPU6050_BW_184HZ);
+    if (res != MPU6050_SUCCESS ) //0x2C->BW=100Hz
+    {
+        return res;
+    }
+
+    res = mpu_i2c_read_block(client, MPU6050_REG_DATA_FORMAT, &data, 1);
+
+    if (res != MPU6050_SUCCESS)
+    {
+        return res;
+    }
+
+    return MPU6050_SUCCESS;
+}
+/*----------------------------------------------------------------------------*/
+static int MPU6050_JudgeTestResult(struct i2c_client *client, s32 prv[MPU6050_AXES_NUM], s32 nxt[MPU6050_AXES_NUM])
+{
+    struct criteria
+    {
+        int min;
+        int max;
+    };
+
+    struct criteria self[4][3] = {
+        {{ 0, 540}, { 0, 540}, { 0, 875}},
+        {{ 0, 270}, { 0, 270}, { 0, 438}},
+        {{ 0, 135}, { 0, 135}, { 0, 219}},            
+        {{ 0,  67}, { 0,  67}, { 0, 110}},            
+    };
+    struct criteria (*ptr)[3] = NULL;
+    u8 format;
+    int res;
+    if ((res = mpu_i2c_read_block(client, MPU6050_REG_DATA_FORMAT, &format, 1)))
+        return res;
+
+    format = format & MPU6050_RANGE_16G;
+
+    switch (format)
+    {
+        case MPU6050_RANGE_2G:
+            GSE_LOG("format use self[0]\n");
+            ptr = &self[0];       
+        break;
+
+        case MPU6050_RANGE_4G:
+            GSE_LOG("format use self[1]\n");
+            ptr = &self[1];       
+        break;
+    
+        case MPU6050_RANGE_8G:
+            GSE_LOG("format use self[2]\n");
+            ptr = &self[2];       
+        break;
+    
+        case MPU6050_RANGE_16G:
+            GSE_LOG("format use self[3]\n");
+            ptr = &self[3];       
+        break;
+    
+        default:
+            GSE_LOG("format unknow use \n");
+        break;
+    }
+
+    if (!ptr)
+    {
+        GSE_ERR("null pointer\n");
+        return -EINVAL;
+    }
+    GSE_LOG("format=0x%x\n",format);
+
+    GSE_LOG("X diff is %ld\n",abs(nxt[MPU6050_AXIS_X] - prv[MPU6050_AXIS_X]));
+    GSE_LOG("Y diff is %ld\n",abs(nxt[MPU6050_AXIS_Y] - prv[MPU6050_AXIS_Y]));
+    GSE_LOG("Z diff is %ld\n",abs(nxt[MPU6050_AXIS_Z] - prv[MPU6050_AXIS_Z]));
+
+
+    if ((abs(nxt[MPU6050_AXIS_X] - prv[MPU6050_AXIS_X]) > (*ptr)[MPU6050_AXIS_X].max) ||
+        (abs(nxt[MPU6050_AXIS_X] - prv[MPU6050_AXIS_X]) < (*ptr)[MPU6050_AXIS_X].min))
+    {
+        GSE_ERR("X is over range\n");
+        res = -EINVAL;
+    }
+    if ((abs(nxt[MPU6050_AXIS_Y] - prv[MPU6050_AXIS_Y]) > (*ptr)[MPU6050_AXIS_Y].max) ||
+        (abs(nxt[MPU6050_AXIS_Y] - prv[MPU6050_AXIS_Y]) < (*ptr)[MPU6050_AXIS_Y].min))
+    {
+        GSE_ERR("Y is over range\n");
+        res = -EINVAL;
+    }
+    if ((abs(nxt[MPU6050_AXIS_Z] - prv[MPU6050_AXIS_Z]) > (*ptr)[MPU6050_AXIS_Z].max) ||
+        (abs(nxt[MPU6050_AXIS_Z] - prv[MPU6050_AXIS_Z]) < (*ptr)[MPU6050_AXIS_Z].min))
+    {
+        GSE_ERR("Z is over range\n");
+        res = -EINVAL;
+    }
+    return res;
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t show_chipinfo_value(struct device_driver *ddri, char *buf)
+{
+    struct i2c_client *client = mpu6050_i2c_client;
+    char strbuf[MPU6050_BUFSIZE];
+    if (NULL == client)
+    {
+        GSE_ERR("i2c client is null!!\n");
+        return 0;
+    }
+
+    if (sensor_power == false)
+    {
+        MPU6050_SetPowerMode(client, true);
+        msleep(50);
+    }
+	
+    MPU6050_ReadAllReg(client, strbuf, MPU6050_BUFSIZE);
+
+    MPU6050_ReadChipInfo(client, strbuf, MPU6050_BUFSIZE);
+    return snprintf(buf, PAGE_SIZE, "%s\n", strbuf);        
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t show_sensordata_value(struct device_driver *ddri, char *buf)
+{
+    struct i2c_client *client = mpu6050_i2c_client;
+    char strbuf[MPU6050_BUFSIZE];
+
+    if (NULL == client)
+    {
+        GSE_ERR("i2c client is null!!\n");
+        return 0;
+    }
+    MPU6050_ReadSensorData(client, strbuf, MPU6050_BUFSIZE);
+    return snprintf(buf, PAGE_SIZE, "%s\n", strbuf);            
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t show_cali_value(struct device_driver *ddri, char *buf)
+{
+    struct i2c_client *client = mpu6050_i2c_client;
+    struct mpu6050_i2c_data *obj;
+    int err, len = 0, mul;
+    int tmp[MPU6050_AXES_NUM];
+
+    if (NULL == client)
+    {
+        GSE_ERR("i2c client is null!!\n");
+        return 0;
+    }
+
+    obj = i2c_get_clientdata(client);
+
+
+    if ((err = MPU6050_ReadOffset(client, obj->offset)))
+    {
+        return -EINVAL;
+    }
+    else if ((err = MPU6050_ReadCalibration(client, tmp)))
+    {
+        return -EINVAL;
+    }
+    else
+    {
+        mul = obj->reso->sensitivity/mpu6050_offset_resolution.sensitivity;
+        len += snprintf(buf+len, PAGE_SIZE-len, "[HW ][%d] (%+3d, %+3d, %+3d) : (0x%02X, 0x%02X, 0x%02X)\n", mul,                        
+                        obj->offset[MPU6050_AXIS_X], obj->offset[MPU6050_AXIS_Y], obj->offset[MPU6050_AXIS_Z],
+                        obj->offset[MPU6050_AXIS_X], obj->offset[MPU6050_AXIS_Y], obj->offset[MPU6050_AXIS_Z]);
+        len += snprintf(buf+len, PAGE_SIZE-len, "[SW ][%d] (%+3d, %+3d, %+3d)\n", 1, 
+                        obj->cali_sw[MPU6050_AXIS_X], obj->cali_sw[MPU6050_AXIS_Y], obj->cali_sw[MPU6050_AXIS_Z]);
+
+        len += snprintf(buf+len, PAGE_SIZE-len, "[ALL]    (%+3d, %+3d, %+3d) : (%+3d, %+3d, %+3d)\n", 
+                        obj->offset[MPU6050_AXIS_X]*mul + obj->cali_sw[MPU6050_AXIS_X],
+                        obj->offset[MPU6050_AXIS_Y]*mul + obj->cali_sw[MPU6050_AXIS_Y],
+                        obj->offset[MPU6050_AXIS_Z]*mul + obj->cali_sw[MPU6050_AXIS_Z],
+                        tmp[MPU6050_AXIS_X], tmp[MPU6050_AXIS_Y], tmp[MPU6050_AXIS_Z]);
+
+        return len;
+    }
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t store_cali_value(struct device_driver *ddri, const char *buf, size_t count)
+{
+    struct i2c_client *client = mpu6050_i2c_client;  
+    int err, x, y, z;
+    int dat[MPU6050_AXES_NUM];
+
+    if (!strncmp(buf, "rst", 3))
+    {
+        if ((err = MPU6050_ResetCalibration(client)))
+        {
+            GSE_ERR("reset offset err = %d\n", err);
+        }
+    }
+    else if (3 == sscanf(buf, "0x%02X 0x%02X 0x%02X", &x, &y, &z))
+    {
+        dat[MPU6050_AXIS_X] = x;
+        dat[MPU6050_AXIS_Y] = y;
+        dat[MPU6050_AXIS_Z] = z;
+        if ((err = MPU6050_WriteCalibration(client, dat)))
+        {
+            GSE_ERR("write calibration err = %d\n", err);
+        }
+    }
+    else
+    {
+        GSE_ERR("invalid format\n");
+    }
+
+    return count;
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t show_self_value(struct device_driver *ddri, char *buf)
+{
+    struct i2c_client *client = mpu6050_i2c_client;
+
+    if (NULL == client)
+    {
+        GSE_ERR("i2c client is null!!\n");
+        return 0;
+    }
+
+    return snprintf(buf, 8, "%s\n", selftestRes);
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t store_self_value(struct device_driver *ddri, const char *buf, size_t count)
+{   /*write anything to this register will trigger the process*/
+    struct item
+    {
+        s16 raw[MPU6050_AXES_NUM];
+    };
+
+    struct i2c_client *client = mpu6050_i2c_client;  
+    int idx, res, num;
+    struct item *prv = NULL, *nxt = NULL;
+    s32 avg_prv[MPU6050_AXES_NUM] = {0, 0, 0};
+    s32 avg_nxt[MPU6050_AXES_NUM] = {0, 0, 0};
+
+
+    if (1 != sscanf(buf, "%d", &num))
+    {
+        GSE_ERR("parse number fail\n");
+        return count;
+    }
+    else if (num == 0)
+    {
+        GSE_ERR("invalid data count\n");
+        return count;
+    }
+
+    prv = kzalloc(sizeof(*prv) * num, GFP_KERNEL);
+    nxt = kzalloc(sizeof(*nxt) * num, GFP_KERNEL);
+    if (!prv || !nxt)
+    {
+        goto exit;
+    }
+
+
+    GSE_LOG("NORMAL:\n");
+    MPU6050_SetPowerMode(client,true);
+    msleep(50);
+
+    for (idx = 0; idx < num; idx++)
+    {
+        if ((res = MPU6050_ReadData(client, prv[idx].raw)))
+        {
+            GSE_ERR("read data fail: %d\n", res);
+            goto exit;
+        }
+
+        avg_prv[MPU6050_AXIS_X] += prv[idx].raw[MPU6050_AXIS_X];
+        avg_prv[MPU6050_AXIS_Y] += prv[idx].raw[MPU6050_AXIS_Y];
+        avg_prv[MPU6050_AXIS_Z] += prv[idx].raw[MPU6050_AXIS_Z];        
+        GSE_LOG("[%5d %5d %5d]\n", prv[idx].raw[MPU6050_AXIS_X], prv[idx].raw[MPU6050_AXIS_Y], prv[idx].raw[MPU6050_AXIS_Z]);
+    }
+
+    avg_prv[MPU6050_AXIS_X] /= num;
+    avg_prv[MPU6050_AXIS_Y] /= num;
+    avg_prv[MPU6050_AXIS_Z] /= num;    
+
+    /*initial setting for self test*/
+    GSE_LOG("SELFTEST:\n");    
+    for (idx = 0; idx < num; idx++)
+    {
+        if ((res = MPU6050_ReadData(client, nxt[idx].raw)))
+        {
+            GSE_ERR("read data fail: %d\n", res);
+            goto exit;
+        }
+        avg_nxt[MPU6050_AXIS_X] += nxt[idx].raw[MPU6050_AXIS_X];
+        avg_nxt[MPU6050_AXIS_Y] += nxt[idx].raw[MPU6050_AXIS_Y];
+        avg_nxt[MPU6050_AXIS_Z] += nxt[idx].raw[MPU6050_AXIS_Z];        
+        GSE_LOG("[%5d %5d %5d]\n", nxt[idx].raw[MPU6050_AXIS_X], nxt[idx].raw[MPU6050_AXIS_Y], nxt[idx].raw[MPU6050_AXIS_Z]);
+    }
+
+    avg_nxt[MPU6050_AXIS_X] /= num;
+    avg_nxt[MPU6050_AXIS_Y] /= num;
+    avg_nxt[MPU6050_AXIS_Z] /= num;    
+
+    GSE_LOG("X: %5d - %5d = %5d \n", avg_nxt[MPU6050_AXIS_X], avg_prv[MPU6050_AXIS_X], avg_nxt[MPU6050_AXIS_X] - avg_prv[MPU6050_AXIS_X]);
+    GSE_LOG("Y: %5d - %5d = %5d \n", avg_nxt[MPU6050_AXIS_Y], avg_prv[MPU6050_AXIS_Y], avg_nxt[MPU6050_AXIS_Y] - avg_prv[MPU6050_AXIS_Y]);
+    GSE_LOG("Z: %5d - %5d = %5d \n", avg_nxt[MPU6050_AXIS_Z], avg_prv[MPU6050_AXIS_Z], avg_nxt[MPU6050_AXIS_Z] - avg_prv[MPU6050_AXIS_Z]); 
+
+    if (!MPU6050_JudgeTestResult(client, avg_prv, avg_nxt))
+    {
+        GSE_LOG("SELFTEST : PASS\n");
+        strcpy(selftestRes,"y");
+    }
+    else
+    {
+        GSE_LOG("SELFTEST : FAIL\n");
+        strcpy(selftestRes,"n");
+    }
+
+    exit:
+    /*restore the setting*/    
+    mpu6050_init_client(client, 0);
+    kfree(prv);
+    kfree(nxt);
+    return count;
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t show_selftest_value(struct device_driver *ddri, char *buf)
+{
+    struct i2c_client *client = mpu6050_i2c_client;
+    struct mpu6050_i2c_data *obj;
+
+    if (NULL == client)
+    {
+        GSE_ERR("i2c client is null!!\n");
+        return 0;
+    }
+
+    obj = i2c_get_clientdata(client);
+    return snprintf(buf, PAGE_SIZE, "%d\n", atomic_read(&obj->selftest));
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t store_selftest_value(struct device_driver *ddri, const char *buf, size_t count)
+{
+    struct mpu6050_i2c_data *obj = obj_i2c_data;
+    int tmp;
+
+    if (NULL == obj)
+    {
+        GSE_ERR("i2c data obj is null!!\n");
+        return 0;
+    }
+
+
+    if (1 == sscanf(buf, "%d", &tmp))
+    {
+        if (atomic_read(&obj->selftest) && !tmp)
+        {
+            /*enable -> disable*/
+            mpu6050_init_client(obj->client, 0);
+        }
+        else if (!atomic_read(&obj->selftest) && tmp)
+        {
+            /*disable -> enable*/
+            MPU6050_InitSelfTest(obj->client);            
+        }
+
+        GSE_LOG("selftest: %d => %d\n", atomic_read(&obj->selftest), tmp);
+        atomic_set(&obj->selftest, tmp); 
+    }
+    else
+    {
+        GSE_ERR("invalid content: '%s', length = %zu\n", buf, count);   
+    }
+    return count;
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t show_firlen_value(struct device_driver *ddri, char *buf)
+{
+#ifdef CONFIG_MPU6050_LOWPASS
+    struct i2c_client *client = mpu6050_i2c_client;
+    struct mpu6050_i2c_data *obj = i2c_get_clientdata(client);
+    if (atomic_read(&obj->firlen))
+    {
+        int idx, len = atomic_read(&obj->firlen);
+        GSE_LOG("len = %2d, idx = %2d\n", obj->fir.num, obj->fir.idx);
+
+        for (idx = 0; idx < len; idx++)
+        {
+            GSE_LOG("[%5d %5d %5d]\n", obj->fir.raw[idx][MPU6050_AXIS_X], obj->fir.raw[idx][MPU6050_AXIS_Y], obj->fir.raw[idx][MPU6050_AXIS_Z]);
+        }
+
+        GSE_LOG("sum = [%5d %5d %5d]\n", obj->fir.sum[MPU6050_AXIS_X], obj->fir.sum[MPU6050_AXIS_Y], obj->fir.sum[MPU6050_AXIS_Z]);
+        GSE_LOG("avg = [%5d %5d %5d]\n", obj->fir.sum[MPU6050_AXIS_X]/len, obj->fir.sum[MPU6050_AXIS_Y]/len, obj->fir.sum[MPU6050_AXIS_Z]/len);
+    }
+    return snprintf(buf, PAGE_SIZE, "%d\n", atomic_read(&obj->firlen));
+#else
+    return snprintf(buf, PAGE_SIZE, "not support\n");
+#endif
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t store_firlen_value(struct device_driver *ddri, const char *buf, size_t count)
+{
+#ifdef CONFIG_MPU6050_LOWPASS
+    struct i2c_client *client = mpu6050_i2c_client;  
+    struct mpu6050_i2c_data *obj = i2c_get_clientdata(client);
+    int firlen;
+
+    if (1 != sscanf(buf, "%d", &firlen))
+    {
+        GSE_ERR("invallid format\n");
+    }
+    else if (firlen > C_MAX_FIR_LENGTH)
+    {
+        GSE_ERR("exceeds maximum filter length\n");
+    }
+    else
+    {
+        atomic_set(&obj->firlen, firlen);
+        if (0 == firlen)
+        {
+            atomic_set(&obj->fir_en, 0);
+        }
+        else
+        {
+            memset(&obj->fir, 0x00, sizeof(obj->fir));
+            atomic_set(&obj->fir_en, 1);
+        }
+    }
+#endif    
+    return count;
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t show_trace_value(struct device_driver *ddri, char *buf)
+{
+    ssize_t res;
+    struct mpu6050_i2c_data *obj = obj_i2c_data;
+    if (obj == NULL)
+    {
+        GSE_ERR("i2c_data obj is null!!\n");
+        return 0;
+    }
+
+    res = snprintf(buf, PAGE_SIZE, "0x%04X\n", atomic_read(&obj->trace));     
+    return res;    
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t store_trace_value(struct device_driver *ddri, const char *buf, size_t count)
+{
+    struct mpu6050_i2c_data *obj = obj_i2c_data;
+    int trace;
+    if (obj == NULL)
+    {
+        GSE_ERR("i2c_data obj is null!!\n");
+        return 0;
+    }
+
+    if (1 == sscanf(buf, "0x%x", &trace))
+    {
+        atomic_set(&obj->trace, trace);
+    }
+    else
+    {
+        GSE_ERR("invalid content: '%s', length = %zu\n", buf, count);
+    }
+
+    return count;    
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t show_status_value(struct device_driver *ddri, char *buf)
+{
+    ssize_t len = 0;    
+    struct mpu6050_i2c_data *obj = obj_i2c_data;
+    if (obj == NULL)
+    {
+        GSE_ERR("i2c_data obj is null!!\n");
+        return 0;
+    }
+
+    if (obj->hw)
+    {
+        len += snprintf(buf+len, PAGE_SIZE-len, "CUST: %d %d (%d %d)\n", 
+                        obj->hw->i2c_num, obj->hw->direction, obj->hw->power_id, obj->hw->power_vol);   
+    }
+    else
+    {
+        len += snprintf(buf+len, PAGE_SIZE-len, "CUST: NULL\n");
+    }
+    return len;    
+}
+/*----------------------------------------------------------------------------*/
+static DRIVER_ATTR(chipinfo,             S_IRUGO, show_chipinfo_value,      NULL);
+static DRIVER_ATTR(sensordata,           S_IRUGO, show_sensordata_value,    NULL);
+static DRIVER_ATTR(cali,       S_IWUSR | S_IRUGO, show_cali_value,          store_cali_value);
+static DRIVER_ATTR(self,       S_IWUSR | S_IRUGO, show_selftest_value,          store_selftest_value);
+static DRIVER_ATTR(selftest,   S_IWUSR | S_IRUGO, show_self_value ,      store_self_value );
+static DRIVER_ATTR(firlen,     S_IWUSR | S_IRUGO, show_firlen_value,        store_firlen_value);
+static DRIVER_ATTR(trace,      S_IWUSR | S_IRUGO, show_trace_value,         store_trace_value);
+static DRIVER_ATTR(status,               S_IRUGO, show_status_value,        NULL);
+/*----------------------------------------------------------------------------*/
+static struct driver_attribute *mpu6050_attr_list[] = {
+    &driver_attr_chipinfo,     /*chip information*/
+    &driver_attr_sensordata,   /*dump sensor data*/
+    &driver_attr_cali,         /*show calibration data*/
+    &driver_attr_self,         /*self test demo*/
+    &driver_attr_selftest,     /*self control: 0: disable, 1: enable*/
+    &driver_attr_firlen,       /*filter length: 0: disable, others: enable*/
+    &driver_attr_trace,        /*trace log*/
+    &driver_attr_status,        
+};
+/*----------------------------------------------------------------------------*/
+static int mpu6050_create_attr(struct device_driver *driver) 
+{
+    int idx, err = 0;
+    int num = (int)(sizeof(mpu6050_attr_list)/sizeof(mpu6050_attr_list[0]));
+    if (driver == NULL)
+    {
+        return -EINVAL;
+    }
+
+    for (idx = 0; idx < num; idx++)
+    {
+        if (0 != (err = driver_create_file(driver, mpu6050_attr_list[idx])))
+        {
+            GSE_ERR("driver_create_file (%s) = %d\n", mpu6050_attr_list[idx]->attr.name, err);
+            break;
+        }
+    }    
+    return err;
+}
+/*----------------------------------------------------------------------------*/
+static int mpu6050_delete_attr(struct device_driver *driver)
+{
+    int idx ,err = 0;
+    int num = (int)(sizeof(mpu6050_attr_list)/sizeof(mpu6050_attr_list[0]));
+
+    if (driver == NULL)
+    {
+        return -EINVAL;
+    }
+
+    for (idx = 0; idx < num; idx++)
+    {
+        driver_remove_file(driver, mpu6050_attr_list[idx]);
+    }
+
+    return err;
+}
+
+/*----------------------------------------------------------------------------*/
+int gsensor_operate(void* self, uint32_t command, void* buff_in, int size_in,
+                    void* buff_out, int size_out, int* actualout)
+{
+    int err = 0;
+    int value, sample_delay;    
+    struct mpu6050_i2c_data *priv = (struct mpu6050_i2c_data*)self;
+    hwm_sensor_data* gsensor_data;
+    char buff[MPU6050_BUFSIZE];
+
+
+    switch (command)
+    {
+    case SENSOR_DELAY:
+        if ((buff_in == NULL) || (size_in < sizeof(int)))
+        {
+            GSE_ERR("Set delay parameter error!\n");
+            err = -EINVAL;
+        }
+        else
+        {
+            value = *(int *)buff_in;
+            
+            if(value <= 5)
+            {
+            	sample_delay = MPU6050_BW_184HZ;
+            }
+            else if(value <= 10)
+            {
+            	sample_delay = MPU6050_BW_94HZ;
+            }
+            else
+            {
+            	sample_delay = MPU6050_BW_44HZ;
+            }
+            GSE_LOG("Set delay parameter value:%d \n", value);
+        
+
+            err = MPU6050_SetBWRate(priv->client, sample_delay);
+            if (err != MPU6050_SUCCESS ) //0x2C->BW=100Hz
+            {
+                GSE_ERR("Set delay parameter error!\n");
+            }
+
+            if (value >= 50)
+            {
+                atomic_set(&priv->filter, 0);
+            }
+            else
+            {
+#if defined(CONFIG_MPU6050_LOWPASS)				
+                priv->fir.num = 0;
+                priv->fir.idx = 0;
+                priv->fir.sum[MPU6050_AXIS_X] = 0;
+                priv->fir.sum[MPU6050_AXIS_Y] = 0;
+                priv->fir.sum[MPU6050_AXIS_Z] = 0;
+#endif
+                atomic_set(&priv->filter, 1);
+            }
+        }
+        break;
+
+    case SENSOR_ENABLE:
+        if ((buff_in == NULL) || (size_in < sizeof(int)))
+        {
+            GSE_ERR("Enable sensor parameter error!\n");
+            err = -EINVAL;
+        }
+        else
+        {
+            value = *(int *)buff_in;
+            if (((value == 0) && (sensor_power == false)) ||((value == 1) && (sensor_power == true)))
+            {
+                GSE_LOG("Gsensor device have updated!\n");
+            }
+            else
+            {
+                err = MPU6050_SetPowerMode( priv->client, !sensor_power);
+            }
+        }
+        break;
+
+    case SENSOR_GET_DATA:
+        if ((buff_out == NULL) || (size_out< sizeof(hwm_sensor_data)))
+        {
+            GSE_ERR("get sensor data parameter error!\n");
+            err = -EINVAL;
+        }
+        else
+        {
+            gsensor_data = (hwm_sensor_data *)buff_out;
+            err = MPU6050_ReadSensorData(priv->client, buff, MPU6050_BUFSIZE);
+            if (!err)
+            {
+                sscanf(buff, "%x %x %x", &gsensor_data->values[0], 
+                       &gsensor_data->values[1], &gsensor_data->values[2]);             
+                gsensor_data->status = SENSOR_STATUS_ACCURACY_MEDIUM;                
+                gsensor_data->value_divide = 1000;
+            }
+        }
+        break;
+    default:
+        GSE_ERR("gsensor operate function no this parameter %d!\n", command);
+        err = -1;
+        break;
+    }
+
+    return err;
+}
+
+/****************************************************************************** 
+ * Function Configuration
+******************************************************************************/
+static int mpu6050_open(struct inode *inode, struct file *file)
+{
+    file->private_data = mpu6050_i2c_client;
+
+    if (file->private_data == NULL)
+    {
+        GSE_ERR("null pointer!!\n");
+        return -EINVAL;
+    }
+    return nonseekable_open(inode, file);
+}
+/*----------------------------------------------------------------------------*/
+static int mpu6050_release(struct inode *inode, struct file *file)
+{
+    file->private_data = NULL;
+    return 0;
+}
+/*----------------------------------------------------------------------------*/
+#ifdef CONFIG_COMPAT
+static long mpu6050_compat_ioctl(struct file *file, unsigned int cmd,
+       unsigned long arg)
+{
+    long err = 0;
+
+	void __user *arg32 = compat_ptr(arg);
+	
+	if (!file->f_op || !file->f_op->unlocked_ioctl)
+		return -ENOTTY;
+	
+    switch (cmd)
+    {
+        case COMPAT_GSENSOR_IOCTL_READ_SENSORDATA:
+            if (arg32 == NULL)
+            {
+                err = -EINVAL;
+                break;    
+            }
+		
+		    err = file->f_op->unlocked_ioctl(file, GSENSOR_IOCTL_READ_SENSORDATA, (unsigned long)arg32);
+		    if (err){
+		        GSE_ERR("GSENSOR_IOCTL_READ_SENSORDATA unlocked_ioctl failed.");
+		        return err;
+		    }
+        break;
+        case COMPAT_GSENSOR_IOCTL_SET_CALI:
+            if (arg32 == NULL)
+            {
+                err = -EINVAL;
+                break;    
+            }
+		
+		    err = file->f_op->unlocked_ioctl(file, GSENSOR_IOCTL_SET_CALI, (unsigned long)arg32);
+		    if (err){
+		        GSE_ERR("GSENSOR_IOCTL_SET_CALI unlocked_ioctl failed.");
+		        return err;
+		    }
+        break;
+        case COMPAT_GSENSOR_IOCTL_GET_CALI:
+            if (arg32 == NULL)
+            {
+                err = -EINVAL;
+                break;    
+            }
+		
+		    err = file->f_op->unlocked_ioctl(file, GSENSOR_IOCTL_GET_CALI, (unsigned long)arg32);
+		    if (err){
+		        GSE_ERR("GSENSOR_IOCTL_GET_CALI unlocked_ioctl failed.");
+		        return err;
+		    }
+        break;
+        case COMPAT_GSENSOR_IOCTL_CLR_CALI:
+            if (arg32 == NULL)
+            {
+                err = -EINVAL;
+                break;    
+            }
+		
+		    err = file->f_op->unlocked_ioctl(file, GSENSOR_IOCTL_CLR_CALI, (unsigned long)arg32);
+		    if (err){
+		        GSE_ERR("GSENSOR_IOCTL_CLR_CALI unlocked_ioctl failed.");
+		        return err;
+		    }
+        break;
+
+        default:
+            GSE_ERR("unknown IOCTL: 0x%08x\n", cmd);
+            err = -ENOIOCTLCMD;
+        break;
+
+    }
+
+    return err;
+}
+#endif
+
+static long mpu6050_unlocked_ioctl(struct file *file, unsigned int cmd,
+       unsigned long arg)
+{
+    struct i2c_client *client = (struct i2c_client*)file->private_data;
+    struct mpu6050_i2c_data *obj = (struct mpu6050_i2c_data*)i2c_get_clientdata(client);    
+    char strbuf[MPU6050_BUFSIZE];
+    void __user *data;
+    SENSOR_DATA sensor_data;
+    long err = 0;
+    int cali[3];
+
+    if (_IOC_DIR(cmd) & _IOC_READ)
+    {
+        err = !access_ok(VERIFY_WRITE, (void __user *)arg, _IOC_SIZE(cmd));
+    }
+    else if (_IOC_DIR(cmd) & _IOC_WRITE)
+    {
+        err = !access_ok(VERIFY_READ, (void __user *)arg, _IOC_SIZE(cmd));
+    }
+
+    if (err)
+    {
+        GSE_ERR("access error: %08X, (%2d, %2d)\n", cmd, _IOC_DIR(cmd), _IOC_SIZE(cmd));
+        return -EFAULT;
+    }
+
+    switch (cmd)
+    {
+    case GSENSOR_IOCTL_INIT:
+        mpu6050_init_client(client, 0);         
+        break;
+
+    case GSENSOR_IOCTL_READ_CHIPINFO:
+        data = (void __user *) arg;
+        if (data == NULL)
+        {
+            err = -EINVAL;
+            break;    
+        }
+
+        MPU6050_ReadChipInfo(client, strbuf, MPU6050_BUFSIZE);
+        if (copy_to_user(data, strbuf, strlen(strbuf)+1))
+        {
+            err = -EFAULT;
+            break;
+        }
+        break;    
+
+    case GSENSOR_IOCTL_READ_SENSORDATA:
+        data = (void __user *) arg;
+        if (data == NULL)
+        {
+            err = -EINVAL;
+            break;    
+        }
+
+        MPU6050_ReadSensorData(client, strbuf, MPU6050_BUFSIZE);
+        if (copy_to_user(data, strbuf, strlen(strbuf)+1))
+        {
+            err = -EFAULT;
+            break;    
+        }
+        break;
+
+    case GSENSOR_IOCTL_READ_GAIN:
+        data = (void __user *) arg;
+        if (data == NULL)
+        {
+            err = -EINVAL;
+            break;    
+        }
+
+        if (copy_to_user(data, &gsensor_gain, sizeof(GSENSOR_VECTOR3D)))
+        {
+            err = -EFAULT;
+            break;
+        }
+        break;
+
+    case GSENSOR_IOCTL_READ_RAW_DATA:
+        data = (void __user *) arg;
+        if (data == NULL)
+        {
+            err = -EINVAL;
+            break;    
+        }
+
+        if (atomic_read(&obj->suspend))
+        {
+            err = -EINVAL;
+        }
+        else
+        {
+        MPU6050_ReadRawData(client, strbuf);
+        if (copy_to_user(data, strbuf, strlen(strbuf)+1))
+        {
+            err = -EFAULT;
+            break;    
+        }
+        }
+        break;    
+
+    case GSENSOR_IOCTL_SET_CALI:
+        data = (void __user*)arg;
+        if (data == NULL)
+        {
+            err = -EINVAL;
+            break;    
+        }
+        if (copy_from_user(&sensor_data, data, sizeof(sensor_data)))
+        {
+            err = -EFAULT;
+            break;    
+        }
+        if (atomic_read(&obj->suspend))
+        {
+            GSE_ERR("Perform calibration in suspend state!!\n");
+            err = -EINVAL;
+        }
+        else
+        {
+            cali[MPU6050_AXIS_X] = sensor_data.x * obj->reso->sensitivity / GRAVITY_EARTH_1000;
+            cali[MPU6050_AXIS_Y] = sensor_data.y * obj->reso->sensitivity / GRAVITY_EARTH_1000;
+            cali[MPU6050_AXIS_Z] = sensor_data.z * obj->reso->sensitivity / GRAVITY_EARTH_1000;           
+            err = MPU6050_WriteCalibration(client, cali);            
+        }
+        break;
+
+    case GSENSOR_IOCTL_CLR_CALI:
+        err = MPU6050_ResetCalibration(client);
+        break;
+
+    case GSENSOR_IOCTL_GET_CALI:
+        data = (void __user*)arg;
+        if (data == NULL)
+        {
+            err = -EINVAL;
+            break;    
+        }
+        if ((err = MPU6050_ReadCalibration(client, cali)))
+        {
+            break;
+        }
+
+        sensor_data.x = cali[MPU6050_AXIS_X] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+        sensor_data.y = cali[MPU6050_AXIS_Y] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+        sensor_data.z = cali[MPU6050_AXIS_Z] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+        if (copy_to_user(data, &sensor_data, sizeof(sensor_data)))
+        {
+            err = -EFAULT;
+            break;
+        }
+        break;
+
+
+    default:
+        GSE_ERR("unknown IOCTL: 0x%08x\n", cmd);
+        err = -ENOIOCTLCMD;
+        break;
+
+    }
+
+    return err;
+}
+
+
+/*----------------------------------------------------------------------------*/
+static struct file_operations mpu6050_fops = {
+    .open = mpu6050_open,
+    .release = mpu6050_release,
+    .unlocked_ioctl = mpu6050_unlocked_ioctl,
+    #ifdef CONFIG_COMPAT
+	.compat_ioctl = mpu6050_compat_ioctl,
+	#endif
+};
+/*----------------------------------------------------------------------------*/
+static struct miscdevice mpu6050_device = {
+    .minor = MISC_DYNAMIC_MINOR,
+    .name = "gsensor",
+    .fops = &mpu6050_fops,
+};
+/*----------------------------------------------------------------------------*/
+#ifndef USE_EARLY_SUSPEND
+/*----------------------------------------------------------------------------*/
+static int mpu6050_suspend(struct i2c_client *client, pm_message_t msg) 
+{
+    struct mpu6050_i2c_data *obj = i2c_get_clientdata(client);    
+    int err = 0;
+    GSE_FUN();    
+
+    if (msg.event == PM_EVENT_SUSPEND)
+    {
+        if (obj == NULL)
+        {
+            GSE_ERR("null pointer!!\n");
+            return -EINVAL;
+        }
+        atomic_set(&obj->suspend, 1);
+
+        if ((err = MPU6050_SetPowerMode(obj->client, false)))
+        {
+            GSE_ERR("write power control fail!!\n");
+            return err;
+        }
+        MPU6050_power(obj->hw, 0);
+        GSE_LOG("mpu6050_suspend ok\n");
+    }
+    return err;
+}
+/*----------------------------------------------------------------------------*/
+static int mpu6050_resume(struct i2c_client *client)
+{
+    struct mpu6050_i2c_data *obj = i2c_get_clientdata(client);        
+    int err;
+    GSE_FUN();
+
+    if (obj == NULL)
+    {
+        GSE_ERR("null pointer!!\n");
+        return -EINVAL;
+    }
+
+    MPU6050_power(obj->hw, 1);
+
+    if ((err = mpu6050_init_client(client, 0)))
+    {
+        GSE_ERR("initialize client fail!!\n");
+        return err;        
+    }
+    atomic_set(&obj->suspend, 0);
+    GSE_LOG("mpu6050_resume ok\n");
+
+    return 0;
+}
+/*----------------------------------------------------------------------------*/
+#else /*CONFIG_HAS_EARLY_SUSPEND is defined*/
+/*----------------------------------------------------------------------------*/
+static void mpu6050_early_suspend(struct early_suspend *h) 
+{
+    struct mpu6050_i2c_data *obj = container_of(h, struct mpu6050_i2c_data, early_drv);   
+    int err;
+    GSE_FUN();    
+
+    if (obj == NULL)
+    {
+        GSE_ERR("null pointer!!\n");
+        return;
+    }
+    atomic_set(&obj->suspend, 1); 
+
+    if ((err = MPU6050_SetPowerMode(obj->client, false)))
+    {
+        GSE_ERR("write power control fail!!\n");
+        return;
+    }
+
+    if (MPU6050_gyro_mode() == false)
+    {
+        MPU6050_Dev_Reset(obj->client);
+        MPU6050_Reset(obj->client);
+    }
+
+    obj->bandwidth = 0;
+
+    sensor_power = false;
+
+    MPU6050_power(obj->hw, 0);
+}
+/*----------------------------------------------------------------------------*/
+static void mpu6050_late_resume(struct early_suspend *h)
+{
+    struct mpu6050_i2c_data *obj = container_of(h, struct mpu6050_i2c_data, early_drv);         
+    int err;
+    GSE_FUN();
+
+    if (obj == NULL)
+    {
+        GSE_ERR("null pointer!!\n");
+        return;
+    }
+
+    MPU6050_power(obj->hw, 1);
+
+    if ((err = mpu6050_init_client(obj->client, 0)))
+    {
+        GSE_ERR("initialize client fail!!\n");
+        return;        
+    }
+    atomic_set(&obj->suspend, 0);    
+}
+/*----------------------------------------------------------------------------*/
+#endif /*CONFIG_HAS_EARLYSUSPEND*/
+/*----------------------------------------------------------------------------*/
+static int mpu6050_i2c_detect(struct i2c_client *client, struct i2c_board_info *info) 
+{    
+    strcpy(info->type, MPU6050_DEV_NAME);
+    return 0;
+}
+
+
+
+// if use  this typ of enable , Gsensor should report inputEvent(x, y, z ,stats, div) to HAL
+static int mpu6050_open_report_data(int open)
+{
+	//should queuq work to report event if  is_report_input_direct=true
+	return 0;
+}
+
+// if use  this typ of enable , Gsensor only enabled but not report inputEvent to HAL
+
+static int mpu6050_enable_nodata(int en)
+{
+	int res =0;
+	int retry = 0;
+	bool power=false;
+	
+	if(1==en)
+	{
+		power=true;
+	}
+	if(0==en)
+	{
+		power =false;
+	}
+
+	for(retry = 0; retry < 3; retry++){
+		res = MPU6050_SetPowerMode(obj_i2c_data->client, power);
+		if(res == 0)
+		{
+			GSE_LOG("MPU6050_SetPowerMode done\n");
+			break;
+		}
+		GSE_LOG("MPU6050_SetPowerMode fail\n");
+	}
+
+	
+	if(res != MPU6050_SUCCESS)
+	{
+		GSE_LOG("MPU6050_SetPowerMode fail!\n");
+		return -1;
+	}
+	GSE_LOG("mpu6050_enable_nodata OK!\n");
+	return 0;
+}
+
+static int mpu6050_set_delay(u64 ns)
+{
+    int value =0;
+	int sample_delay=0;
+	int err;
+	value = (int)ns/1000/1000;
+	if(value <= 5)
+	{
+		sample_delay = MPU6050_BW_184HZ;
+	}
+	else if(value <= 10)
+	{
+		sample_delay = MPU6050_BW_94HZ;
+	}
+	else
+	{
+		sample_delay = MPU6050_BW_44HZ;
+	}
+				
+	err = MPU6050_SetBWRate(obj_i2c_data->client, sample_delay);
+	if(err != MPU6050_SUCCESS ) 
+	{
+		GSE_ERR("mpu6050_set_delay Set delay parameter error!\n");
+		return -1;
+	}
+	GSE_LOG("mpu6050_set_delay (%d)\n",value);
+	return 0;
+}
+
+static int mpu6050_get_data(int* x ,int* y,int* z, int* status)
+{
+	char buff[MPU6050_BUFSIZE];
+	MPU6050_ReadSensorData(obj_i2c_data->client, buff, MPU6050_BUFSIZE);
+	
+	sscanf(buff, "%x %x %x", x, y, z);		
+	*status = SENSOR_STATUS_ACCURACY_MEDIUM;
+
+	return 0;
+}
+
+
+/*----------------------------------------------------------------------------*/
+static int mpu6050_i2c_probe(struct i2c_client *client, const struct i2c_device_id *id)
+{
+	struct i2c_client *new_client;
+	struct mpu6050_i2c_data *obj;
+	int err = 0;
+	struct acc_control_path ctl={0};
+	struct acc_data_path data={0};
+	GSE_FUN();
+
+    if (!(obj = kzalloc(sizeof(*obj), GFP_KERNEL)))
+    {
+        err = -ENOMEM;
+        goto exit;
+    }
+
+    memset(obj, 0, sizeof(struct mpu6050_i2c_data));
+
+    obj->hw = get_cust_acc_hw();
+
+    if ((err = hwmsen_get_convert(obj->hw->direction, &obj->cvt)))
+    {
+        GSE_ERR("invalid direction: %d\n", obj->hw->direction);
+        goto exit;
+    }
+
+    obj_i2c_data = obj;
+    obj->client = client;
+    obj->client->timing = 400;
+
+    new_client = obj->client;
+    i2c_set_clientdata(new_client,obj);
+
+    atomic_set(&obj->trace, 0);
+    atomic_set(&obj->suspend, 0);
+
+#ifdef CONFIG_MPU6050_LOWPASS
+    if (obj->hw->firlen > C_MAX_FIR_LENGTH)
+    {
+        atomic_set(&obj->firlen, C_MAX_FIR_LENGTH);
+    }
+    else
+    {
+        atomic_set(&obj->firlen, obj->hw->firlen);
+    }
+
+    if (atomic_read(&obj->firlen) > 0)
+    {
+        atomic_set(&obj->fir_en, 1);
+    }
+
+#endif
+
+    mpu6050_i2c_client = new_client;    
+    MPU6050_Dev_Reset(new_client);
+    MPU6050_Reset(new_client);
+
+    if ((err = mpu6050_init_client(new_client, 1)))
+    {
+        goto exit_init_failed;
+    }
+
+
+    if ((err = misc_register(&mpu6050_device)))
+    {
+        GSE_ERR("mpu6050_device register failed\n");
+        goto exit_misc_device_register_failed;
+    }
+
+    ctl.is_use_common_factory = false;
+    if ((err = mpu6050_create_attr(&(mpu6050_init_info.platform_diver_addr->driver))))
+    	{
+	        GSE_ERR("create attribute err = %d\n", err);
+	        goto exit_create_attr_failed;
+    	}
+	ctl.open_report_data= mpu6050_open_report_data;
+	ctl.enable_nodata = mpu6050_enable_nodata;
+	ctl.set_delay  = mpu6050_set_delay;
+	ctl.is_report_input_direct = false;
+	ctl.is_support_batch = obj->hw->is_batch_supported;
+
+	err = acc_register_control_path(&ctl);
+	if(err)
+	{
+	 	GSE_ERR("register acc control path err\n");
+		goto exit_kfree;
+	}
+
+	data.get_data = mpu6050_get_data;
+	data.vender_div = 1000;
+	err = acc_register_data_path(&data);
+	if(err)
+	{
+		GSE_ERR("register acc data path err= %d\n", err);
+		goto exit_kfree;
+	}
+
+#ifdef USE_EARLY_SUSPEND
+    obj->early_drv.level    = EARLY_SUSPEND_LEVEL_STOP_DRAWING - 2,
+    obj->early_drv.suspend  = mpu6050_early_suspend,
+    obj->early_drv.resume   = mpu6050_late_resume,    
+    register_early_suspend(&obj->early_drv);
+#endif 
+	mpu6050_init_flag =0;
+
+    	GSE_LOG("%s: OK\n", __func__);    
+    	return 0;
+
+    exit_create_attr_failed:
+    misc_deregister(&mpu6050_device);
+    exit_misc_device_register_failed:
+    exit_init_failed:
+    //i2c_detach_client(new_client);
+    exit_kfree:
+    kfree(obj);
+    exit:
+    GSE_ERR("%s: err = %d\n", __func__, err);
+	mpu6050_init_flag =-1;
+    return err;
+}
+
+/*----------------------------------------------------------------------------*/
+static int mpu6050_i2c_remove(struct i2c_client *client)
+{
+    int err = 0;    
+
+    if ((err =  mpu6050_delete_attr(&(mpu6050_init_info.platform_diver_addr->driver))))
+    {
+        GSE_ERR("mpu6050_delete_attr fail: %d\n", err);
+    }
+
+    if ((err = misc_deregister(&mpu6050_device)))
+    {
+        GSE_ERR("misc_deregister fail: %d\n", err);
+    }
+
+    if ((err = hwmsen_detach(ID_ACCELEROMETER)))
+    {
+        GSE_ERR("hwmsen_detach fail: %d\n", err);
+    }
+
+    mpu6050_i2c_client = NULL;
+    i2c_unregister_device(client);
+    kfree(i2c_get_clientdata(client));
+    return 0;
+}
+/*----------------------------------------------------------------------------*/
+
+/*----------------------------------------------------------------------------*/
+static int mpu6050_remove(void)
+{
+    struct acc_hw *hw = get_cust_acc_hw();
+
+    GSE_FUN();    
+    MPU6050_power(hw, 0);    
+    i2c_del_driver(&mpu6050_i2c_driver);
+    return 0;
+}
+/*----------------------------------------------------------------------------*/
+
+static int  mpu6050_local_init(void)
+{
+    struct acc_hw *hw = get_cust_acc_hw();
+
+	MPU6050_power(hw, 1);
+	if(i2c_add_driver(&mpu6050_i2c_driver))
+	{
+		GSE_ERR("add driver error\n");
+		return -1;
+	}
+	if(-1 == mpu6050_init_flag)
+	{
+	   return -1;
+	}
+
+	return 0;
+}
+
+
+/*----------------------------------------------------------------------------*/
+static int __init mpu6050gse_init(void)
+{
+    	struct acc_hw *hw = get_cust_acc_hw();
+	GSE_LOG("%s: i2c_number=%d\n", __func__,hw->i2c_num); 
+    	i2c_register_board_info(hw->i2c_num, &i2c_mpu6050, 1);
+	acc_driver_add(&mpu6050_init_info);
+    	return 0;    
+}
+/*----------------------------------------------------------------------------*/
+static void __exit mpu6050gse_exit(void)
+{
+    GSE_FUN();
+}
+/*----------------------------------------------------------------------------*/
+module_init(mpu6050gse_init);
+module_exit(mpu6050gse_exit);
+/*----------------------------------------------------------------------------*/
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("MPU6050 gse driver");
+MODULE_AUTHOR("Yucong.Xiong@mediatek.com");
diff --git a/drivers/misc/mediatek/accelerometer/mpu6050g-new/mpu6050.h b/drivers/misc/mediatek/accelerometer/mpu6050g-new/mpu6050.h
new file mode 100644
index 000000000..0d0993f22
--- /dev/null
+++ b/drivers/misc/mediatek/accelerometer/mpu6050g-new/mpu6050.h
@@ -0,0 +1,60 @@
+#ifndef MPU6050_H
+#define MPU6050_H
+
+#include <linux/ioctl.h>
+
+#define MPU6050_I2C_SLAVE_ADDR		0xD0
+
+
+/* MPU6050 Register Map  (Please refer to MPU6050 Specifications) */
+#define MPU6050_REG_DEVID			0x75
+#define	MPU6050_REG_BW_RATE			0x1A
+#define MPU6050_REG_POWER_CTL                   0x6B 
+#define MPU6050_REG_POWER_CTL2                  0x6C
+#define MPU6050_REG_INT_ENABLE		0x38 
+#define MPU6050_REG_DATA_FORMAT		0x1C 
+#define MPU6050_REG_DATAX0			0x3B 
+#define MPU6050_REG_DATAY0			0x3D
+#define MPU6050_REG_DATAZ0			0x3F
+#define MPU6050_REG_RESET               0x68
+
+/* register Value */ 
+#define MPU6050_FIXED_DEVID         0x68 // or 0x69
+													
+                                           // delay(ms)	
+#define MPU6050_BW_260HZ            0x00   //0     
+#define MPU6050_BW_184HZ            0x01   //2.0     
+#define MPU6050_BW_94HZ             0x02   //3.0   
+#define MPU6050_BW_44HZ             0x03   //4.9   
+#define MPU6050_BW_21HZ             0x04   //8.5   
+#define MPU6050_BW_10HZ             0x05   //13.8  
+#define MPU6050_BW_5HZ              0x06   //19.0  
+
+#define MPU6050_DEV_RESET           0x80
+
+//#define MPU6050_FULL_RES			0x08
+#define MPU6050_RANGE_2G			(0x00 << 3)
+#define MPU6050_RANGE_4G			(0x01 << 3)
+#define MPU6050_RANGE_8G			(0x02 << 3)
+#define MPU6050_RANGE_16G			(0x03 << 3)
+//#define MPU6050_SELF_TEST         0x80
+
+
+#define MPU6050_SLEEP				0x40	//enable low power sleep mode
+
+
+
+// below do not modify	 
+#define MPU6050_SUCCESS                     0
+#define MPU6050_ERR_I2C                     -1
+#define MPU6050_ERR_STATUS                  -3
+#define MPU6050_ERR_SETUP_FAILURE           -4
+#define MPU6050_ERR_GETGSENSORDATA          -5
+#define MPU6050_ERR_IDENTIFICATION          -6
+
+
+
+#define MPU6050_BUFSIZE				256  
+
+#endif
+
diff --git a/drivers/misc/mediatek/accelerometer/mpu6050g/Makefile b/drivers/misc/mediatek/accelerometer/mpu6050g/Makefile
new file mode 100644
index 000000000..a21992eba
--- /dev/null
+++ b/drivers/misc/mediatek/accelerometer/mpu6050g/Makefile
@@ -0,0 +1,4 @@
+include $(srctree)/drivers/misc/mediatek/Makefile.custom
+
+obj-y	:=  mpu6050.o mpu6xxx_hwselftest.o
+
diff --git a/drivers/misc/mediatek/accelerometer/mpu6050g/mpu6050.c b/drivers/misc/mediatek/accelerometer/mpu6050g/mpu6050.c
new file mode 100644
index 000000000..df4e6cc8c
--- /dev/null
+++ b/drivers/misc/mediatek/accelerometer/mpu6050g/mpu6050.c
@@ -0,0 +1,2375 @@
+/* MPU6050 motion sensor driver
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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.
+ *
+ */
+
+#include <linux/interrupt.h>
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include <linux/irq.h>
+#include <linux/miscdevice.h>
+#include <asm/uaccess.h>
+#include <linux/delay.h>
+#include <linux/input.h>
+#include <linux/workqueue.h>
+#include <linux/kobject.h>
+#include <linux/earlysuspend.h>
+#include <linux/platform_device.h>
+#include <asm/atomic.h>
+
+#include <cust_acc.h>
+#include <linux/hwmsensor.h>
+#include <linux/hwmsen_dev.h>
+#include <linux/sensors_io.h>
+#include "mpu6050.h"
+#include "mpu6xxx_hwselftest.h"
+#include <linux/hwmsen_helper.h>
+
+#include <mach/mt_typedefs.h>
+#include <mach/mt_gpio.h>
+#include <mach/mt_pm_ldo.h>
+
+#define POWER_NONE_MACRO MT65XX_POWER_NONE
+
+/*----------------------------------------------------------------------------*/
+#define DEBUG 1
+/*----------------------------------------------------------------------------*/
+#define CONFIG_MPU6050_LOWPASS   /*apply low pass filter on output*/       
+#define SW_CALIBRATION
+/*----------------------------------------------------------------------------*/
+#define MPU6050_AXIS_X          0
+#define MPU6050_AXIS_Y          1
+#define MPU6050_AXIS_Z          2
+#define MPU6050_AXES_NUM        3
+#define MPU6050_DATA_LEN        6
+#define MPU6050_DEV_NAME        "MPU6050G"   /* name must different with gyro mpu6050 */
+/*----------------------------------------------------------------------------*/
+static const struct i2c_device_id mpu6050_i2c_id[] = {{MPU6050_DEV_NAME,0},{}};
+static struct i2c_board_info __initdata i2c_mpu6050={ I2C_BOARD_INFO(MPU6050_DEV_NAME, (MPU6050_I2C_SLAVE_ADDR>>1))};
+
+/*----------------------------------------------------------------------------*/
+static int mpu6050_i2c_probe(struct i2c_client *client, const struct i2c_device_id *id); 
+static int mpu6050_i2c_remove(struct i2c_client *client);
+static int mpu6050_i2c_detect(struct i2c_client *client, struct i2c_board_info *info);
+#ifndef USE_EARLY_SUSPEND
+static int mpu6050_suspend(struct i2c_client *client, pm_message_t msg) ;
+static int mpu6050_resume(struct i2c_client *client);
+#endif
+/*----------------------------------------------------------------------------*/
+typedef enum
+{
+    MPU6050_TRC_FILTER  = 0x01,
+    MPU6050_TRC_RAWDATA = 0x02,
+    MPU6050_TRC_IOCTL   = 0x04,
+    MPU6050_TRC_CALI    = 0X08,
+    MPU6050_TRC_INFO    = 0X10,
+} MPU6050_TRC;
+/*----------------------------------------------------------------------------*/
+struct scale_factor
+{
+    u8  whole;
+    u8  fraction;
+};
+/*----------------------------------------------------------------------------*/
+struct data_resolution
+{
+    struct scale_factor scalefactor;
+    int                 sensitivity;
+};
+/*----------------------------------------------------------------------------*/
+#define C_MAX_FIR_LENGTH (32)
+/*----------------------------------------------------------------------------*/
+struct data_filter
+{
+    s16 raw[C_MAX_FIR_LENGTH][MPU6050_AXES_NUM];
+    int sum[MPU6050_AXES_NUM];
+    int num;
+    int idx;
+};
+/*----------------------------------------------------------------------------*/
+struct mpu6050_i2c_data
+{
+    struct i2c_client *client;
+    struct acc_hw *hw;
+    struct hwmsen_convert   cvt;
+
+    /*misc*/
+    struct data_resolution *reso;
+    atomic_t                trace;
+    atomic_t                suspend;
+    atomic_t                selftest;
+    atomic_t                filter;
+    s16                     cali_sw[MPU6050_AXES_NUM+1];
+
+    /*data*/
+    s8                      offset[MPU6050_AXES_NUM+1];  /*+1: for 4-byte alignment*/
+    s16                     data[MPU6050_AXES_NUM+1];
+
+#if defined(CONFIG_MPU6050_LOWPASS)
+    atomic_t                firlen;
+    atomic_t                fir_en;
+    struct data_filter      fir;
+#endif 
+    /*early suspend*/
+#if defined(USE_EARLY_SUSPEND)
+    struct early_suspend    early_drv;
+#endif     
+    u8                      bandwidth;
+#if MPU6XXX_HWSELFTEST
+    struct inv_selftest_device mpu_selftest_device;
+#endif
+};
+/*----------------------------------------------------------------------------*/
+static struct i2c_driver mpu6050_i2c_driver = {
+    .driver = {
+        .name           = MPU6050_DEV_NAME,
+    },
+    .probe              = mpu6050_i2c_probe,
+    .remove             = mpu6050_i2c_remove,
+    .detect             = mpu6050_i2c_detect,
+#if !defined(USE_EARLY_SUSPEND)    
+    .suspend            = mpu6050_suspend,
+    .resume             = mpu6050_resume,
+#endif
+    .id_table = mpu6050_i2c_id,
+};
+
+/*----------------------------------------------------------------------------*/
+static struct i2c_client *mpu6050_i2c_client = NULL;
+static struct platform_driver mpu6050_gsensor_driver;
+static struct mpu6050_i2c_data *obj_i2c_data = NULL;
+static bool sensor_power = false;
+static GSENSOR_VECTOR3D gsensor_gain;
+static char selftestRes[8]= {0}; 
+
+
+/*----------------------------------------------------------------------------*/
+#define GSE_TAG                  "[Gsensor] "
+#define GSE_FUN(f)               printk(GSE_TAG"%s\n", __FUNCTION__)
+#define GSE_ERR(fmt, args...)    printk(GSE_TAG"%s %d : "fmt, __FUNCTION__, __LINE__, ##args)
+#define GSE_LOG(fmt, args...)    printk(GSE_TAG fmt, ##args)
+/*----------------------------------------------------------------------------*/
+static struct data_resolution mpu6050_data_resolution[] = {
+    /*8 combination by {FULL_RES,RANGE}*/
+    {{ 0,  6}, 16384},  /*+/-2g  in 16-bit resolution:  0.06 mg/LSB*/
+    {{ 0, 12}, 8192},   /*+/-4g  in 16-bit resolution:  0.12 mg/LSB*/
+    {{ 0, 24}, 4096},   /*+/-8g  in 16-bit resolution:  0.24 mg/LSB*/
+    {{ 0, 5}, 2048},   	/*+/-16g in 16-bit resolution:  0.49 mg/LSB*/
+};
+/*----------------------------------------------------------------------------*/
+static struct data_resolution mpu6050_offset_resolution = {{ 0, 5}, 2048};
+
+static unsigned int power_on = 0;
+
+extern int MPU6050_gyro_power(void);
+extern int MPU6050_gyro_mode(void);
+
+
+int MPU6050_gse_power( void)
+{
+    return(power_on);
+}
+EXPORT_SYMBOL(MPU6050_gse_power);
+
+int MPU6050_gse_mode(void)
+{
+    return sensor_power;
+}
+EXPORT_SYMBOL(MPU6050_gse_mode);
+
+
+int MPU6050_i2c_master_send(u8 *buf, u8 len)
+{
+    int res = 0;
+    if (NULL == mpu6050_i2c_client)
+    {
+        GSE_ERR("MPU6050_i2c_master_send null ptr!!\n");
+    }
+    else
+    {
+        res = i2c_master_send(mpu6050_i2c_client, buf, len);
+    }
+
+    return res;
+}
+EXPORT_SYMBOL(MPU6050_i2c_master_send);
+
+int MPU6050_i2c_master_recv(u8 *buf, u8 len)
+{
+    int res = 0;
+    if (NULL == mpu6050_i2c_client)
+    {
+        GSE_ERR("MPU6050_i2c_master_recv null ptr!!\n");
+    }
+    else
+    {
+        res = i2c_master_recv(mpu6050_i2c_client, buf, len);
+    }
+
+    return res;
+}
+EXPORT_SYMBOL(MPU6050_i2c_master_recv);
+/*----------------------------------------------------------------------------*/
+static int mpu_i2c_read_block(struct i2c_client *client, u8 addr, u8 *data, u8 len){
+        u8 beg = addr;
+	struct i2c_msg msgs[2] = {
+		{
+			.addr = client->addr,	.flags = 0,
+			.len = 1,	.buf = &beg
+		},
+		{
+			.addr = client->addr,	.flags = I2C_M_RD,
+			.len = len,	.buf = data,
+		}
+	};
+	int err;
+
+	if (!client)
+		return -EINVAL;
+	else if (len > C_I2C_FIFO_SIZE) {
+		GSE_ERR(" length %d exceeds %d\n", len, C_I2C_FIFO_SIZE);
+		return -EINVAL;
+	}
+
+	err = i2c_transfer(client->adapter, msgs, sizeof(msgs)/sizeof(msgs[0]));
+	if (err != 2) {
+		GSE_ERR("i2c_transfer error: (%d %p %d) %d\n",
+			addr, data, len, err);
+		err = -EIO;
+	} else {
+		err = 0;
+	}
+	return err;
+
+}
+int MPU6050_hwmsen_read_block(u8 addr, u8 *buf, u8 len)
+{
+    if (NULL == mpu6050_i2c_client)
+    {
+        GSE_ERR("MPU6050_hwmsen_read_block null ptr!!\n");
+        return MPU6050_ERR_I2C;
+    }
+    return mpu_i2c_read_block(mpu6050_i2c_client, addr, buf, len);
+}
+EXPORT_SYMBOL(MPU6050_hwmsen_read_block);
+
+
+int MPU6050_hwmsen_read_byte(u8 addr, u8 *buf)
+{
+    if (NULL == mpu6050_i2c_client)
+    {
+        GSE_ERR("MPU6050_hwmsen_read_byte null ptr!!\n");
+        return MPU6050_ERR_I2C;
+    }
+    return mpu_i2c_read_block(mpu6050_i2c_client, addr, buf, 1);
+}
+EXPORT_SYMBOL(MPU6050_hwmsen_read_byte);
+/*--------------------mpu6050 power control function----------------------------------*/
+static void MPU6050_power(struct acc_hw *hw, unsigned int on) 
+{
+
+
+    if (hw->power_id != POWER_NONE_MACRO)        // have externel LDO
+    {
+        GSE_LOG("power %s\n", on ? "on" : "off");
+        if (power_on == on)  // power status not change
+        {
+            GSE_LOG("ignore power control: %d\n", on);
+        }
+        else if (on) // power on
+        {
+            if (!hwPowerOn(hw->power_id, hw->power_vol, "MPU6050G"))
+            {
+                GSE_ERR("power on fails!!\n");
+            }
+        }
+        else    // power off
+        {
+            if (MPU6050_gyro_power() == false)
+            {
+                if (!hwPowerDown(hw->power_id, "MPU6050G"))
+                {
+                    GSE_ERR("power off fail!!\n");
+                }
+            }
+        }
+    }
+    power_on = on;    
+}
+
+/*----------------------------------------------------------------------------*/
+static int MPU6050_SetPowerMode(struct i2c_client *client, bool enable)
+{
+    u8 databuf[2];    
+    int res = 0;
+    //u8 addr = MPU6050_REG_POWER_CTL;
+    struct mpu6050_i2c_data *obj = i2c_get_clientdata(client);
+
+
+    if (enable == sensor_power)
+    {
+        GSE_LOG("Sensor power status is newest!\n");
+        return MPU6050_SUCCESS;
+    }
+
+    databuf[0] = MPU6050_REG_POWER_CTL;   
+    res = i2c_master_send(client, databuf, 0x1);
+    if (res <= 0)
+    {
+        return MPU6050_ERR_I2C;
+    }
+
+    udelay(500);
+
+    databuf[0] = 0x0;        
+    /*
+    res = i2c_master_recv(client, databuf, 1);
+    if (res <= 0)
+    {
+        return MPU6050_ERR_I2C;
+    }
+    */
+
+    databuf[0] &= ~MPU6050_SLEEP;
+
+    if (enable == FALSE)
+    {
+        if (MPU6050_gyro_mode() == false)
+        {
+            databuf[0] |= MPU6050_SLEEP;
+        }
+    }
+    else
+    {
+        // do nothing
+    }
+    databuf[1] = databuf[0];
+    databuf[0] = MPU6050_REG_POWER_CTL;
+
+    res = i2c_master_send(client, databuf, 0x2);
+
+    if (res <= 0)
+    {
+        GSE_LOG("set power mode failed!\n");
+        return MPU6050_ERR_I2C;
+    }
+    else if (atomic_read(&obj->trace) & MPU6050_TRC_INFO)
+    {
+        GSE_LOG("set power mode ok %d!\n", databuf[1]);
+    }
+
+    if (enable == true)
+    {
+        msleep(50);
+    }
+
+    sensor_power = enable;
+    return MPU6050_SUCCESS;    
+}
+/*----------------------------------------------------------------------------*/
+static int MPU6050_SetDataResolution(struct mpu6050_i2c_data *obj)
+{
+    int err;
+    u8  dat, reso;
+
+    if ((err = mpu_i2c_read_block(obj->client, MPU6050_REG_DATA_FORMAT, &dat, 1)))
+    {
+        GSE_ERR("write data format fail!!\n");
+        return err;
+    }
+
+    /*the data_reso is combined by 3 bits: {FULL_RES, DATA_RANGE}*/
+    reso = 0x00;
+    reso = (dat & MPU6050_RANGE_16G) >> 3; 
+
+    if (reso < sizeof(mpu6050_data_resolution)/sizeof(mpu6050_data_resolution[0]))
+    {
+        obj->reso = &mpu6050_data_resolution[reso];
+        return 0;
+    }
+    else
+    {
+        return -EINVAL;
+    }
+}
+/*----------------------------------------------------------------------------*/
+static int MPU6050_ReadData(struct i2c_client *client, s16 data[MPU6050_AXES_NUM])
+{
+    struct mpu6050_i2c_data *priv = i2c_get_clientdata(client);        
+    u8 buf[MPU6050_DATA_LEN] = {0};
+    int err = 0;
+
+
+    if (NULL == client)
+    {
+        return -EINVAL;
+    }
+
+    {
+        /* write then burst read */
+        mpu_i2c_read_block(client, MPU6050_REG_DATAX0, buf, MPU6050_DATA_LEN);
+
+        data[MPU6050_AXIS_X] = (s16)((buf[MPU6050_AXIS_X*2] << 8) |
+                                     (buf[MPU6050_AXIS_X*2+1] ));
+        data[MPU6050_AXIS_Y] = (s16)((buf[MPU6050_AXIS_Y*2] << 8) |
+                                     (buf[MPU6050_AXIS_Y*2+1] ));
+        data[MPU6050_AXIS_Z] = (s16)((buf[MPU6050_AXIS_Z*2] << 8) |
+                                     (buf[MPU6050_AXIS_Z*2+1] ));
+
+        if (atomic_read(&priv->trace) & MPU6050_TRC_RAWDATA)
+        {
+            GSE_LOG("[%08X %08X %08X] => [%5d %5d %5d]\n", data[MPU6050_AXIS_X], data[MPU6050_AXIS_Y], data[MPU6050_AXIS_Z],
+                    data[MPU6050_AXIS_X], data[MPU6050_AXIS_Y], data[MPU6050_AXIS_Z]);
+        }
+#ifdef CONFIG_MPU6050_LOWPASS
+        if (atomic_read(&priv->filter))
+        {
+            if (atomic_read(&priv->fir_en) && !atomic_read(&priv->suspend))
+            {
+                int idx, firlen = atomic_read(&priv->firlen);   
+                if (priv->fir.num < firlen)
+                {
+                    priv->fir.raw[priv->fir.num][MPU6050_AXIS_X] = data[MPU6050_AXIS_X];
+                    priv->fir.raw[priv->fir.num][MPU6050_AXIS_Y] = data[MPU6050_AXIS_Y];
+                    priv->fir.raw[priv->fir.num][MPU6050_AXIS_Z] = data[MPU6050_AXIS_Z];
+                    priv->fir.sum[MPU6050_AXIS_X] += data[MPU6050_AXIS_X];
+                    priv->fir.sum[MPU6050_AXIS_Y] += data[MPU6050_AXIS_Y];
+                    priv->fir.sum[MPU6050_AXIS_Z] += data[MPU6050_AXIS_Z];
+                    if (atomic_read(&priv->trace) & MPU6050_TRC_FILTER)
+                    {
+                        GSE_LOG("add [%2d] [%5d %5d %5d] => [%5d %5d %5d]\n", priv->fir.num,
+                                priv->fir.raw[priv->fir.num][MPU6050_AXIS_X], priv->fir.raw[priv->fir.num][MPU6050_AXIS_Y], priv->fir.raw[priv->fir.num][MPU6050_AXIS_Z],
+                                priv->fir.sum[MPU6050_AXIS_X], priv->fir.sum[MPU6050_AXIS_Y], priv->fir.sum[MPU6050_AXIS_Z]);
+                    }
+                    priv->fir.num++;
+                    priv->fir.idx++;
+                }
+                else
+                {
+                    idx = priv->fir.idx % firlen;
+                    priv->fir.sum[MPU6050_AXIS_X] -= priv->fir.raw[idx][MPU6050_AXIS_X];
+                    priv->fir.sum[MPU6050_AXIS_Y] -= priv->fir.raw[idx][MPU6050_AXIS_Y];
+                    priv->fir.sum[MPU6050_AXIS_Z] -= priv->fir.raw[idx][MPU6050_AXIS_Z];
+                    priv->fir.raw[idx][MPU6050_AXIS_X] = data[MPU6050_AXIS_X];
+                    priv->fir.raw[idx][MPU6050_AXIS_Y] = data[MPU6050_AXIS_Y];
+                    priv->fir.raw[idx][MPU6050_AXIS_Z] = data[MPU6050_AXIS_Z];
+                    priv->fir.sum[MPU6050_AXIS_X] += data[MPU6050_AXIS_X];
+                    priv->fir.sum[MPU6050_AXIS_Y] += data[MPU6050_AXIS_Y];
+                    priv->fir.sum[MPU6050_AXIS_Z] += data[MPU6050_AXIS_Z];
+                    priv->fir.idx++;
+                    data[MPU6050_AXIS_X] = priv->fir.sum[MPU6050_AXIS_X]/firlen;
+                    data[MPU6050_AXIS_Y] = priv->fir.sum[MPU6050_AXIS_Y]/firlen;
+                    data[MPU6050_AXIS_Z] = priv->fir.sum[MPU6050_AXIS_Z]/firlen;
+                    if (atomic_read(&priv->trace) & MPU6050_TRC_FILTER)
+                    {
+                        GSE_LOG("add [%2d] [%5d %5d %5d] => [%5d %5d %5d] : [%5d %5d %5d]\n", idx,
+                                priv->fir.raw[idx][MPU6050_AXIS_X], priv->fir.raw[idx][MPU6050_AXIS_Y], priv->fir.raw[idx][MPU6050_AXIS_Z],
+                                priv->fir.sum[MPU6050_AXIS_X], priv->fir.sum[MPU6050_AXIS_Y], priv->fir.sum[MPU6050_AXIS_Z],
+                                data[MPU6050_AXIS_X], data[MPU6050_AXIS_Y], data[MPU6050_AXIS_Z]);
+                    }
+                }
+            }
+        }
+#endif         
+    }
+    return err;
+}
+/*----------------------------------------------------------------------------*/
+static int MPU6050_ReadOffset(struct i2c_client *client, s8 ofs[MPU6050_AXES_NUM])
+{    
+    int err = 0;
+#ifdef SW_CALIBRATION
+    ofs[0]=ofs[1]=ofs[2]=0x0;
+#else
+    if ((err = mpu_i2c_read_block(client, MPU6050_REG_OFSX, ofs, MPU6050_AXES_NUM)))
+    {
+        GSE_ERR("error: %d\n", err);
+    }
+#endif
+    //GSE_LOG("offesx=%x, y=%x, z=%x",ofs[0],ofs[1],ofs[2]);
+
+    return err;    
+}
+/*----------------------------------------------------------------------------*/
+static int MPU6050_ResetCalibration(struct i2c_client *client)
+{
+    struct mpu6050_i2c_data *obj = i2c_get_clientdata(client);
+#ifndef SW_CALIBRATION
+    s8 ofs[MPU6050_AXES_NUM] = {0x00, 0x00, 0x00};
+#endif
+    int err = 0;
+#ifdef SW_CALIBRATION
+    /* do not thing */
+#else
+
+    if ((err = hwmsen_write_block(client, MPU6050_REG_OFSX, ofs, MPU6050_AXES_NUM)))
+    {
+        GSE_ERR("error: %d\n", err);
+    }
+#endif
+
+    memset(obj->cali_sw, 0x00, sizeof(obj->cali_sw));
+    memset(obj->offset, 0x00, sizeof(obj->offset));
+
+    return err;    
+}
+/*----------------------------------------------------------------------------*/
+static int MPU6050_ReadCalibration(struct i2c_client *client, int dat[MPU6050_AXES_NUM])
+{
+    struct mpu6050_i2c_data *obj = i2c_get_clientdata(client);
+#ifdef SW_CALIBRATION
+    int mul;
+#else
+    int err;
+#endif
+#ifdef SW_CALIBRATION
+    mul = 0;//only SW Calibration, disable HW Calibration
+#else
+
+    if ((err = MPU6050_ReadOffset(client, obj->offset)))
+    {
+        GSE_ERR("read offset fail, %d\n", err);
+        return err;
+    }
+    mul = obj->reso->sensitivity/mpu6050_offset_resolution.sensitivity;
+#endif
+
+    dat[obj->cvt.map[MPU6050_AXIS_X]] = obj->cvt.sign[MPU6050_AXIS_X]*(obj->offset[MPU6050_AXIS_X]*mul*GRAVITY_EARTH_1000/(obj->reso->sensitivity) + obj->cali_sw[MPU6050_AXIS_X]);
+    dat[obj->cvt.map[MPU6050_AXIS_Y]] = obj->cvt.sign[MPU6050_AXIS_Y]*(obj->offset[MPU6050_AXIS_Y]*mul*GRAVITY_EARTH_1000/(obj->reso->sensitivity) + obj->cali_sw[MPU6050_AXIS_Y]);
+    dat[obj->cvt.map[MPU6050_AXIS_Z]] = obj->cvt.sign[MPU6050_AXIS_Z]*(obj->offset[MPU6050_AXIS_Z]*mul*GRAVITY_EARTH_1000/(obj->reso->sensitivity) + obj->cali_sw[MPU6050_AXIS_Z]);                        
+
+    return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int MPU6050_ReadCalibrationEx(struct i2c_client *client, int act[MPU6050_AXES_NUM], int raw[MPU6050_AXES_NUM])
+{  
+    /*raw: the raw calibration data; act: the actual calibration data*/
+    struct mpu6050_i2c_data *obj = i2c_get_clientdata(client);
+#ifdef SW_CALIBRATION
+    int mul;
+#else
+    int err;
+#endif
+#ifdef SW_CALIBRATION
+    mul = 0;//only SW Calibration, disable HW Calibration
+#else
+
+    if ((err = MPU6050_ReadOffset(client, obj->offset)))
+    {
+        GSE_ERR("read offset fail, %d\n", err);
+        return err;
+    }
+    mul = obj->reso->sensitivity/mpu6050_offset_resolution.sensitivity;
+#endif
+
+    raw[MPU6050_AXIS_X] = obj->offset[MPU6050_AXIS_X]*mul*GRAVITY_EARTH_1000/(obj->reso->sensitivity) + obj->cali_sw[MPU6050_AXIS_X];
+    raw[MPU6050_AXIS_Y] = obj->offset[MPU6050_AXIS_Y]*mul*GRAVITY_EARTH_1000/(obj->reso->sensitivity) + obj->cali_sw[MPU6050_AXIS_Y];
+    raw[MPU6050_AXIS_Z] = obj->offset[MPU6050_AXIS_Z]*mul*GRAVITY_EARTH_1000/(obj->reso->sensitivity) + obj->cali_sw[MPU6050_AXIS_Z];
+
+    act[obj->cvt.map[MPU6050_AXIS_X]] = obj->cvt.sign[MPU6050_AXIS_X]*raw[MPU6050_AXIS_X];
+    act[obj->cvt.map[MPU6050_AXIS_Y]] = obj->cvt.sign[MPU6050_AXIS_Y]*raw[MPU6050_AXIS_Y];
+    act[obj->cvt.map[MPU6050_AXIS_Z]] = obj->cvt.sign[MPU6050_AXIS_Z]*raw[MPU6050_AXIS_Z];                        
+
+    return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int MPU6050_WriteCalibration(struct i2c_client *client, int dat[MPU6050_AXES_NUM])
+{
+    struct mpu6050_i2c_data *obj = i2c_get_clientdata(client);
+    int err;
+    int cali[MPU6050_AXES_NUM], raw[MPU6050_AXES_NUM];
+#ifndef SW_CALIBRATION
+    int lsb = mpu6050_offset_resolution.sensitivity;
+    int divisor = obj->reso->sensitivity/lsb;
+#endif
+    if ((err = MPU6050_ReadCalibrationEx(client, cali, raw)))    /*offset will be updated in obj->offset*/
+    {
+        GSE_ERR("read offset fail, %d\n", err);
+        return err;
+    }
+
+    GSE_LOG("OLDOFF: (%+3d %+3d %+3d): (%+3d %+3d %+3d) / (%+3d %+3d %+3d)\n", 
+            raw[MPU6050_AXIS_X], raw[MPU6050_AXIS_Y], raw[MPU6050_AXIS_Z],
+            obj->offset[MPU6050_AXIS_X], obj->offset[MPU6050_AXIS_Y], obj->offset[MPU6050_AXIS_Z],
+            obj->cali_sw[MPU6050_AXIS_X], obj->cali_sw[MPU6050_AXIS_Y], obj->cali_sw[MPU6050_AXIS_Z]);
+
+    /*calculate the real offset expected by caller*/
+	#if 0
+	cali[MPU6050_AXIS_X] = cali[MPU6050_AXIS_X] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+	cali[MPU6050_AXIS_Y] = cali[MPU6050_AXIS_Y] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+	cali[MPU6050_AXIS_Z] = cali[MPU6050_AXIS_Z] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;	
+
+	#endif
+    cali[MPU6050_AXIS_X] += dat[MPU6050_AXIS_X];
+    cali[MPU6050_AXIS_Y] += dat[MPU6050_AXIS_Y];
+    cali[MPU6050_AXIS_Z] += dat[MPU6050_AXIS_Z];
+
+    GSE_LOG("UPDATE: (%+3d %+3d %+3d)\n", 
+            dat[MPU6050_AXIS_X], dat[MPU6050_AXIS_Y], dat[MPU6050_AXIS_Z]);
+#ifdef SW_CALIBRATION
+    obj->cali_sw[MPU6050_AXIS_X] = obj->cvt.sign[MPU6050_AXIS_X]*(cali[obj->cvt.map[MPU6050_AXIS_X]]);
+    obj->cali_sw[MPU6050_AXIS_Y] = obj->cvt.sign[MPU6050_AXIS_Y]*(cali[obj->cvt.map[MPU6050_AXIS_Y]]);
+    obj->cali_sw[MPU6050_AXIS_Z] = obj->cvt.sign[MPU6050_AXIS_Z]*(cali[obj->cvt.map[MPU6050_AXIS_Z]]);  
+#else
+
+    obj->offset[MPU6050_AXIS_X] = (s8)(obj->cvt.sign[MPU6050_AXIS_X]*(cali[obj->cvt.map[MPU6050_AXIS_X]])*(obj->reso->sensitivity)/GRAVITY_EARTH_1000/(divisor));
+    obj->offset[MPU6050_AXIS_Y] = (s8)(obj->cvt.sign[MPU6050_AXIS_Y]*(cali[obj->cvt.map[MPU6050_AXIS_Y]])*(obj->reso->sensitivity)/GRAVITY_EARTH_1000/(divisor));
+    obj->offset[MPU6050_AXIS_Z] = (s8)(obj->cvt.sign[MPU6050_AXIS_Z]*(cali[obj->cvt.map[MPU6050_AXIS_Z]])*(obj->reso->sensitivity)/GRAVITY_EARTH_1000/(divisor));
+
+    /*convert software calibration using standard calibration*/
+    obj->cali_sw[MPU6050_AXIS_X] = obj->cvt.sign[MPU6050_AXIS_X]*(cali[obj->cvt.map[MPU6050_AXIS_X]])%(divisor);
+    obj->cali_sw[MPU6050_AXIS_Y] = obj->cvt.sign[MPU6050_AXIS_Y]*(cali[obj->cvt.map[MPU6050_AXIS_Y]])%(divisor);
+    obj->cali_sw[MPU6050_AXIS_Z] = obj->cvt.sign[MPU6050_AXIS_Z]*(cali[obj->cvt.map[MPU6050_AXIS_Z]])%(divisor);
+
+    GSE_LOG("NEWOFF: (%+3d %+3d %+3d): (%+3d %+3d %+3d) / (%+3d %+3d %+3d)\n", 
+            obj->offset[MPU6050_AXIS_X]*divisor + obj->cali_sw[MPU6050_AXIS_X], 
+            obj->offset[MPU6050_AXIS_Y]*divisor + obj->cali_sw[MPU6050_AXIS_Y], 
+            obj->offset[MPU6050_AXIS_Z]*divisor + obj->cali_sw[MPU6050_AXIS_Z], 
+            obj->offset[MPU6050_AXIS_X], obj->offset[MPU6050_AXIS_Y], obj->offset[MPU6050_AXIS_Z],
+            obj->cali_sw[MPU6050_AXIS_X], obj->cali_sw[MPU6050_AXIS_Y], obj->cali_sw[MPU6050_AXIS_Z]);
+
+    if ((err = hwmsen_write_block(obj->client, MPU6050_REG_OFSX, obj->offset, MPU6050_AXES_NUM)))
+    {
+        GSE_ERR("write offset fail: %d\n", err);
+        return err;
+    }
+#endif
+
+    return err;
+}
+/*----------------------------------------------------------------------------*/
+static int MPU6050_CheckDeviceID(struct i2c_client *client)
+{
+    u8 databuf[10];    
+    int res = 0;
+
+    memset(databuf, 0, sizeof(u8)*10);    
+    databuf[0] = MPU6050_REG_DEVID;    
+
+    res = i2c_master_send(client, databuf, 0x1);
+    if (res <= 0)
+    {
+        goto exit_MPU6050_CheckDeviceID;
+    }
+
+    udelay(500);
+
+    databuf[0] = 0x0;        
+    res = i2c_master_recv(client, databuf, 0x01);
+    if (res <= 0)
+    {
+        goto exit_MPU6050_CheckDeviceID;
+    }
+
+    GSE_LOG("MPU6050_CheckDeviceID 0x%x\n", databuf[0]);
+
+    exit_MPU6050_CheckDeviceID:
+    if (res <= 0)
+    {
+        return MPU6050_ERR_I2C;
+    }
+    return MPU6050_SUCCESS;
+}
+
+/*----------------------------------------------------------------------------*/
+static int MPU6050_SetDataFormat(struct i2c_client *client, u8 dataformat)
+{
+    struct mpu6050_i2c_data *obj = i2c_get_clientdata(client);
+    u8 databuf[2];    
+    int res = 0;
+
+    memset(databuf, 0, sizeof(u8)*2);    
+    databuf[0] = MPU6050_REG_DATA_FORMAT;   
+    res = i2c_master_send(client, databuf, 0x1);
+    if (res <= 0)
+    {
+        return MPU6050_ERR_I2C;
+    }
+
+    udelay(500);
+
+    databuf[0] = 0x0;        
+    res = i2c_master_recv(client, databuf, 0x01);
+    if (res <= 0)
+    {
+        return MPU6050_ERR_I2C;
+    }
+
+    /* write */    
+    databuf[1] = databuf[0] | dataformat;
+    databuf[0] = MPU6050_REG_DATA_FORMAT;   
+    res = i2c_master_send(client, databuf, 0x2);
+
+    if (res <= 0)
+    {
+        return MPU6050_ERR_I2C;
+    }
+    return MPU6050_SetDataResolution(obj);    
+}
+/*----------------------------------------------------------------------------*/
+static int MPU6050_SetBWRate(struct i2c_client *client, u8 bwrate)
+{
+    struct mpu6050_i2c_data *obj = i2c_get_clientdata(client);
+    u8 databuf[10];    
+    int res = 0;
+
+    if( (obj->bandwidth != bwrate) || (atomic_read(&obj->suspend)) )
+    {    
+        memset(databuf, 0, sizeof(u8)*10);    
+    
+        /* read */
+        databuf[0] = MPU6050_REG_BW_RATE;    
+        res = i2c_master_send(client, databuf, 0x1);
+        if (res <= 0)
+        {
+            return MPU6050_ERR_I2C;
+        }
+    
+        udelay(500);
+    
+        databuf[0] = 0x0;        
+        res = i2c_master_recv(client, databuf, 0x01);
+        if (res <= 0)
+        {
+            return MPU6050_ERR_I2C;
+        }
+    
+    
+        /* write */
+        databuf[1] = databuf[0] | bwrate;
+        databuf[0] = MPU6050_REG_BW_RATE;    
+    
+        res = i2c_master_send(client, databuf, 0x2);
+    
+        if (res <= 0)
+        {
+            return MPU6050_ERR_I2C;
+        }
+
+    obj->bandwidth = bwrate;
+    }
+
+    return MPU6050_SUCCESS;    
+}
+
+/*----------------------------------------------------------------------------*/
+static int MPU6050_Dev_Reset(struct i2c_client *client)
+{
+    u8 databuf[10];    
+    int res = 0;
+
+    memset(databuf, 0, sizeof(u8)*10);    
+
+    /* read */
+    databuf[0] = MPU6050_REG_POWER_CTL;    
+    res = i2c_master_send(client, databuf, 0x1);
+    if (res <= 0)
+    {
+        return MPU6050_ERR_I2C;
+    }
+
+    udelay(500);
+
+    databuf[0] = 0x0;        
+    res = i2c_master_recv(client, databuf, 0x01);
+    if (res <= 0)
+    {
+        return MPU6050_ERR_I2C;
+    }
+
+
+    /* write */
+    databuf[1] = databuf[0] | MPU6050_DEV_RESET;
+    databuf[0] = MPU6050_REG_POWER_CTL;    
+
+    res = i2c_master_send(client, databuf, 0x2);
+
+    if (res <= 0)
+    {
+        return MPU6050_ERR_I2C;
+    }
+
+    do
+    {
+        databuf[0] = MPU6050_REG_POWER_CTL;    
+        res = i2c_master_send(client, databuf, 0x1);
+        
+        udelay(500);
+        
+        databuf[0] = 0x0;        
+        res = i2c_master_recv(client, databuf, 0x01);
+
+        printk("[Gsensor] check reset bit");
+
+    }while((databuf[0]&MPU6050_DEV_RESET) != 0);
+
+    msleep(50);
+    return MPU6050_SUCCESS;    
+}
+
+
+/*----------------------------------------------------------------------------*/
+static int MPU6050_Reset(struct i2c_client *client)
+{
+    u8 databuf[10];    
+    int res = 0;
+
+    /* write */
+    databuf[1] = 0x7; /* reset gyro, g-sensor, temperature */
+    databuf[0] = MPU6050_REG_RESET;    
+
+    res = i2c_master_send(client, databuf, 0x2);
+
+    if (res <= 0)
+    {
+        return MPU6050_ERR_I2C;
+    }
+
+    msleep(20);
+    return MPU6050_SUCCESS;    
+}
+
+
+/*----------------------------------------------------------------------------*/
+static int MPU6050_SetIntEnable(struct i2c_client *client, u8 intenable)
+{
+    u8 databuf[2];    
+    int res = 0;
+
+    memset(databuf, 0, sizeof(u8)*2);    
+    databuf[0] = MPU6050_REG_INT_ENABLE;    
+    databuf[1] = intenable;
+
+    res = i2c_master_send(client, databuf, 0x2);
+
+    if (res <= 0)
+    {
+        return MPU6050_ERR_I2C;
+    }
+
+    return MPU6050_SUCCESS;    
+}
+/*----------------------------------------------------------------------------*/
+static int mpu6050_gpio_config(void)
+{
+//because we donot use EINT to support low power
+// config to GPIO input mode + PD 
+
+//set to GPIO_GSE_1_EINT_PIN
+	/*
+	mt_set_gpio_mode(GPIO_GSE_1_EINT_PIN, GPIO_GSE_1_EINT_PIN_M_GPIO);
+	mt_set_gpio_dir(GPIO_GSE_1_EINT_PIN, GPIO_DIR_IN);
+	mt_set_gpio_pull_enable(GPIO_GSE_1_EINT_PIN, GPIO_PULL_ENABLE);
+	mt_set_gpio_pull_select(GPIO_GSE_1_EINT_PIN, GPIO_PULL_DOWN);
+	 */
+//set to GPIO_GSE_2_EINT_PIN
+	/*
+	mt_set_gpio_mode(GPIO_GSE_2_EINT_PIN, GPIO_GSE_2_EINT_PIN_M_GPIO);
+	mt_set_gpio_dir(GPIO_GSE_2_EINT_PIN, GPIO_DIR_IN);
+	mt_set_gpio_pull_enable(GPIO_GSE_2_EINT_PIN, GPIO_PULL_ENABLE);
+	 mt_set_gpio_pull_select(GPIO_GSE_2_EINT_PIN, GPIO_PULL_DOWN);
+	 */
+    return 0;
+}
+
+static int mpu6050_init_client(struct i2c_client *client, int reset_cali)
+{
+    struct mpu6050_i2c_data *obj = i2c_get_clientdata(client);
+    int res = 0;
+    bool sensor_power_org;
+
+    mpu6050_gpio_config();
+
+    sensor_power_org = sensor_power;
+    res = MPU6050_SetPowerMode(client, true);
+    if (res != MPU6050_SUCCESS)
+    {
+        GSE_ERR("set power error\n");
+        return res;
+    }
+    res = MPU6050_CheckDeviceID(client); 
+    if (res != MPU6050_SUCCESS)
+    {
+        GSE_ERR("Check ID error\n");
+        return res;
+    }
+
+    res = MPU6050_SetBWRate(client, MPU6050_BW_184HZ);
+    if (res != MPU6050_SUCCESS ) //0x2C->BW=100Hz
+    {
+        GSE_ERR("set power error\n");
+        return res;
+    }
+
+    res = MPU6050_SetDataFormat(client, MPU6050_RANGE_16G);
+    if (res != MPU6050_SUCCESS) //0x2C->BW=100Hz
+    {
+        GSE_ERR("set data format error\n");
+        return res;
+    }
+
+    gsensor_gain.x = gsensor_gain.y = gsensor_gain.z = obj->reso->sensitivity;
+
+    res = MPU6050_SetIntEnable(client, 0x00);//disable INT
+    if (res != MPU6050_SUCCESS)
+    {
+        GSE_ERR("mpu6050_SetIntEnable error\n");
+        return res;
+    }
+
+    if (0 != reset_cali)
+    {
+        /*reset calibration only in power on*/
+        res = MPU6050_ResetCalibration(client);
+        if (res != MPU6050_SUCCESS)
+        {
+            return res;
+        }
+    }
+
+    res = MPU6050_SetPowerMode(client, sensor_power_org);
+    if (res != MPU6050_SUCCESS)
+    {
+        GSE_ERR("set power error\n");
+        return res;
+    }
+
+#ifdef CONFIG_MPU6050_LOWPASS
+    memset(&obj->fir, 0x00, sizeof(obj->fir));  
+#endif
+
+    return MPU6050_SUCCESS;
+}
+/*----------------------------------------------------------------------------*/
+static int MPU6050_ReadAllReg(struct i2c_client *client, char *buf, int bufsize)
+{   
+    u8 total_len= 0x5C; //(0x75-0x19);
+
+    u8 addr = 0x19;
+    u8 buff[total_len+1];
+    int err = 0;
+    int i;
+
+
+    if (sensor_power == FALSE)
+    {
+        err = MPU6050_SetPowerMode(client, true);
+        if (err)
+        {
+            GSE_ERR("Power on mpu6050 error %d!\n", err);
+        }
+    }
+
+    mpu_i2c_read_block(client, addr, buff, total_len);
+
+    for ( i=0; i<=total_len; i++)
+    {
+        GSE_LOG("MPU6050 reg=0x%x, data=0x%x \n",(addr+i), buff[i]);
+    }
+
+    return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int MPU6050_ReadChipInfo(struct i2c_client *client, char *buf, int bufsize)
+{
+    u8 databuf[10];    
+
+    memset(databuf, 0, sizeof(u8)*10);
+
+    if ((NULL == buf)||(bufsize<=30))
+    {
+        return -1;
+    }
+
+    if (NULL == client)
+    {
+        *buf = 0;
+        return -2;
+    }
+
+    sprintf(buf, "MPU6050 Chip");
+    return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int MPU6050_ReadSensorData(struct i2c_client *client, char *buf, int bufsize)
+{
+    struct mpu6050_i2c_data *obj = obj_i2c_data; //(struct mpu6050_i2c_data*)i2c_get_clientdata(client);
+    int acc[MPU6050_AXES_NUM];
+    int res = 0;
+    client = obj->client;
+
+    if (atomic_read(&obj->suspend))
+    {
+        return -3;
+    }
+
+    if (NULL == buf)
+    {
+        return -1;
+    }
+    if (NULL == client)
+    {
+        *buf = 0;
+        return -2;
+    }
+
+    if (sensor_power == FALSE)
+    {
+        res = MPU6050_SetPowerMode(client, true);
+        if (res)
+        {
+            GSE_ERR("Power on mpu6050 error %d!\n", res);
+        }
+    }
+
+    if ((res = MPU6050_ReadData(client, obj->data)))
+    {
+        GSE_ERR("I2C error: ret value=%d", res);
+        return -3;
+    }
+    else
+    {
+
+	#if 1
+        obj->data[MPU6050_AXIS_X] = obj->data[MPU6050_AXIS_X] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+        obj->data[MPU6050_AXIS_Y] = obj->data[MPU6050_AXIS_Y] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+        obj->data[MPU6050_AXIS_Z] = obj->data[MPU6050_AXIS_Z] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;        
+	#endif
+        obj->data[MPU6050_AXIS_X] += obj->cali_sw[MPU6050_AXIS_X];
+        obj->data[MPU6050_AXIS_Y] += obj->cali_sw[MPU6050_AXIS_Y];
+        obj->data[MPU6050_AXIS_Z] += obj->cali_sw[MPU6050_AXIS_Z];
+
+        /*remap coordinate*/
+        acc[obj->cvt.map[MPU6050_AXIS_X]] = obj->cvt.sign[MPU6050_AXIS_X]*obj->data[MPU6050_AXIS_X];
+        acc[obj->cvt.map[MPU6050_AXIS_Y]] = obj->cvt.sign[MPU6050_AXIS_Y]*obj->data[MPU6050_AXIS_Y];
+        acc[obj->cvt.map[MPU6050_AXIS_Z]] = obj->cvt.sign[MPU6050_AXIS_Z]*obj->data[MPU6050_AXIS_Z];
+		
+        //Out put the mg
+        #if 0
+        acc[MPU6050_AXIS_X] = acc[MPU6050_AXIS_X] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+        acc[MPU6050_AXIS_Y] = acc[MPU6050_AXIS_Y] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+        acc[MPU6050_AXIS_Z] = acc[MPU6050_AXIS_Z] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;        
+		#endif
+        sprintf(buf, "%04x %04x %04x", acc[MPU6050_AXIS_X], acc[MPU6050_AXIS_Y], acc[MPU6050_AXIS_Z]);
+        if (atomic_read(&obj->trace) & MPU6050_TRC_IOCTL)
+        {
+            GSE_LOG("gsensor data: %s!\n", buf);
+        }
+    }
+
+    return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int MPU6050_ReadRawData(struct i2c_client *client, char *buf)
+{
+    struct mpu6050_i2c_data *obj = (struct mpu6050_i2c_data*)i2c_get_clientdata(client);
+    int res = 0;
+
+    if (!buf || !client)
+    {
+        return EINVAL;
+    }
+
+
+    if (atomic_read(&obj->suspend))
+    {
+        return EIO;
+    }
+
+    if ((res = MPU6050_ReadData(client, obj->data)))
+    {
+        GSE_ERR("I2C error: ret value=%d", res);
+        return EIO;
+    }
+    else
+    {
+        sprintf(buf, "%04x %04x %04x", obj->data[MPU6050_AXIS_X], 
+                obj->data[MPU6050_AXIS_Y], obj->data[MPU6050_AXIS_Z]);
+
+    }
+
+    return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int MPU6050_InitSelfTest(struct i2c_client *client)
+{
+    int res = 0;
+    u8  data;
+
+    res = MPU6050_SetPowerMode(client, true);
+    if (res != MPU6050_SUCCESS)
+    {
+        GSE_ERR("set power error\n");
+        return res;
+    }
+    
+    res = MPU6050_SetBWRate(client, MPU6050_BW_184HZ);
+    if (res != MPU6050_SUCCESS ) //0x2C->BW=100Hz
+    {
+        return res;
+    }
+
+    res = mpu_i2c_read_block(client, MPU6050_REG_DATA_FORMAT, &data, 1);
+
+    if (res != MPU6050_SUCCESS)
+    {
+        return res;
+    }
+
+    return MPU6050_SUCCESS;
+}
+/*----------------------------------------------------------------------------*/
+#if !MPU6XXX_HWSELFTEST
+static int MPU6050_JudgeTestResult(struct i2c_client *client, s32 prv[MPU6050_AXES_NUM], s32 nxt[MPU6050_AXES_NUM])
+{
+    struct criteria
+    {
+        int min;
+        int max;
+    };
+
+    struct criteria self[4][3] = {
+        {{ 0, 540}, { 0, 540}, { 0, 875}},
+        {{ 0, 270}, { 0, 270}, { 0, 438}},
+        {{ 0, 135}, { 0, 135}, { 0, 219}},            
+        {{ 0,  67}, { 0,  67}, { 0, 110}},            
+    };
+    struct criteria (*ptr)[3] = NULL;
+    u8 format;
+    int res;
+    if ((res = mpu_i2c_read_block(client, MPU6050_REG_DATA_FORMAT, &format, 1)))
+        return res;
+
+    format = format & MPU6050_RANGE_16G;
+
+    switch (format)
+    {
+        case MPU6050_RANGE_2G:
+            GSE_LOG("format use self[0]\n");
+            ptr = &self[0];       
+        break;
+
+        case MPU6050_RANGE_4G:
+            GSE_LOG("format use self[1]\n");
+            ptr = &self[1];       
+        break;
+    
+        case MPU6050_RANGE_8G:
+            GSE_LOG("format use self[2]\n");
+            ptr = &self[2];       
+        break;
+    
+        case MPU6050_RANGE_16G:
+            GSE_LOG("format use self[3]\n");
+            ptr = &self[3];       
+        break;
+    
+        default:
+            GSE_LOG("format unknow use \n");
+        break;
+    }
+
+    if (!ptr)
+    {
+        GSE_ERR("null pointer\n");
+        return -EINVAL;
+    }
+    GSE_LOG("format=0x%x\n",format);
+
+    GSE_LOG("X diff is %ld\n",abs(nxt[MPU6050_AXIS_X] - prv[MPU6050_AXIS_X]));
+    GSE_LOG("Y diff is %ld\n",abs(nxt[MPU6050_AXIS_Y] - prv[MPU6050_AXIS_Y]));
+    GSE_LOG("Z diff is %ld\n",abs(nxt[MPU6050_AXIS_Z] - prv[MPU6050_AXIS_Z]));
+
+
+    if ((abs(nxt[MPU6050_AXIS_X] - prv[MPU6050_AXIS_X]) > (*ptr)[MPU6050_AXIS_X].max) ||
+        (abs(nxt[MPU6050_AXIS_X] - prv[MPU6050_AXIS_X]) < (*ptr)[MPU6050_AXIS_X].min))
+    {
+        GSE_ERR("X is over range\n");
+        res = -EINVAL;
+    }
+    if ((abs(nxt[MPU6050_AXIS_Y] - prv[MPU6050_AXIS_Y]) > (*ptr)[MPU6050_AXIS_Y].max) ||
+        (abs(nxt[MPU6050_AXIS_Y] - prv[MPU6050_AXIS_Y]) < (*ptr)[MPU6050_AXIS_Y].min))
+    {
+        GSE_ERR("Y is over range\n");
+        res = -EINVAL;
+    }
+    if ((abs(nxt[MPU6050_AXIS_Z] - prv[MPU6050_AXIS_Z]) > (*ptr)[MPU6050_AXIS_Z].max) ||
+        (abs(nxt[MPU6050_AXIS_Z] - prv[MPU6050_AXIS_Z]) < (*ptr)[MPU6050_AXIS_Z].min))
+    {
+        GSE_ERR("Z is over range\n");
+        res = -EINVAL;
+    }
+    return res;
+}
+#endif
+/*----------------------------------------------------------------------------*/
+static ssize_t show_chipinfo_value(struct device_driver *ddri, char *buf)
+{
+    struct i2c_client *client = mpu6050_i2c_client;
+    char strbuf[MPU6050_BUFSIZE];
+    if (NULL == client)
+    {
+        GSE_ERR("i2c client is null!!\n");
+        return 0;
+    }
+
+    if (sensor_power == false)
+    {
+        MPU6050_SetPowerMode(client, true);
+    }
+	
+    MPU6050_ReadAllReg(client, strbuf, MPU6050_BUFSIZE);
+
+    MPU6050_ReadChipInfo(client, strbuf, MPU6050_BUFSIZE);
+    return snprintf(buf, PAGE_SIZE, "%s\n", strbuf);        
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t show_sensordata_value(struct device_driver *ddri, char *buf)
+{
+    struct i2c_client *client = mpu6050_i2c_client;
+    char strbuf[MPU6050_BUFSIZE];
+
+    if (NULL == client)
+    {
+        GSE_ERR("i2c client is null!!\n");
+        return 0;
+    }
+    MPU6050_ReadSensorData(client, strbuf, MPU6050_BUFSIZE);
+    return snprintf(buf, PAGE_SIZE, "%s\n", strbuf);            
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t show_cali_value(struct device_driver *ddri, char *buf)
+{
+    struct i2c_client *client = mpu6050_i2c_client;
+    struct mpu6050_i2c_data *obj;
+    int err, len = 0, mul;
+    int tmp[MPU6050_AXES_NUM];
+
+    if (NULL == client)
+    {
+        GSE_ERR("i2c client is null!!\n");
+        return 0;
+    }
+
+    obj = i2c_get_clientdata(client);
+
+
+    if ((err = MPU6050_ReadOffset(client, obj->offset)))
+    {
+        return -EINVAL;
+    }
+    else if ((err = MPU6050_ReadCalibration(client, tmp)))
+    {
+        return -EINVAL;
+    }
+    else
+    {
+        mul = obj->reso->sensitivity/mpu6050_offset_resolution.sensitivity;
+        len += snprintf(buf+len, PAGE_SIZE-len, "[HW ][%d] (%+3d, %+3d, %+3d) : (0x%02X, 0x%02X, 0x%02X)\n", mul,                        
+                        obj->offset[MPU6050_AXIS_X], obj->offset[MPU6050_AXIS_Y], obj->offset[MPU6050_AXIS_Z],
+                        obj->offset[MPU6050_AXIS_X], obj->offset[MPU6050_AXIS_Y], obj->offset[MPU6050_AXIS_Z]);
+        len += snprintf(buf+len, PAGE_SIZE-len, "[SW ][%d] (%+3d, %+3d, %+3d)\n", 1, 
+                        obj->cali_sw[MPU6050_AXIS_X], obj->cali_sw[MPU6050_AXIS_Y], obj->cali_sw[MPU6050_AXIS_Z]);
+
+        len += snprintf(buf+len, PAGE_SIZE-len, "[ALL]    (%+3d, %+3d, %+3d) : (%+3d, %+3d, %+3d)\n", 
+                        obj->offset[MPU6050_AXIS_X]*mul + obj->cali_sw[MPU6050_AXIS_X],
+                        obj->offset[MPU6050_AXIS_Y]*mul + obj->cali_sw[MPU6050_AXIS_Y],
+                        obj->offset[MPU6050_AXIS_Z]*mul + obj->cali_sw[MPU6050_AXIS_Z],
+                        tmp[MPU6050_AXIS_X], tmp[MPU6050_AXIS_Y], tmp[MPU6050_AXIS_Z]);
+
+        return len;
+    }
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t store_cali_value(struct device_driver *ddri, const char *buf, size_t count)
+{
+    struct i2c_client *client = mpu6050_i2c_client;  
+    int err, x, y, z;
+    int dat[MPU6050_AXES_NUM];
+
+    if (!strncmp(buf, "rst", 3))
+    {
+        if ((err = MPU6050_ResetCalibration(client)))
+        {
+            GSE_ERR("reset offset err = %d\n", err);
+        }
+    }
+    else if (3 == sscanf(buf, "0x%02X 0x%02X 0x%02X", &x, &y, &z))
+    {
+        dat[MPU6050_AXIS_X] = x;
+        dat[MPU6050_AXIS_Y] = y;
+        dat[MPU6050_AXIS_Z] = z;
+        if ((err = MPU6050_WriteCalibration(client, dat)))
+        {
+            GSE_ERR("write calibration err = %d\n", err);
+        }
+    }
+    else
+    {
+        GSE_ERR("invalid format\n");
+    }
+
+    return count;
+}
+/*----------------------------------------------------------------------------*/
+#if MPU6XXX_HWSELFTEST
+extern int inv_hw_self_test(struct inv_selftest_device *st);
+int selftestret=0;
+
+static ssize_t show_self_value(struct device_driver *ddri, char *buf)
+{
+    struct i2c_client *client = mpu6050_i2c_client;
+    struct mpu6050_i2c_data *obj = i2c_get_clientdata(client);
+    int len = 0;
+
+    if (NULL == client)
+    {
+        GSE_ERR("i2c client is null!!\n");
+        return 0;
+    }
+
+    if(selftestret & 0x1)
+        len += snprintf(buf+len, PAGE_SIZE-len, "Self-Test result=%d- Gyro = PASS \t",selftestret);
+    else
+        len += snprintf(buf+len, PAGE_SIZE-len, "Self-Test result=%d- Gyro = FAIL \t",selftestret);
+    if(selftestret & 0x2)
+        len += snprintf(buf+len, PAGE_SIZE-len, "Accel = PASS\n");
+    else
+        len += snprintf(buf+len, PAGE_SIZE-len, "Accel = FAIL\n");
+
+    len += snprintf(buf+len, PAGE_SIZE-len, "[gyro_bias](%+3d, %+3d, %+3d)\n",obj->mpu_selftest_device.gyro_bias[MPU6050_AXIS_X], obj->mpu_selftest_device.gyro_bias[MPU6050_AXIS_Y], obj->mpu_selftest_device.gyro_bias[MPU6050_AXIS_Z]);
+    len += snprintf(buf+len, PAGE_SIZE-len, "[accel_bias](%+3d, %+3d, %+3d)\n",obj->mpu_selftest_device.accel_bias[MPU6050_AXIS_X], obj->mpu_selftest_device.accel_bias[MPU6050_AXIS_Y], obj->mpu_selftest_device.accel_bias[MPU6050_AXIS_Z]);
+    len += snprintf(buf+len, PAGE_SIZE-len, "[gyro_bias_st](%+3d, %+3d, %+3d)\n",obj->mpu_selftest_device.gyro_bias_st[MPU6050_AXIS_X], obj->mpu_selftest_device.gyro_bias_st[MPU6050_AXIS_Y], obj->mpu_selftest_device.gyro_bias_st[MPU6050_AXIS_Z]);
+    len += snprintf(buf+len, PAGE_SIZE-len, "[accel_bias_st](%+3d, %+3d, %+3d)\n",obj->mpu_selftest_device.accel_bias_st[MPU6050_AXIS_X], obj->mpu_selftest_device.accel_bias_st[MPU6050_AXIS_Y], obj->mpu_selftest_device.accel_bias_st[MPU6050_AXIS_Z]);
+    return len;
+}
+/*----------------------------------------------------------------------------*/
+
+static ssize_t store_self_value(struct device_driver *ddri, const char *buf, size_t count)
+{
+    struct i2c_client *client = mpu6050_i2c_client;  
+    struct mpu6050_i2c_data *priv = i2c_get_clientdata(client);        
+    int ret;
+    //u8 databuf[2] = {0};    
+    //u8 data;
+#if 1    
+    MPU6050_SetPowerMode(client,true);
+
+    ret = inv_hw_self_test(&(priv->mpu_selftest_device));
+    selftestret = ret;
+    if (ret == 3)
+    {
+        GSE_LOG("SELFTEST : PASS\n");
+        strcpy(selftestRes,"y");
+    }
+    else
+    {
+        GSE_LOG("SELFTEST : FAIL\n");
+        strcpy(selftestRes,"n");
+    }
+#endif
+    return count;	
+}
+#else
+/*----------------------------------------------------------------------------*/
+static ssize_t show_self_value(struct device_driver *ddri, char *buf)
+{
+    struct i2c_client *client = mpu6050_i2c_client;
+
+    if (NULL == client)
+    {
+        GSE_ERR("i2c client is null!!\n");
+        return 0;
+    }
+
+    return snprintf(buf, 8, "%s\n", selftestRes);
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t store_self_value(struct device_driver *ddri, const char *buf, size_t count)
+{   /*write anything to this register will trigger the process*/
+    struct item
+    {
+        s16 raw[MPU6050_AXES_NUM];
+    };
+
+    struct i2c_client *client = mpu6050_i2c_client;  
+    int idx, res, num;
+    struct item *prv = NULL, *nxt = NULL;
+    s32 avg_prv[MPU6050_AXES_NUM] = {0, 0, 0};
+    s32 avg_nxt[MPU6050_AXES_NUM] = {0, 0, 0};
+
+
+    if (1 != sscanf(buf, "%d", &num))
+    {
+        GSE_ERR("parse number fail\n");
+        return count;
+    }
+    else if (num == 0)
+    {
+        GSE_ERR("invalid data count\n");
+        return count;
+    }
+
+    prv = kzalloc(sizeof(*prv) * num, GFP_KERNEL);
+    nxt = kzalloc(sizeof(*nxt) * num, GFP_KERNEL);
+    if (!prv || !nxt)
+    {
+        goto exit;
+    }
+
+
+    GSE_LOG("NORMAL:\n");
+    MPU6050_SetPowerMode(client,true);
+
+    for (idx = 0; idx < num; idx++)
+    {
+        if ((res = MPU6050_ReadData(client, prv[idx].raw)))
+        {
+            GSE_ERR("read data fail: %d\n", res);
+            goto exit;
+        }
+
+        avg_prv[MPU6050_AXIS_X] += prv[idx].raw[MPU6050_AXIS_X];
+        avg_prv[MPU6050_AXIS_Y] += prv[idx].raw[MPU6050_AXIS_Y];
+        avg_prv[MPU6050_AXIS_Z] += prv[idx].raw[MPU6050_AXIS_Z];        
+        GSE_LOG("[%5d %5d %5d]\n", prv[idx].raw[MPU6050_AXIS_X], prv[idx].raw[MPU6050_AXIS_Y], prv[idx].raw[MPU6050_AXIS_Z]);
+    }
+
+    avg_prv[MPU6050_AXIS_X] /= num;
+    avg_prv[MPU6050_AXIS_Y] /= num;
+    avg_prv[MPU6050_AXIS_Z] /= num;    
+
+    /*initial setting for self test*/
+    GSE_LOG("SELFTEST:\n");    
+    for (idx = 0; idx < num; idx++)
+    {
+        if ((res = MPU6050_ReadData(client, nxt[idx].raw)))
+        {
+            GSE_ERR("read data fail: %d\n", res);
+            goto exit;
+        }
+        avg_nxt[MPU6050_AXIS_X] += nxt[idx].raw[MPU6050_AXIS_X];
+        avg_nxt[MPU6050_AXIS_Y] += nxt[idx].raw[MPU6050_AXIS_Y];
+        avg_nxt[MPU6050_AXIS_Z] += nxt[idx].raw[MPU6050_AXIS_Z];        
+        GSE_LOG("[%5d %5d %5d]\n", nxt[idx].raw[MPU6050_AXIS_X], nxt[idx].raw[MPU6050_AXIS_Y], nxt[idx].raw[MPU6050_AXIS_Z]);
+    }
+
+    avg_nxt[MPU6050_AXIS_X] /= num;
+    avg_nxt[MPU6050_AXIS_Y] /= num;
+    avg_nxt[MPU6050_AXIS_Z] /= num;    
+
+    GSE_LOG("X: %5d - %5d = %5d \n", avg_nxt[MPU6050_AXIS_X], avg_prv[MPU6050_AXIS_X], avg_nxt[MPU6050_AXIS_X] - avg_prv[MPU6050_AXIS_X]);
+    GSE_LOG("Y: %5d - %5d = %5d \n", avg_nxt[MPU6050_AXIS_Y], avg_prv[MPU6050_AXIS_Y], avg_nxt[MPU6050_AXIS_Y] - avg_prv[MPU6050_AXIS_Y]);
+    GSE_LOG("Z: %5d - %5d = %5d \n", avg_nxt[MPU6050_AXIS_Z], avg_prv[MPU6050_AXIS_Z], avg_nxt[MPU6050_AXIS_Z] - avg_prv[MPU6050_AXIS_Z]); 
+
+    if (!MPU6050_JudgeTestResult(client, avg_prv, avg_nxt))
+    {
+        GSE_LOG("SELFTEST : PASS\n");
+        strcpy(selftestRes,"y");
+    }
+    else
+    {
+        GSE_LOG("SELFTEST : FAIL\n");
+        strcpy(selftestRes,"n");
+    }
+
+    exit:
+    /*restore the setting*/    
+    mpu6050_init_client(client, 0);
+    kfree(prv);
+    kfree(nxt);
+    return count;
+}
+#endif
+/*----------------------------------------------------------------------------*/
+static ssize_t show_selftest_value(struct device_driver *ddri, char *buf)
+{
+    struct i2c_client *client = mpu6050_i2c_client;
+    struct mpu6050_i2c_data *obj;
+
+    if (NULL == client)
+    {
+        GSE_ERR("i2c client is null!!\n");
+        return 0;
+    }
+
+    obj = i2c_get_clientdata(client);
+    return snprintf(buf, PAGE_SIZE, "%d\n", atomic_read(&obj->selftest));
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t store_selftest_value(struct device_driver *ddri, const char *buf, size_t count)
+{
+    struct mpu6050_i2c_data *obj = obj_i2c_data;
+    int tmp;
+
+    if (NULL == obj)
+    {
+        GSE_ERR("i2c data obj is null!!\n");
+        return 0;
+    }
+
+
+    if (1 == sscanf(buf, "%d", &tmp))
+    {
+        if (atomic_read(&obj->selftest) && !tmp)
+        {
+            /*enable -> disable*/
+            mpu6050_init_client(obj->client, 0);
+        }
+        else if (!atomic_read(&obj->selftest) && tmp)
+        {
+            /*disable -> enable*/
+            MPU6050_InitSelfTest(obj->client);            
+        }
+
+        GSE_LOG("selftest: %d => %d\n", atomic_read(&obj->selftest), tmp);
+        atomic_set(&obj->selftest, tmp); 
+    }
+    else
+    {
+        GSE_ERR("invalid content: '%s', length = %zu\n", buf, count);   
+    }
+    return count;
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t show_firlen_value(struct device_driver *ddri, char *buf)
+{
+#ifdef CONFIG_MPU6050_LOWPASS
+    struct i2c_client *client = mpu6050_i2c_client;
+    struct mpu6050_i2c_data *obj = i2c_get_clientdata(client);
+    if (atomic_read(&obj->firlen))
+    {
+        int idx, len = atomic_read(&obj->firlen);
+        GSE_LOG("len = %2d, idx = %2d\n", obj->fir.num, obj->fir.idx);
+
+        for (idx = 0; idx < len; idx++)
+        {
+            GSE_LOG("[%5d %5d %5d]\n", obj->fir.raw[idx][MPU6050_AXIS_X], obj->fir.raw[idx][MPU6050_AXIS_Y], obj->fir.raw[idx][MPU6050_AXIS_Z]);
+        }
+
+        GSE_LOG("sum = [%5d %5d %5d]\n", obj->fir.sum[MPU6050_AXIS_X], obj->fir.sum[MPU6050_AXIS_Y], obj->fir.sum[MPU6050_AXIS_Z]);
+        GSE_LOG("avg = [%5d %5d %5d]\n", obj->fir.sum[MPU6050_AXIS_X]/len, obj->fir.sum[MPU6050_AXIS_Y]/len, obj->fir.sum[MPU6050_AXIS_Z]/len);
+    }
+    return snprintf(buf, PAGE_SIZE, "%d\n", atomic_read(&obj->firlen));
+#else
+    return snprintf(buf, PAGE_SIZE, "not support\n");
+#endif
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t store_firlen_value(struct device_driver *ddri, const char *buf, size_t count)
+{
+#ifdef CONFIG_MPU6050_LOWPASS
+    struct i2c_client *client = mpu6050_i2c_client;  
+    struct mpu6050_i2c_data *obj = i2c_get_clientdata(client);
+    int firlen;
+
+    if (1 != sscanf(buf, "%d", &firlen))
+    {
+        GSE_ERR("invallid format\n");
+    }
+    else if (firlen > C_MAX_FIR_LENGTH)
+    {
+        GSE_ERR("exceeds maximum filter length\n");
+    }
+    else
+    {
+        atomic_set(&obj->firlen, firlen);
+        if (0 == firlen)
+        {
+            atomic_set(&obj->fir_en, 0);
+        }
+        else
+        {
+            memset(&obj->fir, 0x00, sizeof(obj->fir));
+            atomic_set(&obj->fir_en, 1);
+        }
+    }
+#endif    
+    return count;
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t show_trace_value(struct device_driver *ddri, char *buf)
+{
+    ssize_t res;
+    struct mpu6050_i2c_data *obj = obj_i2c_data;
+    if (obj == NULL)
+    {
+        GSE_ERR("i2c_data obj is null!!\n");
+        return 0;
+    }
+
+    res = snprintf(buf, PAGE_SIZE, "0x%04X\n", atomic_read(&obj->trace));     
+    return res;    
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t store_trace_value(struct device_driver *ddri, const char *buf, size_t count)
+{
+    struct mpu6050_i2c_data *obj = obj_i2c_data;
+    int trace;
+    if (obj == NULL)
+    {
+        GSE_ERR("i2c_data obj is null!!\n");
+        return 0;
+    }
+
+    if (1 == sscanf(buf, "0x%x", &trace))
+    {
+        atomic_set(&obj->trace, trace);
+    }
+    else
+    {
+        GSE_ERR("invalid content: '%s', length = %zu\n", buf, count);
+    }
+
+    return count;    
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t show_status_value(struct device_driver *ddri, char *buf)
+{
+    ssize_t len = 0;    
+    struct mpu6050_i2c_data *obj = obj_i2c_data;
+    if (obj == NULL)
+    {
+        GSE_ERR("i2c_data obj is null!!\n");
+        return 0;
+    }
+
+    if (obj->hw)
+    {
+        len += snprintf(buf+len, PAGE_SIZE-len, "CUST: %d %d (%d %d)\n", 
+                        obj->hw->i2c_num, obj->hw->direction, obj->hw->power_id, obj->hw->power_vol);   
+    }
+    else
+    {
+        len += snprintf(buf+len, PAGE_SIZE-len, "CUST: NULL\n");
+    }
+    return len;    
+}
+/*----------------------------------------------------------------------------*/
+static DRIVER_ATTR(chipinfo,             S_IRUGO, show_chipinfo_value,      NULL);
+static DRIVER_ATTR(sensordata,           S_IRUGO, show_sensordata_value,    NULL);
+static DRIVER_ATTR(cali,       S_IWUSR | S_IRUGO, show_cali_value,          store_cali_value);
+static DRIVER_ATTR(self,       S_IWUSR | S_IRUGO, show_selftest_value,          store_selftest_value);
+static DRIVER_ATTR(selftest,   S_IWUSR | S_IRUGO, show_self_value ,      store_self_value );
+static DRIVER_ATTR(firlen,     S_IWUSR | S_IRUGO, show_firlen_value,        store_firlen_value);
+static DRIVER_ATTR(trace,      S_IWUSR | S_IRUGO, show_trace_value,         store_trace_value);
+static DRIVER_ATTR(status,               S_IRUGO, show_status_value,        NULL);
+/*----------------------------------------------------------------------------*/
+static struct driver_attribute *mpu6050_attr_list[] = {
+    &driver_attr_chipinfo,     /*chip information*/
+    &driver_attr_sensordata,   /*dump sensor data*/
+    &driver_attr_cali,         /*show calibration data*/
+    &driver_attr_self,         /*self test demo*/
+    &driver_attr_selftest,     /*self control: 0: disable, 1: enable*/
+    &driver_attr_firlen,       /*filter length: 0: disable, others: enable*/
+    &driver_attr_trace,        /*trace log*/
+    &driver_attr_status,        
+};
+/*----------------------------------------------------------------------------*/
+static int mpu6050_create_attr(struct device_driver *driver) 
+{
+    int idx, err = 0;
+    int num = (int)(sizeof(mpu6050_attr_list)/sizeof(mpu6050_attr_list[0]));
+    if (driver == NULL)
+    {
+        return -EINVAL;
+    }
+
+    for (idx = 0; idx < num; idx++)
+    {
+        if (0 != (err = driver_create_file(driver, mpu6050_attr_list[idx])))
+        {
+            GSE_ERR("driver_create_file (%s) = %d\n", mpu6050_attr_list[idx]->attr.name, err);
+            break;
+        }
+    }    
+    return err;
+}
+/*----------------------------------------------------------------------------*/
+static int mpu6050_delete_attr(struct device_driver *driver)
+{
+    int idx ,err = 0;
+    int num = (int)(sizeof(mpu6050_attr_list)/sizeof(mpu6050_attr_list[0]));
+
+    if (driver == NULL)
+    {
+        return -EINVAL;
+    }
+
+    for (idx = 0; idx < num; idx++)
+    {
+        driver_remove_file(driver, mpu6050_attr_list[idx]);
+    }
+
+    return err;
+}
+
+/*----------------------------------------------------------------------------*/
+int gsensor_operate(void* self, uint32_t command, void* buff_in, int size_in,
+                    void* buff_out, int size_out, int* actualout)
+{
+    int err = 0;
+    int value, sample_delay;    
+    struct mpu6050_i2c_data *priv = (struct mpu6050_i2c_data*)self;
+    hwm_sensor_data* gsensor_data;
+    char buff[MPU6050_BUFSIZE];
+
+
+    switch (command)
+    {
+    case SENSOR_DELAY:
+        if ((buff_in == NULL) || (size_in < sizeof(int)))
+        {
+            GSE_ERR("Set delay parameter error!\n");
+            err = -EINVAL;
+        }
+        else
+        {
+            value = *(int *)buff_in;
+            
+            if(value <= 5)
+            {
+            	sample_delay = MPU6050_BW_184HZ;
+            }
+            else if(value <= 10)
+            {
+            	sample_delay = MPU6050_BW_94HZ;
+            }
+            else
+            {
+            	sample_delay = MPU6050_BW_44HZ;
+            }
+            GSE_LOG("Set delay parameter value:%d \n", value);
+        
+
+            err = MPU6050_SetBWRate(priv->client, sample_delay);
+            if (err != MPU6050_SUCCESS ) //0x2C->BW=100Hz
+            {
+                GSE_ERR("Set delay parameter error!\n");
+            }
+
+            if (value >= 50)
+            {
+                atomic_set(&priv->filter, 0);
+            }
+            else
+            {
+#if defined(CONFIG_MPU6050_LOWPASS)				
+                priv->fir.num = 0;
+                priv->fir.idx = 0;
+                priv->fir.sum[MPU6050_AXIS_X] = 0;
+                priv->fir.sum[MPU6050_AXIS_Y] = 0;
+                priv->fir.sum[MPU6050_AXIS_Z] = 0;
+#endif
+                atomic_set(&priv->filter, 1);
+            }
+        }
+        break;
+
+    case SENSOR_ENABLE:
+        if ((buff_in == NULL) || (size_in < sizeof(int)))
+        {
+            GSE_ERR("Enable sensor parameter error!\n");
+            err = -EINVAL;
+        }
+        else
+        {
+            value = *(int *)buff_in;
+            if (((value == 0) && (sensor_power == false)) ||((value == 1) && (sensor_power == true)))
+            {
+                GSE_LOG("Gsensor device have updated!\n");
+            }
+            else
+            {
+                err = MPU6050_SetPowerMode( priv->client, !sensor_power);
+            }
+        }
+        break;
+
+    case SENSOR_GET_DATA:
+        if ((buff_out == NULL) || (size_out< sizeof(hwm_sensor_data)))
+        {
+            GSE_ERR("get sensor data parameter error!\n");
+            err = -EINVAL;
+        }
+        else
+        {
+            gsensor_data = (hwm_sensor_data *)buff_out;
+            err = MPU6050_ReadSensorData(priv->client, buff, MPU6050_BUFSIZE);
+            if (!err)
+            {
+                sscanf(buff, "%x %x %x", &gsensor_data->values[0], 
+                       &gsensor_data->values[1], &gsensor_data->values[2]);             
+                gsensor_data->status = SENSOR_STATUS_ACCURACY_MEDIUM;                
+                gsensor_data->value_divide = 1000;
+            }
+        }
+        break;
+    default:
+        GSE_ERR("gsensor operate function no this parameter %d!\n", command);
+        err = -1;
+        break;
+    }
+
+    return err;
+}
+
+/****************************************************************************** 
+ * Function Configuration
+******************************************************************************/
+static int mpu6050_open(struct inode *inode, struct file *file)
+{
+    file->private_data = mpu6050_i2c_client;
+
+    if (file->private_data == NULL)
+    {
+        GSE_ERR("null pointer!!\n");
+        return -EINVAL;
+    }
+    return nonseekable_open(inode, file);
+}
+/*----------------------------------------------------------------------------*/
+static int mpu6050_release(struct inode *inode, struct file *file)
+{
+    file->private_data = NULL;
+    return 0;
+}
+/*----------------------------------------------------------------------------*/
+static long mpu6050_unlocked_ioctl(struct file *file, unsigned int cmd,
+       unsigned long arg)
+{
+    struct i2c_client *client = (struct i2c_client*)file->private_data;
+    struct mpu6050_i2c_data *obj = (struct mpu6050_i2c_data*)i2c_get_clientdata(client);    
+    char strbuf[MPU6050_BUFSIZE];
+    void __user *data;
+    SENSOR_DATA sensor_data;
+    long err = 0;
+    int cali[3];
+
+	memset(strbuf, 0, MPU6050_BUFSIZE);
+
+    if (_IOC_DIR(cmd) & _IOC_READ)
+    {
+        err = !access_ok(VERIFY_WRITE, (void __user *)arg, _IOC_SIZE(cmd));
+    }
+    else if (_IOC_DIR(cmd) & _IOC_WRITE)
+    {
+        err = !access_ok(VERIFY_READ, (void __user *)arg, _IOC_SIZE(cmd));
+    }
+
+    if (err)
+    {
+        GSE_ERR("access error: %08X, (%2d, %2d)\n", cmd, _IOC_DIR(cmd), _IOC_SIZE(cmd));
+        return -EFAULT;
+    }
+
+    switch (cmd)
+    {
+    case GSENSOR_IOCTL_INIT:
+        mpu6050_init_client(client, 0);         
+        break;
+
+    case GSENSOR_IOCTL_READ_CHIPINFO:
+        data = (void __user *) arg;
+        if (data == NULL)
+        {
+            err = -EINVAL;
+            break;    
+        }
+
+        MPU6050_ReadChipInfo(client, strbuf, MPU6050_BUFSIZE);
+        if (copy_to_user(data, strbuf, strlen(strbuf)+1))
+        {
+            err = -EFAULT;
+            break;
+        }
+        break;    
+
+    case GSENSOR_IOCTL_READ_SENSORDATA:
+        data = (void __user *) arg;
+        if (data == NULL)
+        {
+            err = -EINVAL;
+            break;    
+        }
+
+        MPU6050_ReadSensorData(client, strbuf, MPU6050_BUFSIZE);
+        if (copy_to_user(data, strbuf, strlen(strbuf)+1))
+        {
+            err = -EFAULT;
+            break;    
+        }
+        break;
+
+    case GSENSOR_IOCTL_READ_GAIN:
+        data = (void __user *) arg;
+        if (data == NULL)
+        {
+            err = -EINVAL;
+            break;    
+        }
+
+        if (copy_to_user(data, &gsensor_gain, sizeof(GSENSOR_VECTOR3D)))
+        {
+            err = -EFAULT;
+            break;
+        }
+        break;
+
+    case GSENSOR_IOCTL_READ_RAW_DATA:
+        data = (void __user *) arg;
+        if (data == NULL)
+        {
+            err = -EINVAL;
+            break;    
+        }
+
+        if (atomic_read(&obj->suspend))
+        {
+            err = -EINVAL;
+        }
+        else
+        {
+        MPU6050_ReadRawData(client, strbuf);
+        if (copy_to_user(data, strbuf, strlen(strbuf)+1))
+        {
+            err = -EFAULT;
+            break;    
+        }
+        }
+        break;    
+
+    case GSENSOR_IOCTL_SET_CALI:
+        data = (void __user*)arg;
+        if (data == NULL)
+        {
+            err = -EINVAL;
+            break;    
+        }
+        if (copy_from_user(&sensor_data, data, sizeof(sensor_data)))
+        {
+            err = -EFAULT;
+            break;    
+        }
+        if (atomic_read(&obj->suspend))
+        {
+            GSE_ERR("Perform calibration in suspend state!!\n");
+            err = -EINVAL;
+        }
+        else
+        {
+        #if 0
+            cali[MPU6050_AXIS_X] = sensor_data.x * obj->reso->sensitivity / GRAVITY_EARTH_1000;
+            cali[MPU6050_AXIS_Y] = sensor_data.y * obj->reso->sensitivity / GRAVITY_EARTH_1000;
+            cali[MPU6050_AXIS_Z] = sensor_data.z * obj->reso->sensitivity / GRAVITY_EARTH_1000;           
+		#else
+			cali[MPU6050_AXIS_X] = sensor_data.x;
+            cali[MPU6050_AXIS_Y] = sensor_data.y;
+            cali[MPU6050_AXIS_Z] = sensor_data.z;           
+		
+		#endif
+			err = MPU6050_WriteCalibration(client, cali);            
+        }
+        break;
+
+    case GSENSOR_IOCTL_CLR_CALI:
+        err = MPU6050_ResetCalibration(client);
+        break;
+
+    case GSENSOR_IOCTL_GET_CALI:
+        data = (void __user*)arg;
+        if (data == NULL)
+        {
+            err = -EINVAL;
+            break;    
+        }
+        if ((err = MPU6050_ReadCalibration(client, cali)))
+        {
+            break;
+        }
+		#if 0
+        sensor_data.x = cali[MPU6050_AXIS_X] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+        sensor_data.y = cali[MPU6050_AXIS_Y] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+        sensor_data.z = cali[MPU6050_AXIS_Z] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+		#else
+		sensor_data.x = cali[MPU6050_AXIS_X];
+        sensor_data.y = cali[MPU6050_AXIS_Y];
+        sensor_data.z = cali[MPU6050_AXIS_Z];
+		
+		#endif
+		if (copy_to_user(data, &sensor_data, sizeof(sensor_data)))
+        {
+            err = -EFAULT;
+            break;
+        }
+        break;
+
+
+    default:
+        GSE_ERR("unknown IOCTL: 0x%08x\n", cmd);
+        err = -ENOIOCTLCMD;
+        break;
+
+    }
+
+    return err;
+}
+
+
+/*----------------------------------------------------------------------------*/
+static struct file_operations mpu6050_fops = {
+    .open = mpu6050_open,
+    .release = mpu6050_release,
+    .unlocked_ioctl = mpu6050_unlocked_ioctl,
+};
+/*----------------------------------------------------------------------------*/
+static struct miscdevice mpu6050_device = {
+    .minor = MISC_DYNAMIC_MINOR,
+    .name = "gsensor",
+    .fops = &mpu6050_fops,
+};
+/*----------------------------------------------------------------------------*/
+#ifndef USE_EARLY_SUSPEND
+/*----------------------------------------------------------------------------*/
+static int mpu6050_suspend(struct i2c_client *client, pm_message_t msg) 
+{
+    struct mpu6050_i2c_data *obj = i2c_get_clientdata(client);    
+    int err = 0;
+    GSE_FUN();    
+
+    if (msg.event == PM_EVENT_SUSPEND)
+    {
+        if (obj == NULL)
+        {
+            GSE_ERR("null pointer!!\n");
+            return -EINVAL;
+        }
+        atomic_set(&obj->suspend, 1);
+
+        if ((err = MPU6050_SetPowerMode(obj->client, false)))
+        {
+            GSE_ERR("write power control fail!!\n");
+            return err;
+        }
+        MPU6050_power(obj->hw, 0);
+        GSE_LOG("mpu6050_suspend ok\n");
+    }
+    return err;
+}
+/*----------------------------------------------------------------------------*/
+static int mpu6050_resume(struct i2c_client *client)
+{
+    struct mpu6050_i2c_data *obj = i2c_get_clientdata(client);        
+    int err;
+    GSE_FUN();
+
+    if (obj == NULL)
+    {
+        GSE_ERR("null pointer!!\n");
+        return -EINVAL;
+    }
+
+    MPU6050_power(obj->hw, 1);
+
+    if ((err = mpu6050_init_client(client, 0)))
+    {
+        GSE_ERR("initialize client fail!!\n");
+        return err;        
+    }
+    atomic_set(&obj->suspend, 0);
+    GSE_LOG("mpu6050_resume ok\n");
+
+    return 0;
+}
+/*----------------------------------------------------------------------------*/
+#else /*CONFIG_HAS_EARLY_SUSPEND is defined*/
+/*----------------------------------------------------------------------------*/
+static void mpu6050_early_suspend(struct early_suspend *h) 
+{
+    struct mpu6050_i2c_data *obj = container_of(h, struct mpu6050_i2c_data, early_drv);   
+    int err;
+    GSE_FUN();    
+
+    if (obj == NULL)
+    {
+        GSE_ERR("null pointer!!\n");
+        return;
+    }
+    atomic_set(&obj->suspend, 1); 
+
+    if ((err = MPU6050_SetPowerMode(obj->client, false)))
+    {
+        GSE_ERR("write power control fail!!\n");
+        return;
+    }
+
+    /*
+    if (MPU6050_gyro_mode() == false)
+    {
+        MPU6050_Dev_Reset(obj->client);
+        MPU6050_Reset(obj->client);
+    }
+    */
+
+    obj->bandwidth = 0;
+
+    sensor_power = false;
+
+    MPU6050_power(obj->hw, 0);
+}
+/*----------------------------------------------------------------------------*/
+static void mpu6050_late_resume(struct early_suspend *h)
+{
+    struct mpu6050_i2c_data *obj = container_of(h, struct mpu6050_i2c_data, early_drv);         
+    int err;
+    GSE_FUN();
+
+    if (obj == NULL)
+    {
+        GSE_ERR("null pointer!!\n");
+        return;
+    }
+
+    MPU6050_power(obj->hw, 1);
+
+    if ((err = mpu6050_init_client(obj->client, 0)))
+    {
+        GSE_ERR("initialize client fail!!\n");
+        return;        
+    }
+    atomic_set(&obj->suspend, 0);    
+}
+/*----------------------------------------------------------------------------*/
+#endif /*CONFIG_HAS_EARLYSUSPEND*/
+/*----------------------------------------------------------------------------*/
+static int mpu6050_i2c_detect(struct i2c_client *client, struct i2c_board_info *info) 
+{    
+    strcpy(info->type, MPU6050_DEV_NAME);
+    return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int mpu6050_i2c_probe(struct i2c_client *client, const struct i2c_device_id *id)
+{
+    struct i2c_client *new_client;
+    struct mpu6050_i2c_data *obj;
+    struct hwmsen_object sobj;
+    int err = 0;
+    GSE_FUN();
+
+    if (!(obj = kzalloc(sizeof(*obj), GFP_KERNEL)))
+    {
+        err = -ENOMEM;
+        goto exit;
+    }
+
+    memset(obj, 0, sizeof(struct mpu6050_i2c_data));
+
+    obj->hw = get_cust_acc_hw();
+
+    if ((err = hwmsen_get_convert(obj->hw->direction, &obj->cvt)))
+    {
+        GSE_ERR("invalid direction: %d\n", obj->hw->direction);
+        goto exit;
+    }
+
+    obj_i2c_data = obj;
+    obj->client = client;
+    obj->client->timing = 400;
+
+    new_client = obj->client;
+    i2c_set_clientdata(new_client,obj);
+
+    atomic_set(&obj->trace, 0);
+    atomic_set(&obj->suspend, 0);
+
+#ifdef CONFIG_MPU6050_LOWPASS
+    if (obj->hw->firlen > C_MAX_FIR_LENGTH)
+    {
+        atomic_set(&obj->firlen, C_MAX_FIR_LENGTH);
+    }
+    else
+    {
+        atomic_set(&obj->firlen, obj->hw->firlen);
+    }
+
+    if (atomic_read(&obj->firlen) > 0)
+    {
+        atomic_set(&obj->fir_en, 1);
+    }
+
+#endif
+
+    mpu6050_i2c_client = new_client;    
+    MPU6050_Dev_Reset(new_client);
+    MPU6050_Reset(new_client);
+
+    if ((err = mpu6050_init_client(new_client, 1)))
+    {
+        goto exit_init_failed;
+    }
+
+
+    if ((err = misc_register(&mpu6050_device)))
+    {
+        GSE_ERR("mpu6050_device register failed\n");
+        goto exit_misc_device_register_failed;
+    }
+
+
+    if ((err = mpu6050_create_attr(&mpu6050_gsensor_driver.driver)))
+    {
+        GSE_ERR("create attribute err = %d\n", err);
+        goto exit_create_attr_failed;
+    }
+
+    sobj.self = obj;
+    sobj.polling = 1;
+    sobj.sensor_operate = gsensor_operate;
+    if ((err = hwmsen_attach(ID_ACCELEROMETER, &sobj)))
+    {
+        GSE_ERR("attach fail = %d\n", err);
+        goto exit_kfree;
+    }
+
+#ifdef USE_EARLY_SUSPEND
+    obj->early_drv.level    = EARLY_SUSPEND_LEVEL_STOP_DRAWING - 2,
+    obj->early_drv.suspend  = mpu6050_early_suspend,
+    obj->early_drv.resume   = mpu6050_late_resume,    
+    register_early_suspend(&obj->early_drv);
+#endif 
+
+#if MPU6XXX_HWSELFTEST
+    obj->mpu_selftest_device.name = "MPU6XXX";
+    obj->mpu_selftest_device.chip_type = INV_MPU6500;//MPU6515 don't need to modify this chip_type
+    obj->mpu_selftest_device.samples = INIT_ST_SAMPLES;
+#endif
+
+    GSE_LOG("%s: OK\n", __func__);    
+    return 0;
+
+    exit_create_attr_failed:
+    misc_deregister(&mpu6050_device);
+    exit_misc_device_register_failed:
+    exit_init_failed:
+    //i2c_detach_client(new_client);
+    exit_kfree:
+    kfree(obj);
+    exit:
+    GSE_ERR("%s: err = %d\n", __func__, err);        
+    return err;
+}
+
+/*----------------------------------------------------------------------------*/
+static int mpu6050_i2c_remove(struct i2c_client *client)
+{
+    int err = 0;    
+
+    if ((err =  mpu6050_delete_attr(&mpu6050_gsensor_driver.driver)))
+    {
+        GSE_ERR("mpu6050_delete_attr fail: %d\n", err);
+    }
+
+    if ((err = misc_deregister(&mpu6050_device)))
+    {
+        GSE_ERR("misc_deregister fail: %d\n", err);
+    }
+
+    if ((err = hwmsen_detach(ID_ACCELEROMETER)))
+    {
+        GSE_ERR("hwmsen_detach fail: %d\n", err);
+    }
+
+    mpu6050_i2c_client = NULL;
+    i2c_unregister_device(client);
+    kfree(i2c_get_clientdata(client));
+    return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int mpu6050_probe(struct platform_device *pdev) 
+{
+    struct acc_hw *hw = get_cust_acc_hw();
+    GSE_FUN();
+
+    MPU6050_power(hw, 1);
+    if (i2c_add_driver(&mpu6050_i2c_driver))
+    {
+        GSE_ERR("add driver error\n");
+        return -1;
+    }
+    return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int mpu6050_remove(struct platform_device *pdev)
+{
+    struct acc_hw *hw = get_cust_acc_hw();
+
+    GSE_FUN();    
+    MPU6050_power(hw, 0);    
+    i2c_del_driver(&mpu6050_i2c_driver);
+    return 0;
+}
+/*----------------------------------------------------------------------------*/
+#if 1
+#ifdef CONFIG_OF
+	static const struct of_device_id gsensor_of_match[] = {
+		{ .compatible = "mediatek,gsensor", },
+		{},
+	};
+#endif
+	
+	static struct platform_driver mpu6050_gsensor_driver = {
+		.probe		= mpu6050_probe,
+		.remove 	= mpu6050_remove,	 
+		.driver 	= 
+		{
+			.name  = "gsensor",
+		//	.owner  = THIS_MODULE,
+        #ifdef CONFIG_OF
+			.of_match_table = gsensor_of_match,
+		#endif
+		}
+	};
+#else
+
+static struct platform_driver mpu6050_gsensor_driver = {
+    .probe      = mpu6050_probe,
+    .remove     = mpu6050_remove,    
+    .driver     = {
+        .name  = "gsensor",
+    }
+};
+#endif
+/*----------------------------------------------------------------------------*/
+static int __init mpu6050gse_init(void)
+{
+    struct acc_hw *hw = get_cust_acc_hw();
+	GSE_LOG("%s: i2c_number=%d\n", __func__,hw->i2c_num); 
+    i2c_register_board_info(hw->i2c_num, &i2c_mpu6050, 1);
+    if (platform_driver_register(&mpu6050_gsensor_driver))
+    {
+        GSE_ERR("failed to register driver");
+        return -ENODEV;
+    }
+    return 0;    
+}
+/*----------------------------------------------------------------------------*/
+static void __exit mpu6050gse_exit(void)
+{
+    GSE_FUN();
+    platform_driver_unregister(&mpu6050_gsensor_driver);
+}
+/*----------------------------------------------------------------------------*/
+module_init(mpu6050gse_init);
+module_exit(mpu6050gse_exit);
+/*----------------------------------------------------------------------------*/
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("MPU6050 gse driver");
+MODULE_AUTHOR("Yucong.Xiong@mediatek.com");
diff --git a/drivers/misc/mediatek/accelerometer/mpu6050g/mpu6050.h b/drivers/misc/mediatek/accelerometer/mpu6050g/mpu6050.h
new file mode 100644
index 000000000..0d0993f22
--- /dev/null
+++ b/drivers/misc/mediatek/accelerometer/mpu6050g/mpu6050.h
@@ -0,0 +1,60 @@
+#ifndef MPU6050_H
+#define MPU6050_H
+
+#include <linux/ioctl.h>
+
+#define MPU6050_I2C_SLAVE_ADDR		0xD0
+
+
+/* MPU6050 Register Map  (Please refer to MPU6050 Specifications) */
+#define MPU6050_REG_DEVID			0x75
+#define	MPU6050_REG_BW_RATE			0x1A
+#define MPU6050_REG_POWER_CTL                   0x6B 
+#define MPU6050_REG_POWER_CTL2                  0x6C
+#define MPU6050_REG_INT_ENABLE		0x38 
+#define MPU6050_REG_DATA_FORMAT		0x1C 
+#define MPU6050_REG_DATAX0			0x3B 
+#define MPU6050_REG_DATAY0			0x3D
+#define MPU6050_REG_DATAZ0			0x3F
+#define MPU6050_REG_RESET               0x68
+
+/* register Value */ 
+#define MPU6050_FIXED_DEVID         0x68 // or 0x69
+													
+                                           // delay(ms)	
+#define MPU6050_BW_260HZ            0x00   //0     
+#define MPU6050_BW_184HZ            0x01   //2.0     
+#define MPU6050_BW_94HZ             0x02   //3.0   
+#define MPU6050_BW_44HZ             0x03   //4.9   
+#define MPU6050_BW_21HZ             0x04   //8.5   
+#define MPU6050_BW_10HZ             0x05   //13.8  
+#define MPU6050_BW_5HZ              0x06   //19.0  
+
+#define MPU6050_DEV_RESET           0x80
+
+//#define MPU6050_FULL_RES			0x08
+#define MPU6050_RANGE_2G			(0x00 << 3)
+#define MPU6050_RANGE_4G			(0x01 << 3)
+#define MPU6050_RANGE_8G			(0x02 << 3)
+#define MPU6050_RANGE_16G			(0x03 << 3)
+//#define MPU6050_SELF_TEST         0x80
+
+
+#define MPU6050_SLEEP				0x40	//enable low power sleep mode
+
+
+
+// below do not modify	 
+#define MPU6050_SUCCESS                     0
+#define MPU6050_ERR_I2C                     -1
+#define MPU6050_ERR_STATUS                  -3
+#define MPU6050_ERR_SETUP_FAILURE           -4
+#define MPU6050_ERR_GETGSENSORDATA          -5
+#define MPU6050_ERR_IDENTIFICATION          -6
+
+
+
+#define MPU6050_BUFSIZE				256  
+
+#endif
+
diff --git a/drivers/misc/mediatek/accelerometer/mpu6050g/mpu6xxx_hwselftest.c b/drivers/misc/mediatek/accelerometer/mpu6050g/mpu6xxx_hwselftest.c
new file mode 100644
index 000000000..c173ab0b1
--- /dev/null
+++ b/drivers/misc/mediatek/accelerometer/mpu6050g/mpu6xxx_hwselftest.c
@@ -0,0 +1,480 @@
+#include <linux/interrupt.h>
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include <linux/irq.h>
+#include <linux/miscdevice.h>
+#include <asm/uaccess.h>
+#include <linux/delay.h>
+#include <linux/input.h>
+#include <linux/workqueue.h>
+#include <linux/kobject.h>
+#include <linux/earlysuspend.h>
+#include <linux/platform_device.h>
+#include <asm/atomic.h>
+
+#include <cust_acc.h>
+#include <linux/hwmsensor.h>
+#include <linux/hwmsen_dev.h>
+#include <linux/sensors_io.h>
+#include "mpu6050.h"
+#include "mpu6xxx_hwselftest.h"
+#include <linux/hwmsen_helper.h>
+
+#include <mach/mt_typedefs.h>
+#include <mach/mt_gpio.h>
+#include <mach/mt_pm_ldo.h>
+
+extern int MPU6050_i2c_master_send(u8 *buf, u8 len);
+extern int MPU6050_hwmsen_read_block(u8 addr, u8 *buf, u8 len);
+
+//#define pr_debug printk
+
+#if MPU6XXX_HWSELFTEST==1
+
+#define inv_i2c_read(st, reg, len, data) \
+	MPU6050_hwmsen_read_block(reg, data, len)
+#define inv_i2c_single_write(st, reg, data) \
+	inv_i2c_single_write_base(reg, data)
+
+int inv_i2c_single_write_base(u8 reg, u8 data)
+{
+    u8 databuf[2] = {0};
+    int res = 0;
+
+    databuf[1] = data;
+    databuf[0] = reg;
+
+//#ifdef MPU6050_ACCESS_BY_GSE_I2C
+    res = MPU6050_i2c_master_send(databuf, 0x2);
+//#else
+//    res = i2c_master_send(client, databuf, 0x2);
+//#endif
+
+    if (res <= 0)
+    {
+        return MPU6050_ERR_I2C;
+    }
+    else
+    {
+        return MPU6050_SUCCESS;
+    }
+
+}
+
+/**
+* inv_check_6500_gyro_self_test() - check 6500 gyro self test. this function
+*                                   returns zero as success. A non-zero return
+*                                   value indicates failure in self test.
+*  @*st: main data structure.
+*  @*reg_avg: average value of normal test.
+*  @*st_avg:  average value of self test
+*/
+static int inv_check_6500_gyro_self_test(struct inv_selftest_device *st,
+						int *reg_avg, int *st_avg) {
+	u8 regs[3];
+	int ret_val, result;
+	int otp_value_zero = 0;
+	int st_shift_prod[3], st_shift_cust[3], i;
+
+	ret_val = 0;
+	result = inv_i2c_read(st, REG_6500_XG_ST_DATA, 3, regs);
+	if (result)
+		return result;
+	pr_debug("%s self_test gyro shift_code - %02x %02x %02x\n",
+		 st->name, regs[0], regs[1], regs[2]);
+
+	for (i = 0; i < 3; i++) {
+		if (regs[i] != 0) {
+			st_shift_prod[i] = mpu_6500_st_tb[regs[i] - 1];
+		} else {
+			st_shift_prod[i] = 0;
+			otp_value_zero = 1;
+		}
+	}
+	pr_debug("%s self_test gyro st_shift_prod - %+d %+d %+d\n",
+		 st->name, st_shift_prod[0], st_shift_prod[1],
+		 st_shift_prod[2]);
+
+	for (i = 0; i < 3; i++) {
+		st_shift_cust[i] = st_avg[i] - reg_avg[i];
+		if (!otp_value_zero) {
+			/* Self Test Pass/Fail Criteria A */
+			if (st_shift_cust[i] < DEF_6500_GYRO_CT_SHIFT_DELTA
+						* st_shift_prod[i])
+			{
+					ret_val = 1;
+					pr_debug("%s, Fail, A\n", __func__);
+			}
+		} else {
+			/* Self Test Pass/Fail Criteria B */
+			if (st_shift_cust[i] < DEF_GYRO_ST_AL *
+						DEF_SELFTEST_GYRO_SENS *
+						DEF_ST_PRECISION)
+			{
+				ret_val = 1;
+				pr_debug("%s, Fail, B\n", __func__);
+			}
+		}
+	}
+	pr_debug("%s self_test gyro st_shift_cust - %+d %+d %+d\n",
+		 st->name, st_shift_cust[0], st_shift_cust[1],
+		 st_shift_cust[2]);
+
+	if (ret_val == 0) {
+		/* Self Test Pass/Fail Criteria C */
+		for (i = 0; i < 3; i++)
+			if (abs(reg_avg[i]) > DEF_GYRO_OFFSET_MAX *
+						DEF_SELFTEST_GYRO_SENS *
+						DEF_ST_PRECISION)
+			{
+				ret_val = 1;
+				pr_debug("%s, Fail, C\n", __func__);
+			}
+	}
+
+	return ret_val;
+}
+
+/**
+* inv_check_6500_accel_self_test() - check 6500 accel self test. this function
+*                                   returns zero as success. A non-zero return
+*                                   value indicates failure in self test.
+*  @*st: main data structure.
+*  @*reg_avg: average value of normal test.
+*  @*st_avg:  average value of self test
+*/
+static int inv_check_6500_accel_self_test(struct inv_selftest_device *st,
+						int *reg_avg, int *st_avg) {
+	int ret_val, result;
+	int st_shift_prod[3], st_shift_cust[3], st_shift_ratio[3], i;
+	u8 regs[3];
+	int otp_value_zero = 0;
+
+#define ACCEL_ST_AL_MIN ((DEF_ACCEL_ST_AL_MIN * DEF_ST_SCALE \
+				 / DEF_ST_6500_ACCEL_FS_MG) * DEF_ST_PRECISION)
+#define ACCEL_ST_AL_MAX ((DEF_ACCEL_ST_AL_MAX * DEF_ST_SCALE \
+				 / DEF_ST_6500_ACCEL_FS_MG) * DEF_ST_PRECISION)
+
+	ret_val = 0;
+	result = inv_i2c_read(st, REG_6500_XA_ST_DATA, 3, regs);
+	if (result)
+		return result;
+	pr_debug("%s self_test accel shift_code - %02x %02x %02x\n",
+		 st->name, regs[0], regs[1], regs[2]);
+
+	for (i = 0; i < 3; i++) {
+		if (regs[i] != 0) {
+			st_shift_prod[i] = mpu_6500_st_tb[regs[i] - 1];
+		} else {
+			st_shift_prod[i] = 0;
+			otp_value_zero = 1;
+		}
+	}
+	pr_debug("%s self_test accel st_shift_prod - %+d %+d %+d\n",
+		 st->name, st_shift_prod[0], st_shift_prod[1],
+		 st_shift_prod[2]);
+
+	if (!otp_value_zero) {
+		/* Self Test Pass/Fail Criteria A */
+		for (i = 0; i < 3; i++) {
+			st_shift_cust[i] = st_avg[i] - reg_avg[i];
+			st_shift_ratio[i] = abs(st_shift_cust[i] /
+					st_shift_prod[i] - DEF_ST_PRECISION);
+			if (st_shift_ratio[i] > DEF_6500_ACCEL_ST_SHIFT_DELTA)
+			{
+				ret_val = 1;
+				pr_debug("%s, Fail, A\n", __func__);
+			}
+		}
+	} else {
+		/* Self Test Pass/Fail Criteria B */
+		for (i = 0; i < 3; i++) {
+			st_shift_cust[i] = abs(st_avg[i] - reg_avg[i]);
+			if (st_shift_cust[i] < ACCEL_ST_AL_MIN ||
+					st_shift_cust[i] > ACCEL_ST_AL_MAX)
+			{
+				ret_val = 1;
+				pr_debug("%s, Fail, B\n", __func__);
+			}
+		}
+	}
+	pr_debug("%s self_test accel st_shift_cust - %+d %+d %+d\n",
+		 st->name, st_shift_cust[0], st_shift_cust[1],
+		 st_shift_cust[2]);
+
+	return ret_val;
+}
+
+/*
+ *  inv_do_test() - do the actual test of self testing
+ */
+static int inv_do_test(struct inv_selftest_device *st, int self_test_flag,
+		int *gyro_result, int *accel_result)
+{
+	int result, i, j, packet_size;
+	u8 data[BYTES_PER_SENSOR * 2], d;
+	int fifo_count, packet_count, ind, s;
+
+	packet_size = BYTES_PER_SENSOR * 2;
+
+	result = inv_i2c_single_write(st, REG_INT_ENABLE, 0);
+	if (result)
+		return result;
+	/* disable the sensor output to FIFO */
+	result = inv_i2c_single_write(st, REG_FIFO_EN, 0);
+	if (result)
+		return result;
+	/* disable fifo reading */
+	result = inv_i2c_single_write(st, REG_USER_CTRL, 0);
+	if (result)
+		return result;
+	/* clear FIFO */
+	result = inv_i2c_single_write(st, REG_USER_CTRL, BIT_FIFO_RST);
+	if (result)
+		return result;
+	/* setup parameters */
+	result = inv_i2c_single_write(st, REG_CONFIG, INV_FILTER_98HZ);
+	if (result)
+		return result;
+
+	if (INV_MPU6500 == st->chip_type) {
+		/* config accel LPF register for MPU6500 */
+		result = inv_i2c_single_write(st, REG_6500_ACCEL_CONFIG2,
+						DEF_ST_MPU6500_ACCEL_LPF |
+						BIT_FIFO_SIZE_1K);
+		if (result)
+			return result;
+	}
+
+	result = inv_i2c_single_write(st, REG_SAMPLE_RATE_DIV,
+			DEF_SELFTEST_SAMPLE_RATE);
+	if (result)
+		return result;
+	/* wait for the sampling rate change to stabilize */
+	mdelay(INV_MPU_SAMPLE_RATE_CHANGE_STABLE);
+	result = inv_i2c_single_write(st, REG_GYRO_CONFIG,
+		self_test_flag | DEF_SELFTEST_GYRO_FS);
+	if (result)
+		return result;
+	if (INV_MPU6500 == st->chip_type)
+		d = DEF_SELFTEST_6500_ACCEL_FS;
+	else
+		d = DEF_SELFTEST_ACCEL_FS;
+	d |= self_test_flag;
+	result = inv_i2c_single_write(st, REG_ACCEL_CONFIG, d);
+	if (result)
+		return result;
+	/* wait for the output to get stable */
+	if (self_test_flag) {
+		if (INV_MPU6500 == st->chip_type)
+			msleep(DEF_ST_6500_STABLE_TIME);
+		else
+			msleep(DEF_ST_STABLE_TIME);
+	}
+
+	/* enable FIFO reading */
+	result = inv_i2c_single_write(st, REG_USER_CTRL, BIT_FIFO_EN);
+	if (result)
+		return result;
+	/* enable sensor output to FIFO */
+	d = BITS_GYRO_OUT | BIT_ACCEL_OUT;
+	for (i = 0; i < THREE_AXIS; i++) {
+		gyro_result[i] = 0;
+		accel_result[i] = 0;
+	}
+	s = 0;
+	while (s < st->samples) {
+		result = inv_i2c_single_write(st, REG_FIFO_EN, d);
+		if (result)
+			return result;
+		mdelay(DEF_GYRO_WAIT_TIME);
+		result = inv_i2c_single_write(st, REG_FIFO_EN, 0);
+		if (result)
+			return result;
+
+		result = inv_i2c_read(st, REG_FIFO_COUNT_H,
+					FIFO_COUNT_BYTE, data);
+		if (result)
+			return result;
+		fifo_count = be16_to_cpup((__be16 *)(&data[0]));
+		pr_debug("%s self_test fifo_count - %d\n",
+			 st->name, fifo_count);
+		packet_count = fifo_count / packet_size;
+		i = 0;
+		while ((i < packet_count) && (s < st->samples)) {
+			short vals[3];
+			result = inv_i2c_read(st, REG_FIFO_R_W,
+				packet_size/2, data);
+			if (result)
+				return result;
+			ind = 0;
+			for (j = 0; j < THREE_AXIS; j++) {
+				vals[j] = (short)be16_to_cpup(
+				    (__be16 *)(&data[ind + 2 * j]));
+				accel_result[j] += vals[j];
+			}
+			ind += BYTES_PER_SENSOR;
+//			pr_debug(
+//			    "%s self_test accel data - %d %+d %+d %+d",
+//			    st->name, s, vals[0], vals[1], vals[2]);
+
+			result = inv_i2c_read(st, REG_FIFO_R_W,
+				packet_size/2, data+packet_size/2);
+			if (result)
+				return result;
+			for (j = 0; j < THREE_AXIS; j++) {
+				vals[j] = (short)be16_to_cpup(
+					(__be16 *)(&data[ind + 2 * j]));
+				gyro_result[j] += vals[j];
+			}
+//			pr_debug("%s self_test gyro data - %d %+d %+d %+d",
+//				 st->name, s, vals[0], vals[1], vals[2]);
+
+			s++;
+			i++;
+		}
+	}
+
+	for (j = 0; j < THREE_AXIS; j++) {
+		accel_result[j] = accel_result[j] / s;
+		accel_result[j] *= DEF_ST_PRECISION;
+	}
+	for (j = 0; j < THREE_AXIS; j++) {
+		gyro_result[j] = gyro_result[j] / s;
+		gyro_result[j] *= DEF_ST_PRECISION;
+	}
+
+	return 0;
+}
+
+/*
+ *  inv_store_setting() store the old settings before selftest
+ */
+static void inv_store_setting(struct inv_selftest_device *st)
+{
+	int result;
+	u8 data;
+
+	result = inv_i2c_read(st, REG_SAMPLE_RATE_DIV, 1, &data);
+	if (result)
+		pr_debug("%s read REG_SAMPLE_RATE_DIV fail\n",st->name);
+	else st->sample_rate_div = data;
+
+	result = inv_i2c_read(st, REG_CONFIG, 1, &data);
+	if (result)
+		pr_debug("%s read REG_CONFIG fail\n",st->name);
+	else st->config = data;
+
+	result = inv_i2c_read(st, REG_GYRO_CONFIG, 1, &data);
+	if (result)
+		pr_debug("%s read REG_GYRO_CONFIG fail\n",st->name);
+	else st->gyro_config = data;
+
+	result = inv_i2c_read(st, REG_ACCEL_CONFIG, 1, &data);
+	if (result)
+		pr_debug("%s read REG_ACCEL_CONFIG fail\n",st->name);
+	else st->accel_config = data;
+}
+
+/*
+ *  inv_recover_setting() recover the old settings after everything is done
+ */
+static void inv_recover_setting(struct inv_selftest_device *st)
+{
+	inv_i2c_single_write(st, REG_SAMPLE_RATE_DIV, st->sample_rate_div);
+	inv_i2c_single_write(st, REG_CONFIG, st->config);
+	inv_i2c_single_write(st, REG_GYRO_CONFIG, st->gyro_config);
+	inv_i2c_single_write(st, REG_ACCEL_CONFIG, st->accel_config);
+}
+
+/*
+ *  inv_hw_self_test() - main function to do hardware self test
+ */
+int inv_hw_self_test(struct inv_selftest_device *st)
+{
+	int result;
+	int gyro_bias_st[THREE_AXIS], gyro_bias_regular[THREE_AXIS];
+	int accel_bias_st[THREE_AXIS], accel_bias_regular[THREE_AXIS];
+	int test_times, i;
+	char compass_result, accel_result, gyro_result;
+
+	inv_store_setting(st);
+//	result = inv_power_up_self_test(st);
+//	if (result)
+//		return result;
+	compass_result = 0;
+	accel_result = 0;
+	gyro_result = 0;
+	test_times = DEF_ST_TRY_TIMES;
+	while (test_times > 0) {
+		result = inv_do_test(st, 0, gyro_bias_regular,
+			accel_bias_regular);
+		if (result == -EAGAIN)
+			test_times--;
+		else
+			test_times = 0;
+	}
+	if (result)
+		goto test_fail;
+	pr_debug("%s self_test accel bias_regular - %+d %+d %+d\n",
+		 st->name, accel_bias_regular[0],
+		 accel_bias_regular[1], accel_bias_regular[2]);
+	pr_debug("%s self_test gyro bias_regular - %+d %+d %+d\n",
+		 st->name, gyro_bias_regular[0], gyro_bias_regular[1],
+		 gyro_bias_regular[2]);
+
+	for (i = 0; i < 3; i++) {
+		st->gyro_bias[i] = gyro_bias_regular[i];
+		st->accel_bias[i] = accel_bias_regular[i];
+	}
+
+	test_times = DEF_ST_TRY_TIMES;
+	while (test_times > 0) {
+		result = inv_do_test(st, BITS_SELF_TEST_EN, gyro_bias_st,
+					accel_bias_st);
+		if (result == -EAGAIN)
+			test_times--;
+		else
+			break;
+	}
+	if (result)
+		goto test_fail;
+	pr_debug("%s self_test accel bias_st - %+d %+d %+d\n",
+		 st->name, accel_bias_st[0], accel_bias_st[1],
+		 accel_bias_st[2]);
+	pr_debug("%s self_test gyro bias_st - %+d %+d %+d\n",
+		 st->name, gyro_bias_st[0], gyro_bias_st[1],
+		 gyro_bias_st[2]);
+
+	for (i = 0; i < 3; i++) {
+		st->gyro_bias_st[i] = gyro_bias_st[i];
+		st->accel_bias_st[i] = accel_bias_st[i];
+	}
+    //TBD compass selftest
+//	if (st->chip_config.has_compass)
+//		compass_result = !st->slave_compass->self_test(st);
+
+	 if (INV_MPU6050 == st->chip_type) {
+//		accel_result = !inv_check_accel_self_test(st,
+//			accel_bias_regular, accel_bias_st);
+//		gyro_result = !inv_check_6050_gyro_self_test(st,
+//			gyro_bias_regular, gyro_bias_st);
+	} else if (INV_MPU6500 == st->chip_type) {
+		accel_result = !inv_check_6500_accel_self_test(st,
+			accel_bias_regular, accel_bias_st);
+		gyro_result = !inv_check_6500_gyro_self_test(st,
+			gyro_bias_regular, gyro_bias_st);
+	}
+
+test_fail:
+	inv_recover_setting(st);
+
+	result = (compass_result << DEF_ST_COMPASS_RESULT_SHIFT) |
+		(accel_result << DEF_ST_ACCEL_RESULT_SHIFT) | gyro_result;
+	pr_debug("%s, result=0x%x\n", __func__, result);
+	//1:PASS 0:FAIL
+	return result;
+}
+
+#endif//#if MPU6XXX_HWSELFTEST==1
+
diff --git a/drivers/misc/mediatek/accelerometer/mpu6050g/mpu6xxx_hwselftest.h b/drivers/misc/mediatek/accelerometer/mpu6050g/mpu6xxx_hwselftest.h
new file mode 100644
index 000000000..ec0e1f4cf
--- /dev/null
+++ b/drivers/misc/mediatek/accelerometer/mpu6050g/mpu6xxx_hwselftest.h
@@ -0,0 +1,145 @@
+
+#ifndef MPU6XXX_HWSELFTEST_H
+#define MPU6XXX_HWSELFTEST_H
+
+#define MPU6XXX_HWSELFTEST  1
+
+#define REG_6500_XG_ST_DATA     0x0
+#define REG_6500_XA_ST_DATA     0xD
+#define REG_6500_XA_OFFS_H      0x77
+#define REG_6500_YA_OFFS_H      0x7A
+#define REG_6500_ZA_OFFS_H      0x7D
+#define REG_6500_ACCEL_CONFIG2  0x1D
+#define BIT_ACCEL_FCHOCIE_B              0x08
+#define BIT_FIFO_SIZE_1K                 0x40
+
+#define REG_INT_ENABLE 0x38
+#define REG_FIFO_EN 0x23
+#define REG_USER_CTRL 0x6a
+#define BIT_FIFO_RST 0x04
+#define BIT_FIFO_EN 0x40
+#define REG_CONFIG 0x1a
+#define REG_SAMPLE_RATE_DIV 0x19
+#define REG_GYRO_CONFIG 0x1b
+#define REG_ACCEL_CONFIG 0x1c
+#define BITS_GYRO_OUT 0x70
+#define BIT_ACCEL_OUT 0x08
+#define REG_FIFO_COUNT_H 0x72
+#define REG_FIFO_R_W 0x74
+#define BITS_SELF_TEST_EN 0xe0
+#define DEF_ST_COMPASS_RESULT_SHIFT 2
+#define DEF_ST_ACCEL_RESULT_SHIFT 1
+
+
+/* sample rate */
+#define DEF_SELFTEST_SAMPLE_RATE        0
+/* full scale setting dps */
+#define DEF_SELFTEST_GYRO_FS            (0 << 3)
+#define DEF_SELFTEST_ACCEL_FS           (2 << 3)
+#define DEF_SELFTEST_GYRO_SENS          (32768 / 250)
+/* wait time before collecting data */
+#define DEF_GYRO_WAIT_TIME              10
+#define DEF_ST_STABLE_TIME              20
+#define DEF_ST_6500_STABLE_TIME         20
+#define DEF_GYRO_SCALE                  131
+#define DEF_ST_PRECISION                1000
+#define DEF_ST_ACCEL_FS_MG              8000UL
+#define DEF_ST_SCALE                    (1L << 15)
+#define DEF_ST_TRY_TIMES                2
+
+/*---- MPU6500 Self Test Pass/Fail Criteria ----*/
+/* Gyro Offset Max Value (dps) */
+#define DEF_GYRO_OFFSET_MAX             20
+/* Gyro Self Test Absolute Limits ST_AL (dps) */
+#define DEF_GYRO_ST_AL                  60
+/* Accel Self Test Absolute Limits ST_AL (mg) */
+#define DEF_ACCEL_ST_AL_MIN             225
+#define DEF_ACCEL_ST_AL_MAX             675
+#define DEF_6500_ACCEL_ST_SHIFT_DELTA   500
+#define DEF_6500_GYRO_CT_SHIFT_DELTA    500
+#define DEF_ST_MPU6500_ACCEL_LPF        2
+#define DEF_ST_6500_ACCEL_FS_MG         2000UL
+#define DEF_SELFTEST_6500_ACCEL_FS      (0 << 3)
+
+
+#define THREE_AXIS               3
+#define BYTES_PER_SENSOR         6
+
+#define ONE_K_HZ                              1000
+#define INV_MPU_SAMPLE_RATE_CHANGE_STABLE 50
+#define INIT_ST_SAMPLES          200
+#define FIFO_COUNT_BYTE          2
+
+/* device enum */
+enum inv_devices {
+	INV_MPU6050,
+	INV_MPU9150,
+	INV_MPU6500,
+	INV_MPU9250,
+	INV_MPU9350,
+	INV_MPU6515,
+};
+
+enum inv_filter_e {
+	INV_FILTER_256HZ_NOLPF2 = 0,
+	INV_FILTER_188HZ,
+	INV_FILTER_98HZ,
+	INV_FILTER_42HZ,
+	INV_FILTER_20HZ,
+	INV_FILTER_10HZ,
+	INV_FILTER_5HZ,
+	INV_FILTER_2100HZ_NOLPF,
+	NUM_FILTER
+};
+
+struct inv_selftest_device {
+    u8 *name;
+    enum inv_devices chip_type;
+    u16 sample_rate_div;
+    u16 config;
+    u16 gyro_config;
+    u16 accel_config;
+    int accel_bias[3];
+    int gyro_bias[3];
+    int accel_bias_st[3];
+    int gyro_bias_st[3];
+    u16 samples;//selftest sample number
+};
+
+
+static const u16 mpu_6500_st_tb[256] = {
+	2620, 2646, 2672, 2699, 2726, 2753, 2781, 2808,
+	2837, 2865, 2894, 2923, 2952, 2981, 3011, 3041,
+	3072, 3102, 3133, 3165, 3196, 3228, 3261, 3293,
+	3326, 3359, 3393, 3427, 3461, 3496, 3531, 3566,
+	3602, 3638, 3674, 3711, 3748, 3786, 3823, 3862,
+	3900, 3939, 3979, 4019, 4059, 4099, 4140, 4182,
+	4224, 4266, 4308, 4352, 4395, 4439, 4483, 4528,
+	4574, 4619, 4665, 4712, 4759, 4807, 4855, 4903,
+	4953, 5002, 5052, 5103, 5154, 5205, 5257, 5310,
+	5363, 5417, 5471, 5525, 5581, 5636, 5693, 5750,
+	5807, 5865, 5924, 5983, 6043, 6104, 6165, 6226,
+	6289, 6351, 6415, 6479, 6544, 6609, 6675, 6742,
+	6810, 6878, 6946, 7016, 7086, 7157, 7229, 7301,
+	7374, 7448, 7522, 7597, 7673, 7750, 7828, 7906,
+	7985, 8065, 8145, 8227, 8309, 8392, 8476, 8561,
+	8647, 8733, 8820, 8909, 8998, 9088, 9178, 9270,
+	9363, 9457, 9551, 9647, 9743, 9841, 9939, 10038,
+	10139, 10240, 10343, 10446, 10550, 10656, 10763, 10870,
+	10979, 11089, 11200, 11312, 11425, 11539, 11654, 11771,
+	11889, 12008, 12128, 12249, 12371, 12495, 12620, 12746,
+	12874, 13002, 13132, 13264, 13396, 13530, 13666, 13802,
+	13940, 14080, 14221, 14363, 14506, 14652, 14798, 14946,
+	15096, 15247, 15399, 15553, 15709, 15866, 16024, 16184,
+	16346, 16510, 16675, 16842, 17010, 17180, 17352, 17526,
+	17701, 17878, 18057, 18237, 18420, 18604, 18790, 18978,
+	19167, 19359, 19553, 19748, 19946, 20145, 20347, 20550,
+	20756, 20963, 21173, 21385, 21598, 21814, 22033, 22253,
+	22475, 22700, 22927, 23156, 23388, 23622, 23858, 24097,
+	24338, 24581, 24827, 25075, 25326, 25579, 25835, 26093,
+	26354, 26618, 26884, 27153, 27424, 27699, 27976, 28255,
+	28538, 28823, 29112, 29403, 29697, 29994, 30294, 30597,
+	30903, 31212, 31524, 31839, 32157, 32479, 32804
+};
+
+#endif  /* #ifndef MPU6XXX_HWSELFTEST_H */
diff --git a/drivers/misc/mediatek/accelerometer/mpu60x0/Makefile b/drivers/misc/mediatek/accelerometer/mpu60x0/Makefile
new file mode 100644
index 000000000..219e84320
--- /dev/null
+++ b/drivers/misc/mediatek/accelerometer/mpu60x0/Makefile
@@ -0,0 +1,4 @@
+include $(srctree)/drivers/misc/mediatek/Makefile.custom
+
+obj-y	:=  mpu60x0.o
+
diff --git a/drivers/misc/mediatek/accelerometer/mpu60x0/mpu6050c.h b/drivers/misc/mediatek/accelerometer/mpu60x0/mpu6050c.h
new file mode 100644
index 000000000..d4fd30633
--- /dev/null
+++ b/drivers/misc/mediatek/accelerometer/mpu60x0/mpu6050c.h
@@ -0,0 +1,167 @@
+/* mpu6050c.h
+ *
+ * (C) Copyright 2008 
+ * MediaTek <www.mediatek.com>
+ *
+ * mpu300 head file for MT65xx
+ *
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ */
+#ifndef MPU6050C_H
+#define MPU6050C_H
+	 
+#include <linux/ioctl.h>
+	 
+#define MPU6050C_I2C_SLAVE_ADDR		0xD0
+#define MPU6050C_FIXED_DEVID		0xD0
+
+
+/* MPU6050C Register Map  (Please refer to MPU6050C Specifications) */
+
+#define  MPU6050C_REG_SELFTEST_X			0x0D
+#define  MPU6050C_REG_SELFTEST_Y			0x0E
+#define  MPU6050C_REG_SELFTEST_Z			0x0F
+#define  MPU6050C_REG_SELFTEST_XYZ		0x10
+
+#define  MPU6050C_REG_DATA_SMPRT_DIV	0x19
+#define  MPU6050C_REG_DATA_CONFIG	0x1A
+#define  MPU6050C_REG_GYRO_CONFIG	0x1B
+#define	 MPU6050C_REG_ACC_CONFIG	0x1C
+
+#define	 MPU6050C_REG_INT_CONFIG	0x37
+#define	 MPU6050C_REG_INT_ENABLE	0x38
+#define	 MPU6050C_REG_INT_STATUS	0x3A
+
+
+#define  MPU6050C_REG_ACCEL_XOUT_H 		 0x3b
+#define  MPU6050C_REG_ACCEL_XOUT_L 		 0x3c
+#define  MPU6050C_REG_ACCEL_YOUT_H 		 0x3d
+#define  MPU6050C_REG_ACCEL_YOUT_L 		 0x3e
+#define  MPU6050C_REG_ACCEL_ZOUT_H 		 0x3f
+#define  MPU6050C_REG_ACCEL_ZOUT_L 		 0x40
+
+#define  MPU6050C_REG_TEMP_OUT_H 		 0x41
+#define  MPU6050C_REG_TEMP_OUT_L 		 0x42
+
+#define  MPU6050C_REG_GYRO_XOUT_H 		 0x43
+#define  MPU6050C_REG_GYRO_XOUT_L 		 0x44
+#define  MPU6050C_REG_GYRO_YOUT_H 		 0x45
+#define  MPU6050C_REG_GYRO_YOUT_L 		 0x46
+#define  MPU6050C_REG_GYRO_ZOUT_H 		 0x47
+#define  MPU6050C_REG_GYRO_ZOUT_L 		 0x48
+
+#define  MPU6050C_REG_PWR_MGMT_1	0x6B
+#define  MPU6050C_REG_PWR_MGMT_2	0x6C
+
+#define  MPU6050C_REG_DEVICE_ID	0x75	
+
+ 
+
+/*MPU6050C Register Bit definitions*/ 
+
+#define MPU6050C_FS_RANGE			0x03	//set the full-scale range of the gyro sensors
+#define MPU6050C_FS_250 				0x00
+#define MPU6050C_FS_500				0x01
+#define MPU6050C_FS_1000				0x02
+#define MPU6050C_FS_2000				0x03
+#define MPU6050C_FS_MAX				0x03
+
+#define MPU6050C_FS_250_LSB			131		// LSB/(o/s)
+#define MPU6050C_FS_500_LSB			66
+#define MPU6050C_FS_1000_LSB			33
+#define MPU6050C_FS_2000_LSB			16
+#define MPU6050C_FS_MAX_LSB			131
+
+
+#define MPU6050C_SAM_RATE_MASK		0x07	//set sample rate and low padd filter configuration
+#define MPU6050C_RATE_8K_LPFB_256HZ 	0x00
+#define MPU6050C_RATE_1K_LPFB_188HZ	0x01
+#define MPU6050C_RATE_1K_LPFB_98HZ 	0x02
+#define MPU6050C_RATE_1K_LPFB_42HZ 	0x03
+#define MPU6050C_RATE_1K_LPFB_20HZ 	0x04
+#define MPU6050C_RATE_1K_LPFB_10HZ 	0x05
+#define MPU6050C_RATE_1K_LPFB_5HZ 	0x06
+
+#define MPU6050C_GYRO_RNG_250		0x0
+#define MPU6050C_GYRO_RNG_500		0x1
+#define MPU6050C_GYRO_RNG_1000		0x2
+#define MPU6050C_GYRO_RNG_2000		0x3
+
+
+#define MPU6050C_ACC_RNG_2G			0x0
+#define MPU6050C_ACC_RNG_4G			0x1
+#define MPU6050C_ACC_RNG_8G			0x2
+#define MPU6050C_ACC_RNG_16G		0x3
+
+
+#define MPU6050C_CLKSEL_8M		0x0
+#define MPU6050C_CLKSEL_PLL_X		0x1
+#define MPU6050C_CLKSEL_PLL_Y		0x2
+#define MPU6050C_CLKSEL_PLL_Z		0x3
+#define MPU6050C_CLKSEL_STOP		0x7
+
+#define	MPU6050C_SLEEP	0x40
+#define BIT_HW_RESET	0x80
+
+	 
+#define MPU6050C_SUCCESS				    0
+#define MPU6050C_ERR_I2C				  -1
+#define MPU6050C_ERR_STATUS			  -3
+#define MPU6050C_ERR_SETUP_FAILURE	  -4
+#define MPU6050C_ERR_GETGSENSORDATA  -5
+#define MPU6050C_ERR_IDENTIFICATION	  -6
+	 
+
+/*
+typedef enum  {
+    MPU6050C_SYNC_NONE = 0x0,
+    MPU6050C_SYNC_TEMP,
+    MPU6050C_SYNC_GYROX,
+    MPU6050C_SYNC_GYROY,
+    MPU6050C_SYNC_GYROZ,
+    MPU6050C_SYNC_AUXX,
+    MPU6050C_SYNC_AUXY,
+    MPU6050C_SYNC_AUXZ,
+} MPU6050C_EXT_SYNC_SEL;
+
+typedef enum  {
+    MPU6050C_FS_250 = 0x0,
+    MPU6050C_FS_500,
+    MPU6050C_FS_1000,
+    MPU6050C_FS_2000,
+    MPU6050C_FS_MAX = 0x3,
+} MPU6050C_FS_SEL;
+
+typedef enum  {
+    MPU6050C_RATE_8K_LPFB_256HZ = 0x0,
+    MPU6050C_RATE_1K_LPFB_188HZ,
+    MPU6050C_RATE_1K_LPFB_98HZ,
+    MPU6050C_RATE_1K_LPFB_42HZ,
+    MPU6050C_RATE_1K_LPFB_20HZ,
+    MPU6050C_RATE_1K_LPFB_10HZ,
+    MPU6050C_RATE_1K_LPFB_5HZ,
+} MPU6050C_SAMRATE_SEL;
+*/
+
+#define MPU6050C_BUFSIZE 60
+
+// 1 rad = 180/PI degree, MAX_LSB = 131,
+// 180*131/PI = 7506
+#define DEGREE_TO_RAD	7506
+
+//extern struct mpu6050c_gyro_i2c_data* MPU6050C_Gyro_GetI2CData();
+
+#endif //MPU6050C_H
+
diff --git a/drivers/misc/mediatek/accelerometer/mpu60x0/mpu60x0.c b/drivers/misc/mediatek/accelerometer/mpu60x0/mpu60x0.c
new file mode 100644
index 000000000..d97fc0bc3
--- /dev/null
+++ b/drivers/misc/mediatek/accelerometer/mpu60x0/mpu60x0.c
@@ -0,0 +1,1784 @@
+/* MPU6050C_ACC motion sensor driver
+ *
+ *
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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.
+ *
+ */
+
+#include <linux/interrupt.h>
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include <linux/irq.h>
+#include <linux/miscdevice.h>
+#include <asm/uaccess.h>
+#include <linux/delay.h>
+#include <linux/input.h>
+#include <linux/workqueue.h>
+#include <linux/kobject.h>
+#include <linux/earlysuspend.h>
+#include <linux/platform_device.h>
+
+#include <cust_acc.h>
+#include <linux/hwmsensor.h>
+#include <linux/hwmsen_dev.h>
+#include <linux/sensors_io.h>
+#include "mpu60x0.h"
+#include "mpu6050c.h"
+#include <linux/hwmsen_helper.h>
+#include <linux/kernel.h>
+
+//#include <mach/mt_devs.h>
+#include <mach/mt_typedefs.h>
+#include <mach/mt_gpio.h>
+#include <mach/mt_pm_ldo.h>
+#include <mach/mt_boot.h>
+
+#define POWER_NONE_MACRO MT65XX_POWER_NONE
+
+/*----------------------------------------------------------------------------*/
+#define I2C_DRIVERID_MPU6050C_ACC	3000
+/*----------------------------------------------------------------------------*/
+#define MPU6050C_DEFAULT_FS		MPU6050C_FS_1000
+#define MPU6050C_DEFAULT_LSB		MPU6050C_FS_1000_LSB
+/*---------------------------------------------------------------------------*/
+#define DEBUG 0
+/*----------------------------------------------------------------------------*/
+#define CONFIG_MPU6050C_ACC_LOWPASS   /*apply low pass filter on output*/       
+/*----------------------------------------------------------------------------*/
+#define MPU6050C_ACC_AXIS_X          0
+#define MPU6050C_ACC_AXIS_Y          1
+#define MPU6050C_ACC_AXIS_Z          2
+#define MPU6050C_ACC_AXES_NUM        3
+#define MPU6050C_ACC_DATA_LEN        6   
+#define MPU6050C_ACC_DEV_NAME        "MPU6050C_ACC"
+/*----------------------------------------------------------------------------*/
+static const struct i2c_device_id mpu6050c_acc_i2c_id[] = {{MPU6050C_ACC_DEV_NAME,0},{}};
+static struct i2c_board_info __initdata i2c_mpu6050c_acc={ I2C_BOARD_INFO("MPU6050C_ACC", MPU6050C_ACC_I2C_SLAVE_ADDR>>1)};
+/*the adapter id will be available in customization*/
+//static unsigned short mpu6050c_acc_force[] = {0x00, MPU6050C_ACC_I2C_SLAVE_ADDR, I2C_CLIENT_END, I2C_CLIENT_END};
+//static const unsigned short *const mpu6050c_acc_forces[] = { mpu6050c_acc_force, NULL };
+//static struct i2c_client_address_data mpu6050c_acc_addr_data = { .forces = mpu6050c_acc_forces,};
+
+int packet_thresh = 75; // 600 ms / 8ms/sample
+
+/*----------------------------------------------------------------------------*/
+static int mpu6050c_acc_i2c_probe(struct i2c_client *client, const struct i2c_device_id *id); 
+static int mpu6050c_acc_i2c_remove(struct i2c_client *client);
+//static int mpu6050c_acc_i2c_detect(struct i2c_client *client, int kind, struct i2c_board_info *info);
+//static int mpu6050c_acc_suspend(struct i2c_client *client, pm_message_t msg) ;
+//static int mpu6050c_acc_resume(struct i2c_client *client);
+/*----------------------------------------------------------------------------*/
+typedef enum {
+    ACC_TRC_FILTER  = 0x01,
+    ACC_TRC_RAWDATA = 0x02,
+    ACC_TRC_IOCTL   = 0x04,
+    ACC_TRC_CALI	= 0X08,
+    ACC_TRC_INFO	= 0X10,
+    ACC_TRC_DATA	= 0X20,
+} ACC_TRC;
+/*----------------------------------------------------------------------------*/
+struct scale_factor{
+    u8  whole;
+    u8  fraction;
+};
+/*----------------------------------------------------------------------------*/
+struct data_resolution {
+    struct scale_factor scalefactor;
+    int                 sensitivity;
+};
+/*----------------------------------------------------------------------------*/
+#define C_MAX_FIR_LENGTH (32)
+/*----------------------------------------------------------------------------*/
+struct data_filter {
+    s16 raw[C_MAX_FIR_LENGTH][MPU6050C_ACC_AXES_NUM];
+    int sum[MPU6050C_ACC_AXES_NUM];
+    int num;
+    int idx;
+};
+
+typedef enum {
+    CMC_BIT_ACC    = 1,
+    CMC_BIT_GYRO     = 2,
+} CMC_BIT;
+
+/*----------------------------------------------------------------------------*/
+struct mpu6050c_acc_i2c_data {
+    struct i2c_client *client;
+    struct acc_hw *hw;
+    struct hwmsen_convert   cvt;
+    
+    /*misc*/
+    struct data_resolution *reso;
+    atomic_t                trace;
+    atomic_t                suspend;
+    atomic_t                selftest;
+    atomic_t				filter;
+    s16                     acc_cali_sw[MPU6050C_ACC_AXES_NUM+1];
+    s16                     gyro_cali_sw[MPU6050C_ACC_AXES_NUM+1];
+
+    /*data*/
+    s8                      offset[MPU6050C_ACC_AXES_NUM+1];  /*+1: for 4-byte alignment*/
+    s16                     data[MPU6050C_ACC_AXES_NUM+1];
+
+#if defined(CONFIG_MPU6050C_ACC_LOWPASS)
+    atomic_t                firlen;
+    atomic_t                fir_en;
+    struct data_filter      fir;
+#endif 
+    /*early suspend*/
+#if defined(CONFIG_HAS_EARLYSUSPEND)
+    struct early_suspend    early_drv;
+#endif     
+	ulong		enable; 		/*enable mask*/
+
+};
+/*----------------------------------------------------------------------------*/
+static struct i2c_driver mpu6050c_acc_i2c_driver = {
+    .driver = {
+        .name           = MPU6050C_ACC_DEV_NAME,
+    },
+	.probe      		= mpu6050c_acc_i2c_probe,
+	.remove    			= mpu6050c_acc_i2c_remove,
+#if !defined(CONFIG_HAS_EARLYSUSPEND)    
+    .suspend            = mpu6050c_acc_suspend,
+    .resume             = mpu6050c_acc_resume,
+#endif
+	.id_table = mpu6050c_acc_i2c_id,
+};
+
+/*----------------------------------------------------------------------------*/
+struct i2c_client *mpu6050c_acc_i2c_client = NULL;
+static struct platform_driver mpu6050c_acc_driver;
+struct mpu6050c_acc_i2c_data *mpu6050c_obj_i2c_data = NULL;
+
+
+
+/*----------------------------------------------------------------------------*/
+#define GSE_TAG                  "[Gsensor] "
+#define GSE_FUN(f)               printk(KERN_INFO GSE_TAG"%s\n", __FUNCTION__)
+#define GSE_ERR(fmt, args...)    printk(KERN_ERR GSE_TAG"%s %d : "fmt, __FUNCTION__, __LINE__, ##args)
+#define GSE_LOG(fmt, args...)    printk(KERN_INFO GSE_TAG fmt, ##args)
+/*----------------------------------------------------------------------------*/
+/*
+
+//----------------------------------------------------------------------------//
+static struct data_resolution mpu6050c_acc_offset_resolution = {{15, 6}, 64};
+*/
+/*--------------------accscopy power control function----------------------------------*/
+static struct data_resolution mpu6050c_data_resolution[] = {
+ /* combination by {FULL_RES,RANGE}*/
+	 {{ 1, 0}, 16384},   // dataformat +/-2g	in 16-bit resolution;    16384 = (2^16)/(2*2)			
+};
+
+/*----------------------------------------------------------------------------*/
+
+
+struct mpu6050c_acc_i2c_data* MPU6050C_Acc_GetI2CData(void)
+{
+	return mpu6050c_obj_i2c_data;
+}
+EXPORT_SYMBOL_GPL(MPU6050C_Acc_GetI2CData);
+
+
+static void MPU6050C_ACC_power(struct acc_hw *hw, unsigned int on) 
+{
+	static unsigned int power_on = 0;
+
+	if(hw->power_id != POWER_NONE_MACRO)		// have externel LDO
+	{        
+		GSE_LOG("power %s\n", on ? "on" : "off");
+		if(power_on == on)	// power status not change
+		{
+			GSE_LOG("ignore power control: %d\n", on);
+		}
+		else if(on)	// power on
+		{
+			if(!hwPowerOn(hw->power_id, hw->power_vol, "MPU6050C_ACC"))
+			{
+				GSE_ERR("power on fails!!\n");
+			}
+		}
+		else	// power off
+		{
+			if (!hwPowerDown(hw->power_id, "MPU6050C_ACC"))
+			{
+				GSE_ERR("power off fail!!\n");
+			}			  
+		}
+	}
+	power_on = on;    
+}
+/*----------------------------------------------------------------------------*/
+
+
+/*----------------------------------------------------------------------------*/
+static int MPU6050C_ACC_write_rel_calibration(struct mpu6050c_acc_i2c_data *obj, int dat[MPU6050C_ACC_AXES_NUM])
+{
+    obj->acc_cali_sw[MPU6050C_ACC_AXIS_X] = obj->cvt.sign[MPU6050C_ACC_AXIS_X]*dat[obj->cvt.map[MPU6050C_ACC_AXIS_X]];
+    obj->acc_cali_sw[MPU6050C_ACC_AXIS_Y] = obj->cvt.sign[MPU6050C_ACC_AXIS_Y]*dat[obj->cvt.map[MPU6050C_ACC_AXIS_Y]];
+    obj->acc_cali_sw[MPU6050C_ACC_AXIS_Z] = obj->cvt.sign[MPU6050C_ACC_AXIS_Z]*dat[obj->cvt.map[MPU6050C_ACC_AXIS_Z]];
+#if DEBUG		
+		if(atomic_read(&obj->trace) & ACC_TRC_CALI)
+		{
+			GSE_LOG("test  (%5d, %5d, %5d) ->(%5d, %5d, %5d)->(%5d, %5d, %5d))\n", 
+				obj->cvt.sign[MPU6050C_ACC_AXIS_X],obj->cvt.sign[MPU6050C_ACC_AXIS_Y],obj->cvt.sign[MPU6050C_ACC_AXIS_Z],
+				dat[MPU6050C_ACC_AXIS_X], dat[MPU6050C_ACC_AXIS_Y], dat[MPU6050C_ACC_AXIS_Z],
+				obj->cvt.map[MPU6050C_ACC_AXIS_X],obj->cvt.map[MPU6050C_ACC_AXIS_Y],obj->cvt.map[MPU6050C_ACC_AXIS_Z]);
+			GSE_LOG("write acc calibration data  (%5d, %5d, %5d)\n", 
+				obj->acc_cali_sw[MPU6050C_ACC_AXIS_X],obj->acc_cali_sw[MPU6050C_ACC_AXIS_Y],obj->acc_cali_sw[MPU6050C_ACC_AXIS_Z]);
+		}
+#endif
+    return 0;
+}
+static int MPU6050C_ACC_write_gyro_rel_calibration(struct mpu6050c_acc_i2c_data *obj, int dat[MPU6050C_ACC_AXES_NUM])
+{
+    obj->gyro_cali_sw[MPU6050C_ACC_AXIS_X] = obj->cvt.sign[MPU6050C_ACC_AXIS_X]*dat[obj->cvt.map[MPU6050C_ACC_AXIS_X]];
+    obj->gyro_cali_sw[MPU6050C_ACC_AXIS_Y] = obj->cvt.sign[MPU6050C_ACC_AXIS_Y]*dat[obj->cvt.map[MPU6050C_ACC_AXIS_Y]];
+    obj->gyro_cali_sw[MPU6050C_ACC_AXIS_Z] = obj->cvt.sign[MPU6050C_ACC_AXIS_Z]*dat[obj->cvt.map[MPU6050C_ACC_AXIS_Z]];
+#if DEBUG		
+		if(atomic_read(&obj->trace) & ACC_TRC_CALI)
+		{
+			GSE_LOG("test  (%5d, %5d, %5d) ->(%5d, %5d, %5d)->(%5d, %5d, %5d))\n", 
+				obj->cvt.sign[MPU6050C_ACC_AXIS_X],obj->cvt.sign[MPU6050C_ACC_AXIS_Y],obj->cvt.sign[MPU6050C_ACC_AXIS_Z],
+				dat[MPU6050C_ACC_AXIS_X], dat[MPU6050C_ACC_AXIS_Y], dat[MPU6050C_ACC_AXIS_Z],
+				obj->cvt.map[MPU6050C_ACC_AXIS_X],obj->cvt.map[MPU6050C_ACC_AXIS_Y],obj->cvt.map[MPU6050C_ACC_AXIS_Z]);
+			GSE_LOG("write acc calibration data  (%5d, %5d, %5d)\n", 
+				obj->gyro_cali_sw[MPU6050C_ACC_AXIS_X],obj->gyro_cali_sw[MPU6050C_ACC_AXIS_Y],obj->gyro_cali_sw[MPU6050C_ACC_AXIS_Z]);
+		}
+#endif
+    return 0;
+}
+
+
+
+/*----------------------------------------------------------------------------*/
+static int MPU6050C_ACC_ResetCalibration(struct i2c_client *client)
+{
+	struct mpu6050c_acc_i2c_data *obj = i2c_get_clientdata(client);	
+
+	memset(obj->acc_cali_sw, 0x00, sizeof(obj->acc_cali_sw));
+	return 0;    
+}
+
+/*----------------------------------------------------------------------------*/
+static int MPU6050C_ACC_ResetGyroCalibration(struct i2c_client *client)
+{
+	struct mpu6050c_acc_i2c_data *obj = i2c_get_clientdata(client);	
+
+	memset(obj->gyro_cali_sw, 0x00, sizeof(obj->gyro_cali_sw));
+	return 0;    
+}
+
+/*----------------------------------------------------------------------------*/
+static int MPU6050C_ACC_ReadCalibration(struct i2c_client *client, int dat[MPU6050C_ACC_AXES_NUM])
+{
+    struct mpu6050c_acc_i2c_data *obj = i2c_get_clientdata(client);
+
+    dat[obj->cvt.map[MPU6050C_ACC_AXIS_X]] = obj->cvt.sign[MPU6050C_ACC_AXIS_X]*obj->acc_cali_sw[MPU6050C_ACC_AXIS_X];
+    dat[obj->cvt.map[MPU6050C_ACC_AXIS_Y]] = obj->cvt.sign[MPU6050C_ACC_AXIS_Y]*obj->acc_cali_sw[MPU6050C_ACC_AXIS_Y];
+    dat[obj->cvt.map[MPU6050C_ACC_AXIS_Z]] = obj->cvt.sign[MPU6050C_ACC_AXIS_Z]*obj->acc_cali_sw[MPU6050C_ACC_AXIS_Z];
+
+#if DEBUG		
+		if(atomic_read(&obj->trace) & ACC_TRC_CALI)
+		{
+			GSE_LOG("Read acc calibration data  (%5d, %5d, %5d)\n", 
+				dat[MPU6050C_ACC_AXIS_X],dat[MPU6050C_ACC_AXIS_Y],dat[MPU6050C_ACC_AXIS_Z]);
+		}
+#endif
+                                       
+    return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int MPU6050C_ACC_ReadGyroCalibration(struct i2c_client *client, int dat[MPU6050C_ACC_AXES_NUM])
+{
+    struct mpu6050c_acc_i2c_data *obj = i2c_get_clientdata(client);
+
+    dat[obj->cvt.map[MPU6050C_ACC_AXIS_X]] = obj->cvt.sign[MPU6050C_ACC_AXIS_X]*obj->gyro_cali_sw[MPU6050C_ACC_AXIS_X];
+    dat[obj->cvt.map[MPU6050C_ACC_AXIS_Y]] = obj->cvt.sign[MPU6050C_ACC_AXIS_Y]*obj->gyro_cali_sw[MPU6050C_ACC_AXIS_Y];
+    dat[obj->cvt.map[MPU6050C_ACC_AXIS_Z]] = obj->cvt.sign[MPU6050C_ACC_AXIS_Z]*obj->gyro_cali_sw[MPU6050C_ACC_AXIS_Z];
+
+#if DEBUG		
+		if(atomic_read(&obj->trace) & ACC_TRC_CALI)
+		{
+			GSE_LOG("Read acc calibration data  (%5d, %5d, %5d)\n", 
+				dat[MPU6050C_ACC_AXIS_X],dat[MPU6050C_ACC_AXIS_Y],dat[MPU6050C_ACC_AXIS_Z]);
+		}
+#endif
+                                       
+    return 0;
+}
+
+/*----------------------------------------------------------------------------*/
+/*----------------------------------------------------------------------------*/
+static int MPU6050C_ACC_WriteCalibration(struct i2c_client *client, int dat[MPU6050C_ACC_AXES_NUM])
+{
+	struct mpu6050c_acc_i2c_data *obj = i2c_get_clientdata(client);
+	int err = 0;
+	int cali[MPU6050C_ACC_AXES_NUM];
+
+
+	GSE_FUN();
+	if(!obj || ! dat)
+	{
+		GSE_ERR("null ptr!!\n");
+		return -EINVAL;
+	}
+	else
+	{        		
+		cali[obj->cvt.map[MPU6050C_ACC_AXIS_X]] = obj->cvt.sign[MPU6050C_ACC_AXIS_X]*obj->acc_cali_sw[MPU6050C_ACC_AXIS_X];
+		cali[obj->cvt.map[MPU6050C_ACC_AXIS_Y]] = obj->cvt.sign[MPU6050C_ACC_AXIS_Y]*obj->acc_cali_sw[MPU6050C_ACC_AXIS_Y];
+		cali[obj->cvt.map[MPU6050C_ACC_AXIS_Z]] = obj->cvt.sign[MPU6050C_ACC_AXIS_Z]*obj->acc_cali_sw[MPU6050C_ACC_AXIS_Z]; 
+		cali[MPU6050C_ACC_AXIS_X] += dat[MPU6050C_ACC_AXIS_X];
+		cali[MPU6050C_ACC_AXIS_Y] += dat[MPU6050C_ACC_AXIS_Y];
+		cali[MPU6050C_ACC_AXIS_Z] += dat[MPU6050C_ACC_AXIS_Z];
+#if DEBUG		
+		if(atomic_read(&obj->trace) & ACC_TRC_CALI)
+		{
+			GSE_LOG("write acc calibration data  (%5d, %5d, %5d)-->(%5d, %5d, %5d)\n", 
+				dat[MPU6050C_ACC_AXIS_X], dat[MPU6050C_ACC_AXIS_Y], dat[MPU6050C_ACC_AXIS_Z],
+				cali[MPU6050C_ACC_AXIS_X],cali[MPU6050C_ACC_AXIS_Y],cali[MPU6050C_ACC_AXIS_Z]);
+		}
+#endif
+		return MPU6050C_ACC_write_rel_calibration(obj, cali);
+	} 
+
+	return err;
+}
+static int MPU6050C_ACC_WriteGyroCalibration(struct i2c_client *client, int dat[MPU6050C_ACC_AXES_NUM])
+{
+	struct mpu6050c_acc_i2c_data *obj = i2c_get_clientdata(client);
+	int err = 0;
+	int cali[MPU6050C_ACC_AXES_NUM];
+
+
+	GSE_FUN();
+	if(!obj || ! dat)
+	{
+		GSE_ERR("null ptr!!\n");
+		return -EINVAL;
+	}
+	else
+	{        		
+		cali[obj->cvt.map[MPU6050C_ACC_AXIS_X]] = obj->cvt.sign[MPU6050C_ACC_AXIS_X]*obj->gyro_cali_sw[MPU6050C_ACC_AXIS_X];
+		cali[obj->cvt.map[MPU6050C_ACC_AXIS_Y]] = obj->cvt.sign[MPU6050C_ACC_AXIS_Y]*obj->gyro_cali_sw[MPU6050C_ACC_AXIS_Y];
+		cali[obj->cvt.map[MPU6050C_ACC_AXIS_Z]] = obj->cvt.sign[MPU6050C_ACC_AXIS_Z]*obj->gyro_cali_sw[MPU6050C_ACC_AXIS_Z]; 
+		cali[MPU6050C_ACC_AXIS_X] += dat[MPU6050C_ACC_AXIS_X];
+		cali[MPU6050C_ACC_AXIS_Y] += dat[MPU6050C_ACC_AXIS_Y];
+		cali[MPU6050C_ACC_AXIS_Z] += dat[MPU6050C_ACC_AXIS_Z];
+#if DEBUG		
+		if(atomic_read(&obj->trace) & ACC_TRC_CALI)
+		{
+			GSE_LOG("write acc calibration data  (%5d, %5d, %5d)-->(%5d, %5d, %5d)\n", 
+				dat[MPU6050C_ACC_AXIS_X], dat[MPU6050C_ACC_AXIS_Y], dat[MPU6050C_ACC_AXIS_Z],
+				cali[MPU6050C_ACC_AXIS_X],cali[MPU6050C_ACC_AXIS_Y],cali[MPU6050C_ACC_AXIS_Z]);
+		}
+#endif
+		return MPU6050C_ACC_write_gyro_rel_calibration(obj, cali);
+	} 
+
+	return err;
+}
+
+/*----------------------------------------------------------------------------*/
+
+static int MPU6050C_ACC_Reset(struct i2c_client *client)
+{
+	int res = 0;
+	GSE_LOG("Reset MPU6050 B1\n");
+	res = hwmsen_write_byte(client,MPU6050C_ACC_REG_PWR_MGMT_1,BIT_HW_RESET);
+	if (res) 
+	{
+		GSE_ERR(" Reset MPU6050 B1 error,res: %d!\n", res);
+		return res;
+	}
+	msleep(40);
+	return MPU6050C_ACC_SUCCESS;
+}
+
+
+static int MPU6050C_ACC_SetPWR_MGMT_12(struct i2c_client *client)
+{
+	u8 databuf1[2] = {0};
+	u8 databuf2[2] = {0};
+	int res = 0;
+	struct mpu6050c_acc_i2c_data *obj = i2c_get_clientdata(client);
+	//GSE_FUN();
+
+	if((!test_bit(CMC_BIT_GYRO, &obj->enable))
+		&&(test_bit(CMC_BIT_ACC, &obj->enable)))
+	{ 
+		//means it works only for acc,so set gyro as standby mode
+		GSE_LOG("MPU6050C_ACC_SetPWR_MGMT_12 !CMC_BIT_GYRO&&CMC_BIT_ACC \n");
+		//databuf1[0] = 0x24;// CYCLE =1 ;SLEEP =0; TEMP_DIS =1
+		databuf1[0] = 0x28;// CYCLE =1 ;SLEEP =0; TEMP_DIS =1; internal 8M clock
+		databuf2[0] = 0xc7;//STBY_XG =1,STBY_YG =1,STBY_ZG =1,LP_WAKE_CTRL =11
+
+	}
+	else if((test_bit(CMC_BIT_GYRO, &obj->enable))
+		&&(test_bit(CMC_BIT_ACC, &obj->enable)))
+	{
+		//mean  work both
+		GSE_LOG("MPU6050C_ACC_SetPWR_MGMT_12 CMC_BIT_GYR&&CMC_BIT_ACC \n");
+		databuf1[0] = 0x01;//SLEEP=0;CYCLE =0 MPU6050C_ACC_CLKSEL_PLL_X;TEMP_DIS = 0
+		databuf2[0] = 0x00;//STBY_XG =0,STBY_YG =0,STBY_ZG =0;STBY_XA =0,STBY_YA =0,STBY_ZA =0
+	}
+	else if((test_bit(CMC_BIT_GYRO, &obj->enable))
+		&&(!test_bit(CMC_BIT_ACC, &obj->enable)))
+	{
+		//mean	gyro work only,acc sleep
+		GSE_LOG("MPU6050C_ACC_SetPWR_MGMT_12 CMC_BIT_GYR&&CMC_BIT_ACC \n");
+		databuf1[0] = 0x01;//SLEEP=0;CYCLE =0;MPU6050C_ACC_CLKSEL_PLL_X;TEMP_DIS = 0		
+		databuf2[0] = 0x38;//0x34;//STBY_XA =1,STBY_YA =1,STBY_ZA =1
+	}
+	else if((!test_bit(CMC_BIT_GYRO, &obj->enable))
+		&&(!test_bit(CMC_BIT_ACC, &obj->enable)))
+	{
+		//mean both sleep
+		GSE_LOG("MPU6050C_ACC_SetPWR_MGMT_12 !CMC_BIT_GYR&&!CMC_BIT_ACC \n");
+		databuf1[0] |= MPU6050C_ACC_SLEEP;
+		databuf1[0] |= MPU6050C_ACC_CLKSEL_PLL_X;
+
+		databuf2[0] = 0x00;
+		
+	}
+	
+	databuf1[1] = databuf1[0];
+	databuf1[0] = MPU6050C_ACC_REG_PWR_MGMT_1;  
+	res = i2c_master_send(client, databuf1, 0x2);
+	if(res <= 0)
+	{
+		GSE_LOG("set power mode failed!\n");
+		return MPU6050C_ACC_ERR_I2C;
+	}
+
+	databuf2[1] = databuf2[0];
+	databuf2[0] = MPU6050C_ACC_REG_PWR_MGMT_2;
+	res = i2c_master_send(client, databuf2, 0x2);
+	if(res <= 0)
+	{
+		GSE_LOG("set power mode failed!\n");
+		return MPU6050C_ACC_ERR_I2C;
+	}
+	return MPU6050C_ACC_SUCCESS;
+}
+
+/*----------------------------------------------------------------------------*/
+static int MPU6050C_ACC_SetAccDataRange(struct i2c_client *client, u8 range)
+{
+	
+	u8 databuf[2];	  
+	int res = 0;
+
+	memset(databuf, 0, sizeof(u8)*2);	 
+	databuf[0] = MPU6050C_ACC_REG_ACC_CONFIG;    
+	databuf[1] = range;
+	databuf[1] = databuf[1]<<3;//refer to datasheet
+	databuf[1] &= 0x18;
+	res = i2c_master_send(client, databuf, 0x2);
+
+	if(res <= 0)
+	{
+		return MPU6050C_ACC_ERR_I2C;
+	}
+
+	mpu6050c_obj_i2c_data->reso = &mpu6050c_data_resolution[0];
+	return 0;
+
+}
+static int MPU6050C_ACC_SetGyroDataRange(struct i2c_client *client, u8 range)
+{
+	
+	u8 databuf[2];	  
+	int res = 0;
+
+	memset(databuf, 0, sizeof(u8)*2);	 
+	databuf[0] = MPU6050C_ACC_REG_GYRO_CONFIG;    
+	databuf[1] = range;
+	databuf[1] = databuf[1]<<3;//refer to datasheet
+	databuf[1] &= 0x18;
+
+
+	res = i2c_master_send(client, databuf, 0x2);
+
+	if(res <= 0)
+	{
+		return MPU6050C_ACC_ERR_I2C;
+	}
+
+	return 0;
+
+}
+
+static int MPU6050C_ACC_SetDataBandWidth(struct i2c_client *client, u8 BandWidth)
+{
+	
+	u8 databuf[2];	  
+	int res = 0;
+
+	memset(databuf, 0, sizeof(u8)*2);	 
+	databuf[0] = MPU6050C_ACC_REG_DATA_CONFIG;    
+	databuf[1] = BandWidth&0x07;
+
+	res = i2c_master_send(client, databuf, 0x2);
+
+	if(res <= 0)
+	{
+		return MPU6050C_ACC_ERR_I2C;
+	}
+
+	return 0;
+
+}
+
+// set the sample rate
+static int MPU6050C_ACC_SetSampleRate(struct i2c_client *client, int sample_rate)
+{
+	u8 databuf[2] = {0}; 
+	int smplrt_div = 0;
+	int res = 0;
+	GSE_FUN();    
+
+	if(hwmsen_read_byte(client, MPU6050C_ACC_REG_DATA_CONFIG, databuf))
+	{
+		GSE_ERR("read acc data format register err!\n");
+		return MPU6050C_ACC_ERR_I2C;
+	}
+	else
+	{
+		GSE_LOG("read  acc data format register: 0x%x\n", databuf[0]);
+	}
+
+	if((databuf[0] & 0x07) == 0)	//Analog sample rate is 8KHz
+	{
+		smplrt_div = 8 * 1000 / sample_rate - 1;
+	}
+	else	// 1kHz
+	{
+		smplrt_div = 1000 / sample_rate - 1;
+	}
+
+	if(smplrt_div > 255)	// rate_div: 0 to 255;
+	{
+		smplrt_div = 255;
+	}
+	else if(smplrt_div < 0)
+	{
+		smplrt_div = 0;
+	}
+	
+	databuf[0] = MPU6050C_ACC_REG_DATA_SMPRT_DIV;
+	databuf[1] = smplrt_div;
+	res = i2c_master_send(client, databuf, 0x2);
+	if(res <= 0)
+	{
+		GSE_ERR("write sample rate register err!\n");
+		return MPU6050C_ACC_ERR_I2C;
+	}
+
+	//read sample div after written for test	
+	udelay(500);
+	if(hwmsen_read_byte(client, MPU6050C_ACC_REG_DATA_SMPRT_DIV, databuf))
+	{
+		GSE_ERR("read acc sample rate register err!\n");
+		return MPU6050C_ACC_ERR_I2C;
+	}
+	else
+	{
+		GSE_LOG("read  acc sample rate: 0x%x\n", databuf[0]);
+	}
+	
+	return MPU6050C_ACC_SUCCESS;    
+}
+
+/*----------------------------------------------------------------------------*/
+static int MPU6050C_ACC_ReadAccData(struct i2c_client *client, char *buf, int bufsize)
+{
+	char databuf[6];	
+	int data[3];
+	struct mpu6050c_acc_i2c_data *obj = i2c_get_clientdata(client);  
+
+	if(!test_bit(CMC_BIT_ACC, &obj->enable))
+	{
+		set_bit(CMC_BIT_ACC, &obj->enable);
+		MPU6050C_ACC_SetPWR_MGMT_12(client);
+		msleep(50);
+	}
+
+	if(hwmsen_read_block(client, MPU6050C_ACC_REG_ACCEL_XOUT_H, databuf, 6))
+	{
+		GSE_ERR("MPU6050C_ACC read accscope data	error\n");
+		return -2;
+	}
+	else
+	{
+		obj->data[MPU6050C_ACC_AXIS_X] = ((s16)((databuf[MPU6050C_ACC_AXIS_X*2+1]) | (databuf[MPU6050C_ACC_AXIS_X*2] << 8)));
+		obj->data[MPU6050C_ACC_AXIS_Y] = ((s16)((databuf[MPU6050C_ACC_AXIS_Y*2+1]) | (databuf[MPU6050C_ACC_AXIS_Y*2] << 8)));
+		obj->data[MPU6050C_ACC_AXIS_Z] = ((s16)((databuf[MPU6050C_ACC_AXIS_Z*2+1]) | (databuf[MPU6050C_ACC_AXIS_Z*2] << 8)));
+#if DEBUG		
+		if(atomic_read(&obj->trace) & ACC_TRC_RAWDATA)
+		{
+			GSE_LOG("read acc register: %d, %d, %d, %d, %d, %d",
+				databuf[0], databuf[1], databuf[2], databuf[3], databuf[4], databuf[5]);
+			GSE_LOG("get acc raw data (0x%08X, 0x%08X, 0x%08X) -> (%5d, %5d, %5d)\n", 
+				obj->data[MPU6050C_ACC_AXIS_X],obj->data[MPU6050C_ACC_AXIS_Y],obj->data[MPU6050C_ACC_AXIS_Z],
+				obj->data[MPU6050C_ACC_AXIS_X],obj->data[MPU6050C_ACC_AXIS_Y],obj->data[MPU6050C_ACC_AXIS_Z]);
+			GSE_LOG("acc_cali_sw[0] =%d,acc_cali_sw[1] =%d,acc_cali_sw[2] =%d,\n", 
+				obj->acc_cali_sw[0], obj->acc_cali_sw[1], obj->acc_cali_sw[2]);
+		}
+#endif		
+		#if 1
+				obj->data[MPU6050C_ACC_AXIS_X] = obj->data[MPU6050C_ACC_AXIS_X] * GRAVITY_EARTH_1000 / (obj->reso->sensitivity);
+				obj->data[MPU6050C_ACC_AXIS_Y] = obj->data[MPU6050C_ACC_AXIS_Y] * GRAVITY_EARTH_1000 / (obj->reso->sensitivity);
+				obj->data[MPU6050C_ACC_AXIS_Z] = obj->data[MPU6050C_ACC_AXIS_Z] * GRAVITY_EARTH_1000 / (obj->reso->sensitivity);
+		#endif
+
+		obj->data[MPU6050C_ACC_AXIS_X] = obj->data[MPU6050C_ACC_AXIS_X] + obj->acc_cali_sw[MPU6050C_ACC_AXIS_X];
+		obj->data[MPU6050C_ACC_AXIS_Y] = obj->data[MPU6050C_ACC_AXIS_Y] + obj->acc_cali_sw[MPU6050C_ACC_AXIS_Y];
+		obj->data[MPU6050C_ACC_AXIS_Z] = obj->data[MPU6050C_ACC_AXIS_Z] + obj->acc_cali_sw[MPU6050C_ACC_AXIS_Z];
+		/*remap coordinate*/
+		data[obj->cvt.map[MPU6050C_ACC_AXIS_X]] = obj->cvt.sign[MPU6050C_ACC_AXIS_X]*obj->data[MPU6050C_ACC_AXIS_X];
+		data[obj->cvt.map[MPU6050C_ACC_AXIS_Y]] = obj->cvt.sign[MPU6050C_ACC_AXIS_Y]*obj->data[MPU6050C_ACC_AXIS_Y];
+		data[obj->cvt.map[MPU6050C_ACC_AXIS_Z]] = obj->cvt.sign[MPU6050C_ACC_AXIS_Z]*obj->data[MPU6050C_ACC_AXIS_Z];
+
+		////Out put the mg
+		#if 0
+		data[MPU6050C_ACC_AXIS_X] = data[MPU6050C_ACC_AXIS_X] * GRAVITY_EARTH_1000 / 16384;
+		data[MPU6050C_ACC_AXIS_Y] = data[MPU6050C_ACC_AXIS_Y] * GRAVITY_EARTH_1000 / 16384;
+		data[MPU6050C_ACC_AXIS_Z] = data[MPU6050C_ACC_AXIS_Z] * GRAVITY_EARTH_1000 / 16384;
+		#endif
+	}
+	
+	sprintf(buf, "%04x %04x %04x", data[MPU6050C_ACC_AXIS_X],data[MPU6050C_ACC_AXIS_Y],data[MPU6050C_ACC_AXIS_Z]);
+
+#if DEBUG		
+	if(atomic_read(&obj->trace) & ACC_TRC_DATA)
+	{
+		GSE_LOG("get acc data packet:[%d %d %d]\n", data[0], data[1], data[2]);
+	}
+#endif
+	
+	return 0;
+	
+}
+
+/*----------------------------------------------------------------------------*/
+static int MPU6050C_ACC_ReadGyroData(struct i2c_client *client, char *buf, int bufsize)
+{
+	char databuf[6];	
+	int data[3];
+	struct mpu6050c_acc_i2c_data *obj = i2c_get_clientdata(client);  
+	
+	if(!test_bit(CMC_BIT_GYRO, &obj->enable))
+	{
+		set_bit(CMC_BIT_GYRO, &obj->enable);
+		MPU6050C_ACC_SetPWR_MGMT_12(client);
+		msleep(50);
+	}
+
+	if(hwmsen_read_block(client, MPU6050C_ACC_REG_GYRO_XOUT_H, databuf, 6))
+	{
+		GSE_ERR("MPU6050C_ACC read gyroscope data  error\n");
+		return -2;
+	}
+	else
+	{
+		obj->data[MPU6050C_ACC_AXIS_X] = ((s16)((databuf[MPU6050C_ACC_AXIS_X*2+1]) | (databuf[MPU6050C_ACC_AXIS_X*2] << 8)));
+		obj->data[MPU6050C_ACC_AXIS_Y] = ((s16)((databuf[MPU6050C_ACC_AXIS_Y*2+1]) | (databuf[MPU6050C_ACC_AXIS_Y*2] << 8)));
+		obj->data[MPU6050C_ACC_AXIS_Z] = ((s16)((databuf[MPU6050C_ACC_AXIS_Z*2+1]) | (databuf[MPU6050C_ACC_AXIS_Z*2] << 8)));
+#if DEBUG		
+		if(atomic_read(&obj->trace) & ACC_TRC_RAWDATA)
+		{
+			GSE_LOG("read gyro register: %d, %d, %d, %d, %d, %d",
+				databuf[0], databuf[1], databuf[2], databuf[3], databuf[4], databuf[5]);
+			GSE_LOG("get gyro raw data (0x%08X, 0x%08X, 0x%08X) -> (%5d, %5d, %5d)\n", 
+				obj->data[MPU6050C_ACC_AXIS_X],obj->data[MPU6050C_ACC_AXIS_Y],obj->data[MPU6050C_ACC_AXIS_Z],
+				obj->data[MPU6050C_ACC_AXIS_X],obj->data[MPU6050C_ACC_AXIS_Y],obj->data[MPU6050C_ACC_AXIS_Z]);
+			GSE_LOG("gyro_cali_sw[0] =%d,gyro_cali_sw[1] =%d,gyro_cali_sw[2] =%d,\n",
+				obj->gyro_cali_sw[0], obj->gyro_cali_sw[1], obj->gyro_cali_sw[2]);
+
+		}
+#endif		
+		obj->data[MPU6050C_ACC_AXIS_X] = obj->data[MPU6050C_ACC_AXIS_X] + obj->gyro_cali_sw[MPU6050C_ACC_AXIS_X];
+		obj->data[MPU6050C_ACC_AXIS_Y] = obj->data[MPU6050C_ACC_AXIS_Y] + obj->gyro_cali_sw[MPU6050C_ACC_AXIS_Y];
+		obj->data[MPU6050C_ACC_AXIS_Z] = obj->data[MPU6050C_ACC_AXIS_Z] + obj->gyro_cali_sw[MPU6050C_ACC_AXIS_Z];
+
+		/*remap coordinate*/
+		data[obj->cvt.map[MPU6050C_ACC_AXIS_X]] = obj->cvt.sign[MPU6050C_ACC_AXIS_X]*obj->data[MPU6050C_ACC_AXIS_X];
+		data[obj->cvt.map[MPU6050C_ACC_AXIS_Y]] = obj->cvt.sign[MPU6050C_ACC_AXIS_Y]*obj->data[MPU6050C_ACC_AXIS_Y];
+		data[obj->cvt.map[MPU6050C_ACC_AXIS_Z]] = obj->cvt.sign[MPU6050C_ACC_AXIS_Z]*obj->data[MPU6050C_ACC_AXIS_Z];
+
+		//Out put the degree/second(o/s)
+		data[MPU6050C_ACC_AXIS_X] = data[MPU6050C_ACC_AXIS_X] * MPU6050C_FS_MAX_LSB / MPU6050C_DEFAULT_LSB;
+		data[MPU6050C_ACC_AXIS_Y] = data[MPU6050C_ACC_AXIS_Y] * MPU6050C_FS_MAX_LSB / MPU6050C_DEFAULT_LSB;
+		data[MPU6050C_ACC_AXIS_Z] = data[MPU6050C_ACC_AXIS_Z] * MPU6050C_FS_MAX_LSB / MPU6050C_DEFAULT_LSB;
+
+	
+	}
+
+	sprintf(buf, "%04x %04x %04x", data[MPU6050C_ACC_AXIS_X],data[MPU6050C_ACC_AXIS_Y],data[MPU6050C_ACC_AXIS_Z]);
+
+#if DEBUG		
+	if(atomic_read(&obj->trace) & ACC_TRC_DATA)
+	{
+		GSE_LOG("get gyro data packet:[%d %d %d]\n", data[0], data[1], data[2]);
+	}
+#endif
+	
+	return 0;
+	
+}
+
+//for factory mode
+
+/*----------------------------------------------------------------------------*/
+static int MPU6050C_ACC_ReadChipInfo(struct i2c_client *client, char *buf, int bufsize)
+{
+	u8 databuf[10];    
+
+	memset(databuf, 0, sizeof(u8)*10);
+
+	if((NULL == buf)||(bufsize<=30))
+	{
+		return -1;
+	}
+	
+	if(NULL == client)
+	{
+		*buf = 0;
+		return -2;
+	}
+
+	sprintf(buf, "MPU6050C_ACC Chip");
+	return 0;
+}
+
+
+/*----------------------------------------------------------------------------*/
+static ssize_t show_chipinfo_value(struct device_driver *ddri, char *buf)
+{
+	struct i2c_client *client = mpu6050c_acc_i2c_client;
+	char strbuf[MPU6050C_ACC_BUFSIZE];
+	if(NULL == client)
+	{
+		GSE_ERR("i2c client is null!!\n");
+		return 0;
+	}
+	
+	MPU6050C_ACC_ReadChipInfo(client, strbuf, MPU6050C_ACC_BUFSIZE);
+	return snprintf(buf, PAGE_SIZE, "%s\n", strbuf);        
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t show_sensordata_value(struct device_driver *ddri, char *buf)
+{
+	struct i2c_client *client = mpu6050c_acc_i2c_client;
+	char strbuf[MPU6050C_ACC_BUFSIZE];
+	
+	if(NULL == client)
+	{
+		GSE_ERR("i2c client is null!!\n");
+		return 0;
+	}
+	
+	MPU6050C_ACC_ReadAccData(client, strbuf, MPU6050C_ACC_BUFSIZE);
+	return snprintf(buf, PAGE_SIZE, "%s\n", strbuf);;            
+}
+
+/*----------------------------------------------------------------------------*/
+static ssize_t show_trace_value(struct device_driver *ddri, char *buf)
+{
+	ssize_t res;
+	struct mpu6050c_acc_i2c_data *obj = mpu6050c_obj_i2c_data;
+	if (obj == NULL)
+	{
+		GSE_ERR("i2c_data obj is null!!\n");
+		return 0;
+	}
+	
+	res = snprintf(buf, PAGE_SIZE, "0x%04X\n", atomic_read(&obj->trace));     
+	return res;    
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t store_trace_value(struct device_driver *ddri, const char *buf, size_t count)
+{
+	struct mpu6050c_acc_i2c_data *obj = mpu6050c_obj_i2c_data;
+	int trace;
+	if (obj == NULL)
+	{
+		GSE_ERR("i2c_data obj is null!!\n");
+		return 0;
+	}
+	
+	if(1 == sscanf(buf, "0x%x", &trace))
+	{
+		atomic_set(&obj->trace, trace);
+	}	
+	else
+	{
+		GSE_ERR("invalid content: '%s', length = %lld\n", buf, (long long)count);
+	}
+	
+	return count;    
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t show_status_value(struct device_driver *ddri, char *buf)
+{
+	ssize_t len = 0;    
+	struct mpu6050c_acc_i2c_data *obj = mpu6050c_obj_i2c_data;
+	if (obj == NULL)
+	{
+		GSE_ERR("i2c_data obj is null!!\n");
+		return 0;
+	}	
+	
+	if(obj->hw)
+	{
+		len += snprintf(buf+len, PAGE_SIZE-len, "CUST: %d %d (%d %d)\n", 
+	            obj->hw->i2c_num, obj->hw->direction, obj->hw->power_id, obj->hw->power_vol);   
+	}
+	else
+	{
+		len += snprintf(buf+len, PAGE_SIZE-len, "CUST: NULL\n");
+	}
+	return len;    
+}
+/*----------------------------------------------------------------------------*/
+static DRIVER_ATTR(chipinfo,             S_IRUGO, show_chipinfo_value,      NULL);
+static DRIVER_ATTR(sensordata,           S_IRUGO, show_sensordata_value,    NULL);
+static DRIVER_ATTR(trace,      S_IWUSR | S_IRUGO, show_trace_value,         store_trace_value);
+static DRIVER_ATTR(status,               S_IRUGO, show_status_value,        NULL);
+/*----------------------------------------------------------------------------*/
+static struct driver_attribute *MPU6050C_ACC_attr_list[] = {
+	&driver_attr_chipinfo,     /*chip information*/
+	&driver_attr_sensordata,   /*dump sensor data*/	
+	&driver_attr_trace,        /*trace log*/
+	&driver_attr_status,        
+};
+/*----------------------------------------------------------------------------*/
+static int mpu6050c_acc_create_attr(struct device_driver *driver) 
+{
+	int idx, err = 0;
+	int num = (int)(sizeof(MPU6050C_ACC_attr_list)/sizeof(MPU6050C_ACC_attr_list[0]));
+	if (driver == NULL)
+	{
+		return -EINVAL;
+	}
+
+	for(idx = 0; idx < num; idx++)
+	{
+		if(0 != (err = driver_create_file(driver, MPU6050C_ACC_attr_list[idx])))
+		{            
+			GSE_ERR("driver_create_file (%s) = %d\n", MPU6050C_ACC_attr_list[idx]->attr.name, err);
+			break;
+		}
+	}    
+	return err;
+}
+/*----------------------------------------------------------------------------*/
+static int mpu6050c_acc_delete_attr(struct device_driver *driver)
+{
+	int idx ,err = 0;
+	int num = (int)(sizeof(MPU6050C_ACC_attr_list)/sizeof(MPU6050C_ACC_attr_list[0]));
+
+	if(driver == NULL)
+	{
+		return -EINVAL;
+	}
+	
+
+	for(idx = 0; idx < num; idx++)
+	{
+		driver_remove_file(driver, MPU6050C_ACC_attr_list[idx]);
+	}
+	
+
+	return err;
+}
+#if 0 // for build error
+/*----------------------------------------------------------------------------*/
+static int mpu6050c_acc_gpio_config(void)
+{
+    //because we donot use EINT ,to support low power
+    // config to GPIO input mode + PD    
+    //set   GPIO_MSE_EINT_PIN
+#if 0
+    mt_set_gpio_mode(GPIO_GYRO_EINT_PIN, GPIO_GYRO_EINT_PIN_M_GPIO);
+    mt_set_gpio_dir(GPIO_GYRO_EINT_PIN, GPIO_DIR_IN);
+	mt_set_gpio_pull_enable(GPIO_GYRO_EINT_PIN, GPIO_PULL_ENABLE);
+	mt_set_gpio_pull_select(GPIO_GYRO_EINT_PIN, GPIO_PULL_DOWN);
+#endif
+	return 0;
+}
+#endif
+static int mpu6050c_acc_init_client(struct i2c_client *client, bool enable)
+{
+	struct mpu6050c_acc_i2c_data *obj = i2c_get_clientdata(client);
+	int res = 0;
+	GSE_FUN();	
+	//mpu6050c_acc_gpio_config();
+	res = MPU6050C_ACC_Reset(client);
+	if(res != MPU6050C_ACC_SUCCESS)
+	{
+		GSE_ERR("MPU6050C_ACC_Reset failed!\n");
+		return res;
+	}
+	res = MPU6050C_ACC_SetPWR_MGMT_12(client);
+	if(res != MPU6050C_ACC_SUCCESS)
+	{
+		return res;
+	}
+	
+	
+	res = MPU6050C_ACC_SetAccDataRange(client,MPU6050C_ACC_ACC_RNG_2G);
+	if(res != MPU6050C_ACC_SUCCESS) 
+	{
+		GSE_ERR("MPU6050C_ACC_SetAccDataRange failed!\n");
+		return res;
+	}
+	res = MPU6050C_ACC_SetGyroDataRange(client,MPU6050C_ACC_GYRO_RNG_1000);
+	if(res != MPU6050C_ACC_SUCCESS) 
+	{
+		GSE_ERR("MPU6050C_ACC_SetGyroDataRange failed!\n");
+		return res;
+	}
+	res = MPU6050C_ACC_SetDataBandWidth(client,MPU6050C_ACC_RATE_1K_LPFB_98HZ);
+	if(res != MPU6050C_ACC_SUCCESS) 
+	{
+		GSE_ERR("MPU6050C_ACC_SetDataBandWidth failed!\n");
+		return res;
+	}
+
+	// Set 125HZ sample rate
+	res = MPU6050C_ACC_SetSampleRate(client, 125);
+	if(res != MPU6050C_ACC_SUCCESS ) 
+	{
+		GSE_ERR("MPU6050C_ACC_SetSampleRate failed!\n");
+		return res;
+	}
+	GSE_LOG("mpu6050c_acc_init_client OK!\n");
+
+#ifdef CONFIG_MPU6050C_ACC_LOWPASS
+	memset(&obj->fir, 0x00, sizeof(obj->fir));  
+#endif
+
+	return MPU6050C_ACC_SUCCESS;
+}
+
+/*----------------------------------------------------------------------------*/
+int mpu6050c_acc_acc_operate(void* self, uint32_t command, void* buff_in, int size_in,
+		void* buff_out, int size_out, int* actualout)
+{
+	int err = 0;
+	int value;
+	struct mpu6050c_acc_i2c_data *priv = (struct mpu6050c_acc_i2c_data*)self;
+	hwm_sensor_data* acc_data;
+	char buff[MPU6050C_ACC_BUFSIZE];	
+
+	switch (command)
+	{
+		case SENSOR_DELAY:
+			if((buff_in == NULL) || (size_in < sizeof(int)))
+			{
+				GSE_ERR("Set delay parameter error!\n");
+				err = -EINVAL;
+			}
+			else
+			{
+			
+			}
+			break;
+
+		case SENSOR_ENABLE:
+			if((buff_in == NULL) || (size_in < sizeof(int)))
+			{
+				GSE_ERR("Enable accscope parameter error!\n");
+				err = -EINVAL;
+			}
+			else
+			{
+				value = *(int *)buff_in;
+				//GSE_LOG("mpu6050c_acc_acc_operate SENSOR_ENABLE =%d",value);
+				if(((value == 0) && (!test_bit(CMC_BIT_ACC, &mpu6050c_obj_i2c_data->enable))) 
+					||((value == 1) && (test_bit(CMC_BIT_ACC, &mpu6050c_obj_i2c_data->enable))))
+				{
+					GSE_LOG("accscope device have updated!\n");
+				}
+				else
+				{
+					if(value == true)
+					{
+						set_bit(CMC_BIT_ACC, &mpu6050c_obj_i2c_data->enable);
+					}
+					else
+					{
+						clear_bit(CMC_BIT_ACC, &mpu6050c_obj_i2c_data->enable);
+					}
+					err = MPU6050C_ACC_SetPWR_MGMT_12(priv->client);
+				}
+			}
+			break;
+
+		case SENSOR_GET_DATA:
+			if((buff_out == NULL) || (size_out< sizeof(hwm_sensor_data)))
+			{
+				GSE_ERR("get accscope data parameter error!\n");
+				err = -EINVAL;
+			}
+			else
+			{
+				acc_data = (hwm_sensor_data *)buff_out;
+				MPU6050C_ACC_ReadAccData(priv->client, buff, MPU6050C_ACC_BUFSIZE);
+				sscanf(buff, "%x %x %x", &acc_data->values[0], 
+					&acc_data->values[1], &acc_data->values[2]);				
+				acc_data->status = SENSOR_STATUS_ACCURACY_MEDIUM;				
+				acc_data->value_divide = 1000;
+				//GSE_ERR("X :%d,Y: %d, Z: %d\n",gsensor_data->values[0],gsensor_data->values[1],gsensor_data->values[2]);
+			}
+			break;
+		default:
+			GSE_ERR("accscope operate function no this parameter %d!\n", command);
+			err = -1;
+			break;
+	}
+
+	return err;
+}
+
+int mpu6050c_acc_gyro_operate(void* self, uint32_t command, void* buff_in, int size_in,
+		void* buff_out, int size_out, int* actualout)
+{
+	int err = 0;
+	int value;	
+	struct mpu6050c_acc_i2c_data *priv = (struct mpu6050c_acc_i2c_data*)self;
+	hwm_sensor_data* gyro_data;
+	char buff[MPU6050C_ACC_BUFSIZE];	
+
+	switch (command)
+	{
+		case SENSOR_DELAY:
+			if((buff_in == NULL) || (size_in < sizeof(int)))
+			{
+				GSE_ERR("Set delay parameter error!\n");
+				err = -EINVAL;
+			}
+			else
+			{
+			
+			}
+			break;
+
+		case SENSOR_ENABLE:
+			if((buff_in == NULL) || (size_in < sizeof(int)))
+			{
+				GSE_ERR("Enable gyroscope parameter error!\n");
+				err = -EINVAL;
+			}
+			else
+			{
+				value = *(int *)buff_in;
+				//GSE_LOG("mpu6050c_acc_gyro_operate SENSOR_ENABLE =%d",value);
+				if(((value == 0) && (!test_bit(CMC_BIT_GYRO, &mpu6050c_obj_i2c_data->enable))) 
+					||((value == 1) && (test_bit(CMC_BIT_GYRO, &mpu6050c_obj_i2c_data->enable))))
+				{
+					GSE_LOG("accscope device have updated!\n");
+				}
+				else
+				{
+					if(value == true)
+					{
+						set_bit(CMC_BIT_GYRO, &mpu6050c_obj_i2c_data->enable);
+					}
+					else
+					{
+						clear_bit(CMC_BIT_GYRO, &mpu6050c_obj_i2c_data->enable);
+					}
+					err = MPU6050C_ACC_SetPWR_MGMT_12(priv->client);
+				}
+			}
+			break;
+
+		case SENSOR_GET_DATA:
+			if((buff_out == NULL) || (size_out< sizeof(hwm_sensor_data)))
+			{
+				GSE_ERR("get gyroscope data parameter error!\n");
+				err = -EINVAL;
+			}
+			else
+			{
+				gyro_data = (hwm_sensor_data *)buff_out;
+				MPU6050C_ACC_ReadGyroData(priv->client, buff, MPU6050C_ACC_BUFSIZE);
+				sscanf(buff, "%x %x %x", &gyro_data->values[0], 
+									&gyro_data->values[1], &gyro_data->values[2]);				
+				gyro_data->status = SENSOR_STATUS_ACCURACY_MEDIUM;				
+				gyro_data->value_divide = DEGREE_TO_RAD;
+			}
+			break;
+		default:
+			GSE_ERR("gyroscope operate function no this parameter %d!\n", command);
+			err = -1;
+			break;
+	}
+
+	return err;
+}
+
+/****************************************************************************** 
+ * Function Configuration
+******************************************************************************/
+static int mpu6050c_acc_open(struct inode *inode, struct file *file)
+{
+	file->private_data = mpu6050c_acc_i2c_client;
+
+	if(file->private_data == NULL)
+	{
+		GSE_ERR("null pointer!!\n");
+		return -EINVAL;
+	}
+	return nonseekable_open(inode, file);
+}
+/*----------------------------------------------------------------------------*/
+static int mpu6050c_acc_release(struct inode *inode, struct file *file)
+{
+	file->private_data = NULL;
+	return 0;
+}
+
+/*----------------------------------------------------------------------------*/
+//static int mpu6050c_acc_ioctl(struct inode *inode, struct file *file, unsigned int cmd,
+//       unsigned long arg)
+static long mpu6050c_acc_unlocked_ioctl(struct file *file, unsigned int cmd,
+       unsigned long arg)
+{
+	struct i2c_client *client = (struct i2c_client*)file->private_data;
+	struct mpu6050c_acc_i2c_data *obj = (struct mpu6050c_acc_i2c_data*)i2c_get_clientdata(client);	
+	char strbuf[MPU6050C_ACC_BUFSIZE] = {0};
+	//s16 *SMTdata;
+	void __user *data;
+	long err = 0;
+	int copy_cnt = 0;
+	SENSOR_DATA sensor_data;
+	int cali[3];
+	int smtRes=0;
+	//GSE_FUN();
+	
+	if(_IOC_DIR(cmd) & _IOC_READ)
+	{
+		err = !access_ok(VERIFY_WRITE, (void __user *)arg, _IOC_SIZE(cmd));
+	}
+	else if(_IOC_DIR(cmd) & _IOC_WRITE)
+	{
+		err = !access_ok(VERIFY_READ, (void __user *)arg, _IOC_SIZE(cmd));
+	}
+
+	if(err)
+	{
+		GSE_ERR("access error: %08X, (%2d, %2d)\n", cmd, _IOC_DIR(cmd), _IOC_SIZE(cmd));
+		return -EFAULT;
+	}
+
+
+	switch(cmd)
+	{
+		case GYROSCOPE_IOCTL_INIT:
+			mpu6050c_acc_init_client(client, false);			
+			break;
+		case GYROSCOPE_IOCTL_SMT_DATA:
+			data = (void __user *) arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}
+			copy_cnt = copy_to_user(data, &smtRes,  sizeof(smtRes));
+			
+			if(copy_cnt)
+			{
+				err = -EFAULT;
+				GSE_LOG("copy gyro data to user failed!\n");
+			}	
+			GSE_LOG("copy gyro data to user OK: %d!\n", copy_cnt);
+
+			break;
+		case GYROSCOPE_IOCTL_READ_SENSORDATA:
+			data = (void __user *) arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}
+			MPU6050C_ACC_ReadGyroData(client, strbuf, MPU6050C_ACC_BUFSIZE);
+			if(copy_to_user(data, strbuf, sizeof(strbuf)))
+			{
+				err = -EFAULT;
+				break;	  
+			}				 
+			break;
+
+		case GYROSCOPE_IOCTL_SET_CALI:
+			data = (void __user*)arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}
+			if(copy_from_user(&sensor_data, data, sizeof(sensor_data)))
+			{
+				err = -EFAULT;
+				break;	  
+			}
+			
+			else
+			{
+				GSE_LOG("fwq going to set cali\n");
+				cali[MPU6050C_ACC_AXIS_X] = sensor_data.x * MPU6050C_DEFAULT_LSB / MPU6050C_FS_MAX_LSB;
+				cali[MPU6050C_ACC_AXIS_Y] = sensor_data.y * MPU6050C_DEFAULT_LSB / MPU6050C_FS_MAX_LSB;
+				cali[MPU6050C_ACC_AXIS_Z] = sensor_data.z * MPU6050C_DEFAULT_LSB / MPU6050C_FS_MAX_LSB;			  
+				err = MPU6050C_ACC_WriteGyroCalibration(client, cali);			 
+				GSE_LOG("fwq GSENSOR_IOCTL_SET_CALI!!sensor_data .x =%d,sensor_data .z =%d,sensor_data .z =%d \n",sensor_data.x,sensor_data.y,sensor_data.z);
+			}
+			break;
+
+		case GYROSCOPE_IOCTL_CLR_CALI:
+			err = MPU6050C_ACC_ResetGyroCalibration(client);
+			break;
+
+		case GYROSCOPE_IOCTL_GET_CALI:
+			data = (void __user*)arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}
+			err = MPU6050C_ACC_ReadGyroCalibration(client, cali);
+			if(err)
+			{
+				break;
+			}
+			
+			sensor_data.x = cali[MPU6050C_ACC_AXIS_X] * MPU6050C_FS_MAX_LSB / MPU6050C_DEFAULT_LSB;
+			sensor_data.y = cali[MPU6050C_ACC_AXIS_Y] * MPU6050C_FS_MAX_LSB / MPU6050C_DEFAULT_LSB;
+			sensor_data.z = cali[MPU6050C_ACC_AXIS_Z] * MPU6050C_FS_MAX_LSB / MPU6050C_DEFAULT_LSB;
+			if(copy_to_user(data, &sensor_data, sizeof(sensor_data)))
+			{
+				err = -EFAULT;
+				break;
+			}		
+			break;
+			case GSENSOR_IOCTL_INIT:
+				mpu6050c_acc_init_client(client, false);
+			break;
+			case GSENSOR_IOCTL_READ_CHIPINFO:
+				GSE_LOG("fwq GSENSOR_IOCTL_READ_CHIPINFO\n");
+				data = (void __user *) arg;
+				if(data == NULL)
+				{
+					err = -EINVAL;
+					break;	  
+				}
+				
+				MPU6050C_ACC_ReadChipInfo(client, strbuf, MPU6050C_ACC_BUFSIZE);
+				if(copy_to_user(data, strbuf, strlen(strbuf)+1))
+				{
+					err = -EFAULT;
+					break;
+				}				 
+
+			break;
+			case GSENSOR_IOCTL_READ_SENSORDATA:
+				data = (void __user *) arg;
+				if(data == NULL)
+				{
+					err = -EINVAL;
+					break;	  
+				}
+				MPU6050C_ACC_ReadAccData(client, strbuf, MPU6050C_ACC_BUFSIZE);
+				if(copy_to_user(data, strbuf, strlen(strbuf)+1))
+				{
+					err = -EFAULT;
+					break;	  
+				}				 
+			break;
+			case GSENSOR_IOCTL_READ_OFFSET:
+				
+			break;
+			case GSENSOR_IOCTL_READ_RAW_DATA:
+			break;
+			case GSENSOR_IOCTL_SET_CALI:
+			GSE_LOG("fwq GSENSOR_IOCTL_SET_CALI!!\n");
+			data = (void __user*)arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}
+			if(copy_from_user(&sensor_data, data, sizeof(sensor_data)))
+			{
+				err = -EFAULT;
+				break;	  
+			}
+			if(atomic_read(&obj->suspend))
+			{
+				GSE_ERR("Perform calibration in suspend state!!\n");
+				err = -EINVAL;
+			}
+			else
+			{	
+				GSE_LOG("fwq going to set cali\n");
+			#if 0
+				cali[MPU6050C_ACC_AXIS_X] = sensor_data.x * obj->reso->sensitivity / GRAVITY_EARTH_1000;
+				cali[MPU6050C_ACC_AXIS_Y] = sensor_data.y * obj->reso->sensitivity / GRAVITY_EARTH_1000;
+				cali[MPU6050C_ACC_AXIS_Z] = sensor_data.z * obj->reso->sensitivity / GRAVITY_EARTH_1000;
+			#else
+				cali[MPU6050C_ACC_AXIS_X] = sensor_data.x;
+				cali[MPU6050C_ACC_AXIS_Y] = sensor_data.y;
+				cali[MPU6050C_ACC_AXIS_Z] = sensor_data.z;
+			
+			#endif
+				err = MPU6050C_ACC_WriteCalibration(client, cali);
+				GSE_LOG("fwq GSENSOR_IOCTL_SET_CALI!!sensor_data .x =%d,sensor_data .z =%d,sensor_data .z =%d \n",sensor_data.x,sensor_data.y,sensor_data.z);
+			}
+
+			break;			
+			case GSENSOR_IOCTL_CLR_CALI:
+				GSE_LOG("fwq GSENSOR_IOCTL_CLR_CALI!!\n");
+				err = MPU6050C_ACC_ResetCalibration(client);
+			break;
+			case GSENSOR_IOCTL_GET_CALI:
+				GSE_LOG("fwq GSENSOR_IOCTL_GET_CALI\n");
+				data = (void __user*)arg;
+				if(data == NULL)
+				{
+					err = -EINVAL;
+					break;	  
+				}
+                err = MPU6050C_ACC_ReadCalibration(client, cali);
+				if(err)
+				{
+					break;
+				}
+				#if 0
+				sensor_data.x = cali[MPU6050C_ACC_AXIS_X] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+				sensor_data.y = cali[MPU6050C_ACC_AXIS_Y] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+				sensor_data.z = cali[MPU6050C_ACC_AXIS_Z] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+				#else
+				sensor_data.x = cali[MPU6050C_ACC_AXIS_X];
+				sensor_data.y = cali[MPU6050C_ACC_AXIS_Y];
+				sensor_data.z = cali[MPU6050C_ACC_AXIS_Z];
+				
+				#endif
+				if(copy_to_user(data, &sensor_data, sizeof(sensor_data)))
+				{
+					err = -EFAULT;
+					break;
+				}		
+
+			break;
+		default:
+			GSE_ERR("unknown IOCTL: 0x%08x\n", cmd);
+			err = -ENOIOCTLCMD;
+			break;			
+	}
+	return err;
+}
+
+
+/*----------------------------------------------------------------------------*/
+static struct file_operations mpu6050c_acc_fops = {
+//	.owner = THIS_MODULE,//modified
+	.open = mpu6050c_acc_open,
+	.release = mpu6050c_acc_release,
+	.unlocked_ioctl = mpu6050c_acc_unlocked_ioctl,
+};
+/*----------------------------------------------------------------------------*/
+static struct miscdevice mpu6050c_acc_device = {
+	.minor = MISC_DYNAMIC_MINOR,
+	.name = "gsensor",
+	.fops = &mpu6050c_acc_fops,
+};
+#if 1
+/*----------------------------------------------------------------------------*/
+
+static struct miscdevice mpu6050c_acc_gyro_device = {
+	.minor = MISC_DYNAMIC_MINOR,
+	.name = "gyroscope",
+	.fops = &mpu6050c_acc_fops,
+};
+#endif
+/*----------------------------------------------------------------------------*/
+#ifndef CONFIG_HAS_EARLYSUSPEND
+/*----------------------------------------------------------------------------*/
+static int mpu6050c_acc_suspend(struct i2c_client *client, pm_message_t msg) 
+{
+	struct mpu6050c_acc_i2c_data *obj = i2c_get_clientdata(client);    
+	int err; 
+	GSE_FUN();   
+
+	if(msg.event == PM_EVENT_SUSPEND)
+	{   
+		if(obj == NULL)
+		{
+			GSE_ERR("null pointer!!\n");
+			return -EINVAL;
+		}
+		atomic_set(&obj->suspend, 1);		
+
+		err = MPU6050C_ACC_SetPWR_MGMT_12(client);
+		if(err <= 0)
+		{
+			return err;
+		}
+	}
+	return 0;//modified
+}
+/*----------------------------------------------------------------------------*/
+static int mpu6050c_acc_resume(struct i2c_client *client)
+{
+	struct mpu6050c_acc_i2c_data *obj = i2c_get_clientdata(client);        
+	int err;
+	GSE_FUN();
+
+	if(obj == NULL)
+	{
+		GSE_ERR("null pointer!!\n");
+		return -EINVAL;
+	}
+
+	MPU6050C_ACC_power(obj->hw, 1);
+	err = mpu6050c_acc_init_client(client, false);
+	if(err)
+	{
+		GSE_ERR("initialize client fail!!\n");
+		return err;        
+	}
+	atomic_set(&obj->suspend, 0);
+
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+#else /*CONFIG_HAS_EARLY_SUSPEND is defined*/
+/*----------------------------------------------------------------------------*/
+static void mpu6050c_acc_early_suspend(struct early_suspend *h) 
+{
+	struct mpu6050c_acc_i2c_data *obj = container_of(h, struct mpu6050c_acc_i2c_data, early_drv);   
+	int err;
+	u8 databuf1[2]={0};
+	u8 databuf2[2]={0};
+	GSE_FUN();    
+
+	if(obj == NULL)
+	{
+		GSE_ERR("null pointer!!\n");
+		return;
+	}
+	atomic_set(&obj->suspend, 1);
+
+
+	databuf1[0] |= MPU6050C_ACC_SLEEP;
+	databuf1[0] |= MPU6050C_ACC_CLKSEL_PLL_X;
+	
+
+	databuf1[1] = databuf1[0];
+	databuf1[0] = MPU6050C_ACC_REG_PWR_MGMT_1;  
+	err = i2c_master_send(obj->client, databuf1, 0x2);
+	if(err <= 0)
+	{
+		GSE_LOG("set power mode failed!\n");
+		return ;
+	}
+	
+	databuf2[0] = 0x00;
+	databuf2[1] = databuf2[0];
+	databuf2[0] = MPU6050C_ACC_REG_PWR_MGMT_2;
+	err = i2c_master_send(obj->client, databuf2, 0x2);
+	if(err <= 0)
+	{
+		GSE_LOG("set power mode failed!\n");
+		return ;
+	}
+
+	MPU6050C_ACC_power(obj->hw, 0);
+}
+/*----------------------------------------------------------------------------*/
+static void mpu6050c_acc_late_resume(struct early_suspend *h)
+{
+	struct mpu6050c_acc_i2c_data *obj = container_of(h, struct mpu6050c_acc_i2c_data, early_drv);         
+	int err;
+	GSE_FUN();
+
+	if(obj == NULL)
+	{
+		GSE_ERR("null pointer!!\n");
+		return;
+	}
+
+	MPU6050C_ACC_power(obj->hw, 1);
+	err = mpu6050c_acc_init_client(obj->client, false);
+	if(err)
+	{
+		GSE_ERR("initialize client fail! err code %d!\n", err);
+		return;        
+	}
+	atomic_set(&obj->suspend, 0);    
+}
+/*----------------------------------------------------------------------------*/
+#endif /*CONFIG_HAS_EARLYSUSPEND*/
+/*----------------------------------------------------------------------------*/
+/*static int mpu6050c_acc_i2c_detect(struct i2c_client *client, int kind, struct i2c_board_info *info) 
+{    
+	strcpy(info->type, MPU6050C_ACC_DEV_NAME);
+	return 0;
+}
+*/
+/*----------------------------------------------------------------------------*/
+static int mpu6050c_acc_i2c_probe(struct i2c_client *client, const struct i2c_device_id *id)
+{
+	struct i2c_client *new_client;
+	struct mpu6050c_acc_i2c_data *obj;
+	//struct hwmsen_object sobj;
+	struct hwmsen_object sobj_acc, sobj_gyro;
+
+	int err = 0;
+	GSE_FUN();
+
+	if(!(obj = kzalloc(sizeof(*obj), GFP_KERNEL)))
+	{
+		err = -ENOMEM;
+		goto exit;
+	}
+	
+	memset(obj, 0, sizeof(struct mpu6050c_acc_i2c_data));
+
+	obj->hw = get_cust_acc_hw();
+	err = hwmsen_get_convert(obj->hw->direction, &obj->cvt);
+	if(err)
+	{
+		GSE_ERR("invalid direction: %d\n", obj->hw->direction);
+		goto exit;
+	}
+
+	
+	obj->enable = 0;
+	mpu6050c_obj_i2c_data = obj;
+	obj->client = client;
+	new_client = obj->client;
+	i2c_set_clientdata(new_client,obj);
+	
+	atomic_set(&obj->trace, 0);
+	atomic_set(&obj->suspend, 0);
+	
+	set_bit(CMC_BIT_ACC, &obj->enable);
+	set_bit(CMC_BIT_GYRO, &obj->enable);
+
+
+	mpu6050c_acc_i2c_client = new_client;
+	err = mpu6050c_acc_init_client(new_client, false);
+	if(err)
+	{
+		goto exit_init_failed;
+	}
+	
+
+	err = misc_register(&mpu6050c_acc_device);
+	if(err)
+	{
+		GSE_ERR("mpu6050c_acc_device misc register failed!\n");
+		goto exit_misc_device_register_failed;
+	}
+#if 1
+	err = misc_register(&mpu6050c_acc_gyro_device);
+	if(err)
+	{
+		GSE_ERR("mpu6050c_acc_gyro_device misc register failed!\n");
+		goto exit_misc_device_register_failed;
+	}
+#endif
+	err = mpu6050c_acc_create_attr(&mpu6050c_acc_driver.driver);
+	if(err)
+	{
+		GSE_ERR("mpu6050c_acc create attribute err = %d\n", err);
+		goto exit_create_attr_failed;
+	}
+	
+	sobj_acc.self = obj;
+    sobj_acc.polling = 1;
+    sobj_acc.sensor_operate = mpu6050c_acc_acc_operate;
+	err = hwmsen_attach(ID_ACCELEROMETER, &sobj_acc);
+	if(err)
+	{
+		GSE_ERR("hwmsen_attach fail = %d\n", err);
+		goto exit_kfree;
+	}
+	sobj_gyro.self = obj;
+    sobj_gyro.polling = 1;
+    sobj_gyro.sensor_operate = mpu6050c_acc_gyro_operate;
+	err = hwmsen_attach(ID_GYROSCOPE, &sobj_gyro);
+	if(err)
+	{
+		GSE_ERR("hwmsen_attach fail = %d\n", err);
+		goto exit_kfree;
+	}
+#ifdef CONFIG_HAS_EARLYSUSPEND
+	obj->early_drv.level    = EARLY_SUSPEND_LEVEL_STOP_DRAWING - 2,
+	obj->early_drv.suspend  = mpu6050c_acc_early_suspend,
+	obj->early_drv.resume   = mpu6050c_acc_late_resume,    
+	register_early_suspend(&obj->early_drv);
+#endif 
+
+	GSE_LOG("%s: OK\n", __func__);    
+	return 0;
+
+	exit_create_attr_failed:
+	misc_deregister(&mpu6050c_acc_device);
+	
+	exit_misc_device_register_failed:
+	exit_init_failed:
+	//i2c_detach_client(new_client);
+	exit_kfree:
+	kfree(obj);
+	exit:
+	GSE_ERR("%s: err = %d\n", __func__, err);        
+	return err;
+}
+
+/*----------------------------------------------------------------------------*/
+static int mpu6050c_acc_i2c_remove(struct i2c_client *client)
+{
+	int err = 0;	
+	
+	err = mpu6050c_acc_delete_attr(&mpu6050c_acc_driver.driver);
+	if(err)
+	{
+		GSE_ERR("mpu6050c_acc_delete_attr fail: %d\n", err);
+	}
+	
+	err = misc_deregister(&mpu6050c_acc_device);
+	if(err)
+	{
+		GSE_ERR("misc_deregister fail: %d\n", err);
+	}
+#if 1
+	err = misc_deregister(&mpu6050c_acc_gyro_device);
+	if(err)
+	{
+		GSE_ERR("misc_deregister fail: %d\n", err);
+	}
+#endif
+	err = hwmsen_detach(ID_ACCELEROMETER);
+	if(err)
+	{
+		GSE_ERR("hwmsen_detach fail: %d\n", err);
+	}
+	
+	err = hwmsen_detach(ID_GYROSCOPE);
+	if(err)
+	{
+		GSE_ERR("hwmsen_detach fail: %d\n", err);
+	}
+
+	mpu6050c_acc_i2c_client = NULL;
+	i2c_unregister_device(client);
+	kfree(i2c_get_clientdata(client));
+	return 0;
+}
+//extern struct mpu6050c_gyro_i2c_data* MPU6050C_Gyro_GetI2CData(void);
+extern void* MPU6050C_Gyro_GetI2CData(void);
+/*----------------------------------------------------------------------------*/
+static int mpu6050c_acc_probe(struct platform_device *pdev) 
+{
+	struct acc_hw *hw = get_cust_acc_hw();
+	GSE_FUN();
+
+	MPU6050C_ACC_power(hw, 1);
+
+
+	if(MPU6050C_Gyro_GetI2CData()!= NULL)
+	{
+		return 0;
+	}
+	else
+	{
+		if(i2c_add_driver(&mpu6050c_acc_i2c_driver))
+		{
+			GSE_ERR("add driver error\n");
+			return -1;
+		}
+	}
+
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int mpu6050c_acc_remove(struct platform_device *pdev)
+{
+    struct acc_hw *hw = get_cust_acc_hw();
+
+    GSE_FUN();    
+    MPU6050C_ACC_power(hw, 0);
+    i2c_del_driver(&mpu6050c_acc_i2c_driver);
+    return 0;
+}
+/*----------------------------------------------------------------------------*/
+#if 1
+#ifdef CONFIG_OF
+	static const struct of_device_id gsensor_of_match[] = {
+		{ .compatible = "mediatek,gsensor", },
+		{},
+	};
+#endif
+	
+	static struct platform_driver mpu6050c_acc_driver = {
+		.probe		= mpu6050c_acc_probe,
+		.remove 	= mpu6050c_acc_remove,	 
+		.driver 	= 
+		{
+			.name  = "gsensor",
+			.owner  = THIS_MODULE,
+        #ifdef CONFIG_OF
+			.of_match_table = gsensor_of_match,
+		#endif
+		}
+	};
+#else
+
+static struct platform_driver mpu6050c_acc_driver = {
+	.probe      = mpu6050c_acc_probe,
+	.remove     = mpu6050c_acc_remove,    
+	.driver     = {
+		.name  = "gsensor",
+		.owner = THIS_MODULE,//modified
+	}
+};
+#endif
+/*----------------------------------------------------------------------------*/
+static int __init mpu6050c_acc_init(void)
+{
+	//GSE_FUN();
+	struct acc_hw *hw = get_cust_acc_hw();
+	GSE_LOG("%s: i2c_number=%d\n", __func__,hw->i2c_num); 
+	i2c_register_board_info(hw->i2c_num, &i2c_mpu6050c_acc, 1);
+	if(platform_driver_register(&mpu6050c_acc_driver))
+	{
+		GSE_ERR("failed to register driver");
+		return -ENODEV;
+	}
+	return 0;    
+}
+/*----------------------------------------------------------------------------*/
+static void __exit mpu6050c_acc_exit(void)
+{
+	GSE_FUN();
+	platform_driver_unregister(&mpu6050c_acc_driver);
+}
+/*----------------------------------------------------------------------------*/
+module_init(mpu6050c_acc_init);
+module_exit(mpu6050c_acc_exit);
+/*----------------------------------------------------------------------------*/
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("MPU6050C_ACC  driver");
+MODULE_AUTHOR("Chunlei.Wang@mediatek.com");
diff --git a/drivers/misc/mediatek/accelerometer/mpu60x0/mpu60x0.h b/drivers/misc/mediatek/accelerometer/mpu60x0/mpu60x0.h
new file mode 100644
index 000000000..a566177bc
--- /dev/null
+++ b/drivers/misc/mediatek/accelerometer/mpu60x0/mpu60x0.h
@@ -0,0 +1,169 @@
+/* mpu6050c.h
+ *
+ * (C) Copyright 2008 
+ * MediaTek <www.mediatek.com>
+ *
+ * mpu300 head file for MT65xx
+ *
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ */
+#ifndef MPU60X0_H
+#define MPU60X0_H
+	 
+#include <linux/ioctl.h>
+	 
+#define MPU6050C_ACC_I2C_SLAVE_ADDR		0xD0
+#define MPU6050C_ACC_FIXED_DEVID		0xD0
+
+
+/* MPU6050C_ACC Register Map  (Please refer to MPU6050C_ACC Specifications) */
+
+#define  MPU6050C_ACC_REG_SELFTEST_X			0x0D
+#define  MPU6050C_ACC_REG_SELFTEST_Y			0x0E
+#define  MPU6050C_ACC_REG_SELFTEST_Z			0x0F
+#define  MPU6050C_ACC_REG_SELFTEST_XYZ		0x10
+
+#define  MPU6050C_ACC_REG_DATA_SMPRT_DIV	0x19
+#define  MPU6050C_ACC_REG_DATA_CONFIG	0x1A
+#define  MPU6050C_ACC_REG_GYRO_CONFIG	0x1B
+#define	 MPU6050C_ACC_REG_ACC_CONFIG	0x1C
+
+#define	 MPU6050C_ACC_REG_INT_CONFIG	0x37
+#define	 MPU6050C_ACC_REG_INT_ENABLE	0x38
+#define	 MPU6050C_ACC_REG_INT_STATUS	0x3A
+
+
+#define  MPU6050C_ACC_REG_ACCEL_XOUT_H 		 0x3b
+#define  MPU6050C_ACC_REG_ACCEL_XOUT_L 		 0x3c
+#define  MPU6050C_ACC_REG_ACCEL_YOUT_H 		 0x3d
+#define  MPU6050C_ACC_REG_ACCEL_YOUT_L 		 0x3e
+#define  MPU6050C_ACC_REG_ACCEL_ZOUT_H 		 0x3f
+#define  MPU6050C_ACC_REG_ACCEL_ZOUT_L 		 0x40
+
+#define  MPU6050C_ACC_REG_TEMP_OUT_H 		 0x41
+#define  MPU6050C_ACC_REG_TEMP_OUT_L 		 0x42
+
+#define  MPU6050C_ACC_REG_GYRO_XOUT_H 		 0x43
+#define  MPU6050C_ACC_REG_GYRO_XOUT_L 		 0x44
+#define  MPU6050C_ACC_REG_GYRO_YOUT_H 		 0x45
+#define  MPU6050C_ACC_REG_GYRO_YOUT_L 		 0x46
+#define  MPU6050C_ACC_REG_GYRO_ZOUT_H 		 0x47
+#define  MPU6050C_ACC_REG_GYRO_ZOUT_L 		 0x48
+
+#define  MPU6050C_ACC_REG_PWR_MGMT_1	0x6B
+#define  MPU6050C_ACC_REG_PWR_MGMT_2	0x6C
+
+#define  MPU6050C_ACC_REG_DEVICE_ID	0x75	
+
+ 
+
+/*MPU6050C_ACC Register Bit definitions*/ 
+
+#define MPU6050C_FS_RANGE			0x03	//set the full-scale range of the gyro sensors
+#define MPU6050C_FS_250 				0x00
+#define MPU6050C_FS_500				0x01
+#define MPU6050C_FS_1000				0x02
+#define MPU6050C_FS_2000				0x03
+#define MPU6050C_FS_MAX				0x03
+
+
+
+#define MPU6050C_FS_250_LSB			131		// LSB/(o/s)
+#define MPU6050C_FS_500_LSB			66
+#define MPU6050C_FS_1000_LSB			33
+#define MPU6050C_FS_2000_LSB			16
+#define MPU6050C_FS_MAX_LSB			131
+
+
+#define MPU6050C_ACC_SAM_RATE_MASK		0x07	//set sample rate and low padd filter configuration
+#define MPU6050C_ACC_RATE_8K_LPFB_256HZ 	0x00
+#define MPU6050C_ACC_RATE_1K_LPFB_188HZ	0x01
+#define MPU6050C_ACC_RATE_1K_LPFB_98HZ 	0x02
+#define MPU6050C_ACC_RATE_1K_LPFB_42HZ 	0x03
+#define MPU6050C_ACC_RATE_1K_LPFB_20HZ 	0x04
+#define MPU6050C_ACC_RATE_1K_LPFB_10HZ 	0x05
+#define MPU6050C_ACC_RATE_1K_LPFB_5HZ 	0x06
+
+#define MPU6050C_ACC_GYRO_RNG_250		0x0
+#define MPU6050C_ACC_GYRO_RNG_500		0x1
+#define MPU6050C_ACC_GYRO_RNG_1000		0x2
+#define MPU6050C_ACC_GYRO_RNG_2000		0x3
+
+
+#define MPU6050C_ACC_ACC_RNG_2G			0x0
+#define MPU6050C_ACC_ACC_RNG_4G			0x1
+#define MPU6050C_ACC_ACC_RNG_8G			0x2
+#define MPU6050C_ACC_ACC_RNG_16G		0x3
+
+
+#define MPU6050C_ACC_CLKSEL_8M		0x0
+#define MPU6050C_ACC_CLKSEL_PLL_X		0x1
+#define MPU6050C_ACC_CLKSEL_PLL_Y		0x2
+#define MPU6050C_ACC_CLKSEL_PLL_Z		0x3
+#define MPU6050C_ACC_CLKSEL_STOP		0x7
+
+#define	MPU6050C_ACC_SLEEP	0x40
+#define BIT_HW_RESET	0x80
+
+	 
+#define MPU6050C_ACC_SUCCESS				    0
+#define MPU6050C_ACC_ERR_I2C				  -1
+#define MPU6050C_ACC_ERR_STATUS			  -3
+#define MPU6050C_ACC_ERR_SETUP_FAILURE	  -4
+#define MPU6050C_ACC_ERR_GETGSENSORDATA  -5
+#define MPU6050C_ACC_ERR_IDENTIFICATION	  -6
+	 
+
+/*
+typedef enum  {
+    MPU6050C_ACC_SYNC_NONE = 0x0,
+    MPU6050C_ACC_SYNC_TEMP,
+    MPU6050C_ACC_SYNC_GYROX,
+    MPU6050C_ACC_SYNC_GYROY,
+    MPU6050C_ACC_SYNC_GYROZ,
+    MPU6050C_ACC_SYNC_AUXX,
+    MPU6050C_ACC_SYNC_AUXY,
+    MPU6050C_ACC_SYNC_AUXZ,
+} MPU6050C_ACC_EXT_SYNC_SEL;
+
+typedef enum  {
+    MPU6050C_ACC_FS_250 = 0x0,
+    MPU6050C_ACC_FS_500,
+    MPU6050C_ACC_FS_1000,
+    MPU6050C_ACC_FS_2000,
+    MPU6050C_ACC_FS_MAX = 0x3,
+} MPU6050C_ACC_FS_SEL;
+
+typedef enum  {
+    MPU6050C_ACC_RATE_8K_LPFB_256HZ = 0x0,
+    MPU6050C_ACC_RATE_1K_LPFB_188HZ,
+    MPU6050C_ACC_RATE_1K_LPFB_98HZ,
+    MPU6050C_ACC_RATE_1K_LPFB_42HZ,
+    MPU6050C_ACC_RATE_1K_LPFB_20HZ,
+    MPU6050C_ACC_RATE_1K_LPFB_10HZ,
+    MPU6050C_ACC_RATE_1K_LPFB_5HZ,
+} MPU6050C_ACC_SAMRATE_SEL;
+*/
+
+#define MPU6050C_ACC_BUFSIZE 60
+
+// 1 rad = 180/PI degree, MAX_LSB = 131,
+// 180*131/PI = 7506
+#define DEGREE_TO_RAD	7506
+
+//extern struct mpu6050c_acc_i2c_data* MPU6050C_Acc_GetI2CData();
+
+#endif //MPU60X0_H
+
diff --git a/drivers/misc/mediatek/accelerometer/mpu6515/Makefile b/drivers/misc/mediatek/accelerometer/mpu6515/Makefile
new file mode 100755
index 000000000..79ce9e567
--- /dev/null
+++ b/drivers/misc/mediatek/accelerometer/mpu6515/Makefile
@@ -0,0 +1,4 @@
+include $(srctree)/drivers/misc/mediatek/Makefile.custom
+
+obj-y	:=  mpu6515.o
+
diff --git a/drivers/misc/mediatek/accelerometer/mpu6515/mpu6515.c b/drivers/misc/mediatek/accelerometer/mpu6515/mpu6515.c
new file mode 100644
index 000000000..ab5605376
--- /dev/null
+++ b/drivers/misc/mediatek/accelerometer/mpu6515/mpu6515.c
@@ -0,0 +1,2735 @@
+/* MPU6515 motion sensor driver
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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.
+ *
+ */
+
+#include <linux/interrupt.h>
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include <linux/irq.h>
+#include <linux/miscdevice.h>
+#include <asm/uaccess.h>
+#include <linux/delay.h>
+#include <linux/input.h>
+#include <linux/workqueue.h>
+#include <linux/kobject.h>
+#include <linux/earlysuspend.h>
+#include <linux/platform_device.h>
+#include <asm/atomic.h>
+
+#include <cust_acc.h>
+#include <linux/hwmsensor.h>
+#include <linux/hwmsen_dev.h>
+#include <linux/sensors_io.h>
+#include "mpu6515.h"
+#include <linux/hwmsen_helper.h>
+
+#include <mach/mt_typedefs.h>
+#include <mach/mt_gpio.h>
+#include <mach/mt_pm_ldo.h>
+
+#include <accel.h>
+#include <linux/batch.h>
+#ifdef CUSTOM_KERNEL_SENSORHUB
+#include <SCP_sensorHub.h>
+#endif//#ifdef CUSTOM_KERNEL_SENSORHUB
+
+#define POWER_NONE_MACRO MT65XX_POWER_NONE
+
+/*----------------------------------------------------------------------------*/
+#define DEBUG 1
+//#define GSENSOR_UT
+/*----------------------------------------------------------------------------*/
+#define CONFIG_MPU6515_LOWPASS   /*apply low pass filter on output*/       
+#define SW_CALIBRATION
+//#define USE_EARLY_SUSPEND
+/*----------------------------------------------------------------------------*/
+#define MPU6515_AXIS_X          0
+#define MPU6515_AXIS_Y          1
+#define MPU6515_AXIS_Z          2
+#define MPU6515_AXES_NUM        3
+#define MPU6515_DATA_LEN        6
+#define MPU6515_DEV_NAME        "MPU6515G"   /* name must different with gyro mpu6515 */
+/*----------------------------------------------------------------------------*/
+static const struct i2c_device_id mpu6515_i2c_id[] = {{MPU6515_DEV_NAME,0},{}};
+static struct i2c_board_info __initdata i2c_mpu6515={ I2C_BOARD_INFO(MPU6515_DEV_NAME, (MPU6515_I2C_SLAVE_ADDR>>1))};
+
+/*----------------------------------------------------------------------------*/
+static int mpu6515_i2c_probe(struct i2c_client *client, const struct i2c_device_id *id); 
+static int mpu6515_i2c_remove(struct i2c_client *client);
+static int mpu6515_i2c_detect(struct i2c_client *client, struct i2c_board_info *info);
+#if !defined(CONFIG_HAS_EARLYSUSPEND) || !defined(USE_EARLY_SUSPEND)
+static int mpu6515_suspend(struct i2c_client *client, pm_message_t msg) ;
+static int mpu6515_resume(struct i2c_client *client);
+#endif
+
+static int gsensor_local_init(void);
+static int gsensor_remove(void);
+
+#ifdef CUSTOM_KERNEL_SENSORHUB
+static int gsensor_setup_irq(void);
+#endif//#ifdef CUSTOM_KERNEL_SENSORHUB
+static int gsensor_set_delay(u64 ns);
+/*----------------------------------------------------------------------------*/
+typedef enum
+{
+    MPU6515_TRC_FILTER  = 0x01,
+    MPU6515_TRC_RAWDATA = 0x02,
+    MPU6515_TRC_IOCTL   = 0x04,
+    MPU6515_TRC_CALI    = 0X08,
+    MPU6515_TRC_INFO    = 0X10,
+} MPU6515_TRC;
+/*----------------------------------------------------------------------------*/
+struct scale_factor
+{
+    u8  whole;
+    u8  fraction;
+};
+/*----------------------------------------------------------------------------*/
+struct data_resolution
+{
+    struct scale_factor scalefactor;
+    int                 sensitivity;
+};
+/*----------------------------------------------------------------------------*/
+#define C_MAX_FIR_LENGTH (32)
+/*----------------------------------------------------------------------------*/
+struct data_filter
+{
+    s16 raw[C_MAX_FIR_LENGTH][MPU6515_AXES_NUM];
+    int sum[MPU6515_AXES_NUM];
+    int num;
+    int idx;
+};
+/*----------------------------------------------------------------------------*/
+struct mpu6515_i2c_data
+{
+    struct i2c_client *client;
+    struct acc_hw *hw;
+    struct hwmsen_convert   cvt;
+#ifdef CUSTOM_KERNEL_SENSORHUB
+    struct work_struct	irq_work;
+#endif//#ifdef CUSTOM_KERNEL_SENSORHUB
+
+    /*misc*/
+    struct data_resolution *reso;
+    atomic_t                trace;
+    atomic_t                suspend;
+    atomic_t                selftest;
+    atomic_t                filter;
+    s16                     cali_sw[MPU6515_AXES_NUM+1];
+
+    /*data*/
+    s8                      offset[MPU6515_AXES_NUM+1];  /*+1: for 4-byte alignment*/
+    s16                     data[MPU6515_AXES_NUM+1];
+
+#ifdef CUSTOM_KERNEL_SENSORHUB
+    int                     SCP_init_done;
+#endif//#ifdef CUSTOM_KERNEL_SENSORHUB
+
+#if defined(CONFIG_MPU6515_LOWPASS)
+    atomic_t                firlen;
+    atomic_t                fir_en;
+    struct data_filter      fir;
+#endif 
+    /*early suspend*/
+#if defined(CONFIG_HAS_EARLYSUSPEND) && defined(USE_EARLY_SUSPEND)
+    struct early_suspend    early_drv;
+#endif     
+    u8                      bandwidth;
+};
+/*----------------------------------------------------------------------------*/
+static struct i2c_driver mpu6515_i2c_driver = {
+    .driver = {
+        .name           = MPU6515_DEV_NAME,
+    },
+    .probe              = mpu6515_i2c_probe,
+    .remove             = mpu6515_i2c_remove,
+    .detect             = mpu6515_i2c_detect,
+#if !defined(CONFIG_HAS_EARLYSUSPEND) || !defined(USE_EARLY_SUSPEND)
+    .suspend            = mpu6515_suspend,
+    .resume             = mpu6515_resume,
+#endif
+    .id_table = mpu6515_i2c_id,
+};
+
+/*----------------------------------------------------------------------------*/
+static struct i2c_client *mpu6515_i2c_client = NULL;
+static struct mpu6515_i2c_data *obj_i2c_data = NULL;
+static bool sensor_power = false;
+static bool scp_sensor_power = false;
+static GSENSOR_VECTOR3D gsensor_gain;
+static char selftestRes[8]= {0};
+static DEFINE_MUTEX(gsensor_mutex);
+static DEFINE_MUTEX(gsensor_scp_en_mutex);
+static bool enable_status = false;
+
+static int gsensor_init_flag =-1; // 0<==>OK -1 <==> fail
+
+static struct acc_init_info mpu6515_init_info = {
+    .name = "mpu6515",
+    .init = gsensor_local_init,
+    .uninit = gsensor_remove,
+};
+
+/*----------------------------------------------------------------------------*/
+#define GSE_TAG                  "[Gsensor] "
+#define GSE_FUN(f)               printk(GSE_TAG"%s\n", __FUNCTION__)
+#define GSE_ERR(fmt, args...)    printk(GSE_TAG"%s %d : "fmt, __FUNCTION__, __LINE__, ##args)
+#define GSE_LOG(fmt, args...)    printk(GSE_TAG fmt, ##args)
+/*----------------------------------------------------------------------------*/
+static struct data_resolution mpu6515_data_resolution[] = {
+    /*8 combination by {FULL_RES,RANGE}*/
+    {{ 0,  6}, 16384},  /*+/-2g  in 16-bit resolution:  0.06 mg/LSB*/
+    {{ 0, 12}, 8192},   /*+/-4g  in 16-bit resolution:  0.12 mg/LSB*/
+    {{ 0, 24}, 4096},   /*+/-8g  in 16-bit resolution:  0.24 mg/LSB*/
+    {{ 0, 49}, 2048},   	/*+/-16g in 16-bit resolution:  0.49 mg/LSB*/
+};
+/*----------------------------------------------------------------------------*/
+static struct data_resolution mpu6515_offset_resolution = {{ 0, 5}, 2048};
+
+static unsigned int power_on = 0;
+
+extern int MPU6515_gyro_power(void);
+extern int MPU6515_gyro_mode(void);
+
+
+int MPU6515_gse_power( void)
+{
+    return(power_on);
+}
+EXPORT_SYMBOL(MPU6515_gse_power);
+
+int MPU6515_gse_mode(void)
+{
+    return sensor_power;
+}
+EXPORT_SYMBOL(MPU6515_gse_mode);
+
+
+int MPU6515_i2c_master_send(u8 *buf, u8 len)
+{
+#ifndef GSENSOR_UT
+    int res = 0;
+    if (NULL == mpu6515_i2c_client)
+    {
+        GSE_ERR("MPU6515_i2c_master_send null ptr!!\n");
+    }
+    else
+    {
+        res = i2c_master_send(mpu6515_i2c_client, buf, len);
+    }
+
+    return res;
+#else
+    return 1;
+#endif
+}
+EXPORT_SYMBOL(MPU6515_i2c_master_send);
+
+int MPU6515_i2c_master_recv(u8 *buf, u8 len)
+{
+#ifndef GSENSOR_UT
+    int res = 0;
+    if (NULL == mpu6515_i2c_client)
+    {
+        GSE_ERR("MPU6515_i2c_master_recv null ptr!!\n");
+    }
+    else
+    {
+        res = i2c_master_recv(mpu6515_i2c_client, buf, len);
+    }
+
+    return res;
+#else
+    return 1;
+#endif
+}
+EXPORT_SYMBOL(MPU6515_i2c_master_recv);
+
+#ifdef CUSTOM_KERNEL_SENSORHUB
+int MPU6515_SCP_SetPowerMode(bool enable, int sensorType)
+{
+    static bool gsensor_scp_en_status = false;
+    static unsigned int gsensor_scp_en_map = 0;
+    SCP_SENSOR_HUB_DATA req;
+    int len;
+    int err = 0;
+
+    mutex_lock(&gsensor_scp_en_mutex);
+
+    if (sensorType >= 32)
+    {
+        GSE_ERR("Out of index!\n");
+        return -1;
+    }
+
+    if (true == enable)
+    {
+        gsensor_scp_en_map |= (1<<sensorType);
+    }
+    else
+    {
+        gsensor_scp_en_map &= ~(1<<sensorType);
+    }
+
+    if (0 == gsensor_scp_en_map)
+        enable = false;
+    else
+        enable = true;
+
+    if (gsensor_scp_en_status != enable)
+    {
+        gsensor_scp_en_status = enable;
+        
+        req.activate_req.sensorType = ID_ACCELEROMETER;
+        req.activate_req.action = SENSOR_HUB_ACTIVATE;
+        req.activate_req.enable = enable;
+        len = sizeof(req.activate_req);
+        err = SCP_sensorHub_req_send(&req, &len, 1);
+        if (err)
+        {
+            GSE_ERR("SCP_sensorHub_req_send fail!\n");
+        }
+    }
+
+    mutex_unlock(&gsensor_scp_en_mutex);
+
+    return err;
+}
+EXPORT_SYMBOL(MPU6515_SCP_SetPowerMode);
+#endif //#ifdef CUSTOM_KERNEL_SENSORHUB
+/*----------------------------------------------------------------------------*/
+static int mpu_i2c_read_block(struct i2c_client *client, u8 addr, u8 *data, u8 len){
+    int err; 
+    data[0] = addr;
+    client->addr &=I2C_MASK_FLAG;
+    client->addr |=I2C_WR_FLAG;
+    client->addr |=I2C_RS_FLAG;
+    err = i2c_master_send(client, data, (len<<8)|0x1);
+    client->addr &=I2C_MASK_FLAG;
+
+    if (err < 0)
+    {
+        GSE_ERR("i2c_transfer error: (%d %p %d) %d\n", addr, data, len, err);
+    }
+    else
+    {
+        err = 0;
+    }
+    return err;
+}
+int MPU6515_hwmsen_read_block(u8 addr, u8 *buf, u8 len)
+{
+#ifndef GSENSOR_UT
+    if (NULL == mpu6515_i2c_client)
+    {
+        GSE_ERR("MPU6515_hwmsen_read_block null ptr!!\n");
+        return MPU6515_ERR_I2C;
+    }
+    return mpu_i2c_read_block(mpu6515_i2c_client, addr, buf, len);
+#else
+    return 0;
+#endif
+}
+EXPORT_SYMBOL(MPU6515_hwmsen_read_block);
+
+int MPU6515_hwmsen_read_byte(u8 addr, u8 *buf)
+{
+#ifndef GSENSOR_UT
+    if (NULL == mpu6515_i2c_client)
+    {
+        GSE_ERR("MPU6515_hwmsen_read_byte null ptr!!\n");
+        return MPU6515_ERR_I2C;
+    }
+    return mpu_i2c_read_block(mpu6515_i2c_client, addr, buf, 1);
+#else
+    return 0;
+#endif
+}
+EXPORT_SYMBOL(MPU6515_hwmsen_read_byte);
+/*--------------------mpu6515 power control function----------------------------------*/
+static void MPU6515_power(struct acc_hw *hw, unsigned int on) 
+{
+#ifndef FPGA_EARLY_PORTING
+	static unsigned int power_on = 0;
+
+	if(hw->power_id != POWER_NONE_MACRO)		// have externel LDO
+	{        
+		GSE_LOG("power %s\n", on ? "on" : "off");
+		if(power_on == on)	// power status not change
+		{
+			GSE_LOG("ignore power control: %d\n", on);
+		}
+		else if(on)	// power on
+		{
+			if(!hwPowerOn(hw->power_id, hw->power_vol, "MPU6515G"))
+			{
+				GSE_ERR("power on fails!!\n");
+			}
+		}
+		else	// power off
+		{
+			if (!hwPowerDown(hw->power_id, "MPU6515G"))
+			{
+				GSE_ERR("power off fail!!\n");
+			}			  
+		}
+	}
+	power_on = on;    
+#endif
+}
+
+/*----------------------------------------------------------------------------*/
+static int MPU6515_SetPowerMode(struct i2c_client *client, bool enable)
+{
+    int res = 0;
+    struct mpu6515_i2c_data *obj = i2c_get_clientdata(client);
+    u8 databuf[2];
+
+    if (enable == sensor_power)
+    {
+        GSE_LOG("Sensor power status is newest!\n");
+        return MPU6515_SUCCESS;
+    }
+
+#if 0
+    databuf[0] = MPU6515_REG_POWER_CTL;   
+    res = i2c_master_send(client, databuf, 0x1);
+    if (res <= 0)
+    {
+        return MPU6515_ERR_I2C;
+    }
+
+    udelay(500);
+
+    databuf[0] = 0x0;        
+    res = i2c_master_recv(client, databuf, 1);
+    if (res <= 0)
+    {
+        return MPU6515_ERR_I2C;
+    }
+#else
+    if (hwmsen_read_byte(client, MPU6515_REG_POWER_CTL, databuf))
+    {
+        GSE_ERR("read power ctl register err!\n");
+        return MPU6515_ERR_I2C;
+    }
+#endif
+
+    if ((databuf[0] & 0x1f) != 0x1)
+        GSE_ERR("MPU6515 PWR_MGMT_1 = %x\n", databuf[0]);
+
+    databuf[0] &= ~MPU6515_SLEEP;
+
+    if (enable == FALSE)
+    {
+        if (MPU6515_gyro_mode() == false)
+        {
+            databuf[0] |= MPU6515_SLEEP;
+        }
+    }
+    else
+    {
+        // do nothing
+    }
+    databuf[1] = databuf[0];
+    databuf[0] = MPU6515_REG_POWER_CTL;
+
+    res = i2c_master_send(client, databuf, 0x2);
+
+    if (res <= 0)
+    {
+        GSE_LOG("set power mode failed!\n");
+        return MPU6515_ERR_I2C;
+    }
+
+    if (atomic_read(&obj->trace) & MPU6515_TRC_INFO)
+    {
+        GSE_LOG("set power mode ok %d!\n", databuf[1]);
+    }
+
+    sensor_power = enable;
+    return MPU6515_SUCCESS;    
+}
+/*----------------------------------------------------------------------------*/
+static int MPU6515_SetDataResolution(struct mpu6515_i2c_data *obj)
+{
+    int err;
+    u8  dat = 0, reso = 0;
+
+#ifdef GSENSOR_UT
+    GSE_FUN();
+#endif
+
+    if ((err = mpu_i2c_read_block(obj->client, MPU6515_REG_DATA_FORMAT, &dat, 1)))
+    {
+        GSE_ERR("write data format fail!!\n");
+        return err;
+    }
+
+    /*the data_reso is combined by 3 bits: {FULL_RES, DATA_RANGE}*/
+    reso = 0x00;
+    reso = (dat & MPU6515_RANGE_16G) >> 3; 
+
+    if (reso < sizeof(mpu6515_data_resolution)/sizeof(mpu6515_data_resolution[0]))
+    {
+        obj->reso = &mpu6515_data_resolution[reso];
+        return 0;
+    }
+    else
+    {
+        return -EINVAL;
+    }
+}
+/*----------------------------------------------------------------------------*/
+static int MPU6515_ReadData(struct i2c_client *client, s16 data[MPU6515_AXES_NUM])
+{
+    struct mpu6515_i2c_data *priv = i2c_get_clientdata(client);        
+    int err = 0;
+    u8 buf[MPU6515_DATA_LEN] = {0};
+
+#ifdef GSENSOR_UT
+    GSE_FUN();
+#endif
+
+    if (NULL == client)
+    {
+        return -EINVAL;
+    }
+
+    {
+        /* write then burst read */
+        mpu_i2c_read_block(client, MPU6515_REG_DATAX0, buf, MPU6515_DATA_LEN);
+
+        data[MPU6515_AXIS_X] = (s16)((buf[MPU6515_AXIS_X*2] << 8) |
+                                     (buf[MPU6515_AXIS_X*2+1] ));
+        data[MPU6515_AXIS_Y] = (s16)((buf[MPU6515_AXIS_Y*2] << 8) |
+                                     (buf[MPU6515_AXIS_Y*2+1] ));
+        data[MPU6515_AXIS_Z] = (s16)((buf[MPU6515_AXIS_Z*2] << 8) |
+                                     (buf[MPU6515_AXIS_Z*2+1] ));
+
+        if (atomic_read(&priv->trace) & MPU6515_TRC_RAWDATA)
+        {
+            GSE_LOG("[%08X %08X %08X] => [%5d %5d %5d]\n", data[MPU6515_AXIS_X], data[MPU6515_AXIS_Y], data[MPU6515_AXIS_Z],
+                    data[MPU6515_AXIS_X], data[MPU6515_AXIS_Y], data[MPU6515_AXIS_Z]);
+        }
+#ifdef CONFIG_MPU6515_LOWPASS
+        if (atomic_read(&priv->filter))
+        {
+            if (atomic_read(&priv->fir_en) && !atomic_read(&priv->suspend))
+            {
+                int idx, firlen = atomic_read(&priv->firlen);   
+                if (priv->fir.num < firlen)
+                {
+                    priv->fir.raw[priv->fir.num][MPU6515_AXIS_X] = data[MPU6515_AXIS_X];
+                    priv->fir.raw[priv->fir.num][MPU6515_AXIS_Y] = data[MPU6515_AXIS_Y];
+                    priv->fir.raw[priv->fir.num][MPU6515_AXIS_Z] = data[MPU6515_AXIS_Z];
+                    priv->fir.sum[MPU6515_AXIS_X] += data[MPU6515_AXIS_X];
+                    priv->fir.sum[MPU6515_AXIS_Y] += data[MPU6515_AXIS_Y];
+                    priv->fir.sum[MPU6515_AXIS_Z] += data[MPU6515_AXIS_Z];
+                    if (atomic_read(&priv->trace) & MPU6515_TRC_FILTER)
+                    {
+                        GSE_LOG("add [%2d] [%5d %5d %5d] => [%5d %5d %5d]\n", priv->fir.num,
+                                priv->fir.raw[priv->fir.num][MPU6515_AXIS_X], priv->fir.raw[priv->fir.num][MPU6515_AXIS_Y], priv->fir.raw[priv->fir.num][MPU6515_AXIS_Z],
+                                priv->fir.sum[MPU6515_AXIS_X], priv->fir.sum[MPU6515_AXIS_Y], priv->fir.sum[MPU6515_AXIS_Z]);
+                    }
+                    priv->fir.num++;
+                    priv->fir.idx++;
+                }
+                else
+                {
+                    idx = priv->fir.idx % firlen;
+                    priv->fir.sum[MPU6515_AXIS_X] -= priv->fir.raw[idx][MPU6515_AXIS_X];
+                    priv->fir.sum[MPU6515_AXIS_Y] -= priv->fir.raw[idx][MPU6515_AXIS_Y];
+                    priv->fir.sum[MPU6515_AXIS_Z] -= priv->fir.raw[idx][MPU6515_AXIS_Z];
+                    priv->fir.raw[idx][MPU6515_AXIS_X] = data[MPU6515_AXIS_X];
+                    priv->fir.raw[idx][MPU6515_AXIS_Y] = data[MPU6515_AXIS_Y];
+                    priv->fir.raw[idx][MPU6515_AXIS_Z] = data[MPU6515_AXIS_Z];
+                    priv->fir.sum[MPU6515_AXIS_X] += data[MPU6515_AXIS_X];
+                    priv->fir.sum[MPU6515_AXIS_Y] += data[MPU6515_AXIS_Y];
+                    priv->fir.sum[MPU6515_AXIS_Z] += data[MPU6515_AXIS_Z];
+                    priv->fir.idx++;
+                    data[MPU6515_AXIS_X] = priv->fir.sum[MPU6515_AXIS_X]/firlen;
+                    data[MPU6515_AXIS_Y] = priv->fir.sum[MPU6515_AXIS_Y]/firlen;
+                    data[MPU6515_AXIS_Z] = priv->fir.sum[MPU6515_AXIS_Z]/firlen;
+                    if (atomic_read(&priv->trace) & MPU6515_TRC_FILTER)
+                    {
+                        GSE_LOG("add [%2d] [%5d %5d %5d] => [%5d %5d %5d] : [%5d %5d %5d]\n", idx,
+                                priv->fir.raw[idx][MPU6515_AXIS_X], priv->fir.raw[idx][MPU6515_AXIS_Y], priv->fir.raw[idx][MPU6515_AXIS_Z],
+                                priv->fir.sum[MPU6515_AXIS_X], priv->fir.sum[MPU6515_AXIS_Y], priv->fir.sum[MPU6515_AXIS_Z],
+                                data[MPU6515_AXIS_X], data[MPU6515_AXIS_Y], data[MPU6515_AXIS_Z]);
+                    }
+                }
+            }
+        }
+#endif         
+    }
+    
+    return err;
+}
+/*----------------------------------------------------------------------------*/
+static int MPU6515_ReadOffset(struct i2c_client *client, s8 ofs[MPU6515_AXES_NUM])
+{    
+    int err = 0;
+
+#ifdef GSENSOR_UT
+    GSE_FUN();
+#endif
+    
+#ifdef SW_CALIBRATION
+    ofs[0]=ofs[1]=ofs[2]=0x0;
+#else
+    if ((err = mpu_i2c_read_block(client, MPU6515_REG_OFSX, ofs, MPU6515_AXES_NUM)))
+    {
+        GSE_ERR("error: %d\n", err);
+    }
+#endif
+    //GSE_LOG("offesx=%x, y=%x, z=%x",ofs[0],ofs[1],ofs[2]);
+
+    return err;    
+}
+/*----------------------------------------------------------------------------*/
+static int MPU6515_ResetCalibration(struct i2c_client *client)
+{
+    struct mpu6515_i2c_data *obj = i2c_get_clientdata(client);
+    int err = 0;
+#ifdef CUSTOM_KERNEL_SENSORHUB
+    SCP_SENSOR_HUB_DATA data;
+    MPU6515_CUST_DATA *pCustData;
+    unsigned int len;
+#endif
+
+#ifdef GSENSOR_UT
+    GSE_FUN();
+#endif
+
+#ifdef CUSTOM_KERNEL_SENSORHUB
+    if (0 != obj->SCP_init_done)
+    {
+        pCustData = (MPU6515_CUST_DATA *)&data.set_cust_req.custData;
+
+        data.set_cust_req.sensorType = ID_ACCELEROMETER;
+        data.set_cust_req.action = SENSOR_HUB_SET_CUST;
+        pCustData->resetCali.action = MPU6515_CUST_ACTION_RESET_CALI;
+        len = offsetof(SCP_SENSOR_HUB_SET_CUST_REQ, custData) + sizeof(pCustData->resetCali);
+        SCP_sensorHub_req_send(&data, &len, 1);
+    }
+#endif
+
+    memset(obj->cali_sw, 0x00, sizeof(obj->cali_sw));
+    memset(obj->offset, 0x00, sizeof(obj->offset));
+
+    return err;    
+}
+/*----------------------------------------------------------------------------*/
+static int MPU6515_ReadCalibration(struct i2c_client *client, int dat[MPU6515_AXES_NUM])
+{
+    struct mpu6515_i2c_data *obj = i2c_get_clientdata(client);
+#ifdef SW_CALIBRATION
+    int mul;
+#else
+    int err;
+#endif
+
+#ifdef GSENSOR_UT
+    GSE_FUN();
+#endif
+
+#ifdef SW_CALIBRATION
+    mul = 0;//only SW Calibration, disable HW Calibration
+#else
+
+    if ((err = MPU6515_ReadOffset(client, obj->offset)))
+    {
+        GSE_ERR("read offset fail, %d\n", err);
+        return err;
+    }
+    mul = obj->reso->sensitivity/mpu6515_offset_resolution.sensitivity;
+#endif
+
+    dat[obj->cvt.map[MPU6515_AXIS_X]] = obj->cvt.sign[MPU6515_AXIS_X]*(obj->offset[MPU6515_AXIS_X]*mul + obj->cali_sw[MPU6515_AXIS_X]);
+    dat[obj->cvt.map[MPU6515_AXIS_Y]] = obj->cvt.sign[MPU6515_AXIS_Y]*(obj->offset[MPU6515_AXIS_Y]*mul + obj->cali_sw[MPU6515_AXIS_Y]);
+    dat[obj->cvt.map[MPU6515_AXIS_Z]] = obj->cvt.sign[MPU6515_AXIS_Z]*(obj->offset[MPU6515_AXIS_Z]*mul + obj->cali_sw[MPU6515_AXIS_Z]);                        
+
+    return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int MPU6515_ReadCalibrationEx(struct i2c_client *client, int act[MPU6515_AXES_NUM], int raw[MPU6515_AXES_NUM])
+{  
+    /*raw: the raw calibration data; act: the actual calibration data*/
+    struct mpu6515_i2c_data *obj = i2c_get_clientdata(client);
+#ifdef SW_CALIBRATION
+    int mul;
+#else
+    int err;
+#endif
+
+#ifdef GSENSOR_UT
+    GSE_FUN();
+#endif
+
+#ifdef SW_CALIBRATION
+    mul = 0;//only SW Calibration, disable HW Calibration
+#else
+
+    if ((err = MPU6515_ReadOffset(client, obj->offset)))
+    {
+        GSE_ERR("read offset fail, %d\n", err);
+        return err;
+    }
+    mul = obj->reso->sensitivity/mpu6515_offset_resolution.sensitivity;
+#endif
+
+    raw[MPU6515_AXIS_X] = obj->offset[MPU6515_AXIS_X]*mul + obj->cali_sw[MPU6515_AXIS_X];
+    raw[MPU6515_AXIS_Y] = obj->offset[MPU6515_AXIS_Y]*mul + obj->cali_sw[MPU6515_AXIS_Y];
+    raw[MPU6515_AXIS_Z] = obj->offset[MPU6515_AXIS_Z]*mul + obj->cali_sw[MPU6515_AXIS_Z];
+
+    act[obj->cvt.map[MPU6515_AXIS_X]] = obj->cvt.sign[MPU6515_AXIS_X]*raw[MPU6515_AXIS_X];
+    act[obj->cvt.map[MPU6515_AXIS_Y]] = obj->cvt.sign[MPU6515_AXIS_Y]*raw[MPU6515_AXIS_Y];
+    act[obj->cvt.map[MPU6515_AXIS_Z]] = obj->cvt.sign[MPU6515_AXIS_Z]*raw[MPU6515_AXIS_Z];                        
+
+    return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int MPU6515_WriteCalibration(struct i2c_client *client, int dat[MPU6515_AXES_NUM])
+{
+    struct mpu6515_i2c_data *obj = i2c_get_clientdata(client);
+    int err;
+    int cali[MPU6515_AXES_NUM], raw[MPU6515_AXES_NUM];
+#ifdef CUSTOM_KERNEL_SENSORHUB
+    SCP_SENSOR_HUB_DATA data;
+    MPU6515_CUST_DATA *pCustData;
+    unsigned int len;
+#endif
+
+#ifdef GSENSOR_UT
+    GSE_FUN();
+#endif
+
+    if ((err = MPU6515_ReadCalibrationEx(client, cali, raw)))    /*offset will be updated in obj->offset*/
+    {
+        GSE_ERR("read offset fail, %d\n", err);
+        return err;
+    }
+
+    GSE_LOG("OLDOFF: (%+3d %+3d %+3d): (%+3d %+3d %+3d) / (%+3d %+3d %+3d)\n", 
+            raw[MPU6515_AXIS_X], raw[MPU6515_AXIS_Y], raw[MPU6515_AXIS_Z],
+            obj->offset[MPU6515_AXIS_X], obj->offset[MPU6515_AXIS_Y], obj->offset[MPU6515_AXIS_Z],
+            obj->cali_sw[MPU6515_AXIS_X], obj->cali_sw[MPU6515_AXIS_Y], obj->cali_sw[MPU6515_AXIS_Z]);
+
+#ifdef CUSTOM_KERNEL_SENSORHUB
+    pCustData = (MPU6515_CUST_DATA *)data.set_cust_req.custData;
+    data.set_cust_req.sensorType = ID_ACCELEROMETER;
+    data.set_cust_req.action = SENSOR_HUB_SET_CUST;
+    pCustData->setCali.action = MPU6515_CUST_ACTION_SET_CALI;
+    pCustData->setCali.data[MPU6515_AXIS_X] = dat[MPU6515_AXIS_X];
+    pCustData->setCali.data[MPU6515_AXIS_Y] = dat[MPU6515_AXIS_Y];
+    pCustData->setCali.data[MPU6515_AXIS_Z] = dat[MPU6515_AXIS_Z];
+    len = offsetof(SCP_SENSOR_HUB_SET_CUST_REQ, custData) + sizeof(pCustData->setCali);
+    SCP_sensorHub_req_send(&data, &len, 1);
+#endif
+
+    /*calculate the real offset expected by caller*/
+    cali[MPU6515_AXIS_X] += dat[MPU6515_AXIS_X];
+    cali[MPU6515_AXIS_Y] += dat[MPU6515_AXIS_Y];
+    cali[MPU6515_AXIS_Z] += dat[MPU6515_AXIS_Z];
+
+    GSE_LOG("UPDATE: (%+3d %+3d %+3d)\n", 
+            dat[MPU6515_AXIS_X], dat[MPU6515_AXIS_Y], dat[MPU6515_AXIS_Z]);
+    
+    obj->cali_sw[MPU6515_AXIS_X] = obj->cvt.sign[MPU6515_AXIS_X]*(cali[obj->cvt.map[MPU6515_AXIS_X]]);
+    obj->cali_sw[MPU6515_AXIS_Y] = obj->cvt.sign[MPU6515_AXIS_Y]*(cali[obj->cvt.map[MPU6515_AXIS_Y]]);
+    obj->cali_sw[MPU6515_AXIS_Z] = obj->cvt.sign[MPU6515_AXIS_Z]*(cali[obj->cvt.map[MPU6515_AXIS_Z]]);  
+
+    return err;
+}
+/*----------------------------------------------------------------------------*/
+static int MPU6515_CheckDeviceID(struct i2c_client *client)
+{
+    u8 databuf[10];    
+    int res = 0;
+
+#ifdef GSENSOR_UT
+    GSE_FUN();
+#endif
+
+    memset(databuf, 0, sizeof(u8)*10);    
+    databuf[0] = MPU6515_REG_DEVID;    
+
+    res = i2c_master_send(client, databuf, 0x1);
+    if (res <= 0)
+    {
+        GSE_ERR("i2c_master_send failed : %d\n", res);
+        goto exit_MPU6515_CheckDeviceID;
+    }
+
+    udelay(500);
+
+    databuf[0] = 0x0;        
+    res = i2c_master_recv(client, databuf, 0x01);
+    if (res <= 0)
+    {
+        GSE_ERR("i2c_master_recv failed : %d\n", res);
+        goto exit_MPU6515_CheckDeviceID;
+    }
+
+    GSE_LOG("MPU6515_CheckDeviceID 0x%x\n", databuf[0]);
+
+    exit_MPU6515_CheckDeviceID:
+    if (res <= 0)
+    {
+        return MPU6515_ERR_I2C;
+    }
+    return MPU6515_SUCCESS;
+}
+
+/*----------------------------------------------------------------------------*/
+static int MPU6515_SetDataFormat(struct i2c_client *client, u8 dataformat)
+{
+    struct mpu6515_i2c_data *obj = i2c_get_clientdata(client);
+    u8 databuf[2];    
+    int res = 0;
+
+#ifndef GSENSOR_UT
+    memset(databuf, 0, sizeof(u8)*2);    
+    databuf[0] = MPU6515_REG_DATA_FORMAT;   
+    res = i2c_master_send(client, databuf, 0x1);
+    if (res <= 0)
+    {
+        return MPU6515_ERR_I2C;
+    }
+
+    udelay(500);
+
+    databuf[0] = 0x0;        
+    res = i2c_master_recv(client, databuf, 0x01);
+    if (res <= 0)
+    {
+        return MPU6515_ERR_I2C;
+    }
+
+    /* write */    
+    databuf[1] = databuf[0] | dataformat;
+    databuf[0] = MPU6515_REG_DATA_FORMAT;   
+    res = i2c_master_send(client, databuf, 0x2);
+
+    if (res <= 0)
+    {
+        return MPU6515_ERR_I2C;
+    }
+    return MPU6515_SetDataResolution(obj);    
+#else
+    GSE_LOG("dataformat = %d\n", dataformat);
+    obj->reso = &mpu6515_data_resolution[3];
+#endif
+}
+/*----------------------------------------------------------------------------*/
+static int MPU6515_SetBWRate(struct i2c_client *client, u8 bwrate)
+{
+    struct mpu6515_i2c_data *obj = i2c_get_clientdata(client);
+    u8 databuf[10];    
+    int res = 0;
+
+#ifdef GSENSOR_UT
+    GSE_FUN();
+#endif
+
+    if( (obj->bandwidth != bwrate) || (atomic_read(&obj->suspend)) )
+    {    
+        memset(databuf, 0, sizeof(u8)*10);    
+    
+        /* read */
+        databuf[0] = MPU6515_REG_BW_RATE;
+        res = i2c_master_send(client, databuf, 0x1);
+        if (res <= 0)
+        {
+            return MPU6515_ERR_I2C;
+        }
+    
+        udelay(500);
+    
+        databuf[0] = 0x0;        
+        res = i2c_master_recv(client, databuf, 0x01);
+        if (res <= 0)
+        {
+            return MPU6515_ERR_I2C;
+        }
+    
+    
+        /* write */
+        databuf[1] = databuf[0] | bwrate;
+        databuf[0] = MPU6515_REG_BW_RATE;    
+    
+        res = i2c_master_send(client, databuf, 0x2);
+    
+        if (res <= 0)
+        {
+            return MPU6515_ERR_I2C;
+        }
+
+    obj->bandwidth = bwrate;
+    }
+
+    return MPU6515_SUCCESS;    
+}
+
+/*----------------------------------------------------------------------------*/
+static int MPU6515_Dev_Reset(struct i2c_client *client)
+{
+#ifndef CUSTOM_KERNEL_SENSORHUB
+    u8 databuf[10];    
+    int res = 0;
+
+#ifdef GSENSOR_UT
+    GSE_FUN();
+#endif
+
+    memset(databuf, 0, sizeof(u8)*10);    
+
+    /* read */
+    databuf[0] = MPU6515_REG_POWER_CTL;    
+    res = i2c_master_send(client, databuf, 0x1);
+    if (res <= 0)
+    {
+        return MPU6515_ERR_I2C;
+    }
+
+    udelay(500);
+
+    databuf[0] = 0x0;        
+    res = i2c_master_recv(client, databuf, 0x01);
+    if (res <= 0)
+    {
+        return MPU6515_ERR_I2C;
+    }
+
+    if ((databuf[0] & 0x1f) != 0x1)
+        GSE_ERR("MPU6515 PWR_MGMT_1 = %x\n", databuf[0]);
+
+    /* write */
+    databuf[1] = databuf[0] | MPU6515_DEV_RESET;
+    databuf[0] = MPU6515_REG_POWER_CTL;    
+
+    res = i2c_master_send(client, databuf, 0x2);
+
+    if (res <= 0)
+    {
+        return MPU6515_ERR_I2C;
+    }
+
+    do
+    {
+        databuf[0] = MPU6515_REG_POWER_CTL;    
+        res = i2c_master_send(client, databuf, 0x1);
+        
+        udelay(500);
+        
+        databuf[0] = 0x0;        
+        res = i2c_master_recv(client, databuf, 0x01);
+
+        printk("[Gsensor] check reset bit");
+
+    }while((databuf[0]&MPU6515_DEV_RESET) != 0);
+
+    msleep(50);
+#endif //#ifndef CUSTOM_KERNEL_SENSORHUB
+    return MPU6515_SUCCESS;    
+}
+
+
+/*----------------------------------------------------------------------------*/
+static int MPU6515_Reset(struct i2c_client *client)
+{
+#ifndef CUSTOM_KERNEL_SENSORHUB
+    u8 databuf[10];    
+    int res = 0;
+
+#ifdef GSENSOR_UT
+    GSE_FUN();
+#endif
+
+    /* write */
+    databuf[1] = 0x7; /* reset gyro, g-sensor, temperature */
+    databuf[0] = MPU6515_REG_RESET;    
+
+    res = i2c_master_send(client, databuf, 0x2);
+
+    if (res <= 0)
+    {
+        return MPU6515_ERR_I2C;
+    }
+
+    msleep(20);
+#endif //#ifndef CUSTOM_KERNEL_SENSORHUB
+    return MPU6515_SUCCESS;    
+}
+
+
+/*----------------------------------------------------------------------------*/
+static int MPU6515_SetIntEnable(struct i2c_client *client, u8 intenable)
+{
+    u8 databuf[2];    
+    int res = 0;
+
+#ifndef GSENSOR_UT
+
+    memset(databuf, 0, sizeof(u8)*2);    
+    databuf[0] = MPU6515_REG_INT_ENABLE;    
+    databuf[1] = intenable;
+
+    res = i2c_master_send(client, databuf, 0x2);
+
+    if (res <= 0)
+    {
+        return MPU6515_ERR_I2C;
+    }
+#else
+    GSE_FUN();
+#endif
+
+    return MPU6515_SUCCESS;    
+}
+/*----------------------------------------------------------------------------*/
+static int mpu6515_gpio_config(void)
+{
+//because we donot use EINT to support low power
+// config to GPIO input mode + PD 
+
+//set to GPIO_GSE_1_EINT_PIN
+	/*
+	mt_set_gpio_mode(GPIO_GSE_1_EINT_PIN, GPIO_GSE_1_EINT_PIN_M_GPIO);
+	mt_set_gpio_dir(GPIO_GSE_1_EINT_PIN, GPIO_DIR_IN);
+	mt_set_gpio_pull_enable(GPIO_GSE_1_EINT_PIN, GPIO_PULL_ENABLE);
+	mt_set_gpio_pull_select(GPIO_GSE_1_EINT_PIN, GPIO_PULL_DOWN);
+	 */
+//set to GPIO_GSE_2_EINT_PIN
+	/*
+	mt_set_gpio_mode(GPIO_GSE_2_EINT_PIN, GPIO_GSE_2_EINT_PIN_M_GPIO);
+	mt_set_gpio_dir(GPIO_GSE_2_EINT_PIN, GPIO_DIR_IN);
+	mt_set_gpio_pull_enable(GPIO_GSE_2_EINT_PIN, GPIO_PULL_ENABLE);
+	 mt_set_gpio_pull_select(GPIO_GSE_2_EINT_PIN, GPIO_PULL_DOWN);
+	 */
+    return 0;
+}
+
+static int mpu6515_init_client(struct i2c_client *client, int reset_cali)
+{
+    struct mpu6515_i2c_data *obj = i2c_get_clientdata(client);
+    int res = 0;
+
+#ifdef GSENSOR_UT
+    GSE_FUN();
+#endif
+
+    mpu6515_gpio_config();
+
+    res = MPU6515_SetPowerMode(client, true);
+    if (res != MPU6515_SUCCESS)
+    {
+        GSE_ERR("set power error\n");
+        return res;
+    }
+
+    res = MPU6515_CheckDeviceID(client); 
+    if (res != MPU6515_SUCCESS)
+    {
+        GSE_ERR("Check ID error\n");
+        return res;
+    }
+
+    //res = gsensor_set_delay(5000000);
+    res = MPU6515_SetBWRate(client, MPU6515_BW_184HZ);
+    if (res != MPU6515_SUCCESS ) //0x2C->BW=100Hz
+    {
+        GSE_ERR("set BWRate error\n");
+        return res;
+    }
+
+    res = MPU6515_SetDataFormat(client, MPU6515_RANGE_16G);
+    if (res != MPU6515_SUCCESS) //0x2C->BW=100Hz
+    {
+        GSE_ERR("set data format error\n");
+        return res;
+    }
+
+    gsensor_gain.x = gsensor_gain.y = gsensor_gain.z = obj->reso->sensitivity;
+
+#ifdef CUSTOM_KERNEL_SENSORHUB
+    res = gsensor_setup_irq();
+    if(res != MPU6515_SUCCESS)
+	{
+		return res;
+	}
+#endif//#ifdef CUSTOM_KERNEL_SENSORHUB
+
+    res = MPU6515_SetIntEnable(client, 0x00);//disable INT
+    if (res != MPU6515_SUCCESS)
+    {
+        GSE_ERR("mpu6515_SetIntEnable error\n");
+        return res;
+    }
+
+    if (0 != reset_cali)
+    {
+        /*reset calibration only in power on*/
+        res = MPU6515_ResetCalibration(client);
+        if (res != MPU6515_SUCCESS)
+        {
+            return res;
+        }
+    }
+
+    res = MPU6515_SetPowerMode(client, enable_status);
+    if (res != MPU6515_SUCCESS)
+    {
+        GSE_ERR("set power error\n");
+        return res;
+    }
+
+#ifdef CONFIG_MPU6515_LOWPASS
+    memset(&obj->fir, 0x00, sizeof(obj->fir));  
+#endif
+
+    return MPU6515_SUCCESS;
+}
+/*----------------------------------------------------------------------------*/
+static int MPU6515_ReadAllReg(struct i2c_client *client, char *buf, int bufsize)
+{   
+    u8 total_len= 0x5C; //(0x75-0x19);
+
+    u8 addr = 0x19;
+    u8 buff[total_len+1];
+    int err = 0;
+    int i;
+
+
+    if (sensor_power == FALSE)
+    {
+        err = MPU6515_SetPowerMode(client, true);
+        if (err)
+        {
+            GSE_ERR("Power on mpu6515 error %d!\n", err);
+        }
+        msleep(50);
+    }
+
+    mpu_i2c_read_block(client, addr, buff, total_len);
+
+    for ( i=0; i<=total_len; i++)
+    {
+        GSE_LOG("MPU6515 reg=0x%x, data=0x%x \n",(addr+i), buff[i]);
+    }
+
+    return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int MPU6515_ReadChipInfo(struct i2c_client *client, char *buf, int bufsize)
+{
+    u8 databuf[10];
+
+#ifdef GSENSOR_UT
+    GSE_FUN();
+#endif
+
+    memset(databuf, 0, sizeof(u8)*10);
+
+    if ((NULL == buf)||(bufsize<=30))
+    {
+        return -1;
+    }
+
+    if (NULL == client)
+    {
+        *buf = 0;
+        return -2;
+    }
+
+    sprintf(buf, "MPU6515 Chip");
+    return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int MPU6515_ReadSensorData(struct i2c_client *client, char *buf, int bufsize)
+{
+    struct mpu6515_i2c_data *obj = obj_i2c_data; //(struct mpu6515_i2c_data*)i2c_get_clientdata(client);
+    int acc[MPU6515_AXES_NUM];
+    int res = 0;
+    client = obj->client;
+
+#ifdef GSENSOR_UT
+    GSE_FUN();
+#endif
+
+    if (atomic_read(&obj->suspend))
+    {
+        return -3;
+    }
+
+    if (NULL == buf)
+    {
+        return -1;
+    }
+    if (NULL == client)
+    {
+        *buf = 0;
+        return -2;
+    }
+
+    if ((res = MPU6515_ReadData(client, obj->data)))
+    {
+        GSE_ERR("I2C error: ret value=%d", res);
+        return -3;
+    }
+    else
+    {
+        obj->data[MPU6515_AXIS_X] += obj->cali_sw[MPU6515_AXIS_X];
+        obj->data[MPU6515_AXIS_Y] += obj->cali_sw[MPU6515_AXIS_Y];
+        obj->data[MPU6515_AXIS_Z] += obj->cali_sw[MPU6515_AXIS_Z];
+
+        /*remap coordinate*/
+        acc[obj->cvt.map[MPU6515_AXIS_X]] = obj->cvt.sign[MPU6515_AXIS_X]*obj->data[MPU6515_AXIS_X];
+        acc[obj->cvt.map[MPU6515_AXIS_Y]] = obj->cvt.sign[MPU6515_AXIS_Y]*obj->data[MPU6515_AXIS_Y];
+        acc[obj->cvt.map[MPU6515_AXIS_Z]] = obj->cvt.sign[MPU6515_AXIS_Z]*obj->data[MPU6515_AXIS_Z];
+		
+        //Out put the mg
+        acc[MPU6515_AXIS_X] = acc[MPU6515_AXIS_X] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+        acc[MPU6515_AXIS_Y] = acc[MPU6515_AXIS_Y] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+        acc[MPU6515_AXIS_Z] = acc[MPU6515_AXIS_Z] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+
+        sprintf(buf, "%04x %04x %04x", acc[MPU6515_AXIS_X], acc[MPU6515_AXIS_Y], acc[MPU6515_AXIS_Z]);
+        if (atomic_read(&obj->trace) & MPU6515_TRC_IOCTL)
+        {
+            GSE_LOG("gsensor data: %s!\n", buf);
+        }
+    }
+
+    return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int MPU6515_ReadRawData(struct i2c_client *client, char *buf)
+{
+    struct mpu6515_i2c_data *obj = (struct mpu6515_i2c_data*)i2c_get_clientdata(client);
+    int res = 0;
+
+#ifdef GSENSOR_UT
+    GSE_FUN();
+#endif
+
+    if (!buf || !client)
+    {
+        return EINVAL;
+    }
+
+
+    if (atomic_read(&obj->suspend))
+    {
+        return EIO;
+    }
+
+    if ((res = MPU6515_ReadData(client, obj->data)))
+    {
+        GSE_ERR("I2C error: ret value=%d", res);
+        return EIO;
+    }
+    else
+    {
+        sprintf(buf, "%04x %04x %04x", obj->data[MPU6515_AXIS_X], 
+                obj->data[MPU6515_AXIS_Y], obj->data[MPU6515_AXIS_Z]);
+
+    }
+
+    return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int MPU6515_InitSelfTest(struct i2c_client *client)
+{
+    int res = 0;
+    u8  data;
+
+    res = MPU6515_SetBWRate(client, MPU6515_BW_184HZ);
+    if (res != MPU6515_SUCCESS ) //0x2C->BW=100Hz
+    {
+        return res;
+    }
+
+    res = mpu_i2c_read_block(client, MPU6515_REG_DATA_FORMAT, &data, 1);
+
+    if (res != MPU6515_SUCCESS)
+    {
+        return res;
+    }
+
+    return MPU6515_SUCCESS;
+}
+/*----------------------------------------------------------------------------*/
+static int MPU6515_JudgeTestResult(struct i2c_client *client, s32 prv[MPU6515_AXES_NUM], s32 nxt[MPU6515_AXES_NUM])
+{
+    struct criteria
+    {
+        int min;
+        int max;
+    };
+
+    struct criteria self[4][3] = {
+        {{ 0, 540}, { 0, 540}, { 0, 875}},
+        {{ 0, 270}, { 0, 270}, { 0, 438}},
+        {{ 0, 135}, { 0, 135}, { 0, 219}},            
+        {{ 0,  67}, { 0,  67}, { 0, 110}},            
+    };
+    struct criteria (*ptr)[3] = NULL;
+    u8 format;
+    int res;
+    if ((res = mpu_i2c_read_block(client, MPU6515_REG_DATA_FORMAT, &format, 1)))
+        return res;
+
+    format = format & MPU6515_RANGE_16G;
+
+    switch (format)
+    {
+        case MPU6515_RANGE_2G:
+            GSE_LOG("format use self[0]\n");
+            ptr = &self[0];       
+        break;
+
+        case MPU6515_RANGE_4G:
+            GSE_LOG("format use self[1]\n");
+            ptr = &self[1];       
+        break;
+    
+        case MPU6515_RANGE_8G:
+            GSE_LOG("format use self[2]\n");
+            ptr = &self[2];       
+        break;
+    
+        case MPU6515_RANGE_16G:
+            GSE_LOG("format use self[3]\n");
+            ptr = &self[3];       
+        break;
+    
+        default:
+            GSE_LOG("format unknow use \n");
+        break;
+    }
+
+    if (!ptr)
+    {
+        GSE_ERR("null pointer\n");
+        return -EINVAL;
+    }
+    GSE_LOG("format=0x%x\n",format);
+
+    GSE_LOG("X diff is %ld\n",abs(nxt[MPU6515_AXIS_X] - prv[MPU6515_AXIS_X]));
+    GSE_LOG("Y diff is %ld\n",abs(nxt[MPU6515_AXIS_Y] - prv[MPU6515_AXIS_Y]));
+    GSE_LOG("Z diff is %ld\n",abs(nxt[MPU6515_AXIS_Z] - prv[MPU6515_AXIS_Z]));
+
+
+    if ((abs(nxt[MPU6515_AXIS_X] - prv[MPU6515_AXIS_X]) > (*ptr)[MPU6515_AXIS_X].max) ||
+        (abs(nxt[MPU6515_AXIS_X] - prv[MPU6515_AXIS_X]) < (*ptr)[MPU6515_AXIS_X].min))
+    {
+        GSE_ERR("X is over range\n");
+        res = -EINVAL;
+    }
+    if ((abs(nxt[MPU6515_AXIS_Y] - prv[MPU6515_AXIS_Y]) > (*ptr)[MPU6515_AXIS_Y].max) ||
+        (abs(nxt[MPU6515_AXIS_Y] - prv[MPU6515_AXIS_Y]) < (*ptr)[MPU6515_AXIS_Y].min))
+    {
+        GSE_ERR("Y is over range\n");
+        res = -EINVAL;
+    }
+    if ((abs(nxt[MPU6515_AXIS_Z] - prv[MPU6515_AXIS_Z]) > (*ptr)[MPU6515_AXIS_Z].max) ||
+        (abs(nxt[MPU6515_AXIS_Z] - prv[MPU6515_AXIS_Z]) < (*ptr)[MPU6515_AXIS_Z].min))
+    {
+        GSE_ERR("Z is over range\n");
+        res = -EINVAL;
+    }
+    return res;
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t show_chipinfo_value(struct device_driver *ddri, char *buf)
+{
+    struct i2c_client *client = mpu6515_i2c_client;
+    char strbuf[MPU6515_BUFSIZE];
+    if (NULL == client)
+    {
+        GSE_ERR("i2c client is null!!\n");
+        return 0;
+    }
+
+    if (sensor_power == false)
+    {
+        MPU6515_SetPowerMode(client, true);
+        msleep(50);
+    }
+	
+    MPU6515_ReadAllReg(client, strbuf, MPU6515_BUFSIZE);
+
+    MPU6515_ReadChipInfo(client, strbuf, MPU6515_BUFSIZE);
+    return snprintf(buf, PAGE_SIZE, "%s\n", strbuf);        
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t show_sensordata_value(struct device_driver *ddri, char *buf)
+{
+    struct i2c_client *client = mpu6515_i2c_client;
+    char strbuf[MPU6515_BUFSIZE];
+
+    if (NULL == client)
+    {
+        GSE_ERR("i2c client is null!!\n");
+        return 0;
+    }
+    MPU6515_ReadSensorData(client, strbuf, MPU6515_BUFSIZE);
+    return snprintf(buf, PAGE_SIZE, "%s\n", strbuf);            
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t show_cali_value(struct device_driver *ddri, char *buf)
+{
+    struct i2c_client *client = mpu6515_i2c_client;
+    struct mpu6515_i2c_data *obj;
+    int err, len = 0, mul;
+    int tmp[MPU6515_AXES_NUM];
+
+    if (NULL == client)
+    {
+        GSE_ERR("i2c client is null!!\n");
+        return 0;
+    }
+
+    obj = i2c_get_clientdata(client);
+
+
+    if ((err = MPU6515_ReadOffset(client, obj->offset)))
+    {
+        return -EINVAL;
+    }
+    else if ((err = MPU6515_ReadCalibration(client, tmp)))
+    {
+        return -EINVAL;
+    }
+    else
+    {
+        mul = obj->reso->sensitivity/mpu6515_offset_resolution.sensitivity;
+        len += snprintf(buf+len, PAGE_SIZE-len, "[HW ][%d] (%+3d, %+3d, %+3d) : (0x%02X, 0x%02X, 0x%02X)\n", mul,                        
+                        obj->offset[MPU6515_AXIS_X], obj->offset[MPU6515_AXIS_Y], obj->offset[MPU6515_AXIS_Z],
+                        obj->offset[MPU6515_AXIS_X], obj->offset[MPU6515_AXIS_Y], obj->offset[MPU6515_AXIS_Z]);
+        len += snprintf(buf+len, PAGE_SIZE-len, "[SW ][%d] (%+3d, %+3d, %+3d)\n", 1, 
+                        obj->cali_sw[MPU6515_AXIS_X], obj->cali_sw[MPU6515_AXIS_Y], obj->cali_sw[MPU6515_AXIS_Z]);
+
+        len += snprintf(buf+len, PAGE_SIZE-len, "[ALL]    (%+3d, %+3d, %+3d) : (%+3d, %+3d, %+3d)\n", 
+                        obj->offset[MPU6515_AXIS_X]*mul + obj->cali_sw[MPU6515_AXIS_X],
+                        obj->offset[MPU6515_AXIS_Y]*mul + obj->cali_sw[MPU6515_AXIS_Y],
+                        obj->offset[MPU6515_AXIS_Z]*mul + obj->cali_sw[MPU6515_AXIS_Z],
+                        tmp[MPU6515_AXIS_X], tmp[MPU6515_AXIS_Y], tmp[MPU6515_AXIS_Z]);
+
+        return len;
+    }
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t store_cali_value(struct device_driver *ddri, const char *buf, size_t count)
+{
+    struct i2c_client *client = mpu6515_i2c_client;  
+    int err, x, y, z;
+    int dat[MPU6515_AXES_NUM];
+
+    if (!strncmp(buf, "rst", 3))
+    {
+        if ((err = MPU6515_ResetCalibration(client)))
+        {
+            GSE_ERR("reset offset err = %d\n", err);
+        }
+    }
+    else if (3 == sscanf(buf, "0x%02X 0x%02X 0x%02X", &x, &y, &z))
+    {
+        dat[MPU6515_AXIS_X] = x;
+        dat[MPU6515_AXIS_Y] = y;
+        dat[MPU6515_AXIS_Z] = z;
+        if ((err = MPU6515_WriteCalibration(client, dat)))
+        {
+            GSE_ERR("write calibration err = %d\n", err);
+        }
+    }
+    else
+    {
+        GSE_ERR("invalid format\n");
+    }
+
+    return count;
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t show_self_value(struct device_driver *ddri, char *buf)
+{
+    struct i2c_client *client = mpu6515_i2c_client;
+
+    if (NULL == client)
+    {
+        GSE_ERR("i2c client is null!!\n");
+        return 0;
+    }
+
+    return snprintf(buf, 8, "%s\n", selftestRes);
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t store_self_value(struct device_driver *ddri, const char *buf, size_t count)
+{   /*write anything to this register will trigger the process*/
+    struct item
+    {
+        s16 raw[MPU6515_AXES_NUM];
+    };
+
+    struct i2c_client *client = mpu6515_i2c_client;  
+    int idx, res, num;
+    struct item *prv = NULL, *nxt = NULL;
+    s32 avg_prv[MPU6515_AXES_NUM] = {0, 0, 0};
+    s32 avg_nxt[MPU6515_AXES_NUM] = {0, 0, 0};
+
+
+    if (1 != sscanf(buf, "%d", &num))
+    {
+        GSE_ERR("parse number fail\n");
+        return count;
+    }
+    else if (num == 0)
+    {
+        GSE_ERR("invalid data count\n");
+        return count;
+    }
+
+    prv = kzalloc(sizeof(*prv) * num, GFP_KERNEL);
+    nxt = kzalloc(sizeof(*nxt) * num, GFP_KERNEL);
+    if (!prv || !nxt)
+    {
+        goto exit;
+    }
+
+
+    GSE_LOG("NORMAL:\n");
+    MPU6515_SetPowerMode(client,true);
+    msleep(50);
+
+    for (idx = 0; idx < num; idx++)
+    {
+        if ((res = MPU6515_ReadData(client, prv[idx].raw)))
+        {
+            GSE_ERR("read data fail: %d\n", res);
+            goto exit;
+        }
+
+        avg_prv[MPU6515_AXIS_X] += prv[idx].raw[MPU6515_AXIS_X];
+        avg_prv[MPU6515_AXIS_Y] += prv[idx].raw[MPU6515_AXIS_Y];
+        avg_prv[MPU6515_AXIS_Z] += prv[idx].raw[MPU6515_AXIS_Z];        
+        GSE_LOG("[%5d %5d %5d]\n", prv[idx].raw[MPU6515_AXIS_X], prv[idx].raw[MPU6515_AXIS_Y], prv[idx].raw[MPU6515_AXIS_Z]);
+    }
+
+    avg_prv[MPU6515_AXIS_X] /= num;
+    avg_prv[MPU6515_AXIS_Y] /= num;
+    avg_prv[MPU6515_AXIS_Z] /= num;    
+
+    /*initial setting for self test*/
+    GSE_LOG("SELFTEST:\n");    
+    for (idx = 0; idx < num; idx++)
+    {
+        if ((res = MPU6515_ReadData(client, nxt[idx].raw)))
+        {
+            GSE_ERR("read data fail: %d\n", res);
+            goto exit;
+        }
+        avg_nxt[MPU6515_AXIS_X] += nxt[idx].raw[MPU6515_AXIS_X];
+        avg_nxt[MPU6515_AXIS_Y] += nxt[idx].raw[MPU6515_AXIS_Y];
+        avg_nxt[MPU6515_AXIS_Z] += nxt[idx].raw[MPU6515_AXIS_Z];        
+        GSE_LOG("[%5d %5d %5d]\n", nxt[idx].raw[MPU6515_AXIS_X], nxt[idx].raw[MPU6515_AXIS_Y], nxt[idx].raw[MPU6515_AXIS_Z]);
+    }
+
+    avg_nxt[MPU6515_AXIS_X] /= num;
+    avg_nxt[MPU6515_AXIS_Y] /= num;
+    avg_nxt[MPU6515_AXIS_Z] /= num;    
+
+    GSE_LOG("X: %5d - %5d = %5d \n", avg_nxt[MPU6515_AXIS_X], avg_prv[MPU6515_AXIS_X], avg_nxt[MPU6515_AXIS_X] - avg_prv[MPU6515_AXIS_X]);
+    GSE_LOG("Y: %5d - %5d = %5d \n", avg_nxt[MPU6515_AXIS_Y], avg_prv[MPU6515_AXIS_Y], avg_nxt[MPU6515_AXIS_Y] - avg_prv[MPU6515_AXIS_Y]);
+    GSE_LOG("Z: %5d - %5d = %5d \n", avg_nxt[MPU6515_AXIS_Z], avg_prv[MPU6515_AXIS_Z], avg_nxt[MPU6515_AXIS_Z] - avg_prv[MPU6515_AXIS_Z]); 
+
+    if (!MPU6515_JudgeTestResult(client, avg_prv, avg_nxt))
+    {
+        GSE_LOG("SELFTEST : PASS\n");
+        strcpy(selftestRes,"y");
+    }
+    else
+    {
+        GSE_LOG("SELFTEST : FAIL\n");
+        strcpy(selftestRes,"n");
+    }
+
+    exit:
+    /*restore the setting*/    
+    mpu6515_init_client(client, 0);
+    kfree(prv);
+    kfree(nxt);
+    return count;
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t show_selftest_value(struct device_driver *ddri, char *buf)
+{
+    struct i2c_client *client = mpu6515_i2c_client;
+    struct mpu6515_i2c_data *obj;
+
+    if (NULL == client)
+    {
+        GSE_ERR("i2c client is null!!\n");
+        return 0;
+    }
+
+    obj = i2c_get_clientdata(client);
+    return snprintf(buf, PAGE_SIZE, "%d\n", atomic_read(&obj->selftest));
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t store_selftest_value(struct device_driver *ddri, const char *buf, size_t count)
+{
+    struct mpu6515_i2c_data *obj = obj_i2c_data;
+    int tmp;
+
+    if (NULL == obj)
+    {
+        GSE_ERR("i2c data obj is null!!\n");
+        return 0;
+    }
+
+
+    if (1 == sscanf(buf, "%d", &tmp))
+    {
+        if (atomic_read(&obj->selftest) && !tmp)
+        {
+            /*enable -> disable*/
+            mpu6515_init_client(obj->client, 0);
+        }
+        else if (!atomic_read(&obj->selftest) && tmp)
+        {
+            /*disable -> enable*/
+            MPU6515_InitSelfTest(obj->client);            
+        }
+
+        GSE_LOG("selftest: %d => %d\n", atomic_read(&obj->selftest), tmp);
+        atomic_set(&obj->selftest, tmp); 
+    }
+    else
+    {
+        GSE_ERR("invalid content: '%s', length = %d\n", buf, (int)count);   
+    }
+    return count;
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t show_firlen_value(struct device_driver *ddri, char *buf)
+{
+#ifdef CONFIG_MPU6515_LOWPASS
+    struct i2c_client *client = mpu6515_i2c_client;
+    struct mpu6515_i2c_data *obj = i2c_get_clientdata(client);
+    if (atomic_read(&obj->firlen))
+    {
+        int idx, len = atomic_read(&obj->firlen);
+        GSE_LOG("len = %2d, idx = %2d\n", obj->fir.num, obj->fir.idx);
+
+        for (idx = 0; idx < len; idx++)
+        {
+            GSE_LOG("[%5d %5d %5d]\n", obj->fir.raw[idx][MPU6515_AXIS_X], obj->fir.raw[idx][MPU6515_AXIS_Y], obj->fir.raw[idx][MPU6515_AXIS_Z]);
+        }
+
+        GSE_LOG("sum = [%5d %5d %5d]\n", obj->fir.sum[MPU6515_AXIS_X], obj->fir.sum[MPU6515_AXIS_Y], obj->fir.sum[MPU6515_AXIS_Z]);
+        GSE_LOG("avg = [%5d %5d %5d]\n", obj->fir.sum[MPU6515_AXIS_X]/len, obj->fir.sum[MPU6515_AXIS_Y]/len, obj->fir.sum[MPU6515_AXIS_Z]/len);
+    }
+    return snprintf(buf, PAGE_SIZE, "%d\n", atomic_read(&obj->firlen));
+#else
+    return snprintf(buf, PAGE_SIZE, "not support\n");
+#endif
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t store_firlen_value(struct device_driver *ddri, const char *buf, size_t count)
+{
+#ifdef CONFIG_MPU6515_LOWPASS
+    struct i2c_client *client = mpu6515_i2c_client;  
+    struct mpu6515_i2c_data *obj = i2c_get_clientdata(client);
+    int firlen;
+
+    if (1 != sscanf(buf, "%d", &firlen))
+    {
+        GSE_ERR("invallid format\n");
+    }
+    else if (firlen > C_MAX_FIR_LENGTH)
+    {
+        GSE_ERR("exceeds maximum filter length\n");
+    }
+    else
+    {
+        atomic_set(&obj->firlen, firlen);
+        if (0 == firlen)
+        {
+            atomic_set(&obj->fir_en, 0);
+        }
+        else
+        {
+            memset(&obj->fir, 0x00, sizeof(obj->fir));
+            atomic_set(&obj->fir_en, 1);
+        }
+    }
+#endif    
+    return count;
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t show_trace_value(struct device_driver *ddri, char *buf)
+{
+    ssize_t res;
+    struct mpu6515_i2c_data *obj = obj_i2c_data;
+    if (obj == NULL)
+    {
+        GSE_ERR("i2c_data obj is null!!\n");
+        return 0;
+    }
+
+    res = snprintf(buf, PAGE_SIZE, "0x%04X\n", atomic_read(&obj->trace));     
+    return res;    
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t store_trace_value(struct device_driver *ddri, const char *buf, size_t count)
+{
+    struct mpu6515_i2c_data *obj = obj_i2c_data;
+    int trace;
+    if (obj == NULL)
+    {
+        GSE_ERR("i2c_data obj is null!!\n");
+        return 0;
+    }
+
+    if (1 == sscanf(buf, "0x%x", &trace))
+    {
+        atomic_set(&obj->trace, trace);
+    }
+    else
+    {
+        GSE_ERR("invalid content: '%s', length = %d\n", buf, (int)count);
+    }
+
+    return count;    
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t show_status_value(struct device_driver *ddri, char *buf)
+{
+    ssize_t len = 0;    
+    int err;
+    struct mpu6515_i2c_data *obj = obj_i2c_data;
+    u8  dat = 0;
+    if (obj == NULL)
+    {
+        GSE_ERR("i2c_data obj is null!!\n");
+        return 0;
+    }
+
+    if ((err = mpu_i2c_read_block(obj->client, MPU6515_REG_POWER_CTL, &dat, 1)))
+    {
+        GSE_ERR("write data format fail!!\n");
+        return err;
+    }
+
+    if (obj->hw)
+    {
+        len += snprintf(buf+len, PAGE_SIZE-len, "CUST: %d %d (%d %d), %x\n", 
+                        obj->hw->i2c_num, obj->hw->direction, obj->hw->power_id, obj->hw->power_vol, dat);
+    }
+    else
+    {
+        len += snprintf(buf+len, PAGE_SIZE-len, "CUST: NULL\n");
+    }
+    return len;    
+}
+/*----------------------------------------------------------------------------*/
+static DRIVER_ATTR(chipinfo,             S_IRUGO, show_chipinfo_value,      NULL);
+static DRIVER_ATTR(sensordata,           S_IRUGO, show_sensordata_value,    NULL);
+static DRIVER_ATTR(cali,       S_IWUSR | S_IRUGO, show_cali_value,          store_cali_value);
+static DRIVER_ATTR(self,       S_IWUSR | S_IRUGO, show_selftest_value,          store_selftest_value);
+static DRIVER_ATTR(selftest,   S_IWUSR | S_IRUGO, show_self_value ,      store_self_value );
+static DRIVER_ATTR(firlen,     S_IWUSR | S_IRUGO, show_firlen_value,        store_firlen_value);
+static DRIVER_ATTR(trace,      S_IWUSR | S_IRUGO, show_trace_value,         store_trace_value);
+static DRIVER_ATTR(status,               S_IRUGO, show_status_value,        NULL);
+/*----------------------------------------------------------------------------*/
+static struct driver_attribute *mpu6515_attr_list[] = {
+    &driver_attr_chipinfo,     /*chip information*/
+    &driver_attr_sensordata,   /*dump sensor data*/
+    &driver_attr_cali,         /*show calibration data*/
+    &driver_attr_self,         /*self test demo*/
+    &driver_attr_selftest,     /*self control: 0: disable, 1: enable*/
+    &driver_attr_firlen,       /*filter length: 0: disable, others: enable*/
+    &driver_attr_trace,        /*trace log*/
+    &driver_attr_status,        
+};
+/*----------------------------------------------------------------------------*/
+static int mpu6515_create_attr(struct device_driver *driver) 
+{
+    int idx, err = 0;
+    int num = (int)(sizeof(mpu6515_attr_list)/sizeof(mpu6515_attr_list[0]));
+
+#ifdef GSENSOR_UT
+    GSE_FUN();
+#endif
+
+    if (driver == NULL)
+    {
+        return -EINVAL;
+    }
+
+    for (idx = 0; idx < num; idx++)
+    {
+        if (0 != (err = driver_create_file(driver, mpu6515_attr_list[idx])))
+        {
+            GSE_ERR("driver_create_file (%s) = %d\n", mpu6515_attr_list[idx]->attr.name, err);
+            break;
+        }
+    }    
+    return err;
+}
+/*----------------------------------------------------------------------------*/
+static int mpu6515_delete_attr(struct device_driver *driver)
+{
+    int idx ,err = 0;
+    int num = (int)(sizeof(mpu6515_attr_list)/sizeof(mpu6515_attr_list[0]));
+
+    if (driver == NULL)
+    {
+        return -EINVAL;
+    }
+
+    for (idx = 0; idx < num; idx++)
+    {
+        driver_remove_file(driver, mpu6515_attr_list[idx]);
+    }
+
+    return err;
+}
+
+/*----------------------------------------------------------------------------*/
+#ifdef CUSTOM_KERNEL_SENSORHUB
+static void gsensor_irq_work(struct work_struct *work)
+{
+    struct mpu6515_i2c_data *obj = obj_i2c_data;
+    struct scp_acc_hw scp_hw;
+    MPU6515_CUST_DATA *p_cust_data;
+    SCP_SENSOR_HUB_DATA data;
+    int max_cust_data_size_per_packet;
+    int i;
+    uint sizeOfCustData;
+    uint len;
+    char *p = (char *)&scp_hw;
+
+    GSE_FUN();
+
+    scp_hw.i2c_num = obj->hw->i2c_num;
+    scp_hw.direction = obj->hw->direction;
+    scp_hw.power_id = obj->hw->power_id;
+    scp_hw.power_vol = obj->hw->power_vol;
+    scp_hw.firlen = obj->hw->firlen;
+    memcpy(scp_hw.i2c_addr, obj->hw->i2c_addr, sizeof(obj->hw->i2c_addr));
+    scp_hw.power_vio_id = obj->hw->power_vio_id;
+    scp_hw.power_vio_vol = obj->hw->power_vio_vol;
+    scp_hw.is_batch_supported = obj->hw->is_batch_supported;
+
+    p_cust_data = (MPU6515_CUST_DATA *)data.set_cust_req.custData;
+    sizeOfCustData = sizeof(scp_hw);
+    max_cust_data_size_per_packet = sizeof(data.set_cust_req.custData) - offsetof(MPU6515_SET_CUST, data);
+    
+    for (i=0;sizeOfCustData>0;i++)
+    {
+        data.set_cust_req.sensorType = ID_ACCELEROMETER;
+        data.set_cust_req.action = SENSOR_HUB_SET_CUST;
+        p_cust_data->setCust.action = MPU6515_CUST_ACTION_SET_CUST;
+        p_cust_data->setCust.part = i;
+        
+        if (sizeOfCustData > max_cust_data_size_per_packet)
+        {
+            len = max_cust_data_size_per_packet;
+        }
+        else
+        {
+            len = sizeOfCustData;
+        }
+
+        memcpy(p_cust_data->setCust.data, p, len);
+        sizeOfCustData -= len;
+        p += len;
+        
+        len += offsetof(SCP_SENSOR_HUB_SET_CUST_REQ, custData) + offsetof(MPU6515_SET_CUST, data);
+        SCP_sensorHub_req_send(&data, &len, 1);
+    }
+
+    p_cust_data = (MPU6515_CUST_DATA *)&data.set_cust_req.custData;
+
+    data.set_cust_req.sensorType = ID_ACCELEROMETER;
+    data.set_cust_req.action = SENSOR_HUB_SET_CUST;
+    p_cust_data->resetCali.action = MPU6515_CUST_ACTION_RESET_CALI;
+    len = offsetof(SCP_SENSOR_HUB_SET_CUST_REQ, custData) + sizeof(p_cust_data->resetCali);
+    SCP_sensorHub_req_send(&data, &len, 1);
+
+    obj->SCP_init_done = 1;
+}
+/*----------------------------------------------------------------------------*/
+static int gsensor_irq_handler(void* data, uint len)
+{
+	struct mpu6515_i2c_data *obj = obj_i2c_data;
+    SCP_SENSOR_HUB_DATA_P rsp = (SCP_SENSOR_HUB_DATA_P)data;
+
+    GSE_FUN();
+    GSE_ERR("len = %d, type = %d, action = %d, errCode = %d\n", len, rsp->rsp.sensorType, rsp->rsp.action, rsp->rsp.errCode);
+    
+	if(!obj)
+	{
+		return -1;
+	}
+
+    switch(rsp->rsp.action)
+    {
+        case SENSOR_HUB_NOTIFY:
+            switch(rsp->notify_rsp.event)
+            {
+                case SCP_INIT_DONE:
+                    schedule_work(&obj->irq_work);
+                    //schedule_delayed_work(&obj->irq_work, HZ);
+                    break;
+                default:
+                    GSE_ERR("Error sensor hub notify");
+                    break;
+            }
+            break;
+        default:
+            GSE_ERR("Error sensor hub action");
+            break;
+    }
+
+    return 0;
+}
+
+static int gsensor_setup_irq()
+{
+    int err = 0;
+    
+#ifdef GSENSOR_UT
+    GSE_FUN();
+#endif
+
+    err = SCP_sensorHub_rsp_registration(ID_ACCELEROMETER, gsensor_irq_handler);
+    
+	return err;
+}
+#endif//#ifdef CUSTOM_KERNEL_SENSORHUB
+/****************************************************************************** 
+ * Function Configuration
+******************************************************************************/
+static int mpu6515_open(struct inode *inode, struct file *file)
+{
+    file->private_data = mpu6515_i2c_client;
+
+    if (file->private_data == NULL)
+    {
+        GSE_ERR("null pointer!!\n");
+        return -EINVAL;
+    }
+    return nonseekable_open(inode, file);
+}
+/*----------------------------------------------------------------------------*/
+static int mpu6515_release(struct inode *inode, struct file *file)
+{
+    file->private_data = NULL;
+    return 0;
+}
+/*----------------------------------------------------------------------------*/
+static long mpu6515_unlocked_ioctl(struct file *file, unsigned int cmd,
+       unsigned long arg)
+{
+    struct i2c_client *client = (struct i2c_client*)file->private_data;
+    struct mpu6515_i2c_data *obj = (struct mpu6515_i2c_data*)i2c_get_clientdata(client);    
+    char strbuf[MPU6515_BUFSIZE];
+    void __user *data;
+    SENSOR_DATA sensor_data;
+    long err = 0;
+    int cali[3];
+
+#ifdef GSENSOR_UT
+    GSE_FUN();
+#endif
+
+    if (_IOC_DIR(cmd) & _IOC_READ)
+    {
+        err = !access_ok(VERIFY_WRITE, (void __user *)arg, _IOC_SIZE(cmd));
+    }
+    else if (_IOC_DIR(cmd) & _IOC_WRITE)
+    {
+        err = !access_ok(VERIFY_READ, (void __user *)arg, _IOC_SIZE(cmd));
+    }
+
+    if (err)
+    {
+        GSE_ERR("access error: %08X, (%2d, %2d)\n", cmd, _IOC_DIR(cmd), _IOC_SIZE(cmd));
+        return -EFAULT;
+    }
+
+    switch (cmd)
+    {
+    case GSENSOR_IOCTL_INIT:
+        mpu6515_init_client(client, 0);         
+        break;
+
+    case GSENSOR_IOCTL_READ_CHIPINFO:
+        data = (void __user *) arg;
+        if (data == NULL)
+        {
+            err = -EINVAL;
+            break;    
+        }
+
+        MPU6515_ReadChipInfo(client, strbuf, MPU6515_BUFSIZE);
+        if (copy_to_user(data, strbuf, strlen(strbuf)+1))
+        {
+            err = -EFAULT;
+            break;
+        }
+        break;    
+
+    case GSENSOR_IOCTL_READ_SENSORDATA:
+        data = (void __user *) arg;
+        if (data == NULL)
+        {
+            err = -EINVAL;
+            break;    
+        }
+        mutex_lock(&gsensor_mutex);
+        MPU6515_SetPowerMode(client, true);
+        MPU6515_ReadSensorData(client, strbuf, MPU6515_BUFSIZE);
+        mutex_unlock(&gsensor_mutex);
+        if (copy_to_user(data, strbuf, strlen(strbuf)+1))
+        {
+            err = -EFAULT;
+            break;    
+        }
+        break;
+
+    case GSENSOR_IOCTL_READ_GAIN:
+        data = (void __user *) arg;
+        if (data == NULL)
+        {
+            err = -EINVAL;
+            break;    
+        }
+
+        if (copy_to_user(data, &gsensor_gain, sizeof(GSENSOR_VECTOR3D)))
+        {
+            err = -EFAULT;
+            break;
+        }
+        break;
+
+    case GSENSOR_IOCTL_READ_RAW_DATA:
+        data = (void __user *) arg;
+        if (data == NULL)
+        {
+            err = -EINVAL;
+            break;    
+        }
+
+        if (atomic_read(&obj->suspend))
+        {
+            err = -EINVAL;
+        }
+        else
+        {
+        MPU6515_ReadRawData(client, strbuf);
+        if (copy_to_user(data, strbuf, strlen(strbuf)+1))
+        {
+            err = -EFAULT;
+            break;    
+        }
+        }
+        break;    
+
+    case GSENSOR_IOCTL_SET_CALI:
+        data = (void __user*)arg;
+        if (data == NULL)
+        {
+            err = -EINVAL;
+            break;    
+        }
+        if (copy_from_user(&sensor_data, data, sizeof(sensor_data)))
+        {
+            err = -EFAULT;
+            break;    
+        }
+        if (atomic_read(&obj->suspend))
+        {
+            GSE_ERR("Perform calibration in suspend state!!\n");
+            err = -EINVAL;
+        }
+        else
+        {
+            cali[MPU6515_AXIS_X] = sensor_data.x * obj->reso->sensitivity / GRAVITY_EARTH_1000;
+            cali[MPU6515_AXIS_Y] = sensor_data.y * obj->reso->sensitivity / GRAVITY_EARTH_1000;
+            cali[MPU6515_AXIS_Z] = sensor_data.z * obj->reso->sensitivity / GRAVITY_EARTH_1000;           
+            err = MPU6515_WriteCalibration(client, cali);            
+        }
+        break;
+
+    case GSENSOR_IOCTL_CLR_CALI:
+        err = MPU6515_ResetCalibration(client);
+        break;
+
+    case GSENSOR_IOCTL_GET_CALI:
+        data = (void __user*)arg;
+        if (data == NULL)
+        {
+            err = -EINVAL;
+            break;    
+        }
+        if ((err = MPU6515_ReadCalibration(client, cali)))
+        {
+            break;
+        }
+
+        sensor_data.x = cali[MPU6515_AXIS_X] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+        sensor_data.y = cali[MPU6515_AXIS_Y] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+        sensor_data.z = cali[MPU6515_AXIS_Z] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+        if (copy_to_user(data, &sensor_data, sizeof(sensor_data)))
+        {
+            err = -EFAULT;
+            break;
+        }
+        break;
+
+
+    default:
+        GSE_ERR("unknown IOCTL: 0x%08x\n", cmd);
+        err = -ENOIOCTLCMD;
+        break;
+
+    }
+
+    return err;
+}
+#if IS_ENABLED(CONFIG_COMPAT)
+static long compat_mpu6515_unlocked_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
+{
+    long ret;
+
+    GSE_FUN();
+
+    if (!filp->f_op || !filp->f_op->unlocked_ioctl) {
+        GSE_ERR("compat_ion_ioctl file has no f_op or no f_op->unlocked_ioctl.\n");
+        return -ENOTTY;
+    }
+
+    switch (cmd) {
+        case GSENSOR_IOCTL_SET_CALI:
+        case GSENSOR_IOCTL_CLR_CALI:
+        case GSENSOR_IOCTL_GET_CALI:
+            return filp->f_op->unlocked_ioctl(filp, cmd,
+                (unsigned long)compat_ptr(arg));
+        default: {
+            GSE_ERR("compat_ion_ioctl : No such command!! 0x%x\n", cmd);
+            return -ENOIOCTLCMD;
+        }
+    }
+}
+#endif
+/*----------------------------------------------------------------------------*/
+static struct file_operations mpu6515_fops = {
+    .open = mpu6515_open,
+    .release = mpu6515_release,
+    .unlocked_ioctl = mpu6515_unlocked_ioctl,
+#if IS_ENABLED(CONFIG_COMPAT)
+    .compat_ioctl = compat_mpu6515_unlocked_ioctl,
+#endif
+};
+/*----------------------------------------------------------------------------*/
+static struct miscdevice mpu6515_device = {
+    .minor = MISC_DYNAMIC_MINOR,
+    .name = "gsensor",
+    .fops = &mpu6515_fops,
+};
+/*----------------------------------------------------------------------------*/
+#if !defined(CONFIG_HAS_EARLYSUSPEND) || !defined(USE_EARLY_SUSPEND)
+/*----------------------------------------------------------------------------*/
+static int mpu6515_suspend(struct i2c_client *client, pm_message_t msg) 
+{
+    struct mpu6515_i2c_data *obj = i2c_get_clientdata(client);    
+    int err = 0;
+    GSE_FUN();    
+
+    if (msg.event == PM_EVENT_SUSPEND)
+    {
+        if (obj == NULL)
+        {
+            GSE_ERR("null pointer!!\n");
+            return -EINVAL;
+        }
+        //mutex_lock(&gsensor_mutex);
+        atomic_set(&obj->suspend, 1);
+#ifndef CUSTOM_KERNEL_SENSORHUB
+        if ((err = MPU6515_SetPowerMode(obj->client, false)))
+#else //#ifndef CUSTOM_KERNEL_SENSORHUB
+        if (0)//(err = MPU6515_SCP_SetPowerMode(false, ID_ACCELEROMETER))) //need not disable g sensor in suspend mode if use sensor hub.
+#endif //#ifndef CUSTOM_KERNEL_SENSORHUB
+        {
+            GSE_ERR("write power control fail!!\n");
+            return err;
+        }
+        //mutex_unlock(&gsensor_mutex);
+#ifndef CUSTOM_KERNEL_SENSORHUB
+        MPU6515_power(obj->hw, 0);
+#endif //#ifndef CUSTOM_KERNEL_SENSORHUB
+        GSE_LOG("mpu6515_suspend ok\n");
+    }
+    return err;
+}
+/*----------------------------------------------------------------------------*/
+static int mpu6515_resume(struct i2c_client *client)
+{
+    struct mpu6515_i2c_data *obj = i2c_get_clientdata(client);        
+    int err;
+    GSE_FUN();
+
+    if (obj == NULL)
+    {
+        GSE_ERR("null pointer!!\n");
+        return -EINVAL;
+    }
+#ifndef CUSTOM_KERNEL_SENSORHUB
+    MPU6515_power(obj->hw, 1);
+#endif //#ifndef CUSTOM_KERNEL_SENSORHUB
+    //mutex_lock(&gsensor_mutex);
+#ifndef CUSTOM_KERNEL_SENSORHUB
+    if ((err = mpu6515_init_client(client, 0)))
+#else //#ifndef CUSTOM_KERNEL_SENSORHUB
+    if (0)//(err = MPU6515_SCP_SetPowerMode(enable_status, ID_ACCELEROMETER))) //need not disable g sensor in suspend mode if use sensor hub.
+#endif //#ifndef CUSTOM_KERNEL_SENSORHUB
+    {
+        GSE_ERR("initialize client fail!!\n");
+        return err;        
+    }
+    atomic_set(&obj->suspend, 0);
+    //mutex_unlock(&gsensor_mutex);
+    GSE_LOG("mpu6515_resume ok\n");
+
+    return 0;
+}
+/*----------------------------------------------------------------------------*/
+#else //#if !defined(CONFIG_HAS_EARLYSUSPEND) || !defined(USE_EARLY_SUSPEND)
+/*----------------------------------------------------------------------------*/
+static void mpu6515_early_suspend(struct early_suspend *h) 
+{
+    struct mpu6515_i2c_data *obj = container_of(h, struct mpu6515_i2c_data, early_drv);   
+    int err;
+    GSE_FUN();    
+
+    if (obj == NULL)
+    {
+        GSE_ERR("null pointer!!\n");
+        return;
+    }
+    //mutex_lock(&gsensor_mutex);
+    atomic_set(&obj->suspend, 1); 
+#ifndef CUSTOM_KERNEL_SENSORHUB
+    if ((err = MPU6515_SetPowerMode(obj->client, false)))
+#else //#ifndef CUSTOM_KERNEL_SENSORHUB
+    if ((err = MPU6515_SCP_SetPowerMode(false, ID_ACCELEROMETER)))
+#endif //#ifndef CUSTOM_KERNEL_SENSORHUB
+    {
+        GSE_ERR("write power control fail!!\n");
+        return;
+    }
+
+#ifndef CUSTOM_KERNEL_SENSORHUB
+    if (MPU6515_gyro_mode() == false)
+    {
+        MPU6515_Dev_Reset(obj->client);
+        MPU6515_Reset(obj->client);
+        sensor_power = true;
+        MPU6515_SetPowerMode(obj->client, false);
+    }
+
+    obj->bandwidth = 0;
+    //mutex_unlock(&gsensor_mutex);
+    
+    MPU6515_power(obj->hw, 0);
+#endif //#ifndef CUSTOM_KERNEL_SENSORHUB
+}
+/*----------------------------------------------------------------------------*/
+static void mpu6515_late_resume(struct early_suspend *h)
+{
+    struct mpu6515_i2c_data *obj = container_of(h, struct mpu6515_i2c_data, early_drv);         
+    int err;
+    GSE_FUN();
+
+    if (obj == NULL)
+    {
+        GSE_ERR("null pointer!!\n");
+        return;
+    }
+#ifndef CUSTOM_KERNEL_SENSORHUB
+    MPU6515_power(obj->hw, 1);
+#endif //#ifndef CUSTOM_KERNEL_SENSORHUB
+    //mutex_lock(&gsensor_mutex);
+#ifndef CUSTOM_KERNEL_SENSORHUB
+    if ((err = mpu6515_init_client(obj->client, 0)))
+#else //#ifndef CUSTOM_KERNEL_SENSORHUB
+    if ((err = MPU6515_SCP_SetPowerMode(enable_status, ID_ACCELEROMETER)))
+#endif //#ifndef CUSTOM_KERNEL_SENSORHUB
+    {
+        GSE_ERR("initialize client fail!!\n");
+        return;        
+    }
+    atomic_set(&obj->suspend, 0);
+    //mutex_unlock(&gsensor_mutex);
+}
+/*----------------------------------------------------------------------------*/
+#endif //#if !defined(CONFIG_HAS_EARLYSUSPEND) || !defined(USE_EARLY_SUSPEND)
+/*----------------------------------------------------------------------------*/
+// if use  this typ of enable , Gsensor should report inputEvent(x, y, z ,stats, div) to HAL
+static int gsensor_open_report_data(int open)
+{
+	//should queuq work to report event if  is_report_input_direct=true
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+// if use  this typ of enable , Gsensor only enabled but not report inputEvent to HAL
+#ifndef CUSTOM_KERNEL_SENSORHUB
+static int gsensor_enable_nodata(int en)
+{
+    int err = 0;
+
+#ifdef GSENSOR_UT
+    GSE_FUN();
+#endif
+
+    mutex_lock(&gsensor_mutex);
+	if(((en == 0) && (sensor_power == false)) ||((en == 1) && (sensor_power == true)))
+	{
+		enable_status = sensor_power;
+		GSE_LOG("Gsensor device have updated!\n");
+	}
+	else
+	{
+		enable_status = !sensor_power;
+		if (atomic_read(&obj_i2c_data->suspend) == 0)
+		{
+			err = MPU6515_SetPowerMode(obj_i2c_data->client, enable_status);
+			GSE_LOG("Gsensor not in suspend gsensor_SetPowerMode!, enable_status = %d\n",enable_status);
+		}
+		else
+		{
+			GSE_LOG("Gsensor in suspend and can not enable or disable!enable_status = %d\n",enable_status);
+		}
+	}
+	mutex_unlock(&gsensor_mutex);
+
+    if(err != MPU6515_SUCCESS)
+	{
+		printk("gsensor_enable_nodata fail!\n");
+		return -1;
+	}
+
+    printk("gsensor_enable_nodata OK!!!\n");
+	return 0;
+}
+#endif
+/*----------------------------------------------------------------------------*/
+// if use  this typ of enable , Gsensor only enabled but not report inputEvent to HAL
+#ifdef CUSTOM_KERNEL_SENSORHUB
+static int scp_gsensor_enable_nodata(int en)
+{
+    int err = 0;
+
+#ifdef GSENSOR_UT
+    GSE_FUN();
+#endif
+
+    mutex_lock(&gsensor_mutex);
+    if(((en == 0) && (scp_sensor_power == false)) ||((en == 1) && (scp_sensor_power == true)))
+    {
+        enable_status = scp_sensor_power;
+        GSE_LOG("Gsensor device have updated!\n");
+    }
+    else
+    {
+        enable_status = !scp_sensor_power;
+        if (atomic_read(&obj_i2c_data->suspend) == 0)
+        {
+            err = MPU6515_SCP_SetPowerMode(en, ID_ACCELEROMETER);
+            if (0 == err)
+            {
+                scp_sensor_power = enable_status;
+            }
+            GSE_LOG("Gsensor not in suspend gsensor_SetPowerMode!, enable_status = %d\n",scp_sensor_power);
+        }
+        else
+        {
+            GSE_LOG("Gsensor in suspend and can not enable or disable!enable_status = %d\n",scp_sensor_power);
+        }
+    }
+    mutex_unlock(&gsensor_mutex);
+
+    if(err != MPU6515_SUCCESS)
+    {
+        printk("scp_gsensor_enable_nodata fail!\n");
+        return -1;
+    }
+
+    printk("scp_gsensor_enable_nodata OK!!!\n");
+    return 0;
+}
+#endif
+/*----------------------------------------------------------------------------*/
+static int gsensor_set_delay(u64 ns)
+{
+    int err = 0;
+    int value;
+#ifdef CUSTOM_KERNEL_SENSORHUB
+    SCP_SENSOR_HUB_DATA req;
+    int len;
+#else//#ifdef CUSTOM_KERNEL_SENSORHUB
+	int sample_delay;
+#endif//#ifdef CUSTOM_KERNEL_SENSORHUB
+
+#ifdef GSENSOR_UT
+    GSE_FUN();
+#endif
+
+    value = (int)ns/1000/1000;
+
+#ifdef CUSTOM_KERNEL_SENSORHUB
+    req.set_delay_req.sensorType = ID_ACCELEROMETER;
+    req.set_delay_req.action = SENSOR_HUB_SET_DELAY;
+    req.set_delay_req.delay = value;
+    len = sizeof(req.activate_req);
+    err = SCP_sensorHub_req_send(&req, &len, 1);
+    if (err)
+    {
+        GSE_ERR("SCP_sensorHub_req_send!\n");
+        return err;
+    }
+#else//#ifdef CUSTOM_KERNEL_SENSORHUB    
+	if(value <= 5)
+	{
+		sample_delay = MPU6515_BW_184HZ;
+	}
+	else if(value <= 10)
+	{
+		sample_delay = MPU6515_BW_92HZ;
+	}
+	else
+	{
+		sample_delay = MPU6515_BW_41HZ;
+	}
+
+	mutex_lock(&gsensor_mutex);
+	err = MPU6515_SetBWRate(obj_i2c_data->client, sample_delay);
+	mutex_unlock(&gsensor_mutex);
+	if(err != MPU6515_SUCCESS ) //0x2C->BW=100Hz
+	{
+		GSE_ERR("Set delay parameter error!\n");
+        return -1;
+	}
+
+	if(value >= 50)
+	{
+		atomic_set(&obj_i2c_data->filter, 0);
+	}
+	else
+	{	
+	#if defined(CONFIG_MPU6515_LOWPASS)
+		obj_i2c_data->fir.num = 0;
+		obj_i2c_data->fir.idx = 0;
+		obj_i2c_data->fir.sum[MPU6515_AXIS_X] = 0;
+		obj_i2c_data->fir.sum[MPU6515_AXIS_Y] = 0;
+		obj_i2c_data->fir.sum[MPU6515_AXIS_Z] = 0;
+		atomic_set(&obj_i2c_data->filter, 1);
+	#endif
+	}
+#endif//#ifdef CUSTOM_KERNEL_SENSORHUB
+    
+    GSE_LOG("gsensor_set_delay (%d)\n",value);
+
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int gsensor_get_data(int* x ,int* y,int* z, int* status)
+{
+#ifdef CUSTOM_KERNEL_SENSORHUB
+    SCP_SENSOR_HUB_DATA req;
+    int len;
+    int err = 0;
+#else
+    char buff[MPU6515_BUFSIZE];
+#endif //#ifdef CUSTOM_KERNEL_SENSORHUB
+
+    //GSE_FUN();
+
+#ifdef CUSTOM_KERNEL_SENSORHUB
+    req.get_data_req.sensorType = ID_ACCELEROMETER;
+    req.get_data_req.action = SENSOR_HUB_GET_DATA;
+    len = sizeof(req.get_data_req);
+    err = SCP_sensorHub_req_send(&req, &len, 1);
+    if (err)
+    {
+        GSE_ERR("SCP_sensorHub_req_send!\n");
+        return err;
+    }
+
+    if (ID_ACCELEROMETER != req.get_data_rsp.sensorType ||
+        SENSOR_HUB_GET_DATA != req.get_data_rsp.action ||
+        0 != req.get_data_rsp.errCode)
+    {
+        GSE_ERR("error : %d\n", req.get_data_rsp.errCode);
+        return req.get_data_rsp.errCode;
+    }
+
+    //sscanf(buff, "%x %x %x", req.get_data_rsp.int16_Data[0], req.get_data_rsp.int16_Data[1], req.get_data_rsp.int16_Data[2]);
+    *x = (int)req.get_data_rsp.int16_Data[0]*GRAVITY_EARTH_1000/1000;
+    *y = (int)req.get_data_rsp.int16_Data[1]*GRAVITY_EARTH_1000/1000;
+    *z = (int)req.get_data_rsp.int16_Data[2]*GRAVITY_EARTH_1000/1000;
+    //GSE_ERR("x = %d, y = %d, z = %d\n", *x, *y, *z);
+    *status = SENSOR_STATUS_ACCURACY_MEDIUM;
+
+    if(atomic_read(&obj_i2c_data->trace) & MPU6515_TRC_RAWDATA)
+	{
+        GSE_ERR("x = %d, y = %d, z = %d\n", *x, *y, *z);
+	}
+#else//#ifdef CUSTOM_KERNEL_SENSORHUB
+    mutex_lock(&gsensor_mutex);
+	MPU6515_ReadSensorData(obj_i2c_data->client, buff, MPU6515_BUFSIZE);
+	mutex_unlock(&gsensor_mutex);
+	sscanf(buff, "%x %x %x", x, y, z);
+	*status = SENSOR_STATUS_ACCURACY_MEDIUM;
+#endif
+
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int mpu6515_i2c_detect(struct i2c_client *client, struct i2c_board_info *info) 
+{    
+    strcpy(info->type, MPU6515_DEV_NAME);
+    return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int mpu6515_i2c_probe(struct i2c_client *client, const struct i2c_device_id *id)
+{
+    struct i2c_client *new_client;
+    struct mpu6515_i2c_data *obj;
+    struct acc_control_path ctl={0};
+    struct acc_data_path data={0};
+    int err = 0;
+    GSE_FUN();
+
+    if (!(obj = kzalloc(sizeof(*obj), GFP_KERNEL)))
+    {
+        err = -ENOMEM;
+        goto exit;
+    }
+
+    memset(obj, 0, sizeof(struct mpu6515_i2c_data));
+
+    obj->hw = get_cust_acc_hw();
+
+    if ((err = hwmsen_get_convert(obj->hw->direction, &obj->cvt)))
+    {
+        GSE_ERR("invalid direction: %d\n", obj->hw->direction);
+        goto exit;
+    }
+
+#ifdef CUSTOM_KERNEL_SENSORHUB
+    INIT_WORK(&obj->irq_work, gsensor_irq_work);
+#endif//#ifdef CUSTOM_KERNEL_SENSORHUB
+
+    obj_i2c_data = obj;
+    obj->client = client;
+#ifdef FPGA_EARLY_PORTING
+    obj->client->timing = 100; 
+#else
+    obj->client->timing = 400;
+#endif
+
+    new_client = obj->client;
+    i2c_set_clientdata(new_client,obj);
+
+    atomic_set(&obj->trace, 0);
+    atomic_set(&obj->suspend, 0);
+#ifdef CUSTOM_KERNEL_SENSORHUB
+    obj->SCP_init_done = 0;
+#endif//#ifdef CUSTOM_KERNEL_SENSORHUB
+
+#ifdef CONFIG_MPU6515_LOWPASS
+    if (obj->hw->firlen > C_MAX_FIR_LENGTH)
+    {
+        atomic_set(&obj->firlen, C_MAX_FIR_LENGTH);
+    }
+    else
+    {
+        atomic_set(&obj->firlen, obj->hw->firlen);
+    }
+
+    if (atomic_read(&obj->firlen) > 0)
+    {
+        atomic_set(&obj->fir_en, 1);
+    }
+
+#endif
+
+    mpu6515_i2c_client = new_client;    
+    MPU6515_Dev_Reset(new_client);
+    MPU6515_Reset(new_client);
+
+    if ((err = mpu6515_init_client(new_client, 1)))
+    {
+        goto exit_init_failed;
+    }
+
+
+    if ((err = misc_register(&mpu6515_device)))
+    {
+        GSE_ERR("mpu6515_device register failed\n");
+        goto exit_misc_device_register_failed;
+    }
+
+
+    if ((err = mpu6515_create_attr(&mpu6515_init_info.platform_diver_addr->driver)))
+    {
+        GSE_ERR("create attribute err = %d\n", err);
+        goto exit_create_attr_failed;
+    }
+
+    ctl.open_report_data= gsensor_open_report_data;
+#ifdef CUSTOM_KERNEL_SENSORHUB
+    ctl.enable_nodata = scp_gsensor_enable_nodata;
+#else
+    ctl.enable_nodata = gsensor_enable_nodata;
+#endif
+    ctl.set_delay  = gsensor_set_delay;
+    ctl.is_report_input_direct = false;
+#ifdef CUSTOM_KERNEL_SENSORHUB
+    ctl.is_support_batch = obj->hw->is_batch_supported;
+#else
+    ctl.is_support_batch = false;
+#endif
+
+    err = acc_register_control_path(&ctl);
+    if(err)
+    {
+        GSE_ERR("register acc control path err\n");
+        goto exit_create_attr_failed;
+    }
+
+    data.get_data = gsensor_get_data;
+    data.vender_div = 1000;
+    err = acc_register_data_path(&data);
+    if(err)
+    {
+        GSE_ERR("register acc data path err\n");
+        goto exit_create_attr_failed;
+    }
+
+    err = batch_register_support_info(ID_ACCELEROMETER,ctl.is_support_batch, 102, 0); //divisor is 1000/9.8
+    if(err)
+    {
+        GSE_ERR("register gsensor batch support err = %d\n", err);
+        goto exit_create_attr_failed;
+    }
+
+#if defined(CONFIG_HAS_EARLYSUSPEND) && defined(USE_EARLY_SUSPEND)
+    obj->early_drv.level    = EARLY_SUSPEND_LEVEL_STOP_DRAWING - 2,
+    obj->early_drv.suspend  = mpu6515_early_suspend,
+    obj->early_drv.resume   = mpu6515_late_resume,    
+    register_early_suspend(&obj->early_drv);
+#endif 
+
+    gsensor_init_flag =0;
+    GSE_LOG("%s: OK\n", __func__);    
+    return 0;
+
+    exit_create_attr_failed:
+    misc_deregister(&mpu6515_device);
+    exit_misc_device_register_failed:
+    exit_init_failed:
+    //i2c_detach_client(new_client);
+    exit_kfree:
+    kfree(obj);
+    exit:
+    GSE_ERR("%s: err = %d\n", __func__, err);
+    gsensor_init_flag = -1;
+    return err;
+}
+
+/*----------------------------------------------------------------------------*/
+static int mpu6515_i2c_remove(struct i2c_client *client)
+{
+    int err = 0;    
+
+    if ((err =  mpu6515_delete_attr(&mpu6515_init_info.platform_diver_addr->driver)))
+    {
+        GSE_ERR("mpu6515_delete_attr fail: %d\n", err);
+    }
+
+    if ((err = misc_deregister(&mpu6515_device)))
+    {
+        GSE_ERR("misc_deregister fail: %d\n", err);
+    }
+
+    mpu6515_i2c_client = NULL;
+    i2c_unregister_device(client);
+    kfree(i2c_get_clientdata(client));
+    return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int gsensor_local_init(void)
+{
+    struct acc_hw *hw = get_cust_acc_hw();
+
+    GSE_FUN();
+
+	MPU6515_power(hw, 1);
+	if(i2c_add_driver(&mpu6515_i2c_driver))
+	{
+		GSE_ERR("add driver error\n");
+		return -1;
+	}
+	if(-1 == gsensor_init_flag)
+	{
+	   return -1;
+	}
+	return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int gsensor_remove()
+{
+    struct acc_hw *hw = get_cust_acc_hw();
+
+    GSE_FUN();
+    MPU6515_power(hw, 0);    
+    i2c_del_driver(&mpu6515_i2c_driver);
+    return 0;
+}
+/*----------------------------------------------------------------------------*/
+static int __init mpu6515gse_init(void)
+{
+    struct acc_hw *hw = get_cust_acc_hw();
+	GSE_LOG("%s: i2c_number=%d\n", __func__,hw->i2c_num);
+    i2c_register_board_info(hw->i2c_num, &i2c_mpu6515, 1);
+    acc_driver_add(&mpu6515_init_info);
+    return 0;    
+}
+/*----------------------------------------------------------------------------*/
+static void __exit mpu6515gse_exit(void)
+{
+    GSE_FUN();
+}
+/*----------------------------------------------------------------------------*/
+module_init(mpu6515gse_init);
+module_exit(mpu6515gse_exit);
+/*----------------------------------------------------------------------------*/
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("MPU6515 gse driver");
+MODULE_AUTHOR("Yucong.Xiong@mediatek.com");
diff --git a/drivers/misc/mediatek/accelerometer/mpu6515/mpu6515.h b/drivers/misc/mediatek/accelerometer/mpu6515/mpu6515.h
new file mode 100644
index 000000000..a8eb7307f
--- /dev/null
+++ b/drivers/misc/mediatek/accelerometer/mpu6515/mpu6515.h
@@ -0,0 +1,97 @@
+#ifndef MPU6515_H
+#define MPU6515_H
+
+#include <linux/ioctl.h>
+
+#define MPU6515_I2C_SLAVE_ADDR		0xD0 // or 0xD1
+
+
+/* MPU6515 Register Map  (Please refer to MPU6515 Specifications) */
+#define MPU6515_REG_DEVID			0x75
+#define	MPU6515_REG_BW_RATE			0x1D
+#define MPU6515_REG_POWER_CTL       0x6B
+#define MPU6515_REG_POWER_CTL2      0x6C
+#define MPU6515_REG_INT_ENABLE		0x38
+#define MPU6515_REG_DATA_FORMAT		0x1C
+#define MPU6515_REG_DATAX0			0x3B
+#define MPU6515_REG_DATAY0			0x3D
+#define MPU6515_REG_DATAZ0			0x3F
+#define MPU6515_REG_RESET               0x68
+
+/* register Value */ 
+#define MPU6515_FIXED_DEVID         0x74
+													
+                                           // delay(ms)	
+#define MPU6515_BW_460HZ            0x00   //1.94
+#define MPU6515_BW_184HZ            0x01   //5.8
+#define MPU6515_BW_92HZ             0x02   //7.8
+#define MPU6515_BW_41HZ             0x03   //11.8
+#define MPU6515_BW_20HZ             0x04   //19.8
+#define MPU6515_BW_10HZ             0x05   //35.7
+#define MPU6515_BW_5HZ              0x06   //66.96
+
+#define MPU6515_DEV_RESET           0x80
+
+//#define MPU6515_FULL_RES			0x08
+#define MPU6515_RANGE_2G			(0x00 << 3)
+#define MPU6515_RANGE_4G			(0x01 << 3)
+#define MPU6515_RANGE_8G			(0x02 << 3)
+#define MPU6515_RANGE_16G			(0x03 << 3)
+//#define MPU6515_SELF_TEST         0x80
+
+
+#define MPU6515_SLEEP				0x40	//enable low power sleep mode
+
+
+
+// below do not modify	 
+#define MPU6515_SUCCESS                     0
+#define MPU6515_ERR_I2C                     -1
+#define MPU6515_ERR_STATUS                  -3
+#define MPU6515_ERR_SETUP_FAILURE           -4
+#define MPU6515_ERR_GETGSENSORDATA          -5
+#define MPU6515_ERR_IDENTIFICATION          -6
+
+#define MPU6515_BUFSIZE				256
+
+#define MPU6515_AXES_NUM        3
+
+/*----------------------------------------------------------------------------*/
+typedef enum{
+    MPU6515_CUST_ACTION_SET_CUST = 1,
+    MPU6515_CUST_ACTION_SET_CALI,
+    MPU6515_CUST_ACTION_RESET_CALI
+}CUST_ACTION;
+/*----------------------------------------------------------------------------*/
+typedef struct
+{
+    uint16_t    action;
+}MPU6515_CUST;
+/*----------------------------------------------------------------------------*/
+typedef struct
+{
+    uint16_t    action;
+    uint16_t    part;
+    int32_t     data[0];
+}MPU6515_SET_CUST;
+/*----------------------------------------------------------------------------*/
+typedef struct
+{
+    uint16_t    action;
+    int32_t     data[MPU6515_AXES_NUM];
+}MPU6515_SET_CALI;
+/*----------------------------------------------------------------------------*/
+typedef MPU6515_CUST MPU6515_RESET_CALI;
+/*----------------------------------------------------------------------------*/
+typedef union
+{
+    uint32_t                data[10];
+    MPU6515_CUST         cust;
+    MPU6515_SET_CUST     setCust;
+    MPU6515_SET_CALI     setCali;
+    MPU6515_RESET_CALI   resetCali;
+}MPU6515_CUST_DATA;
+/*----------------------------------------------------------------------------*/
+
+#endif
+
diff --git a/drivers/misc/mediatek/accelerometer/mxc400x-new/Makefile b/drivers/misc/mediatek/accelerometer/mxc400x-new/Makefile
new file mode 100755
index 000000000..3f173122f
--- /dev/null
+++ b/drivers/misc/mediatek/accelerometer/mxc400x-new/Makefile
@@ -0,0 +1,4 @@
+include $(srctree)/drivers/misc/mediatek/Makefile.custom
+
+obj-y	:=  mxc400x.o
+
diff --git a/drivers/misc/mediatek/accelerometer/mxc400x-new/mxc400x.c b/drivers/misc/mediatek/accelerometer/mxc400x-new/mxc400x.c
new file mode 100644
index 000000000..0b8d31c7f
--- /dev/null
+++ b/drivers/misc/mediatek/accelerometer/mxc400x-new/mxc400x.c
@@ -0,0 +1,2745 @@
+/******************** (C) COPYRIGHT 2010 STMicroelectronics ********************
+ *
+ * File Name          : mxc400x_acc.c
+ * Description        : MXC400X accelerometer sensor API
+ *
+ *******************************************************************************
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * THE PRESENT SOFTWARE IS PROVIDED ON AN "AS IS" BASIS, WITHOUT WARRANTIES
+ * OR CONDITIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED, FOR THE SOLE
+ * PURPOSE TO SUPPORT YOUR APPLICATION DEVELOPMENT.
+ * AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY DIRECT,
+ * INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING FROM THE
+ * CONTENT OF SUCH SOFTWARE AND/OR THE USE MADE BY CUSTOMERS OF THE CODING
+ * INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
+ *
+ * THIS SOFTWARE IS SPECIFICALLY DESIGNED FOR EXCLUSIVE USE WITH ST PARTS.
+ *
+
+ ******************************************************************************/
+
+#include <linux/interrupt.h>
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include <linux/irq.h>
+#include <linux/miscdevice.h>
+#include <asm/uaccess.h>
+#include <linux/delay.h>
+#include <linux/input.h>
+#include <linux/workqueue.h>
+#include <linux/kobject.h>
+#include <linux/earlysuspend.h>
+#include <linux/platform_device.h>
+#include <asm/atomic.h>
+	 
+#include <accel.h>
+#include <linux/batch.h>
+#ifdef CUSTOM_KERNEL_SENSORHUB
+#include <SCP_sensorHub.h>
+#endif//#ifdef CUSTOM_KERNEL_SENSORHUB
+
+#define POWER_NONE_MACRO MT65XX_POWER_NONE
+//#define KK_OLDARCH 1
+#define L_NewArch 1
+
+
+#include <mach/mt_pm_ldo.h>
+#include <mach/mt_typedefs.h>
+#include <mach/mt_gpio.h>
+#include <mach/mt_boot.h>
+/*-------------------------MT6516&MT6573 define-------------------------------*/
+
+#include <cust_acc.h>
+#include <linux/hwmsensor.h>
+#include <linux/hwmsen_dev.h>
+#include <linux/sensors_io.h>
+#include "mxc400x.h"
+#include <linux/hwmsen_helper.h>
+
+
+#define I2C_DRIVERID_MXC400X 120
+#define DEBUG  1
+#define SW_CALIBRATION
+//#define SW_SIMULATION   0 
+//#define SW_SIMULATION_DEBUG   0
+ 
+#define MXC400X_AXIS_X          0
+#define MXC400X_AXIS_Y          1
+#define MXC400X_AXIS_Z          2
+#define MXC400X_AXES_NUM        3
+#define MXC400X_DATA_LEN        6
+
+static s16 cali_sensor_data[MXC400X_AXES_NUM];
+
+static const struct i2c_device_id mxc400x_i2c_id[] = { { MXC400X_DEV_NAME, 0 }, { }, };
+static struct i2c_board_info __initdata i2c_mxc400x={ I2C_BOARD_INFO(MXC400X_DEV_NAME, MXC400X_I2C_ADDR)};
+static int mxc400x_i2c_probe(struct i2c_client *client, const struct i2c_device_id *id); 
+static int mxc400x_i2c_remove(struct i2c_client *client);
+static int mxc400x_i2c_detect(struct i2c_client *client, int kind, struct i2c_board_info *info);
+#ifndef CONFIG_HAS_EARLYSUSPEND
+static int mxc400x_suspend(struct i2c_client *client, pm_message_t msg);
+static int mxc400x_resume(struct i2c_client *client);
+#endif
+
+static int  mxc400x_local_init(void);
+static int mxc400x_remove(void);
+
+
+typedef enum {
+	ADX_TRC_FILTER  = 0x01,
+	ADX_TRC_RAWDATA = 0x02,
+	ADX_TRC_IOCTL	 = 0x04,
+	ADX_TRC_CALI	 = 0X08,
+	ADX_TRC_INFO	 = 0X10,
+} ADX_TRC;
+
+struct scale_factor{
+	u8  whole;
+	u8  fraction;
+};
+
+struct data_resolution {
+	struct scale_factor scalefactor;
+	int				 sensitivity;
+};
+
+#define C_MAX_FIR_LENGTH (32)
+
+struct data_filter {
+	s16 raw[C_MAX_FIR_LENGTH][MXC400X_AXES_NUM];
+	int sum[MXC400X_AXES_NUM];
+	int num;
+	int idx;
+};
+static int mxc400x_init_flag = -1;
+/*----------------------------------------------------------------------------*/
+static struct acc_init_info mxc400x_init_info = {
+		.name = "mxc400x",
+		.init = mxc400x_local_init,
+		.uninit = mxc400x_remove,
+};
+struct mxc400x_i2c_data {
+		 struct i2c_client *client;
+		 struct acc_hw *hw;
+		 struct hwmsen_convert	 cvt;
+		 atomic_t layout; 
+		 /*misc*/
+		 struct data_resolution *reso;
+		 atomic_t				 trace;
+		 atomic_t				 suspend;
+		 atomic_t				 selftest;
+		 atomic_t				 filter;
+		 s16					 cali_sw[MXC400X_AXES_NUM+1];
+	 
+		 /*data*/
+		 s16 					 offset[MXC400X_AXES_NUM+1];  /*+1: for 4-byte alignment*/
+		 s16					 data[MXC400X_AXES_NUM+1];
+#if defined(CONFIG_MXC400X_LOWPASS)
+		 atomic_t				 firlen;
+		 atomic_t				 fir_en;
+		 struct data_filter 	 fir;
+#endif 
+		 /*early suspend*/
+#if defined(CONFIG_HAS_EARLYSUSPEND)
+		 struct early_suspend	 early_drv;
+#endif 
+};
+
+static struct i2c_driver mxc400x_i2c_driver = {
+	 .driver = {
+		// .owner		   = THIS_MODULE,
+		 .name			 = MXC400X_DEV_NAME,
+	 },
+	 .probe 			 = mxc400x_i2c_probe,
+	 .remove			 = mxc400x_i2c_remove,
+	 .detect			 = mxc400x_i2c_detect,
+	#if !defined(CONFIG_HAS_EARLYSUSPEND)    
+	 .suspend			 = mxc400x_suspend,
+	 .resume			 = mxc400x_resume,
+	#endif
+	 .id_table = mxc400x_i2c_id,
+};
+
+
+
+struct i2c_client      *mxc400x_i2c_client = NULL;
+static struct mxc400x_i2c_data *obj_i2c_data = NULL;
+static struct platform_driver mxc400x_gsensor_driver;
+static bool sensor_power = true;
+static GSENSOR_VECTOR3D gsensor_gain;
+//static DEFINE_MUTEX(mxc400x_mutex);
+static struct mutex mxc400x_mutex;
+static bool enable_status = false;
+
+extern struct acc_hw* get_cust_acc_hw(void);
+
+#ifdef  DEBUG
+#define GSE_TAG                  "[Gsensor] "
+#define GSE_FUN(f)               printk(GSE_TAG"%s\n", __FUNCTION__)
+#define GSE_ERR(fmt, args...)    printk(KERN_ERR GSE_TAG"%s %d : "fmt, __FUNCTION__, __LINE__, ##args)
+#define GSE_LOG(fmt, args...)    printk(GSE_TAG fmt, ##args)
+#else
+#define GSE_TAG
+#define GSE_FUN(f)               do {} while (0)
+#define GSE_ERR(fmt, args...)    do {} while (0)
+#define GSE_LOG(fmt, args...)    do {} while (0)
+#endif
+
+static struct data_resolution mxc400x_data_resolution[] = {
+    {{ 0, 9}, 1024},   /*+/-2g  in 12-bit resolution:  0.9 mg/LSB*/
+    {{ 1, 9}, 512},   /*+/-4g  in 12-bit resolution:  1.9 mg/LSB*/
+    {{ 3, 9},  256},   /*+/-8g  in 12-bit resolution: 3.9 mg/LSB*/      
+};
+static struct data_resolution mxc400x_offset_resolution = {{3, 9}, 256};
+
+
+/**********************************************/
+/**********SW SIMULATE FUNC********************/
+/**********************************************/
+
+#if defined(SW_SIMULATION)	//modified by lyon
+#define MXC400X_I2C_SLAVE_WRITE_ADDR	0x2A	
+static struct mutex g_gsensor_mutex;
+#define GPIO_GSE_SDA_PIN 90
+#define GPIO_GSE_SCL_PIN 89
+#define GPIO_GSE_SDA_PIN_M_GPIO GPIO_MODE_00
+#define GPIO_GSE_SCL_PIN_M_GPIO GPIO_MODE_00
+void cust_i2c_scl_set( unsigned char mode)
+{
+	if(mt_set_gpio_mode(GPIO_GSE_SCL_PIN, GPIO_GSE_SDA_PIN_M_GPIO))
+	{
+		printk("MXC400X cust_i2c_scl_set mode failed \n");
+	}
+	if(mt_set_gpio_dir(GPIO_GSE_SCL_PIN, GPIO_DIR_OUT))
+	{
+		printk("MXC400X cust_i2c_scl_set dir failed \n");
+	}
+	if( mode == 1)
+	{
+		if(mt_set_gpio_out(GPIO_GSE_SCL_PIN, GPIO_OUT_ONE))
+		{
+			printk("MXC400X cust_i2c_scl_set high failed \n");
+		}
+	}
+	else
+	{
+		if(mt_set_gpio_out(GPIO_GSE_SCL_PIN, GPIO_OUT_ZERO))
+		{
+			printk("MXC400X cust_i2c_scl_set low failed \n");
+		}
+	}
+}
+
+void cust_i2c_sda_set( unsigned char mode)
+{
+	if(mt_set_gpio_mode(GPIO_GSE_SDA_PIN, GPIO_GSE_SDA_PIN_M_GPIO))
+	{
+		printk("MXC400X cust_i2c_sda_set mode failed \n");
+	}
+	if(mt_set_gpio_dir(GPIO_GSE_SDA_PIN, GPIO_DIR_OUT))
+	{
+		printk("MXC400X cust_i2c_sda_set dir failed \n");
+	}
+	if( mode == 1)
+	{
+		if(mt_set_gpio_out(GPIO_GSE_SDA_PIN, GPIO_OUT_ONE))
+		{
+			printk("MXC400X cust_i2c_sda_set high failed \n");
+		}
+	}
+	else
+	{
+		if(mt_set_gpio_out(GPIO_GSE_SDA_PIN, GPIO_OUT_ZERO))
+		{
+			printk("MXC400X cust_i2c_sda_set low failed \n");
+		}
+	}
+}
+
+void cust_i2c_sda_dir_set( unsigned char mode)
+{	
+	if(mt_set_gpio_mode(GPIO_GSE_SDA_PIN, GPIO_GSE_SDA_PIN_M_GPIO))
+	{
+		printk("MXC400X cust_i2c_sda_dir_set mode failed \n");
+	}
+	if( mode == GPIO_DIR_IN)
+	{
+		if(mt_set_gpio_dir(GPIO_GSE_SDA_PIN, GPIO_DIR_IN))
+		{
+			printk("MXC400X cust_i2c_sda_dir_set in failed \n");
+		}
+	}
+	else
+	{
+		if(mt_set_gpio_dir(GPIO_GSE_SDA_PIN, GPIO_DIR_OUT))
+		{
+			printk("MXC400X cust_i2c_sda_dir_set out failed \n");
+		}
+	}
+}
+
+unsigned char cust_i2c_sda_get(void)
+{
+	if(mt_set_gpio_mode(GPIO_GSE_SDA_PIN, GPIO_GSE_SDA_PIN_M_GPIO))
+	{
+		printk("MXC400X cust_i2c_sda_get mode failed \n");
+	}
+	if(mt_set_gpio_dir(GPIO_GSE_SDA_PIN,GPIO_DIR_IN))
+	{
+		printk("MXC400X cust_i2c_sda_get dir failed \n");
+	}
+	
+	return  mt_get_gpio_in(GPIO_GSE_SDA_PIN);
+}
+
+void cust_i2c_start(void)
+{
+	//printk("cust_i2c_start \n");
+	cust_i2c_scl_set(1);
+	cust_i2c_sda_set(1);
+	udelay(5);
+	cust_i2c_sda_set(0);
+	udelay(5);
+	cust_i2c_scl_set(0);
+}
+
+void cust_i2c_stop(void)
+{
+	//printk("cust_i2c_stop \n");
+	cust_i2c_scl_set(1);
+	cust_i2c_sda_set(0);
+	udelay(5);
+	cust_i2c_sda_set(1);
+	udelay(5);
+}
+
+char cust_i2c_get_ack(void)
+{
+	unsigned char get_bit;
+	unsigned char i;
+	//printk("cust_i2c_get_ack \n");
+	//cust_i2c_sda_set(1);
+	//udelay(5);
+	cust_i2c_sda_dir_set(GPIO_DIR_IN);
+	cust_i2c_scl_set(1);
+	udelay(5);
+	//cust_i2c_sda_get();
+	for(i=0; i<10; i++)
+	{
+		get_bit =  mt_get_gpio_in(GPIO_GSE_SDA_PIN);
+		udelay(5);
+		if(0 == get_bit)
+		{
+			break;
+		}
+	}
+	cust_i2c_scl_set(0);
+	cust_i2c_sda_dir_set(GPIO_DIR_OUT);
+	if(i == 10)
+	{
+		return -1;
+	}
+	
+	return 0;
+}
+
+char cust_i2c_send_ack(void)
+{
+	//printk("cust_i2c_send_ack \n");
+	cust_i2c_sda_set(0);
+	udelay(5);
+	cust_i2c_scl_set(1);
+	udelay(5);
+	cust_i2c_scl_set(0);
+	return 0;
+}
+
+void cust_i2c_no_ack(void)
+{
+	//printk("cust_i2c_send_ack \n");
+	cust_i2c_sda_set(1);
+	cust_i2c_scl_set(1);
+	udelay(5);
+	cust_i2c_scl_set(0);
+	udelay(5);
+}
+
+void cust_i2c_send_byte( unsigned char udata)
+{
+	char i; 
+
+	//printk("cust_i2c_send_byte \n",udata);
+	for( i = 0; i<8;i++ )
+	{
+		if( ((udata>>(7-i)) & 0x01) == 0x01)
+		{
+			cust_i2c_sda_set(1);
+		}
+		else
+		{
+			cust_i2c_sda_set(0);
+		}
+		
+		udelay(2);
+		cust_i2c_scl_set(1);
+		udelay(5);
+		cust_i2c_scl_set(0);
+	}	
+		udelay(5);
+}
+
+unsigned char cust_i2c_get_byte( void )
+{
+	char i;
+	unsigned char data;
+	unsigned char get_bit;
+	
+	data = 0;
+	//printk("cust_i2c_get_byte \n");
+	cust_i2c_sda_dir_set(GPIO_DIR_IN);	
+	for( i = 0; i<8; i++ )
+	{
+		cust_i2c_scl_set(1);
+		udelay(5);
+		//get_bit = cust_i2c_sda_get();
+		get_bit =  mt_get_gpio_in(GPIO_GSE_SDA_PIN);
+		cust_i2c_scl_set(0);
+		udelay(5);
+		if( 1 == (get_bit &0x01))
+		{
+			data |= get_bit <<(7-i);
+		}
+	}	
+	udelay(5);
+	return data;
+}
+
+char cust_i2c_write_byte(unsigned char addr, unsigned char regaddr, unsigned char udata)
+{
+	char res;
+
+	cust_i2c_start();
+	cust_i2c_send_byte(addr);
+	res = cust_i2c_get_ack();
+	if(0 != res)
+	{
+		printk("MXC400X cust_i2c_write_byte device addr error \n");
+		return -1;
+	}
+	cust_i2c_send_byte(regaddr);
+	res = cust_i2c_get_ack();
+	if( 0 != res )
+	{
+		printk("MXC400X cust_i2c_write_byte reg addr error \n");
+		return -1;
+	}
+	cust_i2c_send_byte(udata);
+	res = cust_i2c_get_ack();
+	if( 0 != res )
+	{
+		printk("MXC400X cust_i2c_write_byte reg data error \n");
+		return -1;
+	}
+	cust_i2c_stop();
+	return 0;
+}
+
+char cust_i2c_read_byte(unsigned char addr, unsigned char regaddr, unsigned char *udata)
+{
+	unsigned char data;
+	char res;
+
+	cust_i2c_start();
+	cust_i2c_send_byte( addr );
+	res = cust_i2c_get_ack();
+	if( 0 != res )
+	{
+		printk("MXC400X cust_i2c_read_byte device addr error \n");
+		return -1;
+	}
+	cust_i2c_send_byte( regaddr );
+	res = cust_i2c_get_ack();
+	if( 0 != res )
+	{
+		printk("MXC400X cust_i2c_read_byte reg addr error \n");
+		return -1;
+	}
+	cust_i2c_start();
+	addr |= 0x01;
+	cust_i2c_send_byte( addr );
+	res = cust_i2c_get_ack();
+	if( 0 != res )
+	{
+		printk("MXC400X cust_i2c_read_byte devce addr error \n");
+		return -1;
+	}
+	*udata = cust_i2c_get_byte();
+	cust_i2c_no_ack();
+	cust_i2c_stop();
+
+	return 0;
+}
+
+char cust_i2c_write_bytes(unsigned char addr, unsigned char regaddr, unsigned char *udata, unsigned int count)
+{
+	u32 i;
+	char res;
+
+	cust_i2c_start();
+	cust_i2c_send_byte( addr );
+	res = cust_i2c_get_ack();
+	if( 0 != res )
+	{
+		printk("MXC400X cust_i2c_write_bytes device addr error \n");
+		return -1;
+	}
+	cust_i2c_send_byte( regaddr );
+	res = cust_i2c_get_ack();
+	if( 0 != res )
+	{
+		printk("MXC400X cust_i2c_write_bytes reg addr error \n");
+		return -1;
+	}
+	for( i = 0; i< count; i++)
+	{
+		cust_i2c_send_byte(udata[i]);
+		res = cust_i2c_get_ack();
+		if(0 != res)
+		{
+			printk("MXC400X cust_i2c_write_bytes reg data error \n",__FUNCTION__,__LINE__);
+			return -1;
+		}
+	}
+	cust_i2c_stop();
+
+	return 0;
+}
+
+char cust_i2c_read_bytes(unsigned char addr, unsigned char regaddr, unsigned char *udata,unsigned int count)
+{
+	unsigned char data;
+	unsigned int i;
+	char res;
+
+	cust_i2c_start();
+	cust_i2c_send_byte( addr );
+	res = cust_i2c_get_ack();
+	if( 0 != res )
+	{
+		printk("MXC400X cust_i2c_read_bytes device addr error \n");
+		return -1;
+	}
+	cust_i2c_send_byte( regaddr );
+	res = cust_i2c_get_ack();
+	if( 0 != res )
+	{
+		printk("MXC400X cust_i2c_read_bytes reg addr error \n");
+		return -1;
+	}
+	cust_i2c_start();
+	addr |= 0x01;
+	cust_i2c_send_byte( addr );
+	res = cust_i2c_get_ack();
+	if( 0 != res )
+	{
+		printk("MXC400X cust_i2c_read_bytes reg addr error \n");
+		return -1;
+	}
+
+	for( i = 0; i < count-1; i++)
+	{
+		udata[i] = cust_i2c_get_byte();
+		res = cust_i2c_send_ack();
+		if(0 != res)
+		{
+			printk("MXC400X cust_i2c_read_bytes reg data error \n");
+			return -1;
+		}
+	}
+	udata[i] = cust_i2c_get_byte();
+	cust_i2c_no_ack();
+
+	cust_i2c_stop();
+
+	return data;
+}
+
+int cust_i2c_master_send(struct i2c_client *client, u8 *buf, u8 count)
+{
+	u8 slave_addr;
+	u8 reg_addr;
+
+	mutex_lock(&g_gsensor_mutex);
+	slave_addr = MXC400X_I2C_SLAVE_WRITE_ADDR ;	
+	reg_addr = buf[0];
+#if defined(SW_SIMULATION_DEBUG)
+	printk("MXC400X cust_i2c_master_send_byte reg_addr=0x x% \n", reg_addr);
+#endif
+	cust_i2c_write_bytes(slave_addr,reg_addr, &buf[1],count-1);
+	mutex_unlock(&g_gsensor_mutex);
+
+	return count;
+}
+
+int cust_i2c_master_read(struct i2c_client *client, u8 *buf, u8 count)
+{
+	u8 slave_addr;
+	u8 reg_addr;
+
+	slave_addr = MXC400X_I2C_SLAVE_WRITE_ADDR ;	
+	reg_addr = buf[0];
+#if defined(SW_SIMULATION_DEBUG)
+	printk("MXC400X cust_i2c_master_read_byte reg_addr=0x %x\n", reg_addr);
+#endif
+	cust_i2c_read_bytes(slave_addr,reg_addr, &buf[0],count);
+
+	return count;
+}
+
+int cust_hwmsen_write_block(struct i2c_client *client, u8 addr, u8 *data, u8 len)
+{
+	u8 slave_addr;
+	u8 reg_addr;
+
+	mutex_lock(&g_gsensor_mutex);
+	slave_addr = MXC400X_I2C_SLAVE_WRITE_ADDR;	
+	reg_addr = addr;
+#if defined(SW_SIMULATION_DEBUG)
+	printk("MXC400X cust_hwmsen_write_block reg_addr=0x%x\n", reg_addr);
+#endif
+	cust_i2c_write_bytes(slave_addr,reg_addr, data,len);
+	mutex_unlock(&g_gsensor_mutex);
+
+	return 0;
+}
+
+int cust_hwmsen_read_block(struct i2c_client *client, u8 addr, u8 *data, u8 len)
+{
+	u8 buf[64] = {0};
+	mutex_lock(&g_gsensor_mutex);
+	buf[0] = addr;
+	cust_i2c_master_read(client, buf, len);
+	mutex_unlock(&g_gsensor_mutex);
+#if defined(SW_SIMULATION_DEBUG)	
+	printk("MXC400X cust_hwmsen_read_block addr=0x%x, buf=0x%x\n", addr, buf[0]);
+#endif
+	memcpy(data, buf, len);
+	return 0;
+}
+#endif
+
+/****************************************/
+/**********SW SIMULATE FUNC**************/
+/****************************************/
+
+static int mxc400x_i2c_read_block(struct i2c_client *client, u8 addr, u8 *data, u8 len)
+{
+    u8 beg = addr;
+	struct i2c_msg msgs[2] = {
+	{
+		.addr = client->addr,	.flags = 0,
+		.len = 1,	.buf = &beg
+	},
+	{
+		.addr = client->addr,	.flags = I2C_M_RD,
+		.len = len,	.buf = data,
+	}
+	};
+	int err;
+
+	if (!client)
+		return -EINVAL;
+	else if (len > C_I2C_FIFO_SIZE) {
+		GSE_ERR(" length %d exceeds %d\n", len, C_I2C_FIFO_SIZE);
+		return -EINVAL;
+	}
+
+	err = i2c_transfer(client->adapter, msgs, sizeof(msgs)/sizeof(msgs[0]));
+	if (err != 2) {
+		GSE_ERR("i2c_transfer error: (%d %p %d) %d\n",
+			addr, data, len, err);
+		err = -EIO;
+	} else {
+		err = 0;
+	}
+	return err;
+
+}
+static void mxc400x_power(struct acc_hw *hw, unsigned int on) 
+{
+	 static unsigned int power_on = 0;
+
+	 if(hw->power_id != POWER_NONE_MACRO)		 // have externel LDO
+	 {		  
+		 GSE_LOG("power %s\n", on ? "on" : "off");
+		 if(power_on == on)  // power status not change
+		 {
+			 GSE_LOG("ignore power control: %d\n", on);
+		 }
+		 else if(on) // power on
+		 {
+			 if(!hwPowerOn(hw->power_id, hw->power_vol, "MXC400X"))
+			 {
+				 GSE_ERR("power on fails!!\n");
+			 }
+		 }
+		 else	 // power off
+		 {
+			 if (!hwPowerDown(hw->power_id, "MXC400X"))
+			 {
+				 GSE_ERR("power off fail!!\n");
+			 }			   
+		 }
+	 }
+	 power_on = on;    
+}
+
+
+
+static int MXC400X_SaveData(int buf[MXC400X_AXES_NUM])
+{ 
+   
+	mutex_lock(&mxc400x_mutex);
+//    memcpy(cali_sensor_data, buf, sizeof(cali_sensor_data));
+	cali_sensor_data[0] = buf[0];
+	cali_sensor_data[1] = buf[1];
+	cali_sensor_data[2] = buf[2];
+	
+	mutex_unlock(&mxc400x_mutex);
+
+    return 0;
+}
+static int  MXC400X_RxData(struct i2c_client *client, s16 data[MXC400X_AXES_NUM])
+{
+	u8 addr = MXC400X_REG_X;
+	u8 buf[MXC400X_DATA_LEN] = {0};
+	int err = 0;
+
+	if(NULL == client)
+	{
+        GSE_LOG("client is null\n");
+		err = -EINVAL;
+	}
+#if defined(SW_SIMULATION)
+	else if(err = cust_hwmsen_read_block(client, addr, buf, MXC400X_DATA_LEN))
+#else
+	else if(err = mxc400x_i2c_read_block(client, addr, buf, MXC400X_DATA_LEN))
+#endif
+	{
+		GSE_ERR("error: %d\n", err);
+	}
+	else 
+    {
+        
+	 	data[MXC400X_AXIS_X] = (s16)(buf[0] << 8 | buf[1]) >> 4;
+		data[MXC400X_AXIS_Y] = (s16)(buf[2] << 8 | buf[3]) >> 4;
+		data[MXC400X_AXIS_Z] = (s16)(buf[4] << 8 | buf[5]) >> 4;
+       
+	}
+
+	return err;	
+}
+static int MXC400X_ReadTemp(struct i2c_client *client, s8 *data)
+{     
+	u8 addr = MXC400X_REG_TEMP;
+	int err = 0;
+    s8 temp = 0;
+
+	if(NULL == client)
+	{
+        GSE_LOG("client is null\n");
+		err = -EINVAL;
+	}
+#if defined(SW_SIMULATION)
+	else if(err = cust_hwmsen_read_block(client, addr, &temp, 0x01))
+#else
+	else if(err = mxc400x_i2c_read_block(client, addr, &temp, 1))
+#endif
+	{
+		GSE_ERR("error: %d\n", err);
+	}
+    *data = temp;
+  
+	return err;
+}
+static int MXC400X_ReadDant(struct i2c_client *client, int dant[MXC400X_AXES_NUM+1])
+{
+	u8 addr = MXC400X_REG_X;
+	u8 buf[MXC400X_DATA_LEN+1] = {0};
+	int err = 0;
+
+	if(NULL == client)
+	{
+        GSE_LOG("client is null\n");
+		err = -EINVAL;
+	}
+#if defined(SW_SIMULATION)
+	else if(err = cust_hwmsen_read_block(client, addr, buf, MXC400X_DATA_LEN+1))
+#else
+	else if(err = mxc400x_i2c_read_block(client, addr, buf, MXC400X_DATA_LEN+1))
+#endif
+	{
+		GSE_ERR("error: %d\n", err);
+	}
+	else 
+    {        
+   
+		dant[MXC400X_AXIS_X] = (s16)((s16)buf[0] << 8 | (s16)buf[1]) >> 4;
+		dant[MXC400X_AXIS_Y] = (s16)((s16)buf[2] << 8 | (s16)buf[3]) >> 4;
+		dant[MXC400X_AXIS_Z] = (s16)((s16)buf[4] << 8 | (s16)buf[5]) >> 4;
+		dant[MXC400X_AXIS_Z+1] = (s16)buf[6];
+
+	   
+	}
+    
+	return err;	
+}
+static int MXC400X_Test1(struct i2c_client *client, u8 data)
+{
+
+  	struct mxc400x_i2c_data *obj = i2c_get_clientdata(client);
+	u8 databuf[10];    
+	int res = 0;
+
+	memset(databuf, 0, sizeof(u8)*10);    
+	databuf[0] = MXC400X_REG_TEST1;    
+	databuf[1] = data;
+
+	res = i2c_master_send(client, databuf, 0x2);
+
+	if(res <= 0)
+	{
+		return MXC400X_ERR_I2C;
+	}
+
+    return 0;
+}
+static int MXC400X_Test2(struct i2c_client *client, u8 data)
+{
+
+  	struct mxc400x_i2c_data *obj = i2c_get_clientdata(client);
+	u8 databuf[10];    
+	int res = 0;
+
+	memset(databuf, 0, sizeof(u8)*10);    
+	databuf[0] = MXC400X_REG_TEST2;    
+	databuf[1] = data;
+
+	res = i2c_master_send(client, databuf, 0x2);
+
+	if(res <= 0)
+	{
+		return MXC400X_ERR_I2C;
+	}
+
+    return 0;
+}
+static int MXC400X_Test3(struct i2c_client *client, u8 data)
+{
+
+  	struct mxc400x_i2c_data *obj = i2c_get_clientdata(client);
+	u8 databuf[10];    
+	int res = 0;
+
+	memset(databuf, 0, sizeof(u8)*10);    
+	databuf[0] = MXC400X_REG_TEST3;    
+	databuf[1] = data;
+
+	res = i2c_master_send(client, databuf, 0x2);
+
+	if(res <= 0)
+	{
+		return MXC400X_ERR_I2C;
+	}
+
+    return 0;
+}
+
+static int MXC400X_SetDataResolution(struct mxc400x_i2c_data *obj)
+{
+ 	obj->reso = &mxc400x_data_resolution[2];
+	return MXC400X_SUCCESS;
+}
+static int MXC400X_ReadData(struct i2c_client *client, s16 data[MXC400X_AXES_NUM])
+{
+	struct mxc400x_i2c_data *priv = i2c_get_clientdata(client);        
+	u8 addr = MXC400X_REG_X;
+	u8 buf[MXC400X_DATA_LEN] = {0};
+	int err = 0;
+
+	if(NULL == client)
+	{
+        GSE_LOG("client is null\n");
+		err = -EINVAL;
+	}
+#if defined(SW_SIMULATION)
+	else if(err = cust_hwmsen_read_block(client, addr, buf, MXC400X_DATA_LEN))
+#else
+	else if((err = mxc400x_i2c_read_block(client, addr, buf, MXC400X_DATA_LEN))!=0)
+#endif
+	{
+		GSE_ERR("error: %d\n", err);
+	}
+	else
+	{
+		data[MXC400X_AXIS_X] = (s16)(buf[0] << 8 | buf[1]) >> 4;
+		data[MXC400X_AXIS_Y] = (s16)(buf[2] << 8 | buf[3]) >> 4;
+		data[MXC400X_AXIS_Z] = (s16)(buf[4] << 8 | buf[5]) >> 4;
+		GSE_LOG("reg data x = %d %d y = %d %d z = %d %d\n", buf[0], buf[1], buf[2], buf[3], buf[4], buf[5]);
+
+#if 0
+		if(data[MXC400X_AXIS_X]&0x80)
+		{
+			 data[MXC400X_AXIS_X] = ~data[MXC400X_AXIS_X];
+			 data[MXC400X_AXIS_X] &= 0xff;
+			 data[MXC400X_AXIS_X]+=1;
+			 data[MXC400X_AXIS_X] = -data[MXC400X_AXIS_X];
+		}
+		if(data[MXC400X_AXIS_Y]&0x80)
+		{
+			 data[MXC400X_AXIS_Y] = ~data[MXC400X_AXIS_Y];
+			 data[MXC400X_AXIS_Y] &= 0xff;
+			 data[MXC400X_AXIS_Y]+=1;
+			 data[MXC400X_AXIS_Y] = -data[MXC400X_AXIS_Y];
+		}
+
+        if(data[MXC400X_AXIS_Z]&0x80)
+		{
+				data[MXC400X_AXIS_Z] = ~data[MXC400X_AXIS_Z];
+				data[MXC400X_AXIS_Z] &= 0xff;
+				data[MXC400X_AXIS_Z]+=1;
+				data[MXC400X_AXIS_Z] = -data[MXC400X_AXIS_Z];
+		}
+		if(atomic_read(&priv->trace) & ADX_TRC_RAWDATA)
+		{
+		 	GSE_LOG("[%08X %08X] => [%5d %5d]\n", data[MXC400X_AXIS_X], data[MXC400X_AXIS_Y],
+									data[MXC400X_AXIS_X], data[MXC400X_AXIS_Y]);
+		}
+
+#endif 
+#ifdef CONFIG_MXC400X_LOWPASS
+		if(atomic_read(&priv->filter))
+		{
+			if(atomic_read(&priv->fir_en) && !atomic_read(&priv->suspend))
+			{
+				int idx, firlen = atomic_read(&priv->firlen);   
+				if(priv->fir.num < firlen)
+				{                
+					priv->fir.raw[priv->fir.num][MXC400X_AXIS_X] = data[MXC400X_AXIS_X];
+					priv->fir.raw[priv->fir.num][MXC400X_AXIS_Y] = data[MXC400X_AXIS_Y];
+					priv->fir.raw[priv->fir.num][MXC400X_AXIS_Z] = data[MXC400X_AXIS_Z];
+					priv->fir.sum[MXC400X_AXIS_X] += data[MXC400X_AXIS_X];
+					priv->fir.sum[MXC400X_AXIS_Y] += data[MXC400X_AXIS_Y];
+					priv->fir.sum[MXC400X_AXIS_Z] += data[MXC400X_AXIS_Z];
+					if(atomic_read(&priv->trace) & ADX_TRC_FILTER)
+					{
+						GSE_LOG("add [%2d] [%5d %5d %5d] => [%5d %5d %5d]\n", priv->fir.num,
+							priv->fir.raw[priv->fir.num][MXC400X_AXIS_X], priv->fir.raw[priv->fir.num][MXC400X_AXIS_Y], priv->fir.raw[priv->fir.num][MXC400X_AXIS_Z],
+							priv->fir.sum[MXC400X_AXIS_X], priv->fir.sum[MXC400X_AXIS_Y], priv->fir.sum[MXC400X_AXIS_Z]);
+					}
+					priv->fir.num++;
+					priv->fir.idx++;
+				}
+				else
+				{
+					idx = priv->fir.idx % firlen;
+					priv->fir.sum[MXC400X_AXIS_X] -= priv->fir.raw[idx][MXC400X_AXIS_X];
+					priv->fir.sum[MXC400X_AXIS_Y] -= priv->fir.raw[idx][MXC400X_AXIS_Y];
+					priv->fir.sum[MXC400X_AXIS_Z] -= priv->fir.raw[idx][MXC400X_AXIS_Z];
+					priv->fir.raw[idx][MXC400X_AXIS_X] = data[MXC400X_AXIS_X];
+					priv->fir.raw[idx][MXC400X_AXIS_Y] = data[MXC400X_AXIS_Y];
+					priv->fir.raw[idx][MXC400X_AXIS_Z] = data[MXC400X_AXIS_Z];
+					priv->fir.sum[MXC400X_AXIS_X] += data[MXC400X_AXIS_X];
+					priv->fir.sum[MXC400X_AXIS_Y] += data[MXC400X_AXIS_Y];
+					priv->fir.sum[MXC400X_AXIS_Z] += data[MXC400X_AXIS_Z];
+					priv->fir.idx++;
+					data[MXC400X_AXIS_X] = priv->fir.sum[MXC400X_AXIS_X]/firlen;
+					data[MXC400X_AXIS_Y] = priv->fir.sum[MXC400X_AXIS_Y]/firlen;
+					data[MXC400X_AXIS_Z] = priv->fir.sum[MXC400X_AXIS_Z]/firlen;
+					if(atomic_read(&priv->trace) & ADX_TRC_FILTER)
+					{
+						GSE_LOG("add [%2d] [%5d %5d %5d] => [%5d %5d %5d] : [%5d %5d %5d]\n", idx,
+						priv->fir.raw[idx][MXC400X_AXIS_X], priv->fir.raw[idx][MXC400X_AXIS_Y], priv->fir.raw[idx][MXC400X_AXIS_Z],
+						priv->fir.sum[MXC400X_AXIS_X], priv->fir.sum[MXC400X_AXIS_Y], priv->fir.sum[MXC400X_AXIS_Z],
+						data[MXC400X_AXIS_X], data[MXC400X_AXIS_Y], data[MXC400X_AXIS_Z]);
+					}
+				}
+			}
+		}	
+#endif 
+	}
+	return err;
+}
+
+
+static int MXC400X_ReadOffset(struct i2c_client *client, s8 ofs[MXC400X_AXES_NUM])
+{    
+	int err;
+	err = 0;
+#ifdef SW_CALIBRATION
+	ofs[0]=ofs[1]=ofs[2]=0x0;
+#endif
+	return err;    
+}
+
+static int MXC400X_ResetCalibration(struct i2c_client *client)
+{
+	struct mxc400x_i2c_data *obj = i2c_get_clientdata(client);
+	int err;
+	err = 0;
+
+	memset(obj->cali_sw, 0x00, sizeof(obj->cali_sw));
+	memset(obj->offset, 0x00, sizeof(obj->offset));
+	return err;    
+}
+
+static int MXC400X_ReadCalibration(struct i2c_client *client, int dat[MXC400X_AXES_NUM])
+{
+    struct mxc400x_i2c_data *obj = i2c_get_clientdata(client);
+    int  err = 0;
+    int mul;
+
+	#ifdef SW_CALIBRATION
+		mul = 0;//only SW Calibration, disable HW Calibration
+	#else
+	    if ((err = MXC400X_ReadOffset(client, obj->offset))) {
+        GSE_ERR("read offset fail, %d\n", err);
+        return err;
+    	}    
+    	mul = obj->reso->sensitivity/mxc400x_offset_resolution.sensitivity;
+	#endif
+
+    dat[obj->cvt.map[MXC400X_AXIS_X]] = obj->cvt.sign[MXC400X_AXIS_X]*(obj->offset[MXC400X_AXIS_X]*mul + obj->cali_sw[MXC400X_AXIS_X]);
+    dat[obj->cvt.map[MXC400X_AXIS_Y]] = obj->cvt.sign[MXC400X_AXIS_Y]*(obj->offset[MXC400X_AXIS_Y]*mul + obj->cali_sw[MXC400X_AXIS_Y]);
+    dat[obj->cvt.map[MXC400X_AXIS_Z]] = obj->cvt.sign[MXC400X_AXIS_Z]*(obj->offset[MXC400X_AXIS_Z]*mul + obj->cali_sw[MXC400X_AXIS_Z]);
+                                       
+    return err;
+}
+static int MXC400X_ReadCalibrationEx(struct i2c_client *client, int act[MXC400X_AXES_NUM], int raw[MXC400X_AXES_NUM])
+{  
+	/*raw: the raw calibration data; act: the actual calibration data*/
+	struct mxc400x_i2c_data *obj = i2c_get_clientdata(client);
+	int err;
+	int mul;
+	err = 0;
+ 
+
+	#ifdef SW_CALIBRATION
+		mul = 0;//only SW Calibration, disable HW Calibration
+	#else
+		if((err = MXC400X_ReadOffset(client, obj->offset)))
+		{
+			GSE_ERR("read offset fail, %d\n", err);
+			return err;
+		}   
+		mul = obj->reso->sensitivity/mxc400x_offset_resolution.sensitivity;
+	#endif
+	
+	raw[MXC400X_AXIS_X] = obj->offset[MXC400X_AXIS_X]*mul + obj->cali_sw[MXC400X_AXIS_X];
+	raw[MXC400X_AXIS_Y] = obj->offset[MXC400X_AXIS_Y]*mul + obj->cali_sw[MXC400X_AXIS_Y];
+	raw[MXC400X_AXIS_Z] = obj->offset[MXC400X_AXIS_Z]*mul + obj->cali_sw[MXC400X_AXIS_Z];
+
+	act[obj->cvt.map[MXC400X_AXIS_X]] = obj->cvt.sign[MXC400X_AXIS_X]*raw[MXC400X_AXIS_X];
+	act[obj->cvt.map[MXC400X_AXIS_Y]] = obj->cvt.sign[MXC400X_AXIS_Y]*raw[MXC400X_AXIS_Y];
+	act[obj->cvt.map[MXC400X_AXIS_Z]] = obj->cvt.sign[MXC400X_AXIS_Z]*raw[MXC400X_AXIS_Z];                        
+	                       
+	return 0;
+}
+
+static int MXC400X_WriteCalibration(struct i2c_client *client, int dat[MXC400X_AXES_NUM])
+{
+	struct mxc400x_i2c_data *obj = i2c_get_clientdata(client);
+	int err = 0;
+	int cali[MXC400X_AXES_NUM], raw[MXC400X_AXES_NUM];
+	int lsb; 
+	lsb = mxc400x_offset_resolution.sensitivity;
+	int divisor = obj->reso->sensitivity/lsb;
+
+	if((err = MXC400X_ReadCalibrationEx(client, cali, raw)))	/*offset will be updated in obj->offset*/
+	{ 
+		GSE_ERR("read offset fail, %d\n", err);
+		return err;
+	}
+
+	GSE_LOG("OLDOFF: (%+3d %+3d %+3d): (%+3d %+3d %+3d) / (%+3d %+3d %+3d)\n", 
+		raw[MXC400X_AXIS_X], raw[MXC400X_AXIS_Y], raw[MXC400X_AXIS_Z],
+		obj->offset[MXC400X_AXIS_X], obj->offset[MXC400X_AXIS_Y], obj->offset[MXC400X_AXIS_Z],
+		obj->cali_sw[MXC400X_AXIS_X], obj->cali_sw[MXC400X_AXIS_Y], obj->cali_sw[MXC400X_AXIS_Z]);
+
+	/*calculate the real offset expected by caller*/
+	cali[MXC400X_AXIS_X] += dat[MXC400X_AXIS_X];
+	cali[MXC400X_AXIS_Y] += dat[MXC400X_AXIS_Y];
+	cali[MXC400X_AXIS_Z] += dat[MXC400X_AXIS_Z];
+
+	GSE_LOG("UPDATE: (%+3d %+3d %+3d)\n", 
+		dat[MXC400X_AXIS_X], dat[MXC400X_AXIS_Y], dat[MXC400X_AXIS_Z]);
+
+#ifdef SW_CALIBRATION
+	obj->cali_sw[MXC400X_AXIS_X] = obj->cvt.sign[MXC400X_AXIS_X]*(cali[obj->cvt.map[MXC400X_AXIS_X]]);
+	obj->cali_sw[MXC400X_AXIS_Y] = obj->cvt.sign[MXC400X_AXIS_Y]*(cali[obj->cvt.map[MXC400X_AXIS_Y]]);
+	obj->cali_sw[MXC400X_AXIS_Z] = obj->cvt.sign[MXC400X_AXIS_Z]*(cali[obj->cvt.map[MXC400X_AXIS_Z]]);	
+#else
+	int divisor = obj->reso->sensitivity/lsb;//modified
+	obj->offset[MXC400X_AXIS_X] = (s8)(obj->cvt.sign[MXC400X_AXIS_X]*(cali[obj->cvt.map[MXC400X_AXIS_X]])/(divisor));
+	obj->offset[MXC400X_AXIS_Y] = (s8)(obj->cvt.sign[MXC400X_AXIS_Y]*(cali[obj->cvt.map[MXC400X_AXIS_Y]])/(divisor));
+	obj->offset[MXC400X_AXIS_Z] = (s8)(obj->cvt.sign[MXC400X_AXIS_Z]*(cali[obj->cvt.map[MXC400X_AXIS_Z]])/(divisor));
+
+	/*convert software calibration using standard calibration*/
+	obj->cali_sw[MXC400X_AXIS_X] = obj->cvt.sign[MXC400X_AXIS_X]*(cali[obj->cvt.map[MXC400X_AXIS_X]])%(divisor);
+	obj->cali_sw[MXC400X_AXIS_Y] = obj->cvt.sign[MXC400X_AXIS_Y]*(cali[obj->cvt.map[MXC400X_AXIS_Y]])%(divisor);
+	obj->cali_sw[MXC400X_AXIS_Z] = obj->cvt.sign[MXC400X_AXIS_Z]*(cali[obj->cvt.map[MXC400X_AXIS_Z]])%(divisor);
+
+	GSE_LOG("NEWOFF: (%+3d %+3d %+3d): (%+3d %+3d %+3d) / (%+3d %+3d %+3d)\n", 
+		obj->offset[MXC400X_AXIS_X]*divisor + obj->cali_sw[MXC400X_AXIS_X], 
+		obj->offset[MXC400X_AXIS_Y]*divisor + obj->cali_sw[MXC400X_AXIS_Y], 
+		obj->offset[MXC400X_AXIS_Z]*divisor + obj->cali_sw[MXC400X_AXIS_Z], 
+		obj->offset[MXC400X_AXIS_X], obj->offset[MXC400X_AXIS_Y], obj->offset[MXC400X_AXIS_Z],
+		obj->cali_sw[MXC400X_AXIS_X], obj->cali_sw[MXC400X_AXIS_Y], obj->cali_sw[MXC400X_AXIS_Z]);
+
+#endif
+	mdelay(1);
+	return err;
+}
+
+static int MXC400X_CheckDeviceID(struct i2c_client *client)
+{
+	u8 databuf[10];    
+	int res = 0;
+
+	memset(databuf, 0, sizeof(u8)*10);    
+	databuf[0] = MXC400X_REG_ID;   
+#if defined(SW_SIMULATION)
+	res = cust_i2c_master_read(client, databuf, 0x1);
+#else
+	res = i2c_master_send(client, databuf, 0x01);
+	if(res <= 0)
+	{
+		goto exit_MXC400X_CheckDeviceID;
+	}
+	udelay(500);
+
+	databuf[0] = 0x0;        
+	res = i2c_master_recv(client, databuf, 0x01);
+#endif
+	if(res <= 0)
+	{
+		goto exit_MXC400X_CheckDeviceID;
+	}
+
+	
+	databuf[0]= (databuf[0]&0x3f);
+	
+	if(databuf[0]!= MXC400X_ID)
+	{
+		return MXC400X_ERR_IDENTIFICATION;
+	}
+	
+	GSE_LOG("MXC400X_CheckDeviceID %d done!\n ", databuf[0]);
+
+	return MXC400X_SUCCESS;
+
+	exit_MXC400X_CheckDeviceID:
+	if (res <= 0)
+	{
+		GSE_ERR("MXC400X_CheckDeviceID %d failt!\n ", MXC400X_ERR_I2C);
+		return MXC400X_ERR_I2C;
+	}
+
+}
+
+static int MXC400X_SetPowerMode(struct i2c_client *client, bool enable)
+{
+	u8 databuf[2] = {0};    
+	int res = 0;
+	struct mxc400x_i2c_data *obj = i2c_get_clientdata(client);
+	
+	if(enable == sensor_power)
+	{
+		GSE_LOG("Sensor power status is newest!\n");
+		return MXC400X_SUCCESS;
+	}
+	if(enable == 1)
+	{
+		databuf[1] = MXC400X_AWAKE;
+	}
+	else
+	{
+		databuf[1] = MXC400X_SLEEP;
+	}
+	databuf[0] = MXC400X_REG_CTRL;
+#if defined(SW_SIMULATION)
+	res = cust_i2c_master_send(client, databuf, 0x2);
+#else	
+	res = i2c_master_send(client, databuf, 0x2);
+#endif
+	if(res <= 0)
+	{
+		GSE_LOG("set power mode failed!\n");
+		return MXC400X_ERR_I2C;
+	}
+	sensor_power = enable;
+	mdelay(300);//lyon
+	
+	return MXC400X_SUCCESS;    
+}
+static int MXC400X_SetDataFormat(struct i2c_client *client, u8 dataformat)
+{
+	struct mxc400x_i2c_data *obj = i2c_get_clientdata(client);
+	u8 databuf[10];    
+	int res = 0;
+
+	memset(databuf, 0, sizeof(u8)*10);  
+	databuf[0] = MXC400X_REG_CTRL;
+	databuf[1] = MXC400X_RANGE_8G;
+#if defined(SW_SIMULATION)
+	res = cust_i2c_master_send(client, databuf, 0x2);
+#else
+	res = i2c_master_send(client, databuf, 0x2);
+#endif
+	if(res <= 0)
+	{
+		GSE_LOG("set power mode failed!\n");
+		return MXC400X_ERR_I2C;
+	}  
+
+	return MXC400X_SetDataResolution(obj);    
+}
+static int MXC400X_SetBWRate(struct i2c_client *client, u8 bwrate)
+{
+	u8 databuf[10];    
+	int res = 0;
+
+	memset(databuf, 0, sizeof(u8)*10);    
+	
+	return MXC400X_SUCCESS;    
+}
+
+static int mxc400x_init_client(struct i2c_client *client, int reset_cali)
+{
+	 struct mxc400x_i2c_data *obj = i2c_get_clientdata(client);
+	 int res = 0;
+
+	 res = MXC400X_CheckDeviceID(client); 
+	 if(res != MXC400X_SUCCESS)
+	 {
+	 	 return res;
+	 }	 
+	 
+	res = MXC400X_SetBWRate(client, MXC400X_BW_50HZ);
+	if(res != MXC400X_SUCCESS ) 
+	{
+		return res;
+	}
+
+	res = MXC400X_SetDataFormat(client, MXC400X_RANGE_8G);
+	if(res != MXC400X_SUCCESS) 
+	{
+		return res;
+	}
+	
+	res = MXC400X_SetPowerMode(client, enable_status);
+	if(res != MXC400X_SUCCESS)
+	{
+		return res;
+	}
+	
+	gsensor_gain.x = gsensor_gain.y = gsensor_gain.z = obj->reso->sensitivity;
+	 
+	if(0 != reset_cali)
+	{ 
+	 	/*reset calibration only in power on*/
+		 res = MXC400X_ResetCalibration(client);
+		 if(res != MXC400X_SUCCESS)
+		 {
+			 return res;
+		 }
+	}
+	GSE_LOG("mxc400x_init_client OK!\n");
+#ifdef CONFIG_MXC400X_LOWPASS
+	memset(&obj->fir, 0x00, sizeof(obj->fir));  
+#endif
+	mdelay(20);
+
+	return MXC400X_SUCCESS;
+}
+
+static int MXC400X_ReadChipInfo(struct i2c_client *client, char *buf, int bufsize)
+{
+	u8 databuf[10];    
+
+	memset(databuf, 0, sizeof(u8)*10);
+
+	if((NULL == buf)||(bufsize<=30))
+	{
+		return -1;
+	}
+	
+	if(NULL == client)
+	{
+		*buf = 0;
+		return -2;
+	}
+
+	sprintf(buf, "MXC400X Chip");
+	return 0;
+}
+
+static int MXC400X_ReadSensorData(struct i2c_client *client, int *buf, int bufsize)
+{
+	struct mxc400x_i2c_data *obj = (struct mxc400x_i2c_data*)i2c_get_clientdata(client);
+	u8 databuf[20];
+	int acc[MXC400X_AXES_NUM];
+	int res = 0;
+
+	memset(databuf, 0, sizeof(u8)*10);
+
+	if(NULL == buf)
+	{
+        GSE_ERR("buf is null !!!\n");
+		return -1;
+	}
+	if(NULL == client)
+	{
+		*buf = 0;
+         GSE_ERR("client is null !!!\n");
+		return MXC400X_ERR_STATUS;
+	}
+
+	if (atomic_read(&obj->suspend))
+	{
+		GSE_LOG("sensor in suspend read not data!\n");
+		return MXC400X_ERR_GETGSENSORDATA;
+	}
+     
+	if(res = MXC400X_ReadData(client, obj->data))
+	{        
+		GSE_ERR("I2C error: ret value=%d", res);
+		return MXC400X_ERR_I2C;
+	}
+	else
+	{   
+        
+		//GSE_LOG("raw data x=%d, y=%d, z=%d \n",obj->data[MXC400X_AXIS_X],obj->data[MXC400X_AXIS_Y],obj->data[MXC400X_AXIS_Z]);
+		obj->data[MXC400X_AXIS_X] += obj->cali_sw[MXC400X_AXIS_X];
+		obj->data[MXC400X_AXIS_Y] += obj->cali_sw[MXC400X_AXIS_Y];
+		obj->data[MXC400X_AXIS_Z] += obj->cali_sw[MXC400X_AXIS_Z];
+
+		//printk("cali_sw x=%d, y=%d, z=%d \n",obj->cali_sw[MXC400X_AXIS_X],obj->cali_sw[MXC400X_AXIS_Y],obj->cali_sw[MXC400X_AXIS_Z]);
+		
+		/*remap coordinate*/
+		acc[obj->cvt.map[MXC400X_AXIS_X]] = obj->cvt.sign[MXC400X_AXIS_X]*obj->data[MXC400X_AXIS_X];
+		acc[obj->cvt.map[MXC400X_AXIS_Y]] = obj->cvt.sign[MXC400X_AXIS_Y]*obj->data[MXC400X_AXIS_Y];
+		acc[obj->cvt.map[MXC400X_AXIS_Z]] = obj->cvt.sign[MXC400X_AXIS_Z]*obj->data[MXC400X_AXIS_Z];
+		//printk("cvt x=%d, y=%d, z=%d \n",obj->cvt.sign[MXC400X_AXIS_X],obj->cvt.sign[MXC400X_AXIS_Y],obj->cvt.sign[MXC400X_AXIS_Z]);
+        
+		//GSE_LOG("Mapped gsensor data: %d, %d, %d!\n", acc[MXC400X_AXIS_X], acc[MXC400X_AXIS_Y], acc[MXC400X_AXIS_Z]);
+  
+		//Out put the mg
+		//printk("mg acc 1 x =%d, GRAVITY=%d, sensityvity=%d \n",acc[MXC400X_AXIS_X],GRAVITY_EARTH_1000,obj->reso->sensitivity);
+		acc[MXC400X_AXIS_X] = acc[MXC400X_AXIS_X] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+		acc[MXC400X_AXIS_Y] = acc[MXC400X_AXIS_Y] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+		acc[MXC400X_AXIS_Z] = acc[MXC400X_AXIS_Z] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;	
+        //printk("mg acc 2 x =%d, y =%d, z =%d \n",acc[MXC400X_AXIS_X],acc[MXC400X_AXIS_Y],acc[MXC400X_AXIS_Z]);
+        buf[0] = acc[MXC400X_AXIS_X];
+        buf[1] = acc[MXC400X_AXIS_Y];
+        buf[2] = acc[MXC400X_AXIS_Z];
+		//sprintf(buf, "%04x %04x %04x", acc[MXC400X_AXIS_X], acc[MXC400X_AXIS_Y], acc[MXC400X_AXIS_Z]);
+		//printk("mg acc 3 x =%d, y =%d, z =%d \n",buf[MXC400X_AXIS_X],buf[MXC400X_AXIS_Y],buf[MXC400X_AXIS_Z]);
+	//	if(atomic_read(&obj->trace) & ADX_TRC_IOCTL)
+	//	{
+	//		GSE_LOG("gsensor data: %s!\n", buf);
+	//	}
+
+	}
+	
+	return res;
+}
+
+static int MXC400X_ReadRawData(struct i2c_client *client, int *buf)
+{
+	struct mxc400x_i2c_data *obj = (struct mxc400x_i2c_data*)i2c_get_clientdata(client);
+	int res = 0;
+
+	if (!buf || !client)
+	{
+        GSE_ERR(" buf or client is null !!\n");
+		return EINVAL;
+	}
+	
+	if(res = MXC400X_ReadData(client, obj->data))
+	{        
+		GSE_ERR("I2C error: ret value=%d\n", res);
+		return EIO;
+	}
+	else
+	{
+		sprintf(buf, "MXC400X_ReadRawData %04x %04x %04x", obj->data[MXC400X_AXIS_X], 
+			obj->data[MXC400X_AXIS_Y], obj->data[MXC400X_AXIS_Z]);
+	
+	}
+	
+	return 0;
+}
+
+
+static ssize_t show_chipinfo_value(struct device_driver *ddri, char *buf)
+{
+	struct i2c_client *client = mxc400x_i2c_client;
+	char strbuf[MXC400X_BUFSIZE];
+	if(NULL == client)
+	{
+		GSE_ERR("i2c client is null!!\n");
+		return 0;
+	}
+	
+	MXC400X_ReadChipInfo(client, strbuf, MXC400X_BUFSIZE);
+	return snprintf(buf, PAGE_SIZE, "%s\n", (char*)strbuf);        
+}
+
+static ssize_t show_sensordata_value(struct device_driver *ddri, char *buf)
+{
+	struct i2c_client *client = mxc400x_i2c_client;
+	int strbuf[MXC400X_BUFSIZE];
+	
+	if(NULL == client)
+	{
+		GSE_ERR("i2c client is null!!\n");
+		return 0;
+	}
+	MXC400X_ReadSensorData(client, strbuf, MXC400X_BUFSIZE);
+	return snprintf(buf, PAGE_SIZE, "%d %d %d \n", strbuf[0],strbuf[1],strbuf[2]);
+	//return snprintf(buf, PAGE_SIZE, "%s\n", (char*)strbuf);            
+}
+
+static ssize_t show_cali_value(struct device_driver *ddri, char *buf)
+{
+	struct i2c_client *client = mxc400x_i2c_client;
+	struct mxc400x_i2c_data *obj;
+	int err, len = 0, mul;
+	int tmp[MXC400X_AXES_NUM];
+
+	if(NULL == client)
+	{
+		GSE_ERR("i2c client is null!!\n");
+		return MXC400X_ERR_STATUS;
+	}
+
+	obj = i2c_get_clientdata(client);
+
+
+
+	if((err = MXC400X_ReadOffset(client, obj->offset)))
+	{
+		return -EINVAL;
+	}
+	else if((err = MXC400X_ReadCalibration(client, tmp)))
+	{
+		return -EINVAL;
+	}
+	else
+	{    
+		mul = obj->reso->sensitivity/mxc400x_offset_resolution.sensitivity;
+		len += snprintf(buf+len, PAGE_SIZE-len, "[HW ][%d] (%+3d, %+3d, %+3d) : (0x%02X, 0x%02X, 0x%02X)\n", mul,                        
+			obj->offset[MXC400X_AXIS_X], obj->offset[MXC400X_AXIS_Y], obj->offset[MXC400X_AXIS_Z],
+			obj->offset[MXC400X_AXIS_X], obj->offset[MXC400X_AXIS_Y], obj->offset[MXC400X_AXIS_Z]);
+		len += snprintf(buf+len, PAGE_SIZE-len, "[SW ][%d] (%+3d, %+3d, %+3d)\n", 1, 
+			obj->cali_sw[MXC400X_AXIS_X], obj->cali_sw[MXC400X_AXIS_Y], obj->cali_sw[MXC400X_AXIS_Z]);
+
+		len += snprintf(buf+len, PAGE_SIZE-len, "[ALL]    (%+3d, %+3d, %+3d) : (%+3d, %+3d, %+3d)\n", 
+			obj->offset[MXC400X_AXIS_X]*mul + obj->cali_sw[MXC400X_AXIS_X],
+			obj->offset[MXC400X_AXIS_Y]*mul + obj->cali_sw[MXC400X_AXIS_Y],
+			obj->offset[MXC400X_AXIS_Z]*mul + obj->cali_sw[MXC400X_AXIS_Z],
+			tmp[MXC400X_AXIS_X], tmp[MXC400X_AXIS_Y], tmp[MXC400X_AXIS_Z]);
+		
+		return len;
+    }
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t store_cali_value(struct device_driver *ddri, const char *buf, size_t count)
+{
+	struct i2c_client *client = mxc400x_i2c_client;  
+	int err, x, y, z;
+	int dat[MXC400X_AXES_NUM];
+
+	if(!strncmp(buf, "rst", 3))
+	{
+		if(err = MXC400X_ResetCalibration(client))
+		{
+			GSE_ERR("reset offset err = %d\n", err);
+		}	
+	}
+	else if(3 == sscanf(buf, "0x%02X 0x%02X 0x%02X", &x, &y, &z))
+	{
+		dat[MXC400X_AXIS_X] = x;
+		dat[MXC400X_AXIS_Y] = y;
+		dat[MXC400X_AXIS_Z] = z;
+		if((err = MXC400X_WriteCalibration(client, dat)))
+		{
+			GSE_ERR("write calibration err = %d\n", err);
+		}		
+	}
+	else
+	{
+		GSE_ERR("invalid format\n");
+	}
+	
+	return 0;
+}
+static ssize_t show_firlen_value(struct device_driver *ddri, char *buf)
+{
+#ifdef CONFIG_MXC400X_LOWPASS
+	struct i2c_client *client = mxc400x_i2c_client;
+	struct mxc400x_i2c_data *obj = i2c_get_clientdata(client);
+	if(atomic_read(&obj->firlen))
+	{
+	 	int idx, len = atomic_read(&obj->firlen);
+	 	GSE_LOG("len = %2d, idx = %2d\n", obj->fir.num, obj->fir.idx);
+
+		for(idx = 0; idx < len; idx++)
+		{
+			GSE_LOG("[%5d %5d %5d]\n", obj->fir.raw[idx][MXC400X_AXIS_X], obj->fir.raw[idx][MXC400X_AXIS_Y], obj->fir.raw[idx][MXC400X_AXIS_Z]);
+		}
+		
+		GSE_LOG("sum = [%5d %5d %5d]\n", obj->fir.sum[MXC400X_AXIS_X], obj->fir.sum[MXC400X_AXIS_Y], obj->fir.sum[MXC400X_AXIS_Z]);
+		GSE_LOG("avg = [%5d %5d %5d]\n", obj->fir.sum[MXC400X_AXIS_X]/len, obj->fir.sum[MXC400X_AXIS_Y]/len, obj->fir.sum[MXC400X_AXIS_Z]/len);
+	}
+	return snprintf(buf, PAGE_SIZE, "%d\n", atomic_read(&obj->firlen));
+#else
+	return snprintf(buf, PAGE_SIZE, "not support\n");
+#endif
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t store_firlen_value(struct device_driver *ddri, const char *buf, size_t count)
+{
+#ifdef CONFIG_MXC400X_LOWPASS
+	struct i2c_client *client = mxc400x_i2c_client;  
+	struct mxc400x_i2c_data *obj = i2c_get_clientdata(client);
+	int firlen;
+
+	if(1 != sscanf(buf, "%d", &firlen))
+	{
+		GSE_ERR("invallid format\n");
+	}
+	else if(firlen > C_MAX_FIR_LENGTH)
+	{
+		GSE_ERR("exceeds maximum filter length\n");
+	}
+	else
+	{ 
+		atomic_set(&obj->firlen, firlen);
+		if(NULL == firlen)
+		{
+			atomic_set(&obj->fir_en, 0);
+		}
+		else
+		{
+			memset(&obj->fir, 0x00, sizeof(obj->fir));
+			atomic_set(&obj->fir_en, 1);
+		}
+	}
+#endif    
+	return 0;
+}
+static ssize_t show_trace_value(struct device_driver *ddri, char *buf)
+{
+	ssize_t res;
+	struct mxc400x_i2c_data *obj = obj_i2c_data;
+	if (obj == NULL)
+	{
+		GSE_ERR("i2c_data obj is null!!\n");
+		return 0;
+	}
+	
+	res = snprintf(buf, PAGE_SIZE, "0x%04X\n", atomic_read(&obj->trace));     
+	return res;    
+}
+static ssize_t store_trace_value(struct device_driver *ddri, const char *buf, size_t count)
+{
+	struct mxc400x_i2c_data *obj = obj_i2c_data;
+	int trace;
+	if (obj == NULL)
+	{
+		GSE_ERR("i2c_data obj is null!!\n");
+		return 0;
+	}
+	
+	if(1 == sscanf(buf, "0x%x", &trace))
+	{
+		atomic_set(&obj->trace, trace);
+	}	
+	else
+	{
+		GSE_ERR("invalid content: '%s', length = %d\n", buf, (int)count);
+	}
+	return count;    
+}
+static ssize_t show_status_value(struct device_driver *ddri, char *buf)
+{
+	ssize_t len = 0;    
+	struct mxc400x_i2c_data *obj = obj_i2c_data;
+	if (obj == NULL)
+	{
+		GSE_ERR("i2c_data obj is null!!\n");
+		return 0;
+	}	
+	
+	if(obj->hw)
+	{
+		len += snprintf(buf+len, PAGE_SIZE-len, "CUST: %d %d (%d %d)\n", 
+	            obj->hw->i2c_num, obj->hw->direction, obj->hw->power_id, obj->hw->power_vol);   
+	}
+	else
+	{
+		len += snprintf(buf+len, PAGE_SIZE-len, "CUST: NULL\n");
+	}
+	return len;    
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t show_power_status_value(struct device_driver *ddri, char *buf)
+{
+	
+	if(sensor_power)
+		GSE_LOG("G sensor is in work mode, sensor_power = %d\n", sensor_power);
+	else
+		GSE_LOG("G sensor is in standby mode, sensor_power = %d\n", sensor_power);
+
+	return snprintf(buf, PAGE_SIZE, "%x\n", sensor_power);
+}
+static ssize_t show_layout_value(struct device_driver *ddri, char *buf)
+{
+	struct i2c_client *client = mxc400x_i2c_client;  
+	struct mxc400x_i2c_data *data = i2c_get_clientdata(client);
+
+	return sprintf(buf, "(%d, %d)\n[%+2d %+2d %+2d]\n[%+2d %+2d %+2d]\n",
+		data->hw->direction,atomic_read(&data->layout),	data->cvt.sign[0], data->cvt.sign[1],
+		data->cvt.sign[2],data->cvt.map[0], data->cvt.map[1], data->cvt.map[2]);            
+}
+/*----------------------------------------------------------------------------*/
+static ssize_t store_layout_value(struct device_driver *ddri, char *buf, size_t count)
+{
+	struct i2c_client *client = mxc400x_i2c_client;  
+	struct mxc400x_i2c_data *data = i2c_get_clientdata(client);
+	int layout = 0;
+
+	if(1 == sscanf(buf, "%d", &layout))
+	{
+		atomic_set(&data->layout, layout);
+		if(!hwmsen_get_convert(layout, &data->cvt))
+		{
+			printk(KERN_ERR "HWMSEN_GET_CONVERT function error!\r\n");
+		}
+		else if(!hwmsen_get_convert(data->hw->direction, &data->cvt))
+		{
+			printk(KERN_ERR "invalid layout: %d, restore to %d\n", layout, data->hw->direction);
+		}
+		else
+		{
+			printk(KERN_ERR "invalid layout: (%d, %d)\n", layout, data->hw->direction);
+			hwmsen_get_convert(0, &data->cvt);
+		}
+	}
+	else
+	{
+		printk(KERN_ERR "invalid format = '%s'\n", buf);
+	}
+	
+	return count;            
+}
+static DRIVER_ATTR(chipinfo,   S_IWUSR | S_IRUGO, show_chipinfo_value,      NULL);
+static DRIVER_ATTR(sensordata, S_IWUSR | S_IRUGO, show_sensordata_value,    NULL);
+static DRIVER_ATTR(cali,       S_IWUSR | S_IRUGO, show_cali_value,          store_cali_value);
+static DRIVER_ATTR(firlen, 	S_IWUSR | S_IRUGO, show_firlen_value,		 store_firlen_value);
+static DRIVER_ATTR(trace,		S_IWUSR | S_IRUGO, show_trace_value,		 store_trace_value);
+static DRIVER_ATTR(layout,      S_IRUGO | S_IWUSR, show_layout_value, store_layout_value);
+static DRIVER_ATTR(status, 			  S_IRUGO, show_status_value,		 NULL);
+static DRIVER_ATTR(powerstatus,			   S_IRUGO, show_power_status_value,		NULL);
+
+static struct driver_attribute *mxc400x_attr_list[] = {
+	&driver_attr_chipinfo,     /*chip information*/
+	&driver_attr_sensordata,   /*dump sensor data*/
+	&driver_attr_cali,         /*show calibration data*/
+	&driver_attr_firlen,	   /*filter length: 0: disable, others: enable*/
+	&driver_attr_trace,		   /*trace log*/
+	&driver_attr_layout,
+	&driver_attr_status,
+	&driver_attr_powerstatus,
+};
+static int mxc400x_create_attr(struct device_driver *driver) 
+{
+	int idx, err = 0;
+	int num = (int)(sizeof(mxc400x_attr_list)/sizeof(mxc400x_attr_list[0]));
+	if (driver == NULL)
+	{
+		return -EINVAL;
+	}
+
+	for(idx = 0; idx < num; idx++)
+	{
+		if((err = driver_create_file(driver, mxc400x_attr_list[idx])))
+		{            
+			GSE_ERR("driver_create_file (%s) = %d\n", mxc400x_attr_list[idx]->attr.name, err);
+			break;
+		}
+	}    
+	return err;
+}
+static int mxc400x_delete_attr(struct device_driver *driver)
+{
+	int idx ,err = 0;
+	int num = (int)(sizeof(mxc400x_attr_list)/sizeof(mxc400x_attr_list[0]));
+
+	if(driver == NULL)
+	{
+		return -EINVAL;
+	}
+	
+
+	for(idx = 0; idx < num; idx++)
+	{
+		driver_remove_file(driver, mxc400x_attr_list[idx]);
+	}
+	
+	return err;
+}
+int gsensor_operate(void* self, uint32_t command, void* buff_in, int size_in,
+			 void* buff_out, int size_out, int* actualout)
+{
+	 int err = 0;
+	 int value, sample_delay;	 
+	 struct mxc400x_i2c_data *priv = (struct mxc400x_i2c_data*)self;
+	 hwm_sensor_data* gsensor_data;
+	 int buff[MXC400X_BUFSIZE] = {0};
+ 
+	 //GSE_FUN(f);
+	 switch (command)
+	 {
+		 case SENSOR_DELAY:
+			 GSE_LOG("mxc400x set delay\n");
+			 break;
+
+		 case SENSOR_ENABLE:
+			 if((buff_in == NULL) || (size_in < sizeof(int)))
+			 {
+				 GSE_ERR("Enable sensor parameter error!\n");
+				 err = -EINVAL;
+			 }
+			 else
+			 {
+				value = *(int *)buff_in;
+				mutex_lock(&mxc400x_mutex);
+                GSE_LOG("Gsensor device enable function enable = %d, sensor_power = %d!\n",value,sensor_power);
+				if(((value == 0) && (sensor_power == false)) ||((value == 1) && (sensor_power == true)))
+				{
+					enable_status = sensor_power;
+					GSE_LOG("Gsensor device have updated !\n");
+				}
+				else
+				{
+					enable_status = !sensor_power;
+					if (atomic_read(&priv->suspend) == 0)
+					{
+						
+						err = MXC400X_SetPowerMode(priv->client, enable_status);
+						GSE_LOG("Gsensor not in suspend MXC400X_SetPowerMode!, enable_status = %d\n",enable_status);
+					}
+					else
+					{
+					
+						GSE_LOG("Gsensor in suspend and can not enable or disable!enable_status = %d\n",enable_status);
+					}
+					
+				}
+				
+				mutex_unlock(&mxc400x_mutex);
+			 }
+			 break;
+
+		 case SENSOR_GET_DATA:
+			 if((buff_out == NULL) || (size_out< sizeof(hwm_sensor_data)))
+			 {
+				 GSE_ERR("get sensor data parameter error!\n");
+				 err = -EINVAL;
+			 }
+			 else
+			 {
+				 gsensor_data = (hwm_sensor_data *)buff_out;
+				 mutex_lock(&mxc400x_mutex);
+				 MXC400X_ReadSensorData(priv->client, buff, MXC400X_DATA_LEN);
+				 gsensor_data->values[0] = buff[0];
+				 gsensor_data->values[1] = buff[1];
+				 gsensor_data->values[2] = buff[2];
+				 
+                 //gsensor_data->values[0] = cali_sensor_data[0];
+				 //gsensor_data->values[1] = cali_sensor_data[1];
+				 //gsensor_data->values[2] = cali_sensor_data[2];
+				 mutex_unlock(&mxc400x_mutex);
+                 //sscanf(buff, "%x %x %x", &gsensor_data->values[0], &gsensor_data->values[1], &gsensor_data->values[2]);
+                 GSE_LOG("Gsensor DATA X = %d, Y = %d, Z = %d!\n",gsensor_data->values[0],gsensor_data->values[1],gsensor_data->values[2]);
+				 gsensor_data->status = SENSOR_STATUS_ACCURACY_MEDIUM;				 
+				 gsensor_data->value_divide = 1000;
+		
+			 }
+			 break;
+		 default:
+			 GSE_ERR("gsensor operate function no this parameter %d!\n", command);
+			 err = -1;
+			 break;
+	 }
+ 
+	return err;
+}
+
+/****************************************************************************** 
+ * Function Configuration
+******************************************************************************/
+static int mxc400x_open(struct inode *inode, struct file *file)
+{
+	 file->private_data = mxc400x_i2c_client;
+
+	 if(file->private_data == NULL)
+	 {
+		 GSE_ERR("null mxc400x!!\n");
+		 return -EINVAL;
+	 }
+	 return nonseekable_open(inode, file);
+}
+/*----------------------------------------------------------------------------*/
+static int mxc400x_release(struct inode *inode, struct file *file)
+{
+	 file->private_data = NULL;
+	 return 0;
+}
+
+#ifdef CONFIG_COMPAT
+static long mxc400x_compat_ioctl(struct file *file, unsigned int cmd,
+       unsigned long arg)
+{
+    long err = 0;
+
+	void __user *arg64 = compat_ptr(arg);
+	
+	if (!file->f_op || !file->f_op->unlocked_ioctl)
+		return -ENOTTY;
+	
+	switch (cmd)
+	{
+		case COMPAT_GSENSOR_IOCTL_INIT:
+			err = file->f_op->unlocked_ioctl(file, GSENSOR_IOCTL_INIT, (unsigned long)arg64);
+			if(err < 0)
+				{
+					printk(KERN_ERR "COMPAT_MMC31XX_IOC_TM is failed!\n");
+				}
+				break;
+		case COMPAT_GSENSOR_IOCTL_READ_CHIPINFO:
+			err = file->f_op->unlocked_ioctl(file, GSENSOR_IOCTL_READ_CHIPINFO, (unsigned long)arg64);
+			if(err < 0)
+				{
+					printk(KERN_ERR "COMPAT_GSENSOR_IOCTL_READ_CHIPINFO is failed!\n");
+				}
+				break;
+		case COMPAT_GSENSOR_IOCTL_READ_SENSORDATA:
+			err = file->f_op->unlocked_ioctl(file, GSENSOR_IOCTL_READ_SENSORDATA, (unsigned long)arg64);
+			if(err < 0)
+				{
+					printk(KERN_ERR "COMPAT_GSENSOR_IOCTL_READ_SENSORDATA is failed!\n");
+				}
+				break;
+		case COMPAT_GSENSOR_IOCTL_READ_GAIN:
+			err = file->f_op->unlocked_ioctl(file, GSENSOR_IOCTL_READ_GAIN, (unsigned long)arg64);
+			if(err < 0)
+				{
+					printk(KERN_ERR "COMPAT_GSENSOR_IOCTL_READ_GAIN is failed!\n");
+				}
+				break;
+		case COMPAT_GSENSOR_IOCTL_READ_RAW_DATA:
+			err = file->f_op->unlocked_ioctl(file, GSENSOR_IOCTL_READ_RAW_DATA, (unsigned long)arg64);
+			if(err < 0)
+				{
+					printk(KERN_ERR "COMPAT_GSENSOR_IOCTL_READ_RAW_DATA is failed!\n");
+				}
+				break;
+
+		case COMPAT_GSENSOR_IOCTL_SET_CALI:
+			err = file->f_op->unlocked_ioctl(file, GSENSOR_IOCTL_SET_CALI, (unsigned long)arg64);
+			if(err < 0)
+				{
+					printk(KERN_ERR "COMPAT_GSENSOR_IOCTL_SET_CALI is failed!\n");
+				}
+				break;
+		case COMPAT_GSENSOR_IOCTL_CLR_CALI:
+			err = file->f_op->unlocked_ioctl(file, GSENSOR_IOCTL_CLR_CALI, (unsigned long)arg64);
+			if(err < 0)
+				{
+					printk(KERN_ERR "COMPAT_GSENSOR_IOCTL_CLR_CALI is failed!\n");
+				}
+				break;
+
+		case COMPAT_GSENSOR_IOCTL_GET_CALI:
+			err = file->f_op->unlocked_ioctl(file, GSENSOR_IOCTL_GET_CALI, (unsigned long)arg64);
+			if(err < 0)
+				{
+					printk(KERN_ERR "COMPAT_GSENSOR_IOCTL_GET_CALI is failed!\n");
+				}
+				break;
+		case COMPAT_GSENSOR_IOCTL_GET_DELAY:
+			err = file->f_op->unlocked_ioctl(file, GSENSOR_IOCTL_GET_DELAY, (unsigned long)arg64);
+			if(err < 0)
+				{
+					printk(KERN_ERR "COMPAT_GSENSOR_IOCTL_GET_DELAY is failed!\n");
+				}
+				break;
+
+		case COMPAT_GSENSOR_IOCTL_GET_STATUS:
+			err = file->f_op->unlocked_ioctl(file, GSENSOR_IOCTL_GET_STATUS, (unsigned long)arg64);
+			if(err < 0)
+				{
+					printk(KERN_ERR "COMPAT_GSENSOR_IOCTL_GET_STATUS is failed!\n");
+				}
+				break;
+		case COMPAT_GSENSOR_IOCTL_GET_DATA:
+			err = file->f_op->unlocked_ioctl(file, GSENSOR_IOCTL_GET_DATA, (unsigned long)arg64);
+			if(err < 0)
+				{
+					printk(KERN_ERR "COMPAT_GSENSOR_IOCTL_GET_DATA is failed!\n");
+				}
+				break;
+
+		case COMPAT_GSENSOR_IOCTL_SET_DATA:
+			err = file->f_op->unlocked_ioctl(file, GSENSOR_IOCTL_SET_DATA, (unsigned long)arg64);
+			if(err < 0)
+				{
+					printk(KERN_ERR "COMPAT_GSENSOR_IOCTL_SET_DATA is failed!\n");
+				}
+				break;
+		case COMPAT_GSENSOR_IOCTL_GET_TEMP:
+			err = file->f_op->unlocked_ioctl(file, GSENSOR_IOCTL_GET_TEMP, (unsigned long)arg64);
+			if(err < 0)
+				{
+					printk(KERN_ERR "COMPAT_GSENSOR_IOCTL_GET_TEMP is failed!\n");
+				}
+				break;
+
+		case COMPAT_GSENSOR_IOCTL_GET_DANT:
+			err = file->f_op->unlocked_ioctl(file, GSENSOR_IOCTL_GET_DANT, (unsigned long)arg64);
+			if(err < 0)
+				{
+					printk(KERN_ERR "COMPAT_GSENSOR_IOCTL_GET_DANT is failed!\n");
+				}
+				break;
+		case COMPAT_GSENSOR_IOCTL_READ_REG:
+			err = file->f_op->unlocked_ioctl(file, GSENSOR_IOCTL_READ_REG, (unsigned long)arg64);
+			if(err < 0)
+				{
+					printk(KERN_ERR "COMPAT_GSENSOR_IOCTL_READ_REG is failed!\n");
+				}
+				break;
+
+		case COMPAT_GSENSOR_IOCTL_WRITE_REG:
+			err = file->f_op->unlocked_ioctl(file, GSENSOR_IOCTL_WRITE_REG, (unsigned long)arg64);
+			if(err < 0)
+				{
+					printk(KERN_ERR "COMPAT_GSENSOR_IOCTL_WRITE_REG is failed!\n");
+				}
+				break;
+		default:
+			 printk(KERN_ERR "%s not supported = 0x%04x", __FUNCTION__, cmd);
+			 return -ENOIOCTLCMD;
+			 break;
+	}
+	return 0;
+}
+#endif
+static long mxc400x_unlocked_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
+{
+
+	struct i2c_client *client = (struct i2c_client*)file->private_data;
+	struct mxc400x_i2c_data *obj = (struct mxc400x_i2c_data*)i2c_get_clientdata(client);  
+	int strbuf[MXC400X_BUFSIZE];
+	void __user *data;
+	SENSOR_DATA sensor_data;
+	int err = 0;
+	int cali[3];
+        int value[3] = {0};
+        int vec[3] = {0};
+        s8 temp = 0 ;
+	int dant[4] = {0};
+	u8 test=0;
+	u8 buf;
+	u8 reg[2];
+
+
+	//printk(KERN_INFO "%s: %s call with cmd 0x%x and arg 0x%x\n",MXC400X_DEV_NAME, __func__, cmd, (unsigned int)arg);
+	if(_IOC_DIR(cmd) & _IOC_READ)
+	{
+	 	err = !access_ok(VERIFY_WRITE, (void __user *)arg, _IOC_SIZE(cmd));
+	}
+	else if(_IOC_DIR(cmd) & _IOC_WRITE)
+	{
+	 	err = !access_ok(VERIFY_READ, (void __user *)arg, _IOC_SIZE(cmd));
+	}
+
+	if(err)
+	{
+	 	GSE_ERR("access error: %08X, (%2d, %2d)\n", cmd, _IOC_DIR(cmd), _IOC_SIZE(cmd));
+	 	return -EFAULT;
+	}
+	switch (cmd) {
+		case GSENSOR_IOCTL_INIT:
+			mxc400x_init_client(client, 0); 		 
+			break;
+
+		case GSENSOR_IOCTL_READ_CHIPINFO:
+			data = (void __user *) arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}
+			
+			MXC400X_ReadChipInfo(client, strbuf, MXC400X_BUFSIZE);
+			if(copy_to_user(data, strbuf, strlen(strbuf)+1))
+			{
+				err = -EFAULT;
+				break;
+			}				 
+			break;	  
+
+		case GSENSOR_IOCTL_READ_SENSORDATA:
+			data = (void __user *) arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}
+			MXC400X_SetPowerMode(client,true);	
+			MXC400X_ReadSensorData(client, strbuf, MXC400X_BUFSIZE);
+			if(copy_to_user(data, strbuf, strlen(strbuf)+1))
+			{
+				err = -EFAULT;
+				break;	  
+			}				 
+			break;
+		case GSENSOR_IOCTL_READ_GAIN:
+			data = (void __user *) arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}			
+			
+			if(copy_to_user(data, &gsensor_gain, sizeof(GSENSOR_VECTOR3D)))
+			{
+				err = -EFAULT;
+				break;
+			}				 
+			break;
+		case GSENSOR_IOCTL_READ_RAW_DATA:
+			data = (void __user *) arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}
+			MXC400X_ReadRawData(client, strbuf);
+			if(copy_to_user(data, strbuf, strlen(strbuf)+1))
+			{
+				err = -EFAULT;
+				break;	  
+			}
+			break;	  
+		case GSENSOR_IOCTL_SET_CALI:
+			data = (void __user*)arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}
+			if(copy_from_user(&sensor_data, data, sizeof(sensor_data)))
+			{
+				err = -EFAULT;
+				break;	  
+			}
+			if(atomic_read(&obj->suspend))
+			{
+				GSE_ERR("Perform calibration in suspend state!!\n");
+				err = -EINVAL;
+			}
+			else
+			{
+				cali[MXC400X_AXIS_X] = sensor_data.x * obj->reso->sensitivity / GRAVITY_EARTH_1000;
+				cali[MXC400X_AXIS_Y] = sensor_data.y * obj->reso->sensitivity / GRAVITY_EARTH_1000;
+				cali[MXC400X_AXIS_Z] = sensor_data.z * obj->reso->sensitivity / GRAVITY_EARTH_1000;			  
+				err = MXC400X_WriteCalibration(client, cali);			 
+			}
+			break;
+		case GSENSOR_IOCTL_CLR_CALI:
+			err = MXC400X_ResetCalibration(client);
+			break;
+		case GSENSOR_IOCTL_GET_CALI:
+			data = (void __user*)arg;
+			if(data == NULL)
+			{
+				err = -EINVAL;
+				break;	  
+			}
+			if(err = MXC400X_ReadCalibration(client, cali))
+			{
+				break;
+			}
+			
+			sensor_data.x = cali[MXC400X_AXIS_X] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+			sensor_data.y = cali[MXC400X_AXIS_Y] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+			sensor_data.z = cali[MXC400X_AXIS_Z] * GRAVITY_EARTH_1000 / obj->reso->sensitivity;
+			if(copy_to_user(data, &sensor_data, sizeof(sensor_data)))
+			{
+				err = -EFAULT;
+				break;
+			}		
+			break;
+//  begin.................
+		case GSENSOR_IOCTL_GET_DELAY:
+           //  GSE_LOG("Gsensor get delay!!\n");
+             break;
+		case GSENSOR_IOCTL_GET_STATUS:
+			 data = (void __user *) arg;
+			 if(data == NULL)
+			 {
+				err = -EINVAL;
+			 	break;	  
+			 }
+			 mutex_lock(&mxc400x_mutex);
+			 if(copy_to_user(data, &enable_status, sizeof(enable_status)))
+			 {
+				err = -EFAULT;
+				mutex_unlock(&mxc400x_mutex);
+				break;
+			 }	
+             	
+             mutex_unlock(&mxc400x_mutex);
+             break;
+		case GSENSOR_IOCTL_GET_DATA:
+			 data = (void __user *) arg;
+			 if(data == NULL)
+			 {
+				 err = -EINVAL;
+				 break;	  
+			 }
+			 	
+			 //MXC400X_RxData(client, vec);
+             err = MXC400X_ReadSensorData(client, vec,MXC400X_AXES_NUM);
+             
+			 if(copy_to_user(data, vec, sizeof(vec)))
+			 {
+				 err = -EFAULT;
+				 break;	  
+			 }
+             break;
+		case GSENSOR_IOCTL_SET_DATA:
+			 data = (void __user *) arg;
+			 if(data == NULL)
+			 {
+				 err = -EINVAL;
+				 break;	  
+			 }
+			 
+			 if(copy_from_user(value, data, sizeof(value)))
+			 {
+				 err = -EFAULT;
+				 break;	  
+			 }
+             
+			 MXC400X_SaveData(value);
+             break;
+        case GSENSOR_IOCTL_GET_TEMP:
+			 data = (void __user *) arg;
+			 if(data == NULL)
+			 {
+				 err = -EINVAL;
+				 break;	  
+			 }
+            
+			 err = MXC400X_ReadTemp(client, &temp);
+			
+			 if(copy_to_user(data, &temp, sizeof(temp)))
+			 {
+				 err = -EFAULT;
+				 break;	  
+			 }
+             break;
+		case GSENSOR_IOCTL_GET_DANT:
+             data = (void __user *) arg;
+			 if(data == NULL)
+			 {
+				 err = -EINVAL;
+				 break;	  
+			 }
+			 err = MXC400X_ReadDant(client, dant);
+             //GSE_LOG("dant 2 = %d %d %d %d\n", dant[0], dant[1], dant[2], dant[3]);
+			 if(copy_to_user(data, dant, sizeof(dant)))
+			 {
+				 err = -EFAULT;
+				 break;	  
+			 } 			
+			break;
+#if 0
+		case GSENSOR_IOCTL_SET_TEST1:
+			 data = (void __user *) arg;
+			 if(data == NULL)
+			 {
+				 err = -EINVAL;
+				 break;	  
+			 }
+			 if(copy_from_user(&test, data, sizeof(test)))
+			 {
+				 err = -EFAULT;
+				 break;	  
+			 }
+			
+			err = MXC400X_Test1(client, test);
+            break;
+
+		case GSENSOR_IOCTL_SET_TEST2:
+			 data = (void __user *) arg;
+			 if(data == NULL)
+			 {
+				 err = -EINVAL;
+				 break;	  
+			 }
+			 if(copy_from_user(&test, data, sizeof(test)))
+			 {
+				 err = -EFAULT;
+				 break;	  
+			 }
+			
+			err = MXC400X_Test2(client, test);
+			break;
+		case GSENSOR_IOCTL_SET_TEST3:
+			 data = (void __user *) arg;
+			 if(data == NULL)
+			 {
+				 err = -EINVAL;
+				 break;	  
+			 }
+			 if(copy_from_user(&test, data, sizeof(test)))
+			 {
+				 err = -EFAULT;
+				 break;	  
+			 }
+			
+			err = MXC400X_Test3(client, test);
+			break;
+#endif
+
+		case GSENSOR_IOCTL_READ_REG:
+			data = (void __user *) arg;
+			if(data == NULL)
+			{
+				 err = -EINVAL;
+				 break;	  
+			}
+		        if(copy_from_user(&test, data, sizeof(test)))
+			{
+				 err = -EFAULT;
+				 break;	  
+			}
+			if(err = mxc400x_i2c_read_block(client, test, &buf, 1));
+			{
+				GSE_ERR("error: %d\n", err);
+			}
+ 			if(copy_to_user(data, &buf, 1))
+			{
+				err = -EFAULT;
+				break;	  
+			} 
+			break;
+
+		case GSENSOR_IOCTL_WRITE_REG:
+			data = (void __user *) arg;
+			if(data == NULL)
+			{
+				 err = -EINVAL;
+				 break;	  
+			}
+			if(copy_from_user(reg, data, sizeof(reg)))
+			{
+				 err = -EFAULT;
+				 break;	  
+			}
+			err = i2c_master_send(client, reg, 0x2);
+			if(err <= 0)
+			{
+				GSE_LOG("write reg failed!\n");
+				err = -EFAULT;
+				break;
+			}
+			break;
+//  end.....
+		default:
+			GSE_ERR("unknown IOCTL: 0x%08x\n", cmd);
+			err = -ENOIOCTLCMD;
+			break;
+	}
+	
+	return err;
+}
+
+
+static struct file_operations mxc400x_fops = {
+		 //.owner = THIS_MODULE,
+		 .open = mxc400x_open,
+		 .release = mxc400x_release,
+		 .unlocked_ioctl = mxc400x_unlocked_ioctl,
+		#ifdef CONFIG_COMPAT
+		.compat_ioctl = mxc400x_compat_ioctl,
+		#endif
+};
+
+static struct miscdevice mxc400x_device = {
+		 .minor = MISC_DYNAMIC_MINOR,
+		 .name = "gsensor",
+		 .fops = &mxc400x_fops,
+};
+
+#ifndef CONFIG_HAS_EARLYSUSPEND
+
+static int mxc400x_suspend(struct i2c_client *client, pm_message_t msg) 
+{
+	 struct mxc400x_i2c_data *obj = i2c_get_clientdata(client);	  
+	 int err = 0; 
+
+	 if(msg.event == PM_EVENT_SUSPEND)
+	 {	 
+		 if(obj == NULL)
+		 {
+			 GSE_ERR("null mxc400x!!\n");
+			 return -EINVAL;
+		 }
+		 mutex_lock(&mxc400x_mutex);
+		 atomic_set(&obj->suspend, 1);
+		 if(err == MXC400X_SetPowerMode(obj->client, false))
+		 {
+			 GSE_ERR("write power control fail!!\n");
+			 mutex_unlock(&mxc400x_mutex);
+			 return -EINVAL;
+		 } 
+		 mutex_unlock(&mxc400x_mutex);
+	 }
+	 return err;
+}
+
+static int mxc400x_resume(struct i2c_client *client)
+{
+	 struct mxc400x_i2c_data *obj = i2c_get_clientdata(client);		  
+	 int err = 0;
+
+
+	 if(obj == NULL)
+	 {
+		 GSE_ERR("null mxc400x!!\n");
+		 return -EINVAL;
+	 }
+     mutex_lock(&mxc400x_mutex);
+	 if(err = mxc400x_init_client(client, 0))
+	 {
+		 GSE_ERR("initialize client fail!!\n");
+		 mutex_unlock(&mxc400x_mutex);
+		 return -EINVAL;		
+	 }
+	 atomic_set(&obj->suspend, 0);
+     mutex_unlock(&mxc400x_mutex);
+	 return err;
+}
+
+#else /*CONFIG_HAS_EARLY_SUSPEND is defined*/
+	
+static void mxc400x_early_suspend(struct early_suspend *h) 
+{
+	 struct mxc400x_i2c_data *obj = container_of(h, struct mxc400x_i2c_data, early_drv);	 
+	 int err; 
+
+	 if(obj == NULL)
+	 {
+		 GSE_ERR("null mxc400x!!\n");
+		 return;
+	 }
+	mutex_lock(&mxc400x_mutex);
+	atomic_set(&obj->suspend, 1); 
+	if(err = MXC400X_SetPowerMode(obj->client, false))
+	{
+		GSE_ERR("write power control fail!!\n");
+		mutex_unlock(&mxc400x_mutex);
+		return;
+	}
+	mutex_unlock(&mxc400x_mutex);
+	mxc400x_power(obj->hw, 0);
+}
+	
+static void mxc400x_late_resume(struct early_suspend *h)
+{
+	 struct mxc400x_i2c_data *obj = container_of(h, struct mxc400x_i2c_data, early_drv);		   
+	 int err;
+
+	 if(obj == NULL)
+	 {
+		 GSE_ERR("null mxc400x!!\n");
+		 return;
+	 }
+
+	 mxc400x_power(obj->hw, 1);
+	 mutex_lock(&mxc400x_mutex);
+	 if(err = mxc400x_init_client(obj->client, 0))
+	 {
+		 GSE_ERR("initialize client fail!!\n");
+		 mutex_unlock(&mxc400x_mutex);
+		 return;		
+	 }
+	 atomic_set(&obj->suspend, 0);	  
+	 mutex_unlock(&mxc400x_mutex);
+}
+
+#endif /*CONFIG_HAS_EARLYSUSPEND*/
+
+static int mxc400x_i2c_detect(struct i2c_client *client,int kind,  struct i2c_board_info *info) 
+{    
+	strcpy(info->type, MXC400X_DEV_NAME);
+	return 0;
+}
+
+// if use  this typ of enable , Gsensor should report inputEvent(x, y, z ,stats, div) to HAL
+static int mxc400x_open_report_data(int open)
+{
+	//should queuq work to report event if  is_report_input_direct=true
+	return 0;
+}
+
+// if use  this typ of enable , Gsensor only enabled but not report inputEvent to HAL
+
+static int mxc400x_enable_nodata(int en)
+{
+	int res =0;
+	bool power = false;
+	
+	if(1==en)
+	{
+		power = true;
+
+	}
+	if(0==en)
+	{
+		power = false;
+	}
+	res = MXC400X_SetPowerMode(obj_i2c_data->client, power);
+	if(res != MXC400X_SUCCESS)
+	{
+		GSE_ERR("MXC400X_SetPowerMode fail!\n");
+		return -1;
+	}
+	GSE_LOG("MXC400X_enable_nodata OK!\n");
+	enable_status = en;
+	return 0;
+}
+
+static int mxc400x_set_delay(u64 ns)
+{
+/*
+    int value =0;
+	int sample_delay=0;
+	int err;
+	value = (int)ns/1000/1000;
+	if(value <= 5)
+	{
+		sample_delay = LIS3DH_BW_200HZ;
+	}
+	else if(value <= 10)
+	{
+		sample_delay = LIS3DH_BW_100HZ;
+	}
+	else
+	{
+		sample_delay = LIS3DH_BW_50HZ;
+	}
+	mutex_lock(&lis3dh_op_mutex);
+	err = LIS3DH_SetBWRate(obj_i2c_data->client, sample_delay);
+	if(err != LIS3DH_SUCCESS ) //0x2C->BW=100Hz
+	{
+		GSE_ERR("Set delay parameter error!\n");
+	}
+	mutex_unlock(&lis3dh_op_mutex);
+	if(value >= 50)
+	{
+		atomic_set(&obj_i2c_data->filter, 0);
+	}
+	else
+	{					
+		obj_i2c_data->fir.num = 0;
+		obj_i2c_data->fir.idx = 0;
+		obj_i2c_data->fir.sum[LIS3DH_AXIS_X] = 0;
+		obj_i2c_data->fir.sum[LIS3DH_AXIS_Y] = 0;
+		obj_i2c_data->fir.sum[LIS3DH_AXIS_Z] = 0;
+		atomic_set(&obj_i2c_data->filter, 1);
+	}
+	
+	GSE_LOG("lis3dh_set_delay (%d)\n",value);
+*/
+	return 0;
+}
+
+static int mxc400x_get_data(int* x ,int* y,int* z, int* status)
+{
+
+	struct i2c_client *client = mxc400x_i2c_client;
+	int strbuf[MXC400X_BUFSIZE];
+	
+	if(NULL == client)
+	{
+		GSE_ERR("i2c client is null!!\n");
+		return 0;
+	}
+	MXC400X_ReadSensorData(client, strbuf, MXC400X_BUFSIZE);     
+	*x = strbuf[0];
+	*y = strbuf[1];
+	*z = strbuf[2];
+	
+//	sscanf(buff, "%x %x %x", x, y, z);		
+	*status = SENSOR_STATUS_ACCURACY_MEDIUM;
+
+	return 0;
+}
+
+
+/*----------------------------------------------------------------------------*/
+static int mxc400x_i2c_probe(struct i2c_client *client, const struct i2c_device_id *id)
+{
+	 struct i2c_client *new_client;
+	 struct mxc400x_i2c_data *obj;
+	 
+	 #ifdef KK_OLDARCH
+	 struct hwmsen_object sobj;
+	 #endif
+	 
+	 int err = 0;
+	 
+	#ifdef L_NewArch
+	struct acc_control_path ctl={0};
+	struct acc_data_path data={0};
+    #endif
+	
+	 if(!(obj = kzalloc(sizeof(*obj), GFP_KERNEL)))
+	 {
+		 err = -ENOMEM;
+		 goto exit;
+	 }
+	 
+	 memset(obj, 0, sizeof(struct mxc400x_i2c_data));
+ 
+	 obj->hw = get_cust_acc_hw();
+	 atomic_set(&obj->layout, obj->hw->direction);
+	 if(err = hwmsen_get_convert(obj->hw->direction, &obj->cvt))
+	 {
+		 GSE_ERR("invalid direction: %d\n", obj->hw->direction);
+		 goto exit;
+	 }
+
+	 obj_i2c_data = obj;
+	 obj->client = client;
+	 new_client = obj->client;
+	 i2c_set_clientdata(new_client,obj);
+	 
+	 atomic_set(&obj->trace, 0);
+	 atomic_set(&obj->suspend, 0);
+ 
+ 
+#ifdef CONFIG_MXC400X_LOWPASS
+	if(obj->hw->firlen > C_MAX_FIR_LENGTH)
+	{
+	 	atomic_set(&obj->firlen, C_MAX_FIR_LENGTH);
+	}	 
+	else
+	{
+	 	atomic_set(&obj->firlen, obj->hw->firlen);
+	}
+
+	if(atomic_read(&obj->firlen) > 0)
+	{
+	 	atomic_set(&obj->fir_en, 1);
+	}
+
+#endif
+	 mxc400x_i2c_client = new_client; 
+ 
+	 if(err = mxc400x_init_client(new_client, 1))
+	 {
+		 goto exit_init_failed;
+	 }
+	 
+ 
+	 if(err = misc_register(&mxc400x_device))
+	 {
+		 GSE_ERR("mxc400x_device register failed\n");
+		 goto exit_misc_device_register_failed;
+	 }
+ 
+	 if(err = mxc400x_create_attr(&mxc400x_init_info.platform_diver_addr->driver))
+	 {
+		 GSE_ERR("create attribute err = %d\n", err);
+		 goto exit_create_attr_failed;
+	 }
+	 
+ #ifdef KK_OLDARCH
+	 sobj.self = obj;
+	 sobj.polling = 1;
+	 sobj.sensor_operate = gsensor_operate;
+	 if(err = hwmsen_attach(ID_ACCELEROMETER, &sobj))
+	 {
+		 GSE_ERR("attach fail = %d\n", err);
+		 goto exit_kfree;
+	 }
+#endif
+
+  #ifdef L_NewArch
+ 	ctl.open_report_data= mxc400x_open_report_data;
+	ctl.enable_nodata = mxc400x_enable_nodata;
+	ctl.set_delay  = mxc400x_set_delay;
+	ctl.is_report_input_direct = false;
+	
+	err = acc_register_control_path(&ctl);
+	if(err)
+	{
+	 	GSE_ERR("register acc control path err\n");
+		goto exit_kfree;
+	}
+	
+	data.get_data = mxc400x_get_data;
+	data.vender_div = 1000;
+	err = acc_register_data_path(&data);
+	if(err)
+	{
+	 	GSE_ERR("register acc data path err\n");
+		goto exit_kfree;
+	}
+#endif
+	
+#ifdef CONFIG_HAS_EARLYSUSPEND
+	 obj->early_drv.level	 = EARLY_SUSPEND_LEVEL_DISABLE_FB - 1,
+	 obj->early_drv.suspend  = mxc400x_early_suspend,
+	 obj->early_drv.resume	 = mxc400x_late_resume,	  
+	 register_early_suspend(&obj->early_drv);
+#endif 
+     mxc400x_init_flag = 0;
+	 GSE_LOG("%s: OK\n", __func__);    
+	 return 0;
+ 
+	 exit_create_attr_failed:
+	 misc_deregister(&mxc400x_device);
+	 exit_misc_device_register_failed:
+	 exit_init_failed:
+	 //i2c_detach_client(new_client);
+	 exit_kfree:
+	 kfree(obj);
+	 exit:
+	 GSE_ERR("%s: err = %d\n", __func__, err);
+      mxc400x_init_flag = -1;
+	 return err;
+}
+ 
+static int mxc400x_i2c_remove(struct i2c_client *client)
+{
+	 int err = 0;	 
+	 
+	 if(err = mxc400x_delete_attr(&mxc400x_init_info.platform_diver_addr->driver))
+	 {
+		 GSE_ERR("mxc400x_delete_attr fail: %d\n", err);
+	 }
+	 
+	 if(err = misc_deregister(&mxc400x_device))
+	 {
+		 GSE_ERR("misc_deregister fail: %d\n", err);
+	 }
+
+	 if(err = hwmsen_detach(ID_ACCELEROMETER))
+		 
+
+	 mxc400x_i2c_client = NULL;
+	 i2c_unregister_device(client);
+	 kfree(i2c_get_clientdata(client));
+	 return 0;
+}
+
+
+static int  mxc400x_local_init(void)
+{
+	struct acc_hw *hw = get_cust_acc_hw();
+
+	mxc400x_power(hw, 1);
+
+	if(i2c_add_driver(&mxc400x_i2c_driver))
+	{
+		 GSE_ERR("add driver error\n");
+		 return -1;
+	}
+	return 0;
+}
+static int mxc400x_remove(void)
+{
+	 struct acc_hw *hw = get_cust_acc_hw();
+ 
+	 mxc400x_power(hw, 0);	 
+	 i2c_del_driver(&mxc400x_i2c_driver);
+	 return 0;
+}
+#if 0
+static struct platform_driver mxc400x_gsensor_driver = {
+		 .probe 	 = mxc400x_probe,
+		 .remove	 = mxc400x_remove,	 
+		 .driver	 = {
+			 .name	= "gsensor",
+			 //.owner = THIS_MODULE,
+		 }
+};
+#endif
+
+static int __init mxc400x_driver_init(void)
+{
+   	
+	 struct acc_hw *hw = get_cust_acc_hw();
+	 GSE_LOG("%s: i2c_number=%d\n", __func__,hw->i2c_num); 
+
+	 i2c_register_board_info(hw->i2c_num, &i2c_mxc400x, 1);
+	 acc_driver_add(&mxc400x_init_info);
+/*
+if(platform_driver_register(&mxc400x_gsensor_driver))
+	 {
+		GSE_ERR("failed to register driver");
+		return -ENODEV;
+	 }
+*/
+     mutex_init(&mxc400x_mutex);
+#if defined(SW_SIMULATION)	
+     mutex_init(&g_gsensor_mutex);
+#endif
+
+	 return 0;	 
+}
+
+static void __exit mxc400x_driver_exit(void)
+{
+	
+     platform_driver_unregister(&mxc400x_gsensor_driver);
+     mutex_destroy(&mxc400x_mutex);
+#if defined(SW_SIMULATION)	//modified by lyon
+     mutex_destroy(&g_gsensor_mutex);
+#endif
+
+}
+
+module_init(mxc400x_driver_init);
+module_exit(mxc400x_driver_exit);
+
+
+MODULE_AUTHOR("Lyon Miao<xlmiao@memsic.com>");
+MODULE_DESCRIPTION("MEMSIC MXC400x Accelerometer Sensor Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/misc/mediatek/accelerometer/mxc400x-new/mxc400x.h b/drivers/misc/mediatek/accelerometer/mxc400x-new/mxc400x.h
new file mode 100644
index 000000000..5125257b7
--- /dev/null
+++ b/drivers/misc/mediatek/accelerometer/mxc400x-new/mxc400x.h
@@ -0,0 +1,97 @@
+/*
+ * Copyright (C) 2010 MEMSIC, Inc.
+ *
+ * Initial Code:
+ *	Robbie Cao
+ *
+ * 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307 USA
+ *
+ */
+
+/*
+ * Definitions for MXC400X accelorometer sensor chip.
+ */
+#ifndef __MXC400X_H__
+#define __MXC400X_H__
+
+
+#include	<linux/ioctl.h>	/* For IOCTL macros */
+#include	<linux/input.h>
+
+
+
+/************************************************/
+/* 	Accelerometer defines section	 	*/
+/************************************************/
+#define MXC400X_DEV_NAME		"MXC400X"
+
+#define MXC400X_I2C_ADDR     	0x15
+#define MXC400X_ID				0x02	
+
+/* MXC400X register address */
+#define MXC400X_REG_X			0x03
+#define MXC400X_REG_Y			0x05
+#define MXC400X_REG_Z			0x07
+#define MXC400X_REG_TEMP        0x09
+#define MXC400X_REG_CTRL		0x0D
+#define MXC400X_REG_ID          0x0E
+//#define MXC400X_REG_FAC         0x0E
+//#define MXC400X_REG_FSRE        0x17
+#define MXC400X_REG_TEST1        0x13
+#define MXC400X_REG_TEST2        0x14
+#define MXC400X_REG_TEST3        0x15 
+
+
+/*para setting*/
+//#define MXC400X_PASSWORD        0x93
+//#define MXC400X_RANGE_8G        0x5B    /*full scale range +/-8g*/
+#define MXC400X_AWAKE		0x40	/* power on */
+#define MXC400X_SLEEP		0x01	/* power donw */
+
+#define MXC400X_BW_50HZ    0x00
+#define MXC400X_RANGE_2G   0x00
+#define MXC400X_RANGE_4G   0x20
+#define MXC400X_RANGE_8G   0x40
+
+
+/*ERR code*/
+#define MXC400X_SUCCESS						0
+#define MXC400X_ERR_I2C						-1
+#define MXC400X_ERR_STATUS					-3
+#define MXC400X_ERR_SETUP_FAILURE			-4
+#define MXC400X_ERR_GETGSENSORDATA			-5
+#define MXC400X_ERR_IDENTIFICATION			-6
+
+#define MXC400X_BUFSIZE				256
+
+#if defined(C2_PROJ)
+#define ACC_USE_SYS_I2C
+#endif
+
+#ifdef	__KERNEL__
+struct mxc400x_acc_platform_data {
+	int poll_interval;
+	int min_interval;
+
+	int (*init)(void);
+	void (*exit)(void);
+	int (*power_on)(void);
+	int (*power_off)(void);
+
+};
+#endif	/* __KERNEL__ */
+
+#endif /* __MXC400X_H__ */
+