static int read_accel_raw_xyz(s16 *x, s16 *y, s16 *z)
{
u8 regs[6];
s16 raw[3], orien_raw[3];
int i = 2;
int result;
result =
mpu6050_i2c_read_reg(gb_mpu_data->client,
MPUREG_ACCEL_XOUT_H, 6, regs);
raw[0] = ((s16) ((s16) regs[0] << 8)) | regs[1];
raw[1] = ((s16) ((s16) regs[2] << 8)) | regs[3];
raw[2] = ((s16) ((s16) regs[4] << 8)) | regs[5];
do {
orien_raw[i] = gb_mpu_data->pdata->orientation[i*3] * raw[0]
+ gb_mpu_data->pdata->orientation[i*3+1] * raw[1]
+ gb_mpu_data->pdata->orientation[i*3+2] * raw[2];
} while (i-- != 0);
pr_info("%s : x = %d, y = %d, z = %d\n", __func__,
orien_raw[0], orien_raw[1], orien_raw[2]);
*x = orien_raw[0];
*y = orien_raw[1];
*z = orien_raw[2];
return 0;
}
static int mpu6050_input_resume_gyro(struct mpu6050_input_data *data)
{
int result;
unsigned regs[2] = { 0, };
CHECK_RESULT(mpu6050_i2c_read_reg
(data->client, MPUREG_PWR_MGMT_1, 2, data->gyro_pwr_mgnt));
CHECK_RESULT(mpu6050_i2c_write_single_reg
(data->client, MPUREG_PWR_MGMT_1,
data->gyro_pwr_mgnt[0] & ~BIT_SLEEP));
data->gyro_pwr_mgnt[1] &= ~(BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG);
CHECK_RESULT(mpu6050_i2c_write_single_reg
(data->client, MPUREG_PWR_MGMT_2, data->gyro_pwr_mgnt[1]));
regs[0] = 3 << 3; /* 2000dps */
CHECK_RESULT(mpu6050_i2c_write_single_reg
(data->client, MPUREG_GYRO_CONFIG, regs[0]));
regs[0] = MPU_FILTER_20HZ | 0x18;
CHECK_RESULT(mpu6050_i2c_write_single_reg
(data->client, MPUREG_CONFIG, regs[0]));
return result;
}
static int mpu6050_input_set_fsr(struct mpu6050_input_data *data, int fsr)
{
unsigned char fsr_mask;
int result;
unsigned char reg;
if (fsr <= 2000) {
fsr_mask = 0x00;
} else if (fsr <= 4000) {
fsr_mask = 0x08;
} else if (fsr <= 8000) {
fsr_mask = 0x10;
} else { /* fsr = [8001, oo) */
fsr_mask = 0x18;
}
result =
mpu6050_i2c_read_reg(data->client,
MPUREG_ACCEL_CONFIG, 1, ®);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
result =
mpu6050_i2c_write_single_reg(data->client, MPUREG_ACCEL_CONFIG,
reg | fsr_mask);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
return result;
}
static void mpu6050_input_report_gyro_xyz(struct mpu6050_input_data *data)
{
u8 regs[6];
s16 raw[3], orien_raw[3];
int i = 2;
int result;
result =
mpu6050_i2c_read_reg(data->client, MPUREG_GYRO_XOUT_H, 6, regs);
raw[0] = (((s16) ((s16) regs[0] << 8)) | regs[1])
- (s16)data->gyro_bias[0];
raw[1] = (((s16) ((s16) regs[2] << 8)) | regs[3])
- (s16)data->gyro_bias[1];
raw[2] = (((s16) ((s16) regs[4] << 8)) | regs[5])
- (s16)data->gyro_bias[2];
do {
orien_raw[i] = data->pdata->orientation[i*3] * raw[0]
+ data->pdata->orientation[i*3+1] * raw[1]
+ data->pdata->orientation[i*3+2] * raw[2];
} while (i-- != 0);
input_report_rel(data->input, REL_RX, orien_raw[0]);
input_report_rel(data->input, REL_RY, orien_raw[1]);
input_report_rel(data->input, REL_RZ, orien_raw[2]);
input_sync(data->input);
}
static irqreturn_t mpu6050_input_irq_thread(int irq, void *dev)
{
struct mpu6050_input_data *data = (struct mpu6050_input_data *)dev;
unsigned char reg;
unsigned int timediff = 0;
if (!atomic_read(&data->reactive_enable)) {
#ifndef CONFIG_INPUT_MPU6050_POLLING
if (data->enabled_sensors & MPU6050_SENSOR_ACCEL)
mpu6050_input_report_accel_xyz(data);
if (data->enabled_sensors & MPU6050_SENSOR_GYRO)
mpu6050_input_report_gyro_xyz(data);
#endif
} else {
mpu6050_i2c_read_reg(data->client, MPUREG_INT_STATUS, 1, ®);
/* skip erronous interrupt in 100ms */
timediff = jiffies_to_msecs(jiffies - (data->motion_st_time));
if (timediff < 100 && !(data->factory_mode)) {
pr_info("%s: timediff = %d msec\n", __func__, timediff);
goto done;
}
if (reg & (1 << 6) || data->factory_mode) {
/*handle motion recognition*/
atomic_set(&data->reactive_state, true);
data->factory_mode = false;
pr_info("%s: motion interrupt happened\n",
__func__);
wake_lock_timeout(&data->reactive_wake_lock,
msecs_to_jiffies(2000));
}
}
done:
return IRQ_HANDLED;
}
static void mpu6050_input_report_accel_xyz(struct mpu6050_input_data *data)
{
u8 regs[6];
s16 raw[3], orien_raw[3];
int i = 2;
int result;
result =
mpu6050_i2c_read_reg(data->client,
MPUREG_ACCEL_XOUT_H, 6, regs);
raw[0] = ((s16) ((s16) regs[0] << 8)) | regs[1];
raw[1] = ((s16) ((s16) regs[2] << 8)) | regs[3];
raw[2] = ((s16) ((s16) regs[4] << 8)) | regs[5];
do {
orien_raw[i] = data->pdata->orientation[i*3] * raw[0]
+ data->pdata->orientation[i*3+1] * raw[1]
+ data->pdata->orientation[i*3+2] * raw[2];
} while (i-- != 0);
pr_debug("%s : x = %d, y = %d, z = %d\n", __func__,
orien_raw[0], orien_raw[1], orien_raw[2]);
input_report_rel(data->input, REL_X, orien_raw[0] - data->acc_cal[0]);
input_report_rel(data->input, REL_Y, orien_raw[1] - data->acc_cal[1]);
input_report_rel(data->input, REL_Z, orien_raw[2] - data->acc_cal[2]);
input_sync(data->input);
}
static int mpu6050_input_activate_devices(struct mpu6050_input_data *data,
int sensors, bool enable)
{
int result;
unsigned char reg;
struct motion_int_data *mot_data = &data->mot_data;
if (sensors & MPU6050_SENSOR_ACCEL)
CHECK_RESULT(mpu6050_input_actiave_accel(data, enable));
if (sensors & MPU6050_SENSOR_GYRO)
CHECK_RESULT(mpu6050_input_activate_gyro(data, enable));
if (data->enabled_sensors) {
CHECK_RESULT(mpu6050_set_delay(data));
} else {
CHECK_RESULT(mpu6050_input_set_irq(data, 0x0));
CHECK_RESULT(mpu6050_i2c_read_reg
(data->client, MPUREG_PWR_MGMT_1, 1, ®));
if (!(reg & BIT_SLEEP)) {
CHECK_RESULT(mpu6050_i2c_write_single_reg(data->client,
MPUREG_PWR_MGMT_1,
reg |
BIT_SLEEP));
}
if (atomic_read(&data->reactive_enable) && !mot_data->is_set) {
usleep_range(5000, 5100);
mpu6050_input_set_motion_interrupt(data, true, false);
}
}
return result;
}
static int mpu6050_gyro_self_test(struct i2c_client *client,
int *reg_avg, int *st_avg, int *ratio){
int result;
int ret_val;
int ct_shift_prod[3], st_shift_cust[3], st_shift_ratio[3], i;
u8 regs[3];
ret_val = 0;
result = mpu6050_i2c_read_reg(client, REG_ST_GCT_X, 3, regs);
if (result)
return result;
regs[0] &= 0x1f;
regs[1] &= 0x1f;
regs[2] &= 0x1f;
for (i = 0; i < 3; i++) {
if (regs[i] != 0)
ct_shift_prod[i] = gyro_6050_st_tb[regs[i] - 1];
else
ct_shift_prod[i] = 0;
}
for (i = 0; i < 3; i++) {
st_shift_cust[i] = abs(reg_avg[i] - st_avg[i]);
if (ct_shift_prod[i]) {
st_shift_ratio[i] = abs(st_shift_cust[i] /
ct_shift_prod[i] - DEF_ST_PRECISION);
ratio[i] = st_shift_ratio[i];
if (st_shift_ratio[i] > DEF_GYRO_CT_SHIFT_DELTA)
ret_val |= 1 << i;
} else {
if (st_shift_cust[i] < DEF_ST_PRECISION *
DEF_GYRO_CT_SHIFT_MIN * DEF_SELFTEST_GYRO_SENS)
ret_val |= 1 << i;
if (st_shift_cust[i] > DEF_ST_PRECISION *
DEF_GYRO_CT_SHIFT_MAX * DEF_SELFTEST_GYRO_SENS)
ret_val |= 1 << i;
}
}
pr_info("ct_shift_prod : %d %d %d\n",
ct_shift_prod[0], ct_shift_prod[1], ct_shift_prod[2]);
pr_info("st_shift_cust : %d %d %d\n",
st_shift_cust[0], st_shift_cust[1], st_shift_cust[2]);
pr_info("st_shift_ratio : %d %d %d\n",
st_shift_ratio[0], st_shift_ratio[1], st_shift_ratio[2]);
/* check for absolute value passing criterion. Using DEF_ST_TOR
* for certain degree of tolerance */
for (i = 0; i < 3; i++) {
if (abs(reg_avg[i]) > DEF_ST_TOR * DEF_ST_ABS_THRESH *
DEF_ST_PRECISION * DEF_GYRO_SCALE)
ret_val |= (1 << i);
}
return ret_val;
}
static int mpu6050_input_set_fp_mode(struct mpu6050_input_data *data)
{
unsigned char b;
/* Resetting the cycle bit and LPA wake up freq */
mpu6050_i2c_read_reg(data->client, MPUREG_PWR_MGMT_1, 1, &b);
b &= ~BIT_CYCLE & ~BIT_PD_PTAT;
mpu6050_i2c_write_single_reg(data->client, MPUREG_PWR_MGMT_1, b);
mpu6050_i2c_read_reg(data->client, MPUREG_PWR_MGMT_2, 1, &b);
b &= ~BITS_LPA_WAKE_CTRL;
mpu6050_i2c_write_single_reg(data->client, MPUREG_PWR_MGMT_2, b);
/* Resetting the duration setting for fp mode */
b = (unsigned char)10 / ACCEL_MOT_DUR_LSB;
mpu6050_i2c_write_single_reg(data->client, MPUREG_ACCEL_MOT_DUR, b);
return 0;
}
static int mpu6050_input_resume_accel(struct mpu6050_input_data *data)
{
int result;
unsigned char reg;
CHECK_RESULT(mpu6050_i2c_read_reg
(data->client, MPUREG_PWR_MGMT_1, 1, ®));
if (reg & BIT_SLEEP) {
CHECK_RESULT(mpu6050_i2c_write_single_reg(data->client,
MPUREG_PWR_MGMT_1,
reg & ~BIT_SLEEP));
}
usleep_range(2000, 2100);
CHECK_RESULT(mpu6050_i2c_read_reg
(data->client, MPUREG_PWR_MGMT_2, 1, ®));
reg &= ~(BIT_STBY_XA | BIT_STBY_YA | BIT_STBY_ZA);
CHECK_RESULT(mpu6050_i2c_write_single_reg
(data->client, MPUREG_PWR_MGMT_2, reg));
/* settings */
/*----- LPF configuration : 44hz ---->*/
reg = MPU_FILTER_20HZ;
CHECK_RESULT(mpu6050_i2c_write_single_reg
(data->client, MPUREG_CONFIG, reg));
/*<----- LPF configuration : 44hz ---- */
CHECK_RESULT(mpu6050_i2c_read_reg
(data->client, MPUREG_ACCEL_CONFIG, 1, ®));
CHECK_RESULT(mpu6050_i2c_write_single_reg
(data->client, MPUREG_ACCEL_CONFIG, reg | 0x0));
CHECK_RESULT(mpu6050_input_set_fsr(data, 2000));
return result;
}
static int mpu6050_input_suspend_accel(struct mpu6050_input_data *data)
{
unsigned char reg;
int result;
CHECK_RESULT(mpu6050_i2c_read_reg
(data->client, MPUREG_PWR_MGMT_2, 1, ®));
reg |= (BIT_STBY_XA | BIT_STBY_YA | BIT_STBY_ZA);
CHECK_RESULT(mpu6050_i2c_write_single_reg
(data->client, MPUREG_PWR_MGMT_2, reg));
return result;
}
static int mpu6050_input_suspend_gyro(struct mpu6050_input_data *data)
{
int result;
CHECK_RESULT(mpu6050_i2c_read_reg
(data->client, MPUREG_PWR_MGMT_1, 2, data->gyro_pwr_mgnt));
data->gyro_pwr_mgnt[1] |=
(BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG);
CHECK_RESULT(mpu6050_i2c_write_single_reg
(data->client, MPUREG_PWR_MGMT_2, data->gyro_pwr_mgnt[1]));
return result;
}
static int mpu6050_do_powerup(struct i2c_client *client)
{
int result = 0;
char reg;
mpu6050_i2c_write_single_reg
(client, MPUREG_PWR_MGMT_1, 0x1);
msleep(20);
mpu6050_i2c_read_reg
(client, MPUREG_PWR_MGMT_2, 1, ®);
reg &= ~(BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG);
mpu6050_i2c_write_single_reg
(client, MPUREG_PWR_MGMT_2, reg);
return result;
}
static ssize_t mpu6050_get_temp(struct device *dev,
struct device_attribute *attr, char *buf)
{
int count = 0;
short temperature = 0;
unsigned char reg[2];
int result;
CHECK_RESULT(mpu6050_i2c_read_reg
(gb_mpu_data->client, MPUREG_TEMP_OUT_H, 2, reg));
temperature = (short) (((reg[0]) << 8) | reg[1]);
temperature = (((temperature + 521) / 340) + 35);
pr_info("read temperature = %d\n", temperature);
count = sprintf(buf, "%d\n", temperature);
return count;
}
int mpu6050_i2c_read_fifo(struct i2c_client *i2c_client,
unsigned short length, unsigned char *data)
{
int result;
unsigned short bytes_read = 0;
unsigned short this_len;
while (bytes_read < length) {
this_len = length - bytes_read;
result =
mpu6050_i2c_read_reg(i2c_client,
MPUREG_FIFO_R_W, this_len, &data[bytes_read]);
if (result)
return result;
bytes_read += this_len;
}
return 0;
}
static int mpu6050_backup_register(struct i2c_client *client)
{
int result = 0;
result =
mpu6050_i2c_read_reg(client, MPUREG_PWR_MGMT_1,
2, mpu6050_selftest.pwm_mgmt);
if (result)
return result;
result =
mpu6050_i2c_read_reg(client, MPUREG_CONFIG,
1, &mpu6050_selftest.config);
if (result)
return result;
result =
mpu6050_i2c_read_reg(client, MPUREG_GYRO_CONFIG,
1, &mpu6050_selftest.gyro_config);
if (result)
return result;
result =
mpu6050_i2c_read_reg(client, MPUREG_USER_CTRL,
1, &mpu6050_selftest.user_ctrl);
if (result)
return result;
result =
mpu6050_i2c_read_reg(client, MPUREG_INT_ENABLE,
1, &mpu6050_selftest.int_enable);
if (result)
return result;
result =
mpu6050_i2c_read_reg(client, MPUREG_SMPLRT_DIV,
1, &mpu6050_selftest.smplrt_div);
pr_info("mpu6050_backup_register : result=%d\n", result);
if (result)
return result;
return result;
}
static int mpu6050_input_set_odr(struct mpu6050_input_data *data, int odr)
{
int result;
unsigned char b;
struct motion_int_data *mot_data = &data->mot_data;
if (mot_data->is_set) {
result = 0;
goto done;
}
b = (unsigned char)(odr);
CHECK_RESULT(mpu6050_i2c_write_single_reg
(data->client, MPUREG_SMPLRT_DIV, b));
mpu6050_i2c_read_reg(data->client, MPUREG_PWR_MGMT_1, 1, &b);
b &= BIT_CYCLE;
if (b == BIT_CYCLE) {
pr_info("Accel LP - > FP mode.\n ");
mpu6050_input_set_fp_mode(data);
}
done:
return result;
}
static int mpu6050_input_set_motion_interrupt(struct mpu6050_input_data *data,
bool enable, bool factory_test)
{
struct motion_int_data *mot_data = &data->mot_data;
unsigned char reg;
if (enable) {
if (!mot_data->is_set) {
mpu6050_i2c_read_reg(data->client,
MPUREG_PWR_MGMT_1, 2,
mot_data->pwr_mnt);
mpu6050_i2c_read_reg(data->client,
MPUREG_CONFIG, 1, &mot_data->cfg);
mpu6050_i2c_read_reg(data->client,
MPUREG_ACCEL_CONFIG, 1,
&mot_data->accel_cfg);
mpu6050_i2c_read_reg(data->client,
MPUREG_INT_ENABLE, 1,
&mot_data->int_cfg);
mpu6050_i2c_read_reg(data->client,
MPUREG_SMPLRT_DIV, 1,
&mot_data->smplrt_div);
}
/* 1) initialize */
mpu6050_i2c_read_reg(data->client, MPUREG_INT_STATUS, 1, ®);
pr_info("%s: Initialize motion interrupt : INT_STATUS=%x\n",
__func__, reg);
reg = 0x01;
mpu6050_i2c_write_single_reg(data->client,
MPUREG_PWR_MGMT_1, reg);
msleep(50);
/* 2. mpu& accel config */
if (factory_test)
reg = 0x0; /*260Hz LPF */
else
reg = 0x1; /*44Hz LPF */
mpu6050_i2c_write_single_reg(data->client, MPUREG_CONFIG, reg);
reg = 0x0; /* Clear Accel Config. */
mpu6050_i2c_write_single_reg(data->client,
MPUREG_ACCEL_CONFIG, reg);
/* 3. set motion thr & dur */
reg = 0x40; /* Make the motion interrupt enable */
mpu6050_i2c_write_single_reg(data->client,
MPUREG_INT_ENABLE, reg);
reg = 0x02; /* Motion Duration =1 ms */
mpu6050_i2c_write_single_reg(data->client,
MPUREG_ACCEL_MOT_DUR, reg);
/* Motion Threshold =1mg, based on the data sheet. */
if (factory_test)
reg = 0x00;
else
reg = 0x03;
mpu6050_i2c_write_single_reg(data->client,
MPUREG_ACCEL_MOT_THR, reg);
/* 5. */
/* Steps to setup the lower power mode for PWM-2 register */
reg = mot_data->pwr_mnt[1];
reg |= (BITS_LPA_WAKE_20HZ); /* setup the frequency of wakeup */
reg |= 0x07; /* put gyro in standby. */
reg &= ~(BIT_STBY_XA | BIT_STBY_YA | BIT_STBY_ZA);
mpu6050_i2c_write_single_reg(data->client,
MPUREG_PWR_MGMT_2, reg);
reg = 0x1;
reg |= 0x20; /* Set the cycle bit to be 1. LOW POWER MODE */
reg &= ~0x08; /* Clear the temp disp bit. */
mpu6050_i2c_write_single_reg(data->client,
MPUREG_PWR_MGMT_1,
reg & ~BIT_SLEEP);
data->motion_st_time = jiffies;
} else {
if (mot_data->is_set) {
mpu6050_i2c_write_single_reg(data->client,
MPUREG_PWR_MGMT_1,
mot_data->pwr_mnt[0]);
msleep(50);
mpu6050_i2c_write_single_reg(data->client,
MPUREG_PWR_MGMT_2,
mot_data->pwr_mnt[1]);
mpu6050_i2c_write_single_reg(data->client,
MPUREG_CONFIG,
mot_data->cfg);
mpu6050_i2c_write_single_reg(data->client,
MPUREG_ACCEL_CONFIG,
mot_data->accel_cfg);
mpu6050_i2c_write_single_reg(data->client,
MPUREG_INT_ENABLE,
mot_data->int_cfg);
reg = 0xff; /* Motion Duration =1 ms */
mpu6050_i2c_write_single_reg(data->client,
MPUREG_ACCEL_MOT_DUR, reg);
/* Motion Threshold =1mg, based on the data sheet. */
reg = 0xff;
mpu6050_i2c_write_single_reg(data->client,
MPUREG_ACCEL_MOT_THR, reg);
mpu6050_i2c_read_reg(data->client,
MPUREG_INT_STATUS, 1, ®);
mpu6050_i2c_write_single_reg(data->client,
MPUREG_SMPLRT_DIV,
mot_data->smplrt_div);
pr_info("%s: disable interrupt\n", __func__);
}
}
mot_data->is_set = enable;
return 0;
}
static int mpu6050_do_test(struct i2c_client *client, int self_test_flag,
int *gyro_result)
{
int result, i, j, packet_size;
u8 data[BYTES_PER_SENSOR * 2], d;
int fifo_count, packet_count, ind, s;
packet_size = BYTES_PER_SENSOR;
result = mpu6050_i2c_write_single_reg(client, MPUREG_INT_ENABLE, 0);
if (result)
return result;
/* disable the sensor output to FIFO */
result = mpu6050_i2c_write_single_reg(client, MPUREG_FIFO_EN, 0);
if (result)
return result;
/* disable fifo reading */
result = mpu6050_i2c_write_single_reg(client, MPUREG_USER_CTRL, 0);
if (result)
return result;
/* clear FIFO */
result = mpu6050_i2c_write_single_reg(client,
MPUREG_USER_CTRL, BIT_FIFO_RST);
if (result)
return result;
/* setup parameters */
result = mpu6050_i2c_write_single_reg(client, MPUREG_CONFIG,
MPU_FILTER_188HZ);
if (result)
return result;
result = mpu6050_i2c_write_single_reg(client, MPUREG_SMPLRT_DIV,
0x0);
if (result)
return result;
result = mpu6050_i2c_write_single_reg(client, MPUREG_GYRO_CONFIG,
self_test_flag | (MPU_FS_250DPS << 3));
if (result)
return result;
/* wait for the output to get stable */
msleep(DEF_ST_STABLE_TIME);
/* enable FIFO reading */
result = mpu6050_i2c_write_single_reg(client, MPUREG_USER_CTRL,
BIT_FIFO_EN);
if (result)
return result;
/* enable sensor output to FIFO */
d = BITS_GYRO_OUT;
result = mpu6050_i2c_write_single_reg(client, MPUREG_FIFO_EN, d);
if (result)
return result;
for (i = 0; i < THREE_AXIS; i++)
gyro_result[i] = 0;
s = 0;
while (s < INIT_ST_SAMPLES) {
msleep(DEF_GYRO_WAIT_TIME);
result = mpu6050_i2c_read_reg(client, MPUREG_FIFO_COUNTH,
FIFO_COUNT_BYTE, data);
if (result)
return result;
fifo_count = be16_to_cpup((__be16 *)(&data[0]));
packet_count = fifo_count / packet_size;
result = mpu6050_i2c_read_reg(client, MPUREG_FIFO_R_W,
packet_size, data);
if (result)
return result;
i = 0;
while ((i < packet_count) && (s < INIT_ST_SAMPLES)) {
result = mpu6050_i2c_read_reg(client, MPUREG_FIFO_R_W,
packet_size, data);
if (result)
return result;
ind = 0;
for (j = 0; j < THREE_AXIS; j++)
gyro_result[j] +=
(short)be16_to_cpup(
(__be16 *)(&data[ind + 2 * j]));
s++;
i++;
}
}
/* stop sending data to FIFO */
result = mpu6050_i2c_write_single_reg(client, MPUREG_FIFO_EN, 0);
if (result)
return result;
for (j = 0; j < THREE_AXIS; j++) {
gyro_result[j] = gyro_result[j]/s;
gyro_result[j] *= DEF_ST_PRECISION;
}
return 0;
}
static int __devinit mpu6050_input_probe(struct i2c_client *client,
const struct i2c_device_id *id)
{
struct mpu6050_input_platform_data *mpu_pdata =
client->dev.platform_data;
const struct mpu6050_input_cfg *cfg;
struct mpu6050_input_data *data;
int error;
unsigned char reg;
if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
dev_err(&client->dev, "i2c_check_functionality error\n");
return -EIO;
}
data = kzalloc(sizeof(struct mpu6050_input_data), GFP_KERNEL);
if (!data)
return -ENOMEM;
data->client = client;
data->pdata = mpu_pdata;
gb_mpu_data = data;
if (mpu_pdata->power_on)
mpu_pdata->power_on(true);
error = mpu6050_i2c_read_reg(data->client,
MPUREG_WHOAMI, 1, ®);
if (error < 0) {
pr_err("%s failed : threre is no such device.\n", __func__);
goto err_free_mem;
}
pr_info("%s: WHOAMI (0x68) = 0x%x\n", __func__, reg);
error = sensors_register(data->accel_sensor_device,
NULL, accel_sensor_attrs,
"accelerometer_sensor");
if (error) {
pr_err("%s: cound not register accelerometer sensor device(%d).\n",
__func__, error);
goto acc_sensor_register_failed;
}
error = sensors_register(data->gyro_sensor_device,
NULL, gyro_sensor_attrs,
"gyro_sensor");
if (error) {
pr_err("%s: cound not register gyro sensor device(%d).\n",
__func__, error);
goto gyro_sensor_register_failed;
}
#ifdef CONFIG_INPUT_MPU6050_POLLING
INIT_DELAYED_WORK(&data->accel_work, mpu6050_work_func_acc);
INIT_DELAYED_WORK(&data->gyro_work, mpu6050_work_func_gyro);
#endif
wake_lock_init(&data->reactive_wake_lock, WAKE_LOCK_SUSPEND,
"reactive_wake_lock");
error = mpu6050_input_initialize(data, cfg);
if (error)
goto err_input_initialize;
if (client->irq > 0) {
error = mpu6050_input_register_input_device(data);
if (error < 0)
goto err_input_initialize;
error = request_threaded_irq(client->irq,
NULL, mpu6050_input_irq_thread,
IRQF_TRIGGER_RISING | IRQF_ONESHOT,
MPU6050_INPUT_DRIVER, data);
if (error < 0) {
dev_err(&client->dev,
"irq request failed %d, error %d\n",
client->irq, error);
input_unregister_device(data->input);
goto err_input_initialize;
}
#ifdef CONFIG_INPUT_MPU6050_POLLING
disable_irq(client->irq);
#endif
}
i2c_set_clientdata(client, data);
pr_info("mpu6050_input_probe success\n");
return 0;
err_input_initialize:
wake_lock_destroy(&data->reactive_wake_lock);
gyro_sensor_register_failed:
acc_sensor_register_failed:
err_free_mem:
if (mpu_pdata->power_on)
mpu_pdata->power_on(false);
kfree(data);
return error;
}
int mpu6050_selftest_run(struct i2c_client *client,
int packet_cnt[3],
int gyro_bias[3],
int gyro_rms[3], int gyro_lsb_bias[3])
{
int ret_val = 0;
int result;
int total_count = 0;
int total_count_axis[3] = { 0, 0, 0 };
int packet_count;
long avg[3];
long rms[3];
short *x;
short *y;
short *z;
int buf_size = sizeof(short) * DEF_PERIOD_CAL
* DEF_TESTS_PER_AXIS / 8 * 4;
int i, j, tmp;
char tmpStr[200];
unsigned char regs[7] = { 0, };
unsigned char dataout[20];
int perform_full_test = 0;
int dps_rms[3] = { 0, };
u32 temp;
struct mpu6050_selftest_info test_setup = {
DEF_GYRO_SENS, DEF_GYRO_FULLSCALE, DEF_PACKET_THRESH,
DEF_TOTAL_TIMING_TOL, (int)DEF_BIAS_THRESH_SELF,
DEF_RMS_LSB_THRESH_SELF * DEF_RMS_LSB_THRESH_SELF,
/* now obsolete - has no effect */
DEF_TESTS_PER_AXIS, DEF_N_ACCEL_SAMPLES
};
char a_name[3][2] = { "X", "Y", "Z" };
x = kzalloc(buf_size, GFP_KERNEL);
y = kzalloc(buf_size, GFP_KERNEL);
z = kzalloc(buf_size, GFP_KERNEL);
/*backup registers */
result = mpu6050_backup_register(client);
if (result) {
pr_err("register backup error=%d", result);
goto err_state;
}
if (mpu6050_selftest.pwm_mgmt[0] & 0x40) {
result =
mpu6050_i2c_write_single_reg(client, MPUREG_PWR_MGMT_1,
0x00);
if (result) {
pr_err("init PWR_MGMT error=%d", result);
goto err_state;
}
}
regs[0] =
mpu6050_selftest.pwm_mgmt[1] &
~(BIT_STBY_XG | BIT_STBY_YG | BIT_STBY_ZG);
result =
mpu6050_i2c_write_single_reg(client,
MPUREG_PWR_MGMT_2, regs[0]);
/* make sure the DMP is disabled first */
result = mpu6050_i2c_write_single_reg(client, MPUREG_USER_CTRL, 0x00);
if (result) {
pr_info("DMP disable error=%d", result);
goto err_state;
}
/* reset the gyro offset values */
regs[0] = MPUREG_XG_OFFS_USRH;
result = mpu6050_i2c_write(client, 6, regs);
if (result)
goto err_state;
/* sample rate */
if (perform_full_test) {
/* = 8ms */
result =
mpu6050_i2c_write_single_reg(client, MPUREG_SMPLRT_DIV,
0x07);
test_setup.bias_thresh = DEF_BIAS_LSB_THRESH_CAL;
} else {
/* = 1ms */
result =
mpu6050_i2c_write_single_reg(client, MPUREG_SMPLRT_DIV,
0x00);
test_setup.bias_thresh = DEF_BIAS_LSB_THRESH_SELF;
}
if (result)
goto err_state;
regs[0] = 0x03; /* filter = 42Hz, analog_sample rate = 1 KHz */
switch (test_setup.gyro_fs) {
case 2000:
regs[0] |= 0x18;
break;
case 1000:
regs[0] |= 0x10;
break;
case 500:
regs[0] |= 0x08;
break;
case 250:
default:
regs[0] |= 0x00;
break;
}
result = mpu6050_i2c_write_single_reg(client, MPUREG_CONFIG, regs[0]);
if (result)
goto err_state;
switch (test_setup.gyro_fs) {
case 2000:
regs[0] = 0x03;
break;
case 1000:
regs[0] = 0x02;
break;
case 500:
regs[0] = 0x01;
break;
case 250:
default:
regs[0] = 0x00;
break;
}
result =
mpu6050_i2c_write_single_reg(client, MPUREG_GYRO_CONFIG,
regs[0] << 3);
if (result)
goto err_state;
result = mpu6050_i2c_write_single_reg(client, MPUREG_INT_ENABLE, 0x00);
if (result)
goto err_state;
/* 1st, timing test */
for (j = 0; j < 3; j++) {
pr_info("Collecting gyro data from %s gyro PLL\n", a_name[j]);
/* turn on all gyros, use gyro X for clocking
Set to Y and Z for 2nd and 3rd iteration */
result =
mpu6050_i2c_write_single_reg(client, MPUREG_PWR_MGMT_1,
j + 1);
if (result)
goto err_state;
/* wait for 50 ms after switching clock source */
msleep(50);
/* enable & reset FIFO */
result =
mpu6050_i2c_write_single_reg(client, MPUREG_USER_CTRL,
BIT_FIFO_EN | BIT_FIFO_RST);
if (result)
goto err_state;
tmp = test_setup.tests_per_axis;
while (tmp-- > 0) {
const unsigned char fifo_en_reg = MPUREG_FIFO_EN;
/* enable XYZ gyro in FIFO and nothing else */
result =
mpu6050_i2c_write_single_reg(client, fifo_en_reg,
BIT_GYRO_XOUT |
BIT_GYRO_YOUT |
BIT_GYRO_ZOUT);
if (result)
goto err_state;
pr_info("%s : before sampling ...\n", __func__);
/* wait one period for data */
if (perform_full_test)
msleep(DEF_PERIOD_CAL);
else
msleep(DEF_PERIOD_SELF);
pr_info("%s : after sampling ...\n", __func__);
/* stop storing gyro in the FIFO */
result =
mpu6050_i2c_write_single_reg(client,
fifo_en_reg, 0x00);
if (result)
goto err_state;
/* Getting number of bytes in FIFO */
result =
mpu6050_i2c_read_reg(client, MPUREG_FIFO_COUNTH,
2, dataout);
if (result)
goto err_state;
/* number of 6 B packets in the FIFO */
packet_count = mpu6050_big8_to_int16(dataout) / 6;
pr_info("%s : sampling result packet_count =%d\n",
__func__, packet_count);
sprintf(tmpStr, "Packet Count: %d -", packet_count);
if (packet_count - (test_setup.packet_thresh - 3)
> 0) {
for (i = 0; i < packet_count; i++) {
/* getting FIFO data */
result =
mpu6050_i2c_read_fifo(client, 6,
dataout);
if (result)
goto err_state;
x[total_count + i] =
mpu6050_big8_to_int16(&dataout[0]);
y[total_count + i] =
mpu6050_big8_to_int16(&dataout[2]);
z[total_count + i] =
mpu6050_big8_to_int16(&dataout[4]);
}
total_count += packet_count;
total_count_axis[j] += packet_count;
packet_cnt[j] = packet_count;
sprintf(tmpStr, "%sOK success", tmpStr);
} else {
ret_val |= 1 << j;
sprintf(tmpStr, "%sNOK - samples ignored ...",
tmpStr);
}
pr_info("%s\n", tmpStr);
}
/* remove gyros from FIFO */
result =
mpu6050_i2c_write_single_reg(client, MPUREG_FIFO_EN, 0x00);
if (result)
goto err_state;
}
pr_info("\nTotal %d samples\n", total_count);
/* 2nd, check bias from X, Y, and Z PLL clock source */
tmp = total_count != 0 ? total_count : 1;
for (i = 0, avg[0] = 0, avg[1] = 0, avg[2] = 0; i < total_count; i++) {
avg[0] += x[i];
avg[1] += y[i];
avg[2] += z[i];
}
avg[0] /= tmp;
avg[1] /= tmp;
avg[2] /= tmp;
pr_info("bias : %+8ld %+8ld %+8ld (LSB)\n", avg[0], avg[1], avg[2]);
gyro_bias[0] = (avg[0] * DEF_SCALE_FOR_FLOAT) / DEF_GYRO_SENS;
gyro_bias[1] = (avg[1] * DEF_SCALE_FOR_FLOAT) / DEF_GYRO_SENS;
gyro_bias[2] = (avg[2] * DEF_SCALE_FOR_FLOAT) / DEF_GYRO_SENS;
gyro_lsb_bias[0] = avg[0];
gyro_lsb_bias[1] = avg[1];
gyro_lsb_bias[2] = avg[2];
if (VERBOSE_OUT) {
pr_info("abs bias : %+8d.%03d %+8d.%03d %+8d.%03d (dps)\n",
(int)abs(gyro_bias[0]) / DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_bias[0]) % DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_bias[1]) / DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_bias[1]) % DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_bias[2]) / DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_bias[2]) % DEF_SCALE_FOR_FLOAT);
}
for (j = 0; j < 3; j++) {
if (abs(avg[j]) > test_setup.bias_thresh) {
pr_info("%s-Gyro bias (%ld) exceeded threshold "
"(threshold = %d LSB)\n", a_name[j],
avg[j], test_setup.bias_thresh);
ret_val |= 1 << (3 + j);
}
}
/* 3rd, check RMS for dead gyros
If any of the RMS noise value returns zero,
then we might have dead gyro or FIFO/register failure,
the part is sleeping, or the part is not responsive */
for (i = 0, rms[0] = 0, rms[1] = 0, rms[2] = 0; i < total_count; i++) {
rms[0] += (long)(x[i] - avg[0]) * (x[i] - avg[0]);
rms[1] += (long)(y[i] - avg[1]) * (y[i] - avg[1]);
rms[2] += (long)(z[i] - avg[2]) * (z[i] - avg[2]);
}
if (rms[0] == 0 || rms[1] == 0 || rms[2] == 0)
ret_val |= 1 << 6;
if (VERBOSE_OUT) {
pr_info("RMS ^ 2 : %+8ld %+8ld %+8ld\n",
(long)rms[0] / total_count,
(long)rms[1] / total_count, (long)rms[2] / total_count);
}
for (j = 0; j < 3; j++) {
if (rms[j] / total_count > test_setup.rms_thresh) {
pr_info("%s-Gyro rms (%ld) exceeded threshold "
"(threshold = %d LSB)\n", a_name[j],
rms[j] / total_count, test_setup.rms_thresh);
ret_val |= 1 << (7 + j);
}
}
for (i = 0; i < 3; i++) {
if (rms[i] > 10000) {
temp =
((u32) (rms[i] / total_count)) *
DEF_RMS_SCALE_FOR_RMS;
} else {
temp =
((u32) (rms[i] * DEF_RMS_SCALE_FOR_RMS)) /
total_count;
}
if (rms[i] < 0)
temp = 1 << 31;
dps_rms[i] = mpu6050_selftest_sqrt(temp) / DEF_GYRO_SENS;
gyro_rms[i] =
dps_rms[i] * DEF_SCALE_FOR_FLOAT / DEF_SQRT_SCALE_FOR_RMS;
}
pr_info("RMS : %+8d.%03d %+8d.%03d %+8d.%03d (dps)\n",
(int)abs(gyro_rms[0]) / DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_rms[0]) % DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_rms[1]) / DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_rms[1]) % DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_rms[2]) / DEF_SCALE_FOR_FLOAT,
(int)abs(gyro_rms[2]) % DEF_SCALE_FOR_FLOAT);
/*recover registers */
result = mpu6050_recover_register(client);
if (result) {
pr_err("register recovering error=%d", result);
goto err_state;
}
result = ret_val;
err_state:
kfree(x);
kfree(y);
kfree(z);
return result;
}
