/* 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 #include #include #include #include #include #include #include #include #include #include #include #include #include #define MT6582 #ifdef MT6516 #include #include #include #include #endif #ifdef MT6573 #include #include #include #include #endif #ifdef MT6575 #include #include #include #include #endif #ifdef MT6577 #include #include #include #include #endif #ifdef MT6572 #include #include #include #include #endif #ifdef MT6582 #include #include #include #include #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 #include #include #include #include "k2dh.h" #include /*----------------------------------------------------------------------------*/ //#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", __func__) #define GSE_ERR(fmt, args...) printk(KERN_ERR GSE_TAG"[ERROR] %s() line=%d : "fmt, __func__, __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");