diff --git a/drivers/input/sensors/accel/lis3dh.c b/drivers/input/sensors/accel/lis3dh.c old mode 100755 new mode 100644 index f74ec77463ac..ece78214a2ea --- a/drivers/input/sensors/accel/lis3dh.c +++ b/drivers/input/sensors/accel/lis3dh.c @@ -31,12 +31,11 @@ #endif #include - #define LIS3DH_INT_COUNT (0x0E) -#define LIS3DH_WHO_AM_I (0x0F) +#define LIS3DH_WHO_AM_I (0x0F) /* full scale setting - register & mask */ -#define LIS3DH_TEMP_CFG_REG (0x1F) +#define LIS3DH_TEMP_CFG_REG (0x1F) #define LIS3DH_CTRL_REG1 (0x20) #define LIS3DH_CTRL_REG2 (0x21) #define LIS3DH_CTRL_REG3 (0x22) @@ -51,33 +50,20 @@ #define LIS3DH_OUT_Y_H (0x2b) #define LIS3DH_OUT_Z_L (0x2c) #define LIS3DH_OUT_Z_H (0x2d) -#define LIS3DH_FIFO_CTRL_REG (0x2E) +#define LIS3DH_FIFO_CTRL_REG (0x2E) #define LIS3DH_INT1_CFG (0x30) #define LIS3DH_INT1_SRC (0x31) #define LIS3DH_INT1_THS (0x32) -#define LIS3DH_INT1_DURATION (0x33) +#define LIS3DH_INT1_DURATION (0x33) -#define LIS3DH_DEVID (0x33) //chip id +#define LIS3DH_DEVID (0x33) #define LIS3DH_ACC_DISABLE (0x08) #define LIS3DH_RANGE 2000000 /* LIS3DH */ #define LIS3DH_PRECISION 16 -#define LIS3DH_BOUNDARY (0x1 << (LIS3DH_PRECISION - 1)) -#define LIS3DH_GRAVITY_STEP (LIS3DH_RANGE / LIS3DH_BOUNDARY) - -#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 */ -#define ODR400 0x70 /* 400Hz output data rate */ -#define ODR1250 0x90 /* 1250Hz output data rate */ - - struct sensor_reg_data { char reg; @@ -85,251 +71,216 @@ struct sensor_reg_data { }; /****************operate according to sensor chip:start************/ +/* odr table, hz */ +static const int odr_table[7] = { + 1, 10, 25, 50, 100, 200, 400 +}; + +static int lis3dh_select_odr(int want) +{ + int i; + int max_index = ARRAY_SIZE(odr_table); + + for (i = 0; i < max_index; i++) { + if (want <= odr_table[i]) + return i + 1; + } + + return max_index; +} static int sensor_active(struct i2c_client *client, int enable, int rate) { struct sensor_private_data *sensor = - (struct sensor_private_data *) i2c_get_clientdata(client); + (struct sensor_private_data *) i2c_get_clientdata(client); int result = 0; int status = 0; - - sensor->ops->ctrl_data = sensor_read_reg(client, sensor->ops->ctrl_reg); + int odr_rate = 0; - sensor->ops->ctrl_data |= ODR100; //100HZ,if 0 then power down - - //register setting according to chip datasheet - if(!enable) - { - status = LIS3DH_ACC_DISABLE; //lis3dh - sensor->ops->ctrl_data |= status; + if (rate == 0) { + dev_err(&client->dev, "%s: rate == 0!!!\n", __func__); + return -1; } - else - { - status = ~LIS3DH_ACC_DISABLE; //lis3dh + odr_rate = 1000 / rate; + + sensor->ops->ctrl_data = sensor_read_reg(client, sensor->ops->ctrl_reg); + result = lis3dh_select_odr(odr_rate); + sensor->ops->ctrl_data &= 0x0f; + sensor->ops->ctrl_data |= (result << 4); + + if (!enable) { + status = LIS3DH_ACC_DISABLE; + sensor->ops->ctrl_data |= status; + } else { + status = ~LIS3DH_ACC_DISABLE; sensor->ops->ctrl_data &= status; } - DBG("%s:reg=0x%x,reg_ctrl=0x%x,enable=%d\n",__func__,sensor->ops->ctrl_reg, sensor->ops->ctrl_data, enable); result = sensor_write_reg(client, sensor->ops->ctrl_reg, sensor->ops->ctrl_data); - if(result) - printk("%s:fail to active sensor\n",__func__); - - return result; + if (result) + dev_err(&client->dev, "%s:fail to active sensor\n", __func__); + return result; } static int sensor_init(struct i2c_client *client) -{ +{ struct sensor_private_data *sensor = - (struct sensor_private_data *) i2c_get_clientdata(client); + (struct sensor_private_data *) i2c_get_clientdata(client); int result = 0; int i; - struct sensor_reg_data reg_data[] = - { - {LIS3DH_CTRL_REG2,0X00}, - {LIS3DH_CTRL_REG4,0x08}, //High resolution output mode: 1, Normal mode - {LIS3DH_CTRL_REG6,0x40}, - {LIS3DH_TEMP_CFG_REG,0x00}, // - {LIS3DH_FIFO_CTRL_REG,0x00}, // - {LIS3DH_INT1_CFG,0xFF}, //6 direction position recognition - {LIS3DH_INT1_THS,0x7F}, //Interrupt 1 threshold - {LIS3DH_INT1_DURATION,0x7F}, //Duration value 0x00->ox7f - }; - - result = sensor->ops->active(client,0,0); - if(result) - { - printk("%s:line=%d,error\n",__func__,__LINE__); + + struct sensor_reg_data reg_data[] = { + {LIS3DH_CTRL_REG2, 0X00}, + {LIS3DH_CTRL_REG4, 0x08}, + {LIS3DH_CTRL_REG6, 0x40}, + {LIS3DH_TEMP_CFG_REG, 0x00}, + {LIS3DH_FIFO_CTRL_REG, 0x00}, + {LIS3DH_INT1_CFG, 0xFF}, + {LIS3DH_INT1_THS, 0x7F}, + {LIS3DH_INT1_DURATION, 0x7F}, + }; + + result = sensor->ops->active(client, 0, sensor->pdata->poll_delay_ms); + if (result) { + dev_err(&client->dev, "%s:line=%d,error\n", __func__, __LINE__); return result; } sensor->status_cur = SENSOR_OFF; - - for(i=0;i<(sizeof(reg_data)/sizeof(struct sensor_reg_data));i++) - { + + for (i = 0; i < (sizeof(reg_data) / sizeof(struct sensor_reg_data)); i++) { result = sensor_write_reg(client, reg_data[i].reg, reg_data[i].data); - if(result) - { - printk("%s:line=%d,i=%d,error\n",__func__,__LINE__,i); + if (result) { + dev_err(&client->dev, "%s:line=%d,i=%d,error\n", __func__, __LINE__, i); return result; } } - - if(sensor->pdata->irq_enable) - { - - result = sensor_write_reg(client, LIS3DH_CTRL_REG3, 0x40);//I1_AOI1 =1 if motion - if(result) - { - printk("%s:line=%d,error\n",__func__,__LINE__); + if (sensor->pdata->irq_enable) { + result = sensor_write_reg(client, LIS3DH_CTRL_REG3, 0x40); + if (result) { + dev_err(&client->dev, "%s:line=%d,error\n", __func__, __LINE__); return result; } result = sensor_write_reg(client, LIS3DH_CTRL_REG5, 0x08); - if(result) - { - printk("%s:line=%d,error\n",__func__,__LINE__); + if (result) { + dev_err(&client->dev, "%s:line=%d,error\n", __func__, __LINE__); return result; } - } - + return result; } -static int sensor_convert_data(struct i2c_client *client, char high_byte, char low_byte) -{ - s64 result; - struct sensor_private_data *sensor = - (struct sensor_private_data *) i2c_get_clientdata(client); - //int precision = sensor->ops->precision; - switch (sensor->devid) { - case LIS3DH_DEVID: - result = ((int)high_byte << 8) | (int)low_byte; - if (result < LIS3DH_BOUNDARY) - result = result* LIS3DH_GRAVITY_STEP; - else - result = ~( ((~result & (0x7fff>>(16-LIS3DH_PRECISION)) ) + 1) - * LIS3DH_GRAVITY_STEP) + 1; - break; - - default: - printk(KERN_ERR "%s: devid wasn't set correctly\n",__func__); - return -EFAULT; - } - - return (int)result; -} - static int gsensor_report_value(struct i2c_client *client, struct sensor_axis *axis) { struct sensor_private_data *sensor = - (struct sensor_private_data *) i2c_get_clientdata(client); + (struct sensor_private_data *) i2c_get_clientdata(client); - /* Report acceleration sensor information */ - input_report_abs(sensor->input_dev, ABS_X, axis->x); - input_report_abs(sensor->input_dev, ABS_Y, axis->y); - input_report_abs(sensor->input_dev, ABS_Z, axis->z); - input_sync(sensor->input_dev); - DBG("Gsensor x==%d y==%d z==%d\n",axis->x,axis->y,axis->z); + if (sensor->status_cur == SENSOR_ON) { + /* Report acceleration sensor information */ + input_report_abs(sensor->input_dev, ABS_X, axis->x); + input_report_abs(sensor->input_dev, ABS_Y, axis->y); + input_report_abs(sensor->input_dev, ABS_Z, axis->z); + input_sync(sensor->input_dev); + } return 0; } -#define GSENSOR_MIN 10 static int sensor_report_value(struct i2c_client *client) { struct sensor_private_data *sensor = - (struct sensor_private_data *) i2c_get_clientdata(client); - struct sensor_platform_data *pdata = sensor->pdata; + (struct sensor_private_data *) i2c_get_clientdata(client); + struct sensor_platform_data *pdata = sensor->pdata; int ret = 0; - int x,y,z; - struct sensor_axis axis; - char buffer[6] = {0}; + short x, y, z; + struct sensor_axis axis; + char buffer[6] = {0}; char value = 0; - - if(sensor->ops->read_len < 6) //sensor->ops->read_len = 6 - { - printk("%s:lenth is error,len=%d\n",__func__,sensor->ops->read_len); + + if (sensor->ops->read_len < 6) { + dev_err(&client->dev, "%s:lenth is error,len=%d\n", __func__, sensor->ops->read_len); return -1; } - + memset(buffer, 0, 6); value = sensor_read_reg(client, LIS3DH_STATUS_REG); - if((value & 0x0f) == 0) - { - printk("%s:line=%d,value=0x%x,data is not ready\n",__func__,__LINE__,value); + if ((value & 0x0f) == 0) { + dev_err(&client->dev, "%s:line=%d,value=0x%x,data is not ready\n", __func__, __LINE__, value); return -1; } - - - /* Data bytes from hardware xL, xH, yL, yH, zL, zH */ + + /* Data bytes from hardware xL, xH, yL, yH, zL, zH */ do { *buffer = sensor->ops->read_reg; ret = sensor_rx_data(client, buffer, sensor->ops->read_len); if (ret < 0) - return ret; + return ret; } while (0); - //this gsensor need 6 bytes buffer - x = sensor_convert_data(sensor->client, buffer[1], buffer[0]); //buffer[1]:high bit - y = sensor_convert_data(sensor->client, buffer[3], buffer[2]); - z = sensor_convert_data(sensor->client, buffer[5], buffer[4]); + x = ((buffer[1] << 8) & 0xff00) + (buffer[0] & 0xFF); + y = ((buffer[3] << 8) & 0xff00) + (buffer[2] & 0xFF); + z = ((buffer[5] << 8) & 0xff00) + (buffer[4] & 0xFF); - axis.x = (pdata->orientation[0])*x + (pdata->orientation[1])*y + (pdata->orientation[2])*z; - axis.y = (pdata->orientation[3])*x + (pdata->orientation[4])*y + (pdata->orientation[5])*z; - axis.z = (pdata->orientation[6])*x + (pdata->orientation[7])*y + (pdata->orientation[8])*z; + axis.x = (pdata->orientation[0]) * x + (pdata->orientation[1]) * y + (pdata->orientation[2]) * z; + axis.y = (pdata->orientation[3]) * x + (pdata->orientation[4]) * y + (pdata->orientation[5]) * z; + axis.z = (pdata->orientation[6]) * x + (pdata->orientation[7]) * y + (pdata->orientation[8]) * z; - DBG( "%s: axis = %d %d %d \n", __func__, axis.x, axis.y, axis.z); + gsensor_report_value(client, &axis); - //Report event only while value is changed to save some power - if((abs(sensor->axis.x - axis.x) > GSENSOR_MIN) || (abs(sensor->axis.y - axis.y) > GSENSOR_MIN) || (abs(sensor->axis.z - axis.z) > GSENSOR_MIN)) - { - gsensor_report_value(client, &axis); + mutex_lock(&(sensor->data_mutex)); + sensor->axis = axis; + mutex_unlock(&(sensor->data_mutex)); - /* 互斥地缓存数据. */ - mutex_lock(&(sensor->data_mutex) ); - sensor->axis = axis; - mutex_unlock(&(sensor->data_mutex) ); - } - - if((sensor->pdata->irq_enable)&& (sensor->ops->int_status_reg >= 0)) //read sensor intterupt status register - { - + if ((sensor->pdata->irq_enable) && (sensor->ops->int_status_reg >= 0)) value = sensor_read_reg(client, sensor->ops->int_status_reg); - DBG("%s:sensor int status :0x%x\n",__func__,value); - } - + return ret; } struct sensor_operate gsensor_lis3dh_ops = { .name = "lis3dh", - .type = SENSOR_TYPE_ACCEL, //sensor type and it should be correct - .id_i2c = ACCEL_ID_LIS3DH, //i2c id number - .read_reg = (LIS3DH_OUT_X_L | 0x80), //read data - .read_len = 6, //data length - .id_reg = LIS3DH_WHO_AM_I, //read device id from this register - .id_data = LIS3DH_DEVID, //device id - .precision = LIS3DH_PRECISION, //12 bits - .ctrl_reg = LIS3DH_CTRL_REG1, //enable or disable - .int_status_reg = LIS3DH_INT1_SRC, //intterupt status register - .range = {-LIS3DH_RANGE,LIS3DH_RANGE}, //range - .trig = (IRQF_TRIGGER_LOW|IRQF_ONESHOT), - .active = sensor_active, - .init = sensor_init, + .type = SENSOR_TYPE_ACCEL, + .id_i2c = ACCEL_ID_LIS3DH, + .read_reg = (LIS3DH_OUT_X_L | 0x80), + .read_len = 6, + .id_reg = LIS3DH_WHO_AM_I, + .id_data = LIS3DH_DEVID, + .precision = LIS3DH_PRECISION, + .ctrl_reg = LIS3DH_CTRL_REG1, + .int_status_reg = LIS3DH_INT1_SRC, + .range = {-32768, +32768}, + .trig = (IRQF_TRIGGER_LOW | IRQF_ONESHOT), + .active = sensor_active, + .init = sensor_init, .report = sensor_report_value, }; /****************operate according to sensor chip:end************/ - -//function name should not be changed static struct sensor_operate *gsensor_get_ops(void) { return &gsensor_lis3dh_ops; } - static int __init gsensor_lis3dh_init(void) { struct sensor_operate *ops = gsensor_get_ops(); - int result = 0; int type = ops->type; - result = sensor_register_slave(type, NULL, NULL, gsensor_get_ops); - return result; + + return sensor_register_slave(type, NULL, NULL, gsensor_get_ops); } static void __exit gsensor_lis3dh_exit(void) { struct sensor_operate *ops = gsensor_get_ops(); int type = ops->type; + sensor_unregister_slave(type, NULL, NULL, gsensor_get_ops); } - module_init(gsensor_lis3dh_init); module_exit(gsensor_lis3dh_exit); - - diff --git a/drivers/input/sensors/accel/mc3230.c b/drivers/input/sensors/accel/mc3230.c old mode 100755 new mode 100644 index 7b2e0deb56ec..0a0fed4ee67c --- a/drivers/input/sensors/accel/mc3230.c +++ b/drivers/input/sensors/accel/mc3230.c @@ -145,8 +145,6 @@ static int g_value; /* Addresses to scan -- protected by sense_data_mutex */ static struct i2c_client *this_client; -static DECLARE_WAIT_QUEUE_HEAD(data_ready_wq); - #ifdef CONFIG_HAS_EARLYSUSPEND static struct early_suspend mc3230_early_suspend; #endif @@ -370,7 +368,7 @@ static int mc3230_reg_init(struct i2c_client *client) mc3230_active(client, 0); pcode = sensor_read_reg(client, MC3230_REG_PRODUCT_CODE); - GSE_LOG("mc3230_reg_init pcode=%x\n", pcode); + printk(KERN_INFO "mc3230_reg_init pcode=%x\n", pcode); if ((pcode == 0x19) || (pcode == 0x29)) { mc32x0_type = IS_MC3230; } else if ((pcode == 0x90) || (pcode == 0xA8) || (pcode == 0x88)) { @@ -523,36 +521,24 @@ static inline int mc3230_convert_to_int(s16 value) int result; if ((mc32x0_type == IS_MC3230) || (mc32x0_type == IS_MC2234)) { - if (value < MC3230_BOUNDARY) { - result = value * MC3230_GRAVITY_STEPS; - } else { - result = - ~(((~value & 0x7f) + 1) * MC3230_GRAVITY_STEPS) + 1; - } + result = value * 192; } else if (mc32x0_type == IS_MC3236) { - if (value < MC3230_BOUNDARY) { - result = value * MC3236_GRAVITY_STEP; - } else { - result = - ~(((~value & 0x7f) + 1) * MC3236_GRAVITY_STEP) + 1; - } + result = value * 256; } else if (mc32x0_type == IS_MC3210) { - if (value < MC3210_BOUNDARY) { - result = value * MC3210_GRAVITY_STEP; - } else { - result = - ~(((~value & 0x7f) + 1) * MC3210_GRAVITY_STEP) + 1; - } + result = value * 16; } return result; } static void mc3230_report_value(struct i2c_client *client, - struct mc3230_axis *axis) + struct sensor_axis *axis) { struct sensor_private_data *mc3230 = i2c_get_clientdata(client); + if (mc3230->status_cur == SENSOR_OFF) + return; + if (mc32x0_type == IS_MC2234) { input_report_abs(mc3230->input_dev, ABS_X, (axis->x)); input_report_abs(mc3230->input_dev, ABS_Y, -(axis->y)); @@ -582,7 +568,8 @@ static int mc3230_get_data(struct i2c_client *client) int ret; int x, y, z; int value = 0; - struct mc3230_axis axis; + static int flag; + struct sensor_axis axis; if (load_cali_flg > 0) { ret = mcube_read_cali_file(client); @@ -626,17 +613,28 @@ static int mc3230_get_data(struct i2c_client *client) (pdata->orientation[6]) * x + (pdata->orientation[7]) * y + (pdata->orientation[8]) * z; + /* input dev will ignore report data if data value is the same with last_value, + sample rate will not enough by this way, so just avoid this case */ + if ((sensor->axis.x == axis.x) && (sensor->axis.y == axis.y) && (sensor->axis.z == axis.z)) { + if (flag) { + flag = 0; + axis.x += 1; + axis.y += 1; + axis.z += 1; + } else { + flag = 1; + axis.x -= 1; + axis.y -= 1; + axis.z -= 1; + } + } + mc3230_report_value(client, &axis); mutex_lock(&sensor->data_mutex); - /* get data from buffer */ - memcpy(&axis, &sensor->axis, sizeof(sensor->axis)); + sensor->axis = axis; mutex_unlock(&sensor->data_mutex); - /* data_ready */ - atomic_set(&sensor->data_ready, 1); - wake_up(&sensor->data_ready_wq); - return 0; } @@ -1040,7 +1038,7 @@ long mc3230_ioctl(struct file *file, unsigned int cmd, unsigned long arg, int cali[3]; struct mc3230_data *p_mc3230_data = get_3230_ctl_data(); - struct mc3230_axis sense_data = { 0 }; + struct sensor_axis sense_data = { 0 }; mcprintkreg("mc3230_ioctl cmd is %d.", cmd); @@ -1169,6 +1167,23 @@ long mc3230_ioctl(struct file *file, unsigned int cmd, unsigned long arg, return 0; } +/* odr table, hz */ +static const int odr_table[8] = { + 1, 2, 4, 8, 16, 32, 64, 128 +}; + +static int mc3230_select_odr(int want) +{ + int i; + int max_index = ARRAY_SIZE(odr_table); + + for (i = 0; i < max_index; i++) { + if (want <= odr_table[i]) + return max_index - i - 1; + } + + return 0; +} static int sensor_active(struct i2c_client *client, int enable, int rate) { struct sensor_private_data *sensor = @@ -1176,7 +1191,14 @@ static int sensor_active(struct i2c_client *client, int enable, int rate) int result = 0; int mc3230_rate = 0; - mc3230_rate = 0xf8 | (0x07 & rate); + if (rate == 0) { + dev_err(&client->dev, "%s: rate == 0!!!\n", __func__); + return -1; + } + + mc3230_rate = mc3230_select_odr(1000 / rate); + + mc3230_rate = 0xf8 | (0x07 & mc3230_rate); if (rate != 0xff) result = @@ -1211,19 +1233,11 @@ static int sensor_init(struct i2c_client *client) struct sensor_private_data *sensor = (struct sensor_private_data *)i2c_get_clientdata(client); int result = 0; - int retry = 5; - static int MC3230_is_init; - if (MC3230_is_init == 0) { - while (retry--) { - if (init_3230_ctl_data(client) == 0) - break; - } - } + if (init_3230_ctl_data(client)) + return -1; - MC3230_is_init = 1; - - result = sensor->ops->active(client, 0, 0); + result = sensor->ops->active(client, 0, sensor->pdata->poll_delay_ms); if (result) { GSE_LOG("%s:line=%d,error\n", __func__, __LINE__); return result; @@ -1237,7 +1251,7 @@ static int sensor_init(struct i2c_client *client) return result; } - result = sensor->ops->active(client, 1, MC3230_RATE_32); + result = sensor->ops->active(client, 1, 31); if (result) { GSE_LOG("%s:line=%d,error\n", __func__, __LINE__); return result; @@ -1275,7 +1289,7 @@ static struct sensor_operate gsensor_ops = { .ctrl_reg = MC32X0_Mode_Feature_REG, /* intterupt status register */ .int_status_reg = MC32X0_Interrupt_Enable_REG, - .range = {-MC3230_RANGE, MC3230_RANGE}, + .range = {-32768, 32768}, .trig = (IRQF_TRIGGER_HIGH | IRQF_ONESHOT), .active = sensor_active, .init = sensor_init, diff --git a/drivers/input/sensors/accel/mpu6500_acc.c b/drivers/input/sensors/accel/mpu6500_acc.c index 07a807ffedff..004679133da9 100644 --- a/drivers/input/sensors/accel/mpu6500_acc.c +++ b/drivers/input/sensors/accel/mpu6500_acc.c @@ -32,122 +32,147 @@ #include #include +static int mpu6500_set_lpf(struct i2c_client *client, int rate) +{ + const short hz[] = {184, 98, 41, 20, 10, 5}; + const int d[] = {DLPF_CFG_184HZ, DLPF_CFG_98HZ, + DLPF_CFG_41HZ, DLPF_CFG_20HZ, + DLPF_CFG_10HZ, DLPF_CFG_5HZ}; + int i, h, data, result; + h = (rate >> 1); + i = 0; + while ((h < hz[i]) && (i < ARRAY_SIZE(d) - 1)) + i++; + data = d[i]; + + result = sensor_write_reg(client, MPU6500_CONFIG, data); + if (result) + return -1; + + return 0; +} + +static int mpu6500_set_rate(struct i2c_client *client, int rate) +{ + u8 data; + int result; + u16 fifo_rate; + + if ((rate < 1) || (rate > 250)) + return -1; + + data = rate - 1; + result = sensor_write_reg(client, MPU6500_SMPLRT_DIV, data); + if (result) + return result; + + fifo_rate = 1000 / rate; + + result = mpu6500_set_lpf(client, fifo_rate); + if (result) + return -1; + + return 0; +} static int sensor_active(struct i2c_client *client, int enable, int rate) { struct sensor_private_data *sensor = - (struct sensor_private_data *) i2c_get_clientdata(client); + (struct sensor_private_data *) i2c_get_clientdata(client); int result = 0; int status = 0; - u8 pwrm1 = 0; - + u8 pwrm1 = 0; + sensor->ops->ctrl_data = sensor_read_reg(client, sensor->ops->ctrl_reg); pwrm1 = sensor_read_reg(client, MPU6500_PWR_MGMT_1); - //关闭 - if(!enable) - { - status = BIT_ACCEL_STBY; - sensor->ops->ctrl_data |= status; - //gyro和acc都不工作时，模块进入休眠 - if(sensor->ops->ctrl_data && (BIT_ACCEL_STBY | BIT_GYRO_STBY) != 0) - { + + if (!enable) { + status = BIT_ACCEL_STBY; + sensor->ops->ctrl_data |= status; + if (sensor->ops->ctrl_data && (BIT_ACCEL_STBY | BIT_GYRO_STBY) != 0) { pwrm1 |= MPU6500_PWRM1_SLEEP; } - } - else//打开 - { - status = ~BIT_ACCEL_STBY; + } else { + status = ~BIT_ACCEL_STBY; sensor->ops->ctrl_data &= status; - pwrm1 &=~MPU6500_PWRM1_SLEEP; + pwrm1 &= ~MPU6500_PWRM1_SLEEP; + + mpu6500_set_rate(client, rate); } result = sensor_write_reg(client, sensor->ops->ctrl_reg, sensor->ops->ctrl_data); - if(result) - { - printk("%s:fail to set pwrm2\n",__func__); + if (result) { + dev_err(&client->dev, "%s:fail to set pwrm2\n", __func__); return -1; } msleep(20); - result = sensor_write_reg(client, MPU6500_PWR_MGMT_1,pwrm1); - if(result) - { - printk("%s:fail to set pwrm1\n",__func__); + result = sensor_write_reg(client, MPU6500_PWR_MGMT_1, pwrm1); + if (result) { + dev_err(&client->dev, "%s:fail to set pwrm1\n", __func__); return -1; } - msleep(20); - + msleep(100); + return result; - } static int sensor_init(struct i2c_client *client) { - int res=0; + int res = 0; u8 read_data = 0; struct sensor_private_data *sensor = - (struct sensor_private_data *) i2c_get_clientdata(client); + (struct sensor_private_data *) i2c_get_clientdata(client); - //检测IC是否为MPU6500 - read_data = sensor_read_reg(client,sensor->ops->id_reg); - - if(read_data != sensor->ops->id_data) - { - printk("%s:check id err,read_data:%d,ops->id_data:%d\n",__func__,read_data,sensor->ops->id_data); + read_data = sensor_read_reg(client, sensor->ops->id_reg); + + if (read_data != sensor->ops->id_data) { + dev_err(&client->dev, "%s:check id err,read_data:%d,ops->id_data:%d\n", __func__, read_data, sensor->ops->id_data); return -1; } - - //寄存器初始化 - res = sensor_write_reg(client, MPU6500_PWR_MGMT_1,0x80); - if (res) - { - printk("set MPU6500_PWR_MGMT_1 error,res: %d!\n", res); + + res = sensor_write_reg(client, MPU6500_PWR_MGMT_1, 0x80); + if (res) { + dev_err(&client->dev, "set MPU6500_PWR_MGMT_1 error,res: %d!\n", res); return res; } msleep(40); - res = sensor_write_reg(client, MPU6500_GYRO_CONFIG,0x18); //config gyro for 2000dps - if (res) - { - printk("set MPU6500_GYRO_CONFIG error,res: %d!\n", res); + res = sensor_write_reg(client, MPU6500_GYRO_CONFIG, 0x18); + if (res) { + dev_err(&client->dev, "set MPU6500_GYRO_CONFIG error,res: %d!\n", res); return res; } msleep(10); - res = sensor_write_reg(client, MPU6500_ACCEL_CONFIG,0x00); //config Accel for +_2G - if (res) - { - printk("set MPU6500_ACCEL_CONFIG error,res: %d!\n", res); + res = sensor_write_reg(client, MPU6500_ACCEL_CONFIG, 0x00); + if (res) { + dev_err(&client->dev, "set MPU6500_ACCEL_CONFIG error,res: %d!\n", res); return res; } msleep(10); - res = sensor_write_reg(client, MPU6500_ACCEL_CONFIG2,0x00); - if (res) - { - printk("set MPU6500_ACCEL_CONFIG2 error,res: %d!\n", res); + res = sensor_write_reg(client, MPU6500_ACCEL_CONFIG2, 0x00); + if (res) { + dev_err(&client->dev, "set MPU6500_ACCEL_CONFIG2 error,res: %d!\n", res); return res; } - res = sensor_write_reg(client, MPU6500_PWR_MGMT_2,0x3F); //set accl and gyro all axis into standby mode - if (res) - { - printk("set MPU6500_PWR_MGMT_2 error,res: %d!\n", res); + res = sensor_write_reg(client, MPU6500_PWR_MGMT_2, 0x3F); + if (res) { + dev_err(&client->dev, "set MPU6500_PWR_MGMT_2 error,res: %d!\n", res); return res; - } - msleep(10); - res = sensor_write_reg(client, MPU6500_PWR_MGMT_1,0x41); - if (res) - { - printk("set MPU6500_PWR_MGMT_1 error,res: %d!\n", res); + } + msleep(10); + res = sensor_write_reg(client, MPU6500_PWR_MGMT_1, 0x41); + if (res) { + dev_err(&client->dev, "set MPU6500_PWR_MGMT_1 error,res: %d!\n", res); return res; - } - msleep(10); + } + msleep(10); - //默认关闭 - res = sensor->ops->active(client,0,0); - if(res) - { - printk("%s:line=%d,error\n",__func__,__LINE__); + res = sensor->ops->active(client, 0, sensor->pdata->poll_delay_ms); + if (res) { + dev_err(&client->dev, "%s:line=%d,error\n", __func__, __LINE__); return res; } return res; @@ -156,131 +181,105 @@ static int sensor_init(struct i2c_client *client) static int gsensor_report_value(struct i2c_client *client, struct sensor_axis *axis) { struct sensor_private_data *sensor = - (struct sensor_private_data *) i2c_get_clientdata(client); + (struct sensor_private_data *) i2c_get_clientdata(client); - /* Report acceleration sensor information */ - input_report_abs(sensor->input_dev, ABS_X, axis->x); - input_report_abs(sensor->input_dev, ABS_Y, axis->y); - input_report_abs(sensor->input_dev, ABS_Z, axis->z); - input_sync(sensor->input_dev); - DBG("Gsensor x==%d y==%d z==%d\n",axis->x,axis->y,axis->z); + if (sensor->status_cur == SENSOR_ON) { + /* Report acceleration sensor information */ + input_report_abs(sensor->input_dev, ABS_X, axis->x); + input_report_abs(sensor->input_dev, ABS_Y, axis->y); + input_report_abs(sensor->input_dev, ABS_Z, axis->z); + input_sync(sensor->input_dev); + } return 0; } -#define GSENSOR_MIN 10 static int sensor_report_value(struct i2c_client *client) { struct sensor_private_data *sensor = - (struct sensor_private_data *) i2c_get_clientdata(client); + (struct sensor_private_data *) i2c_get_clientdata(client); struct sensor_platform_data *pdata = sensor->pdata; int ret = 0; - short x,y,z; + short x, y, z; struct sensor_axis axis; - u8 buffer[6] = {0}; + u8 buffer[6] = {0}; char value = 0; - - if(sensor->ops->read_len < 6) //sensor->ops->read_len = 6 - { - printk("%s:lenth is error,len=%d\n",__func__,sensor->ops->read_len); + + if (sensor->ops->read_len < 6) { + dev_err(&client->dev, "%s:lenth is error,len=%d\n", __func__, sensor->ops->read_len); return -1; } - + memset(buffer, 0, 6); - - /* Data bytes from hardware xL, xH, yL, yH, zL, zH */ + + /* Data bytes from hardware xL, xH, yL, yH, zL, zH */ do { *buffer = sensor->ops->read_reg; ret = sensor_rx_data(client, buffer, sensor->ops->read_len); if (ret < 0) - return ret; + return ret; } while (0); x = ((buffer[0] << 8) & 0xff00) + (buffer[1] & 0xFF); y = ((buffer[2] << 8) & 0xff00) + (buffer[3] & 0xFF); z = ((buffer[4] << 8) & 0xff00) + (buffer[5] & 0xFF); - /* - printk("mpu6500_acc: x:%d,y:%d,z:%d,-4:%d\n",x,y,z,-4); - printk("mpu6500_acc:orientation:\n%d %d %d\n%d %d %d\n%d %d %d\n",pdata->orientation[0], - pdata->orientation[1],pdata->orientation[2],pdata->orientation[3],pdata->orientation[4], - pdata->orientation[5],pdata->orientation[6],pdata->orientation[7],pdata->orientation[8]); - */ - axis.x = (pdata->orientation[0])*x + (pdata->orientation[1])*y + (pdata->orientation[2])*z; - axis.y = (pdata->orientation[3])*x + (pdata->orientation[4])*y + (pdata->orientation[5])*z; - axis.z = (pdata->orientation[6])*x + (pdata->orientation[7])*y + (pdata->orientation[8])*z; - //为了不修改hal层代码，数据转换后上报 - axis.x = 61*axis.x; - axis.y = 61*axis.y; - axis.z = 61*axis.z; - - //if((abs(sensor->axis.x - axis.x) > GSENSOR_MIN) || (abs(sensor->axis.y - axis.y) > GSENSOR_MIN) || (abs(sensor->axis.z - axis.z) > GSENSOR_MIN)) - { - gsensor_report_value(client, &axis); + axis.x = (pdata->orientation[0]) * x + (pdata->orientation[1]) * y + (pdata->orientation[2]) * z; + axis.y = (pdata->orientation[3]) * x + (pdata->orientation[4]) * y + (pdata->orientation[5]) * z; + axis.z = (pdata->orientation[6]) * x + (pdata->orientation[7]) * y + (pdata->orientation[8]) * z; - /* 互斥地缓存数据. */ - mutex_lock(&(sensor->data_mutex) ); - sensor->axis = axis; - mutex_unlock(&(sensor->data_mutex) ); - } + gsensor_report_value(client, &axis); - if((sensor->pdata->irq_enable)&& (sensor->ops->int_status_reg >= 0)) //read sensor intterupt status register - { - + mutex_lock(&(sensor->data_mutex)); + sensor->axis = axis; + mutex_unlock(&(sensor->data_mutex)); + + if ((sensor->pdata->irq_enable) && (sensor->ops->int_status_reg >= 0)) value = sensor_read_reg(client, sensor->ops->int_status_reg); - DBG("%s:sensor int status :0x%x\n",__func__,value); - } - + return ret; } - struct sensor_operate gsensor_mpu6500_ops = { .name = "mpu6500_acc", - .type = SENSOR_TYPE_ACCEL, //sensor type and it should be correct - .id_i2c = ACCEL_ID_MPU6500, //i2c id number - .read_reg = MPU6500_ACCEL_XOUT_H, //read data - .read_len = 6, //data length - .id_reg = MPU6500_WHOAMI, //read device id from this register - .id_data = MPU6500_DEVICE_ID, //device id - .precision = MPU6500_PRECISION, //16 bit - .ctrl_reg = MPU6500_PWR_MGMT_2, //enable or disable - .int_status_reg = MPU6500_INT_STATUS, //intterupt status register - .range = {-32768*61,32768*61}, //range - .trig = IRQF_TRIGGER_HIGH |IRQF_ONESHOT, - .active = sensor_active, - .init = sensor_init, - .report = sensor_report_value, + .type = SENSOR_TYPE_ACCEL, + .id_i2c = ACCEL_ID_MPU6500, + .read_reg = MPU6500_ACCEL_XOUT_H, + .read_len = 6, + .id_reg = MPU6500_WHOAMI, + .id_data = MPU6500_DEVICE_ID, + .precision = MPU6500_PRECISION, + .ctrl_reg = MPU6500_PWR_MGMT_2, + .int_status_reg = MPU6500_INT_STATUS, + .range = {-32768, 32768}, + .trig = IRQF_TRIGGER_HIGH |IRQF_ONESHOT, + .active = sensor_active, + .init = sensor_init, + .report = sensor_report_value, }; /****************operate according to sensor chip:end************/ -//function name should not be changed static struct sensor_operate *gsensor_get_ops(void) { return &gsensor_mpu6500_ops; } - static int __init gsensor_mpu6500_init(void) { struct sensor_operate *ops = gsensor_get_ops(); - int result = 0; int type = ops->type; - result = sensor_register_slave(type, NULL, NULL, gsensor_get_ops); - return result; + + return sensor_register_slave(type, NULL, NULL, gsensor_get_ops); } static void __exit gsensor_mpu6500_exit(void) { struct sensor_operate *ops = gsensor_get_ops(); int type = ops->type; + sensor_unregister_slave(type, NULL, NULL, gsensor_get_ops); } - module_init(gsensor_mpu6500_init); module_exit(gsensor_mpu6500_exit); - - - diff --git a/drivers/input/sensors/accel/mpu6880_acc.c b/drivers/input/sensors/accel/mpu6880_acc.c index 0b5ca796aff0..c85d607b8034 100644 --- a/drivers/input/sensors/accel/mpu6880_acc.c +++ b/drivers/input/sensors/accel/mpu6880_acc.c @@ -32,121 +32,145 @@ #include #include +static int mpu6880_set_lpf(struct i2c_client *client, int rate) +{ + const short hz[] = {184, 98, 41, 20, 10, 5}; + const int d[] = {DLPF_CFG_184HZ, DLPF_CFG_98HZ, + DLPF_CFG_41HZ, DLPF_CFG_20HZ, + DLPF_CFG_10HZ, DLPF_CFG_5HZ}; + int i, h, data, result; + h = (rate >> 1); + i = 0; + while ((h < hz[i]) && (i < ARRAY_SIZE(d) - 1)) + i++; + data = d[i]; + + result = sensor_write_reg(client, MPU6880_CONFIG, data); + if (result) + return -1; + + return 0; +} + +static int mpu6880_set_rate(struct i2c_client *client, int rate) +{ + u8 data; + int result; + u16 fifo_rate; + + if ((rate < 1) || (rate > 250)) + return -1; + + data = rate - 1; + result = sensor_write_reg(client, MPU6880_SMPLRT_DIV, data); + if (result) + return result; + + fifo_rate = 1000 / rate; + + result = mpu6880_set_lpf(client, fifo_rate); + if (result) + return -1; + + return 0; +} static int sensor_active(struct i2c_client *client, int enable, int rate) { struct sensor_private_data *sensor = - (struct sensor_private_data *) i2c_get_clientdata(client); + (struct sensor_private_data *) i2c_get_clientdata(client); int result = 0; int status = 0; - u8 pwrm1 = 0; - + u8 pwrm1 = 0; + sensor->ops->ctrl_data = sensor_read_reg(client, sensor->ops->ctrl_reg); pwrm1 = sensor_read_reg(client, MPU6880_PWR_MGMT_1); - //关闭 - if(!enable) - { - status = BIT_ACCEL_STBY; - sensor->ops->ctrl_data |= status; - //gyro和acc都不工作时，模块进入休眠 - if(sensor->ops->ctrl_data && (BIT_ACCEL_STBY | BIT_GYRO_STBY) != 0) - { + + if (!enable) { + status = BIT_ACCEL_STBY; + sensor->ops->ctrl_data |= status; + if (sensor->ops->ctrl_data && (BIT_ACCEL_STBY | BIT_GYRO_STBY) != 0) pwrm1 |= MPU6880_PWRM1_SLEEP; - } - } - else//打开 - { - status = ~BIT_ACCEL_STBY; + } else { + status = ~BIT_ACCEL_STBY; sensor->ops->ctrl_data &= status; - pwrm1 &=~MPU6880_PWRM1_SLEEP; + pwrm1 &= ~MPU6880_PWRM1_SLEEP; + + mpu6880_set_rate(client, rate); } result = sensor_write_reg(client, sensor->ops->ctrl_reg, sensor->ops->ctrl_data); - if(result) - { - printk("%s:fail to set pwrm2\n",__func__); + if (result) { + dev_err(&client->dev, "%s:fail to set pwrm2\n", __func__); return -1; } msleep(20); - result = sensor_write_reg(client, MPU6880_PWR_MGMT_1,pwrm1); - if(result) - { - printk("%s:fail to set pwrm1\n",__func__); + result = sensor_write_reg(client, MPU6880_PWR_MGMT_1, pwrm1); + if (result) { + dev_err(&client->dev, "%s:fail to set pwrm1\n", __func__); return -1; } - msleep(20); - + msleep(100); + return result; - } static int sensor_init(struct i2c_client *client) { - int res=0; + int res = 0; u8 read_data = 0; struct sensor_private_data *sensor = - (struct sensor_private_data *) i2c_get_clientdata(client); + (struct sensor_private_data *) i2c_get_clientdata(client); - //检测IC是否为MPU6880 - read_data = sensor_read_reg(client,sensor->ops->id_reg); - if(read_data != sensor->ops->id_data) - { - printk("%s:check id err,read_data:%d,ops->id_data:%d\n",__func__,read_data,sensor->ops->id_data); + read_data = sensor_read_reg(client, sensor->ops->id_reg); + if (read_data != sensor->ops->id_data) { + dev_err(&client->dev, "%s:check id err,read_data:%d,ops->id_data:%d\n", __func__, read_data, sensor->ops->id_data); return -1; } - - //寄存器初始化 - res = sensor_write_reg(client, MPU6880_PWR_MGMT_1,0x80); - if (res) - { - printk("set MPU6880_PWR_MGMT_1 error,res: %d!\n", res); + + res = sensor_write_reg(client, MPU6880_PWR_MGMT_1, 0x80); + if (res) { + dev_err(&client->dev, "set MPU6880_PWR_MGMT_1 error,res: %d!\n", res); return res; } msleep(40); - res = sensor_write_reg(client, MPU6880_GYRO_CONFIG,0x18); //config gyro for 2000dps - if (res) - { - printk("set MPU6880_GYRO_CONFIG error,res: %d!\n", res); + res = sensor_write_reg(client, MPU6880_GYRO_CONFIG, 0x18); + if (res) { + dev_err(&client->dev, "set MPU6880_GYRO_CONFIG error,res: %d!\n", res); return res; } msleep(10); - res = sensor_write_reg(client, MPU6880_ACCEL_CONFIG,0x00); //config Accel for +_2G - if (res) - { - printk("set MPU6880_ACCEL_CONFIG error,res: %d!\n", res); + res = sensor_write_reg(client, MPU6880_ACCEL_CONFIG, 0x00); + if (res) { + dev_err(&client->dev, "set MPU6880_ACCEL_CONFIG error,res: %d!\n", res); return res; } msleep(10); - res = sensor_write_reg(client, MPU6880_ACCEL_CONFIG2,0x00); - if (res) - { - printk("set MPU6880_ACCEL_CONFIG2 error,res: %d!\n", res); + res = sensor_write_reg(client, MPU6880_ACCEL_CONFIG2, 0x00); + if (res) { + dev_err(&client->dev, "set MPU6880_ACCEL_CONFIG2 error,res: %d!\n", res); return res; } - res = sensor_write_reg(client, MPU6880_PWR_MGMT_2,0x3F); //set accl and gyro all axis into standby mode - if (res) - { - printk("set MPU6880_PWR_MGMT_2 error,res: %d!\n", res); + res = sensor_write_reg(client, MPU6880_PWR_MGMT_2, 0x3F); + if (res) { + dev_err(&client->dev, "set MPU6880_PWR_MGMT_2 error,res: %d!\n", res); return res; - } - msleep(10); - res = sensor_write_reg(client, MPU6880_PWR_MGMT_1,0x41); - if (res) - { - printk("set MPU6880_PWR_MGMT_1 error,res: %d!\n", res); + } + msleep(10); + res = sensor_write_reg(client, MPU6880_PWR_MGMT_1, 0x41); + if (res) { + dev_err(&client->dev, "set MPU6880_PWR_MGMT_1 error,res: %d!\n", res); return res; - } - msleep(10); + } + msleep(10); - //默认关闭 - res = sensor->ops->active(client,0,0); - if(res) - { - printk("%s:line=%d,error\n",__func__,__LINE__); + res = sensor->ops->active(client, 0, sensor->pdata->poll_delay_ms); + if (res) { + dev_err(&client->dev, "%s:line=%d,error\n", __func__, __LINE__); return res; } return res; @@ -155,130 +179,103 @@ static int sensor_init(struct i2c_client *client) static int gsensor_report_value(struct i2c_client *client, struct sensor_axis *axis) { struct sensor_private_data *sensor = - (struct sensor_private_data *) i2c_get_clientdata(client); + (struct sensor_private_data *) i2c_get_clientdata(client); - /* Report acceleration sensor information */ - input_report_abs(sensor->input_dev, ABS_X, axis->x); - input_report_abs(sensor->input_dev, ABS_Y, axis->y); - input_report_abs(sensor->input_dev, ABS_Z, axis->z); - input_sync(sensor->input_dev); - DBG("Gsensor x==%d y==%d z==%d\n",axis->x,axis->y,axis->z); + if (sensor->status_cur == SENSOR_ON) { + /* Report acceleration sensor information */ + input_report_abs(sensor->input_dev, ABS_X, axis->x); + input_report_abs(sensor->input_dev, ABS_Y, axis->y); + input_report_abs(sensor->input_dev, ABS_Z, axis->z); + input_sync(sensor->input_dev); + } return 0; } -#define GSENSOR_MIN 10 static int sensor_report_value(struct i2c_client *client) { struct sensor_private_data *sensor = - (struct sensor_private_data *) i2c_get_clientdata(client); + (struct sensor_private_data *) i2c_get_clientdata(client); struct sensor_platform_data *pdata = sensor->pdata; int ret = 0; - short x,y,z; + short x, y, z; struct sensor_axis axis; - u8 buffer[6] = {0}; + u8 buffer[6] = {0}; char value = 0; - - if(sensor->ops->read_len < 6) //sensor->ops->read_len = 6 - { - printk("%s:lenth is error,len=%d\n",__func__,sensor->ops->read_len); + + if (sensor->ops->read_len < 6) { + dev_err(&client->dev, "%s:lenth is error,len=%d\n", __func__, sensor->ops->read_len); return -1; } - + memset(buffer, 0, 6); - - /* Data bytes from hardware xL, xH, yL, yH, zL, zH */ + + /* Data bytes from hardware xL, xH, yL, yH, zL, zH */ do { *buffer = sensor->ops->read_reg; ret = sensor_rx_data(client, buffer, sensor->ops->read_len); if (ret < 0) - return ret; + return ret; } while (0); x = ((buffer[0] << 8) & 0xff00) + (buffer[1] & 0xFF); y = ((buffer[2] << 8) & 0xff00) + (buffer[3] & 0xFF); z = ((buffer[4] << 8) & 0xff00) + (buffer[5] & 0xFF); - - printk("mpu6880_acc: x:%d,y:%d,z:%d,-4:%d\n",x,y,z,-4); - printk("mpu6880_acc:orientation:\n%d %d %d\n%d %d %d\n%d %d %d\n",pdata->orientation[0], - pdata->orientation[1],pdata->orientation[2],pdata->orientation[3],pdata->orientation[4], - pdata->orientation[5],pdata->orientation[6],pdata->orientation[7],pdata->orientation[8]); - axis.x = (pdata->orientation[0])*x + (pdata->orientation[1])*y + (pdata->orientation[2])*z; - axis.y = (pdata->orientation[3])*x + (pdata->orientation[4])*y + (pdata->orientation[5])*z; - axis.z = (pdata->orientation[6])*x + (pdata->orientation[7])*y + (pdata->orientation[8])*z; + axis.x = (pdata->orientation[0]) * x + (pdata->orientation[1]) * y + (pdata->orientation[2]) * z; + axis.y = (pdata->orientation[3]) * x + (pdata->orientation[4]) * y + (pdata->orientation[5]) * z; + axis.z = (pdata->orientation[6]) * x + (pdata->orientation[7]) * y + (pdata->orientation[8]) * z; - //为了不修改hal层代码，数据转换后上报 - axis.x = 61*axis.x; - axis.y = 61*axis.y; - axis.z = 61*axis.z; - - //if((abs(sensor->axis.x - axis.x) > GSENSOR_MIN) || (abs(sensor->axis.y - axis.y) > GSENSOR_MIN) || (abs(sensor->axis.z - axis.z) > GSENSOR_MIN)) - { - gsensor_report_value(client, &axis); + gsensor_report_value(client, &axis); - /* 互斥地缓存数据. */ - mutex_lock(&(sensor->data_mutex) ); - sensor->axis = axis; - mutex_unlock(&(sensor->data_mutex) ); - } + mutex_lock(&(sensor->data_mutex)); + sensor->axis = axis; + mutex_unlock(&(sensor->data_mutex)); - if((sensor->pdata->irq_enable)&& (sensor->ops->int_status_reg >= 0)) //read sensor intterupt status register - { - + if ((sensor->pdata->irq_enable) && (sensor->ops->int_status_reg >= 0)) value = sensor_read_reg(client, sensor->ops->int_status_reg); - DBG("%s:sensor int status :0x%x\n",__func__,value); - } - + return ret; } - struct sensor_operate gsensor_mpu6880_ops = { .name = "mpu6880_acc", - .type = SENSOR_TYPE_ACCEL, //sensor type and it should be correct - .id_i2c = ACCEL_ID_MPU6880, //i2c id number - .read_reg = MPU6880_ACCEL_XOUT_H, //read data - .read_len = 6, //data length - .id_reg = MPU6880_WHOAMI, //read device id from this register - .id_data = MPU6880_DEVICE_ID, //device id - .precision = MPU6880_PRECISION, //16 bit - .ctrl_reg = MPU6880_PWR_MGMT_2, //enable or disable - .int_status_reg = MPU6880_INT_STATUS, //intterupt status register - .range = {-32768*61,32768*61}, //range - .trig = IRQF_TRIGGER_HIGH |IRQF_ONESHOT, - .active = sensor_active, - .init = sensor_init, - .report = sensor_report_value, + .type = SENSOR_TYPE_ACCEL, + .id_i2c = ACCEL_ID_MPU6880, + .read_reg = MPU6880_ACCEL_XOUT_H, + .read_len = 6, + .id_reg = MPU6880_WHOAMI, + .id_data = MPU6880_DEVICE_ID, + .precision = MPU6880_PRECISION, + .ctrl_reg = MPU6880_PWR_MGMT_2, + .int_status_reg = MPU6880_INT_STATUS, + .range = {-32768, 32768}, + .trig = IRQF_TRIGGER_HIGH | IRQF_ONESHOT, + .active = sensor_active, + .init = sensor_init, + .report = sensor_report_value, }; /****************operate according to sensor chip:end************/ - -//function name should not be changed static struct sensor_operate *gsensor_get_ops(void) { return &gsensor_mpu6880_ops; } - static int __init gsensor_mpu6880_init(void) { struct sensor_operate *ops = gsensor_get_ops(); - int result = 0; int type = ops->type; - result = sensor_register_slave(type, NULL, NULL, gsensor_get_ops); - return result; + + return sensor_register_slave(type, NULL, NULL, gsensor_get_ops); } static void __exit gsensor_mpu6880_exit(void) { struct sensor_operate *ops = gsensor_get_ops(); int type = ops->type; + sensor_unregister_slave(type, NULL, NULL, gsensor_get_ops); } - module_init(gsensor_mpu6880_init); module_exit(gsensor_mpu6880_exit); - - -