diff --git a/drivers/input/sensors/accel/mma7660.c b/drivers/input/sensors/accel/mma7660.c index 59f3060bb402..77dad66141cb 100644 --- a/drivers/input/sensors/accel/mma7660.c +++ b/drivers/input/sensors/accel/mma7660.c @@ -31,129 +31,95 @@ #endif #include - -#define MMA7660_ENABLE 1 - -#define MMA7660_REG_X_OUT 0x0 -#define MMA7660_REG_Y_OUT 0x1 -#define MMA7660_REG_Z_OUT 0x2 -#define MMA7660_REG_TILT 0x3 -#define MMA7660_REG_SRST 0x4 -#define MMA7660_REG_SPCNT 0x5 -#define MMA7660_REG_INTSU 0x6 -#define MMA7660_REG_MODE 0x7 -#define MMA7660_REG_SR 0x8 -#define MMA7660_REG_PDET 0x9 -#define MMA7660_REG_PD 0xa - - -#define MMA7660_RANGE 1500000 - -/* LIS3DH */ -#define MMA7660_PRECISION 6 -#define MMA7660_BOUNDARY (0x1 << (MMA7660_PRECISION - 1)) -#define MMA7660_GRAVITY_STEP (MMA7660_RANGE / MMA7660_BOUNDARY) - -#define MMA7660_COUNT_AVERAGE 2 - -struct sensor_axis_average { - int x_average; - int y_average; - int z_average; - int count; -}; - -static struct sensor_axis_average axis_average; +#define MMA7660_ENABLE 1 +#define MMA7660_REG_X_OUT 0x0 +#define MMA7660_REG_Y_OUT 0x1 +#define MMA7660_REG_Z_OUT 0x2 +#define MMA7660_REG_TILT 0x3 +#define MMA7660_REG_SRST 0x4 +#define MMA7660_REG_SPCNT 0x5 +#define MMA7660_REG_INTSU 0x6 +#define MMA7660_REG_MODE 0x7 +#define MMA7660_REG_SR 0x8 +#define MMA7660_REG_PDET 0x9 +#define MMA7660_REG_PD 0xa +#define MMA7660_PRECISION 6 /****************operate according to sensor chip:start************/ 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); - - //register setting according to chip datasheet - if(enable) - { - status = MMA7660_ENABLE; //mma7660 + + if (enable) { + status = MMA7660_ENABLE; sensor->ops->ctrl_data |= status; - } - else - { - status = ~MMA7660_ENABLE; //mma7660 + } else { + status = ~MMA7660_ENABLE; 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; - - result = sensor->ops->active(client,0,0); - if(result) - { - printk("%s:line=%d,error\n",__func__,__LINE__); + + result = sensor->ops->active(client, 0, 0); + if (result) { + dev_err(&client->dev, "%s:line=%d,error\n", __func__, __LINE__); return result; } - + sensor->status_cur = SENSOR_OFF; - DBG("%s:MMA7660_REG_TILT=0x%x\n",__func__,sensor_read_reg(client, MMA7660_REG_TILT)); - - result = sensor_write_reg(client, MMA7660_REG_SR, (0x01<<5)| 0x02); //32 Samples/Second Active and Auto-Sleep Mode - if(result) - { - printk("%s:line=%d,error\n",__func__,__LINE__); + /*120 Samples/Second Active and Auto-Sleep Mode */ + result = sensor_write_reg(client, MMA7660_REG_SR, 0x01 << 5); + if (result) { + dev_err(&client->dev, "%s:line=%d,error\n", __func__, __LINE__); return result; } - if(sensor->pdata->irq_enable) //open interrupt - { - result = sensor_write_reg(client, MMA7660_REG_INTSU, 1<<4);//enable int,GINT=1 - if(result) - { - printk("%s:line=%d,error\n",__func__,__LINE__); + if (sensor->pdata->irq_enable) { + result = sensor_write_reg(client, MMA7660_REG_INTSU, 1 << 4); + if (result) { + dev_err(&client->dev, "%s:line=%d,error\n", __func__, __LINE__); return result; } } - - sensor->ops->ctrl_data = 1<<6; //Interrupt output INT is push-pull + + sensor->ops->ctrl_data = 1 << 6; result = sensor_write_reg(client, sensor->ops->ctrl_reg, sensor->ops->ctrl_data); - 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; } - - memset(&axis_average, 0, sizeof(struct sensor_axis_average)); - return result; } - static int sensor_convert_data(struct i2c_client *client, char high_byte, char low_byte) { - s64 result; + int result = (int)low_byte; - result = (int)low_byte; - result *= 768; + if (low_byte & 0x20) + result = ((~result & 0x1f) + 1) * (-768); + else + result = (result & 0x1f) * 768; - return (int)result; + return result; } static int gsensor_report_value(struct i2c_client *client, struct sensor_axis *axis) @@ -172,43 +138,58 @@ static int gsensor_report_value(struct i2c_client *client, struct sensor_axis *a return 0; } -#define GSENSOR_MIN 2 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; - int x,y,z; + int x, y, z; struct sensor_axis axis; - char buffer[3] = {0}; + char buffer[3] = {0}; char value = 0; - - if(sensor->ops->read_len < 3) //sensor->ops->read_len = 3 - { - printk("%s:lenth is error,len=%d\n",__func__,sensor->ops->read_len); + static int flag; + + if (sensor->ops->read_len < 3) { + dev_err(&client->dev, "%s:lenth is error,len=%d\n", __func__, sensor->ops->read_len); return -1; } - + memset(buffer, 0, 3); - - /* 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, 0, buffer[0]); //buffer[1]:high bit + x = sensor_convert_data(sensor->client, 0, buffer[0]); y = sensor_convert_data(sensor->client, 0, buffer[1]); - z = sensor_convert_data(sensor->client, 0, buffer[2]); + z = sensor_convert_data(sensor->client, 0, buffer[2]); - 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; + + /* + *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; + } + } gsensor_report_value(client, &axis); @@ -216,17 +197,12 @@ static int sensor_report_value(struct i2c_client *client) 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_mma7660_ops = { .name = "mma7660", .type = SENSOR_TYPE_ACCEL, @@ -238,7 +214,7 @@ struct sensor_operate gsensor_mma7660_ops = { .precision = MMA7660_PRECISION, .ctrl_reg = MMA7660_REG_MODE, .int_status_reg = SENSOR_UNKNOW_DATA, - .range = {-24576, 24576}, + .range = {-24576, 24576}, .trig = IRQF_TRIGGER_LOW | IRQF_ONESHOT, .active = sensor_active, .init = sensor_init, @@ -253,26 +229,21 @@ static struct sensor_operate *gsensor_get_ops(void) return &gsensor_mma7660_ops; } - static int __init gsensor_mma7660_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_mma7660_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_mma7660_init); module_exit(gsensor_mma7660_exit); - - -