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; }