diff options
Diffstat (limited to 'drivers/input/misc/mpu/inv_gyro.c')
-rw-r--r-- | drivers/input/misc/mpu/inv_gyro.c | 2559 |
1 files changed, 1656 insertions, 903 deletions
diff --git a/drivers/input/misc/mpu/inv_gyro.c b/drivers/input/misc/mpu/inv_gyro.c index cfa76ab46865..65e375f9d2e6 100644 --- a/drivers/input/misc/mpu/inv_gyro.c +++ b/drivers/input/misc/mpu/inv_gyro.c @@ -23,6 +23,7 @@ * @details This driver currently works for the ITG3500, MPU6050, MPU9150 * MPU3050 */ +#define DEBUG_FIFO_DATA_SPEW (0) #include <linux/module.h> #include <linux/init.h> @@ -72,18 +73,25 @@ static struct inv_reg_map_s chip_reg = { .pwr_mgmt_2 = 0x6C, .mem_start_addr = 0x6E, .mem_r_w = 0x6F, - .prgm_strt_addrh = 0x70 + .prgm_strt_addrh = 0x70, + + .accl_fifo_en = BIT_ACCEL_OUT, + .fifo_reset = BIT_FIFO_RST, + .i2c_mst_reset = BIT_I2C_MST_RST, + .cycle = BIT_CYCLE }; static const struct inv_hw_s hw_info[INV_NUM_PARTS] = { {119, "ITG3500"}, { 63, "MPU3050"}, - {118, "MPU6050"}, - {118, "MPU9150"} + {117, "MPU6050"}, + {118, "MPU9150"}, + {119, "MPU6500"}, + {118, "MPU9250"}, }; static unsigned long nvi_lpf_us_tbl[] = { - 3906, /* 256Hz */ + 0, /* WAR: disabled 3906, 256Hz */ 5319, /* 188Hz */ 10204, /* 98Hz */ 23810, /* 42Hz */ @@ -92,16 +100,31 @@ static unsigned long nvi_lpf_us_tbl[] = { /* 200000, 5Hz */ }; -static unsigned long nvi_lpa_delay_us_tbl[] = { - 800000, - 200000, - 50000, - /* 25000, */ +static unsigned long nvi_lpa_delay_us_tbl_6050[] = { + 800000, /* 800ms */ + 200000, /* 200ms */ + 50000, /* 50ms */ + /* 25000, 25ms */ +}; + +static unsigned long nvi_lpa_delay_us_tbl_6500[] = { + 4096000,/* 4096ms */ + 2048000,/* 2048ms */ + 1024000,/* 1024ms */ + 512000, /* 512ms */ + 256000, /* 256ms */ + 128000, /* 128ms */ + 64000, /* 64ms */ + 32000, /* 32ms */ + 16000, /* 16ms */ + 8000, /* 8ms */ + 4000, /* 4ms */ + /* 2000, 2ms */ }; static struct inv_gyro_state_s *inf_local; -s64 get_time_ns(void) +s64 nvi_ts_ns(void) { struct timespec ts; ktime_get_ts(&ts); @@ -190,8 +213,7 @@ int inv_i2c_single_write_base(struct inv_gyro_state_s *st, /* Register SMPLRT_DIV (0x19) */ -static int nvi_smplrt_div_wr(struct inv_gyro_state_s *inf, - unsigned char smplrt_div) +static int nvi_smplrt_div_wr(struct inv_gyro_state_s *inf, u8 smplrt_div) { int err = 0; @@ -205,24 +227,33 @@ static int nvi_smplrt_div_wr(struct inv_gyro_state_s *inf, } /* Register CONFIG (0x1A) */ -static int nvi_config_wr(struct inv_gyro_state_s *inf, unsigned char lpf) +static int nvi_config_wr(struct inv_gyro_state_s *inf, u8 val) { int err = 0; - if (lpf != inf->hw.config) { - err = inv_i2c_single_write(inf, inf->reg->lpf, lpf); - if (!err) - inf->hw.config = lpf; + if (val != inf->hw.config) { + err = inv_i2c_single_write(inf, inf->reg->lpf, val); + if (!err) { + inf->hw.config = val; + err = 1; /* flag change made */ + } } return err; } /* Register GYRO_CONFIG (0x1B) */ -static int nvi_gyro_config_wr(struct inv_gyro_state_s *inf, unsigned char fsr) +static int nvi_gyro_config_wr(struct inv_gyro_state_s *inf, u8 fsr) { - unsigned char val; + u8 val; int err = 0; + if (inf->chip_type == INV_MPU3050) { + val = inf->hw.config; + val &= 0xE7; + val |= fsr << 3; + return nvi_config_wr(inf, val); + } + if (fsr != inf->hw.gyro_config) { val = (fsr << 3); err = inv_i2c_single_write(inf, inf->reg->gyro_config, val); @@ -234,29 +265,61 @@ static int nvi_gyro_config_wr(struct inv_gyro_state_s *inf, unsigned char fsr) return err; } -/* Register ACCEL_CONFIG (0x1C) */ -static int nvi_accel_config_wr(struct inv_gyro_state_s *inf, - unsigned char fsr, unsigned char hpf) +/* Register ACCEL_CONFIG2 (0x1D) */ +static int nvi_accel_config2_wr(struct inv_gyro_state_s *inf, u8 val) { - unsigned char val; int err = 0; - val = (fsr << 3) | hpf; + if (val != inf->hw.accl_config2) { + err = inv_i2c_single_write(inf, 0x1D, val); + if (!err) { + inf->hw.accl_config2 = val; + err = 1; /* flag change made */ + } + } + return err; +} + +/* Register ACCEL_CONFIG (0x1C) */ +static int nvi_accel_config_wr(struct inv_gyro_state_s *inf, u8 fsr, u8 hpf) +{ + u8 val; + int err; + int err_t = 0; + + val = (fsr << 3); + if (inf->chip_type == INV_MPU6500) + err_t = nvi_accel_config2_wr(inf, hpf); + else + val |= hpf; if (val != inf->hw.accl_config) { - err = inv_i2c_single_write(inf, inf->reg->accl_config, - val); + err = inv_i2c_single_write(inf, inf->reg->accl_config, val); if (!err) { inf->hw.accl_config = val; - err = 1; /* flag change made */ - if (hpf != 7) - inf->mot_enable = false; + err_t |= 1; /* flag change made */ + } else { + err_t |= err; } } + return err_t; +} + +/* Register LP_ACCEL_ODR (0x1E) */ +static int nvi_lp_accel_odr_wr(struct inv_gyro_state_s *inf, u8 lposc_clksel) +{ + int err = 0; + + if (lposc_clksel != inf->hw.lposc_clksel) { + err = inv_i2c_single_write(inf, REG_6500_LP_ACCEL_ODR, + lposc_clksel); + if (!err) + inf->hw.lposc_clksel = lposc_clksel; + } return err; } /* Register MOT_THR (0x1F) */ -static int nvi_mot_thr_wr(struct inv_gyro_state_s *inf, unsigned char mot_thr) +static int nvi_mot_thr_wr(struct inv_gyro_state_s *inf, u8 mot_thr) { int err = 0; @@ -269,7 +332,7 @@ static int nvi_mot_thr_wr(struct inv_gyro_state_s *inf, unsigned char mot_thr) } /* Register MOT_DUR (0x20) */ -static int nvi_mot_dur_wr(struct inv_gyro_state_s *inf, unsigned char mot_dur) +static int nvi_mot_dur_wr(struct inv_gyro_state_s *inf, u8 mot_dur) { int err = 0; @@ -282,7 +345,7 @@ static int nvi_mot_dur_wr(struct inv_gyro_state_s *inf, unsigned char mot_dur) } /* Register FIFO_EN (0x23) */ -static int nvi_fifo_en_wr(struct inv_gyro_state_s *inf, unsigned char fifo_en) +static int nvi_fifo_en_wr(struct inv_gyro_state_s *inf, u8 fifo_en) { int err = 0; @@ -298,11 +361,14 @@ static int nvi_fifo_en_wr(struct inv_gyro_state_s *inf, unsigned char fifo_en) static int nvi_i2c_mst_ctrl_wr(struct inv_gyro_state_s *inf, bool port3_fifo_en) { - unsigned char val; + u8 val; int err = 0; + if (inf->chip_type == INV_MPU3050) + return 0; + val = inf->aux.clock_i2c; - val |= BIT_WAIT_FOR_ES | BIT_I2C_MST_P_NSR; + val |= BIT_WAIT_FOR_ES; if (port3_fifo_en) val |= BIT_SLV3_FIFO_EN; if (val != inf->hw.i2c_mst_ctrl) { @@ -313,10 +379,68 @@ static int nvi_i2c_mst_ctrl_wr(struct inv_gyro_state_s *inf, return err; } +/* Register I2C_SLV0_CTRL (0x25) */ +/* Register I2C_SLV1_CTRL (0x28) */ +/* Register I2C_SLV2_CTRL (0x2B) */ +/* Register I2C_SLV3_CTRL (0x2E) */ +/* Register I2C_SLV4_CTRL (0x31) */ +static int nvi_i2c_slv_addr_wr(struct inv_gyro_state_s *inf, int port, u8 addr) +{ + u8 reg; + int err = 0; + + reg = (REG_I2C_SLV0_ADDR + (port * 3)); + if (addr != inf->hw.i2c_slv_addr[port]) { + err = inv_i2c_single_write(inf, reg, addr); + if (!err) + inf->hw.i2c_slv_addr[port] = addr; + } + return err; +} + +/* Register I2C_SLV0_CTRL (0x26) */ +/* Register I2C_SLV1_CTRL (0x29) */ +/* Register I2C_SLV2_CTRL (0x2C) */ +/* Register I2C_SLV3_CTRL (0x2F) */ +/* Register I2C_SLV4_CTRL (0x32) */ +static int nvi_i2c_slv_reg_wr(struct inv_gyro_state_s *inf, int port, u8 val) +{ + u8 reg; + int err = 0; + + reg = (REG_I2C_SLV0_REG + (port * 3)); + if (val != inf->hw.i2c_slv_reg[port]) { + err = inv_i2c_single_write(inf, reg, val); + if (!err) + inf->hw.i2c_slv_reg[port] = val; + } + return err; +} + +/* Register I2C_SLV0_CTRL (0x27) */ +/* Register I2C_SLV1_CTRL (0x2A) */ +/* Register I2C_SLV2_CTRL (0x2D) */ +/* Register I2C_SLV3_CTRL (0x30) */ +static int nvi_i2c_slv_ctrl_wr(struct inv_gyro_state_s *inf, int port, u8 val) +{ + u8 reg; + int err = 0; + + reg = (REG_I2C_SLV0_CTRL + (port * 3)); + if (val != inf->hw.i2c_slv_ctrl[port]) { + err = inv_i2c_single_write(inf, reg, val); + if (!err) { + inf->hw.i2c_slv_ctrl[port] = val; + err = 1; /* flag change made */ + } + } + return err; +} + /* Register I2C_SLV4_CTRL (0x34) */ static int nvi_i2c_slv4_ctrl_wr(struct inv_gyro_state_s *inf, bool slv4_en) { - unsigned char val; + u8 val; int err = 0; val = inf->aux.delay_hw; @@ -326,14 +450,16 @@ static int nvi_i2c_slv4_ctrl_wr(struct inv_gyro_state_s *inf, bool slv4_en) val |= BIT_SLV_EN; if (val != inf->hw.i2c_slv4_ctrl) { err = inv_i2c_single_write(inf, REG_I2C_SLV4_CTRL, val); - if (!err) + if (!err) { inf->hw.i2c_slv4_ctrl = val; + err = 1; /* flag change made */ + } } return err; } /* Register INT_PIN_CFG (0x37) */ -static int nvi_int_pin_cfg_wr(struct inv_gyro_state_s *inf, unsigned char val) +static int nvi_int_pin_cfg_wr(struct inv_gyro_state_s *inf, u8 val) { int err = 0; @@ -348,18 +474,23 @@ static int nvi_int_pin_cfg_wr(struct inv_gyro_state_s *inf, unsigned char val) /* Register INT_ENABLE (0x38) */ static int nvi_int_enable_wr(struct inv_gyro_state_s *inf, bool enable) { - unsigned char val = 0; + u8 val = 0; int err = 0; if (enable) { if ((inf->hw.user_ctrl & BIT_I2C_MST_EN) || inf->chip_config.gyro_enable) { - val = BIT_DATA_RDY_EN; + if ((inf->hw.user_ctrl & BIT_FIFO_EN) && + (!inf->chip_config.fifo_thr)) + val = BIT_FIFO_OVERFLOW; + else + val = BIT_DATA_RDY_EN; } else if (inf->chip_config.accl_enable) { - if (inf->mot_enable && (!inf->mot_cnt)) { + if (((inf->hw.accl_config & 0x07) == 0x07) && + (!inf->mot_cnt)) { val = BIT_MOT_EN; - if (inf->mot_dbg) - pr_info("%s motion detect on", + if (inf->chip_config.mot_enable == NVI_MOT_DBG) + pr_info("%s motion detect on\n", __func__); } else { val = BIT_DATA_RDY_EN; @@ -368,15 +499,44 @@ static int nvi_int_enable_wr(struct inv_gyro_state_s *inf, bool enable) } if ((val != inf->hw.int_enable) && (inf->pm > NVI_PM_OFF)) { err = inv_i2c_single_write(inf, inf->reg->int_enable, val); - if (!err) + if (!err) { inf->hw.int_enable = val; + dev_dbg(&inf->i2c->dev, "%s: %x\n", __func__, val); + } + } + return err; +} + +/* Register I2C_SLV0_CTRL (0x63) */ +/* Register I2C_SLV1_CTRL (0x64) */ +/* Register I2C_SLV2_CTRL (0x65) */ +/* Register I2C_SLV3_CTRL (0x66) */ +/* Register I2C_SLV4_CTRL (0x33) */ +static int nvi_i2c_slv_do_wr(struct inv_gyro_state_s *inf, + int port, u8 data_out) +{ + u8 *hw; + u8 reg; + int err = 0; + + if (port == AUX_PORT_SPECIAL) { + hw = &inf->hw.i2c_slv4_do; + reg = REG_I2C_SLV4_DO; + } else { + hw = &inf->hw.i2c_slv_do[port]; + reg = REG_I2C_SLV0_DO + port; + } + if (data_out != *hw) { + err = inv_i2c_single_write(inf, reg, data_out); + if (!err) + *hw = data_out; } return err; } /* Register I2C_MST_DELAY_CTRL (0x67) */ static int nvi_i2c_mst_delay_ctrl_wr(struct inv_gyro_state_s *inf, - unsigned char i2c_mst_delay_ctrl) + u8 i2c_mst_delay_ctrl) { int err = 0; @@ -390,8 +550,7 @@ static int nvi_i2c_mst_delay_ctrl_wr(struct inv_gyro_state_s *inf, } /* Register MOT_DETECT_CTRL (0x69) */ -static int nvi_mot_detect_ctrl_wr(struct inv_gyro_state_s *inf, - unsigned char val) +static int nvi_mot_detect_ctrl_wr(struct inv_gyro_state_s *inf, u8 val) { int err = 0; @@ -404,8 +563,7 @@ static int nvi_mot_detect_ctrl_wr(struct inv_gyro_state_s *inf, } /* Register USER_CTRL (0x6A) */ -static int nvi_user_ctrl_reset_wr(struct inv_gyro_state_s *inf, - unsigned char val) +static int nvi_user_ctrl_reset_wr(struct inv_gyro_state_s *inf, u8 val) { int i; int err; @@ -415,7 +573,7 @@ static int nvi_user_ctrl_reset_wr(struct inv_gyro_state_s *inf, for (i = 0; i < POWER_UP_TIME; i++) { val = -1; err = inv_i2c_read(inf, inf->reg->user_ctrl, 1, &val); - if (!(val & (BIT_FIFO_RST | BIT_I2C_MST_RST))) + if (!(val & (inf->reg->fifo_reset | inf->reg->i2c_mst_reset))) break; mdelay(1); @@ -429,61 +587,95 @@ static int nvi_user_ctrl_reset_wr(struct inv_gyro_state_s *inf, static int nvi_user_ctrl_en_wr(struct inv_gyro_state_s *inf, bool fifo_enable, bool i2c_enable) { - unsigned char val; + u8 val; + u16 fifo_sample_size; bool en; int i; - int err = 0; + int err; + int err_t = 0; - if (inf->lpa_enable) + dev_dbg(&inf->i2c->dev, "%s: FIFO=%x I2C=%x\n", + __func__, fifo_enable, i2c_enable); + if (inf->hw.pwr_mgmt_1 & inf->reg->cycle) fifo_enable = false; val = 0; + fifo_sample_size = 0; + inf->fifo_sample_size = 0; + en = false; if (fifo_enable) { - for (i = 0; i < (AUX_PORT_SPECIAL - 1); i++) { - if (inf->aux.port[i].fifo_en && inf->aux.port[i].hw_en) - val |= (1 << i); + if (inf->chip_type == INV_MPU3050) { + val |= BIT_3050_FIFO_FOOTER; + fifo_sample_size += 2; + } + if (inf->chip_config.accl_fifo_enable) { + val |= inf->reg->accl_fifo_en; + fifo_sample_size += 6; } - if (inf->chip_config.gyro_fifo_enable) - val |= (inf->chip_config.gyro_enable << 4); - if (inf->chip_config.accl_fifo_enable) - val |= BIT_ACCEL_OUT; - if (inf->chip_config.temp_fifo_enable) + if (inf->chip_config.temp_enable && + inf->chip_config.temp_fifo_enable) { val |= BIT_TEMP_FIFO_EN; - if (inf->aux.port[3].fifo_en && inf->aux.port[3].hw_en) - en = true; - else - en = false; - err |= nvi_i2c_mst_ctrl_wr(inf, en); + fifo_sample_size += 2; + } + if (inf->chip_config.gyro_fifo_enable) { + val |= (inf->chip_config.gyro_fifo_enable << 4); + if (val & BIT_GYRO_XOUT) + fifo_sample_size += 2; + if (val & BIT_GYRO_YOUT) + fifo_sample_size += 2; + if (val & BIT_GYRO_ZOUT) + fifo_sample_size += 2; + } + for (i = 0; i < AUX_PORT_SPECIAL; i++) { + if (inf->aux.port[i].fifo_en && + (inf->aux.port[i].nmp.addr & BIT_I2C_READ) && + (inf->hw.i2c_slv_ctrl[i] & BIT_SLV_EN)) { + if (i == 3) + en = true; + else + val |= (1 << i); + fifo_sample_size += inf->aux.port[i].nmp.ctrl & + BITS_I2C_SLV_CTRL_LEN; + } + } + err_t |= nvi_i2c_mst_ctrl_wr(inf, en); if (val || en) en = true; else en = false; + inf->fifo_sample_size = fifo_sample_size; } else { - err |= nvi_i2c_mst_ctrl_wr(inf, false); - en = false; + err_t |= nvi_i2c_mst_ctrl_wr(inf, false); } - err |= nvi_fifo_en_wr(inf, val); + err_t |= nvi_fifo_en_wr(inf, val); val = 0; if (fifo_enable && en) val |= BIT_FIFO_EN; - if (i2c_enable && inf->aux.enable) + if (i2c_enable && (inf->aux.enable || inf->aux.en3050)) val |= BIT_I2C_MST_EN; if (val != inf->hw.user_ctrl) { - err |= inv_i2c_single_write(inf, inf->reg->user_ctrl, val); - if (!err) + err = inv_i2c_single_write(inf, inf->reg->user_ctrl, val); + if (err) { + err_t |= err; + dev_err(&inf->i2c->dev, "%s ERR FIFO=%x I2C=%x", + __func__, fifo_enable, i2c_enable); + } else { inf->hw.user_ctrl = val; + dev_dbg(&inf->i2c->dev, "%s FIFO=%x I2C=%x", __func__, + (val & BIT_FIFO_EN), (val & BIT_I2C_MST_EN)); + } } - return err; + return err_t; } /* Register PWR_MGMT_1 (0x6B) */ -static int nvi_pwr_mgmt_1_wr(struct inv_gyro_state_s *inf, unsigned char pm1) +static int nvi_pwr_mgmt_1_war(struct inv_gyro_state_s *inf) { - unsigned char val; + u8 val; int i; int err; for (i = 0; i < POWER_UP_TIME; i++) { - inv_i2c_single_write(inf, inf->reg->pwr_mgmt_1, pm1); + inv_i2c_single_write(inf, inf->reg->pwr_mgmt_1, 0); val = -1; err = inv_i2c_read(inf, inf->reg->pwr_mgmt_1, 1, &val); if (!val) @@ -495,28 +687,65 @@ static int nvi_pwr_mgmt_1_wr(struct inv_gyro_state_s *inf, unsigned char pm1) return err; } -static bool nvi_lpa_able(struct inv_gyro_state_s *inf) +/* Register PWR_MGMT_1 (0x6B) */ +static int nvi_pwr_mgmt_1_wr(struct inv_gyro_state_s *inf, u8 pwr_mgmt_1) { - bool lpa_enable; - int i; + int err = 0; - if (inf->mot_enable) { - lpa_enable = true; - } else if (inf->chip_config.lpa_delay_us) { - if (inf->chip_config.accl_delay_us < - inf->chip_config.lpa_delay_us) - lpa_enable = false; - else - lpa_enable = true; - } else { - lpa_enable = false; + if (pwr_mgmt_1 != inf->hw.pwr_mgmt_1) { + err = inv_i2c_single_write(inf, inf->reg->pwr_mgmt_1, + pwr_mgmt_1); + if (!err) + inf->hw.pwr_mgmt_1 = pwr_mgmt_1; } - for (i = 0; i < ARRAY_SIZE(nvi_lpa_delay_us_tbl); i++) { - if (inf->chip_config.accl_delay_us >= nvi_lpa_delay_us_tbl[i]) - break; + return err; +} + +/* Register PWR_MGMT_2 (0x6C) */ +static int nvi_pwr_mgmt_2_wr(struct inv_gyro_state_s *inf, u8 pwr_mgmt_2) +{ + int err = 0; + + if (pwr_mgmt_2 != inf->hw.pwr_mgmt_2) { + err = inv_i2c_single_write(inf, inf->reg->pwr_mgmt_2, + pwr_mgmt_2); + if (!err) + inf->hw.pwr_mgmt_2 = pwr_mgmt_2; } - inf->lpa_hw = i; - return lpa_enable; + return err; +} + +static int nvi_motion_detect_enable(struct inv_gyro_state_s *inf, u8 mot_thr) +{ + int err; + int err_t = 0; + + if (inf->chip_type != INV_MPU6050) + return 0; + + if (mot_thr) { + err = nvi_accel_config_wr(inf, inf->chip_config.accl_fsr, 0); + if (err < 0) + err_t |= err; + err = nvi_config_wr(inf, 0); + if (err < 0) + err_t |= err; + err_t |= nvi_mot_dur_wr(inf, inf->chip_config.mot_dur); + err_t |= nvi_mot_detect_ctrl_wr(inf, + inf->chip_config.mot_ctrl); + err_t |= nvi_mot_thr_wr(inf, mot_thr); + mdelay(5); + err = nvi_accel_config_wr(inf, inf->chip_config.accl_fsr, 7); + if (err < 0) + err_t |= err; + if (err_t) + nvi_accel_config_wr(inf, inf->chip_config.accl_fsr, 0); + } else { + err = nvi_accel_config_wr(inf, inf->chip_config.accl_fsr, 0); + if (err < 0) + err_t |= err; + } + return err_t; } static int nvi_vreg_dis(struct inv_gyro_state_s *inf, unsigned int i) @@ -613,16 +842,17 @@ static int nvi_vreg_init(struct inv_gyro_state_s *inf) return err; } -static int nvi_pm_wr_on(struct inv_gyro_state_s *inf, u8 pm1, u8 pm2) +static int nvi_pm_wr(struct inv_gyro_state_s *inf, + u8 pwr_mgmt_1, u8 pwr_mgmt_2, u8 lpa) { - unsigned char val; + u8 val; int i; int err; int err_t = 0; err = nvi_vreg_en_all(inf); if (err) { - err_t |= nvi_pwr_mgmt_1_wr(inf, 0); + err_t |= nvi_pwr_mgmt_1_war(inf); err_t |= inv_i2c_single_write(inf, inf->reg->pwr_mgmt_1, BIT_RESET); for (i = 0; i < POWER_UP_TIME; i++) { @@ -634,110 +864,91 @@ static int nvi_pm_wr_on(struct inv_gyro_state_s *inf, u8 pm1, u8 pm2) mdelay(1); } err_t |= err; - err_t |= nvi_pwr_mgmt_1_wr(inf, 0); - err_t |= nvi_user_ctrl_reset_wr(inf, (BIT_FIFO_RST | - BIT_I2C_MST_RST)); - for (i = 0; i < AUX_PORT_MAX; i++) { - inf->aux.port[i].hw_valid = false; - inf->aux.port[i].hw_en = false; - } + err_t |= nvi_pwr_mgmt_1_war(inf); + err_t |= nvi_user_ctrl_reset_wr(inf, (inf->reg->fifo_reset | + inf->reg->i2c_mst_reset)); memset(&inf->hw, 0, sizeof(struct nvi_hw)); inf->sample_delay_us = 0; - inf->mot_enable = false; } else { - err_t |= nvi_pwr_mgmt_1_wr(inf, 0); - } - if (pm2 != inf->hw.pwr_mgmt_2) { - err = inv_i2c_single_write(inf, inf->reg->pwr_mgmt_2, pm2); + err_t |= nvi_pwr_mgmt_1_war(inf); + } + switch (inf->chip_type) { + case INV_MPU3050: + pwr_mgmt_1 &= (BIT_SLEEP | INV_CLK_PLL); + if (pwr_mgmt_1 & INV_CLK_PLL) { + err = nvi_pwr_mgmt_1_wr(inf, (BITS_3050_POWER1 | + INV_CLK_PLL)); + err |= nvi_pwr_mgmt_1_wr(inf, (BITS_3050_POWER2 | + INV_CLK_PLL)); + err |= nvi_pwr_mgmt_1_wr(inf, INV_CLK_PLL); + } else { + pwr_mgmt_1 |= (pwr_mgmt_2 & 0x07) << 3; + err = nvi_pwr_mgmt_1_wr(inf, pwr_mgmt_1); + } if (err) err_t |= err; else - inf->hw.pwr_mgmt_2 = pm2; - } - if (pm1 != inf->hw.pwr_mgmt_1) { - err = inv_i2c_single_write(inf, inf->reg->pwr_mgmt_1, pm1); + inf->hw.pwr_mgmt_2 = pwr_mgmt_2; + break; + + case INV_MPU6050: + pwr_mgmt_2 |= lpa << 6; + err = nvi_pwr_mgmt_2_wr(inf, pwr_mgmt_2); if (err) err_t |= err; else - inf->hw.pwr_mgmt_1 = pm1; - } - return err_t; -} - -static int nvi_pm_wr(struct inv_gyro_state_s *inf, int pm, int stby) -{ - int err = 0; - - if ((pm == inf->pm) && (stby == inf->stby)) - return err; - - switch (pm) { - case NVI_PM_OFF_FORCE: - case NVI_PM_OFF: - err = nvi_pm_wr_on(inf, BIT_SLEEP, stby); - err |= nvi_vreg_dis_all(inf); - break; - - case NVI_PM_STDBY: - err = nvi_pm_wr_on(inf, BIT_SLEEP, stby); + inf->hw.lposc_clksel = lpa; + err_t |= nvi_pwr_mgmt_1_wr(inf, pwr_mgmt_1); break; - case NVI_PM_ON_CYCLE: - err = nvi_pm_wr_on(inf, BIT_CYCLE, stby); - break; - - case NVI_PM_ON: - err = nvi_pm_wr_on(inf, INV_CLK_INTERNAL, stby); - break; - - case NVI_PM_ON_FULL: - err = nvi_pm_wr_on(inf, INV_CLK_PLL, stby); - break; - - default: - err = -EINVAL; + default: /* INV_MPU6500 */ + err_t |= nvi_lp_accel_odr_wr(inf, lpa); + err_t |= nvi_pwr_mgmt_2_wr(inf, pwr_mgmt_2); + err_t |= nvi_pwr_mgmt_1_wr(inf, pwr_mgmt_1); break; } - if (err < 0) { - dev_err(&inf->i2c->dev, "%s requested pm=%d ERR=%d\n", - __func__, pm, err); - pm = NVI_PM_ERR; - } else { - inf->stby = stby; - } - inf->pm = pm; - dev_dbg(&inf->i2c->dev, "%s pm=%d stby=%x\n", __func__, pm, stby); - if (err > 0) - err = 0; - return err; + return err_t; } static int nvi_pm(struct inv_gyro_state_s *inf, int pm_req) { bool irq; - int stby; + bool mot_det_dis; + u8 pwr_mgmt_2; + u8 lpa; int pm; - int err; + int err = 0; - nvi_int_enable_wr(inf, false); - inf->lpa_enable = false; - if ((pm_req == NVI_PM_OFF_FORCE) || (pm_req == NVI_PM_OFF)) { - stby = 0x3F; + mot_det_dis = false; + lpa = inf->hw.lposc_clksel; + if ((pm_req == NVI_PM_OFF_FORCE) || (pm_req == NVI_PM_OFF) || + inf->suspend) { + pwr_mgmt_2 = 0x3F; pm = NVI_PM_OFF_FORCE; } else { - stby = ((~inf->chip_config.accl_enable) & 0x07) << 3; - stby |= (~inf->chip_config.gyro_enable) & 0x07; + pwr_mgmt_2 = ((~inf->chip_config.accl_enable) & 0x07) << 3; + pwr_mgmt_2 |= (~inf->chip_config.gyro_enable) & 0x07; if (inf->chip_config.gyro_enable || + (inf->chip_config.temp_enable & NVI_TEMP_EN) || (inf->hw.user_ctrl & BIT_I2C_MST_EN)) { + mot_det_dis = true; if (inf->chip_config.gyro_enable) pm = NVI_PM_ON_FULL; else pm = NVI_PM_ON; } else if (inf->chip_config.accl_enable) { - if (nvi_lpa_able(inf)) { - inf->lpa_enable = true; - stby |= (inf->lpa_hw << 6); + if (((inf->hw.accl_config & 0x07) == 0x07) || + (inf->chip_config.lpa_delay_us && + inf->hal.lpa_tbl_n && + (inf->chip_config.accl_delay_us >= + inf->chip_config.lpa_delay_us))) { + for (lpa = 0; lpa < inf->hal.lpa_tbl_n; + lpa++) { + if (inf->chip_config.accl_delay_us >= + inf->hal.lpa_tbl[lpa]) + break; + } pm = NVI_PM_ON_CYCLE; } else { pm = NVI_PM_ON; @@ -750,9 +961,59 @@ static int nvi_pm(struct inv_gyro_state_s *inf, int pm_req) } if (pm_req > pm) pm = pm_req; - err = nvi_pm_wr(inf, pm, stby); - if (pm_req == NVI_PM_AUTO) { + if ((pm != inf->pm) || (pwr_mgmt_2 != inf->hw.pwr_mgmt_2) || + (lpa != inf->hw.lposc_clksel)) { + nvi_int_enable_wr(inf, false); + if (((pwr_mgmt_2 & 0x38) != (inf->hw.pwr_mgmt_2 & 0x38)) || + (pm != NVI_PM_ON_FULL)) + inf->gyro_start_ts = 0; + switch (pm) { + case NVI_PM_OFF_FORCE: + case NVI_PM_OFF: + err = nvi_pm_wr(inf, BIT_SLEEP, pwr_mgmt_2, lpa); + err |= nvi_vreg_dis_all(inf); + break; + + case NVI_PM_STDBY: + err = nvi_pm_wr(inf, BIT_SLEEP, pwr_mgmt_2, lpa); + break; + + case NVI_PM_ON_CYCLE: + err = nvi_pm_wr(inf, inf->reg->cycle, pwr_mgmt_2, lpa); + break; + + case NVI_PM_ON: + err = nvi_pm_wr(inf, INV_CLK_INTERNAL, pwr_mgmt_2, lpa); + if (mot_det_dis) + nvi_motion_detect_enable(inf, 0); + break; + + case NVI_PM_ON_FULL: + err = nvi_pm_wr(inf, INV_CLK_PLL, pwr_mgmt_2, lpa); + nvi_motion_detect_enable(inf, 0); + break; + + default: + err = -EINVAL; + break; + } + + if (err < 0) { + dev_err(&inf->i2c->dev, "%s requested pm=%d ERR=%d\n", + __func__, pm, err); + pm = NVI_PM_ERR; + } + inf->pm = pm; + dev_dbg(&inf->i2c->dev, "%s pm=%d pwr_mgmt_2=%x lpa=%x\n", + __func__, pm, pwr_mgmt_2, lpa); + if (err > 0) + err = 0; + } + if ((pm_req == NVI_PM_AUTO) && (pm > NVI_PM_STDBY)) { nvi_user_ctrl_en_wr(inf, true, true); + if ((pm == NVI_PM_ON_FULL) && (!inf->gyro_start_ts)) + inf->gyro_start_ts = nvi_ts_ns() + + inf->chip_config.gyro_start_delay_ns; irq = true; } else { irq = false; @@ -773,62 +1034,40 @@ static int nvi_pm_init(struct inv_gyro_state_s *inf) nvi_vreg_init(inf); inf->pm = NVI_PM_ERR; - inf->stby = 0; err = nvi_pm(inf, NVI_PM_ON_FULL); return err; } -static int nvi_motion_detect_enable(struct inv_gyro_state_s *inf, u8 mot_thr) -{ - int err; - int err_t = 0; - - if (mot_thr) { - err = nvi_accel_config_wr(inf, inf->chip_config.accl_fsr, 0); - if (err < 0) - err_t |= err; - err_t |= nvi_config_wr(inf, 0); - if (!inf->hw.mot_dur) - err_t |= nvi_mot_dur_wr(inf, 1); - err_t |= nvi_mot_detect_ctrl_wr(inf, inf->chip_config.mot_ctrl); - err_t |= nvi_mot_thr_wr(inf, mot_thr); - mdelay(5); - err = nvi_accel_config_wr(inf, inf->chip_config.accl_fsr, 7); - if (err < 0) - err_t |= err; - if (!err_t) - inf->mot_enable = true; - } else { - err = nvi_accel_config_wr(inf, inf->chip_config.accl_fsr, 0); - if (err < 0) - err_t |= err; - } - return err_t; -} - static int nvi_aux_delay(struct inv_gyro_state_s *inf, int port, unsigned int delay_ms) { - unsigned char val; - unsigned char i; - unsigned int ms; + struct aux_port *ap; + u8 val; + u8 i; unsigned int delay_new; int delay_rtn; - int err; if (port != AUX_PORT_BYPASS) inf->aux.port[port].nmp.delay_ms = delay_ms; /* determine valid delays by ports enabled */ delay_new = 0; delay_rtn = 0; - for (i = 0; i < AUX_PORT_MAX; i++) { - if (delay_rtn < inf->aux.port[i].nmp.delay_ms) - delay_rtn = inf->aux.port[i].nmp.delay_ms; - if (inf->aux.port[i].hw_en) { - if (delay_new < inf->aux.port[i].nmp.delay_ms) - delay_new = inf->aux.port[i].nmp.delay_ms; + for (i = 0; i < AUX_PORT_SPECIAL; i++) { + ap = &inf->aux.port[i]; + if (delay_rtn < ap->nmp.delay_ms) + delay_rtn = ap->nmp.delay_ms; + if (inf->hw.i2c_slv_ctrl[i] & BIT_SLV_EN) { + if (delay_new < ap->nmp.delay_ms) + delay_new = ap->nmp.delay_ms; } } + ap = &inf->aux.port[AUX_PORT_SPECIAL]; + if (delay_rtn < ap->nmp.delay_ms) + delay_rtn = ap->nmp.delay_ms; + if (inf->hw.i2c_slv4_ctrl & BIT_SLV_EN) { + if (delay_new < ap->nmp.delay_ms) + delay_new = ap->nmp.delay_ms; + } if (!(inf->hw.user_ctrl & BIT_I2C_MST_EN)) { /* delay will execute when re-enabled */ if (delay_ms) @@ -838,18 +1077,16 @@ static int nvi_aux_delay(struct inv_gyro_state_s *inf, } /* HW global delay */ - for (i = 1; i < (BITS_I2C_MST_DLY - 1); i++) { - ms = (inf->sample_delay_us - (inf->sample_delay_us / - (i + 1))) * 1000; - if (ms >= delay_new) - break; - } - inf->aux.delay_hw = i; - err = nvi_i2c_slv4_ctrl_wr(inf, inf->aux.port[AUX_PORT_SPECIAL].hw_en); + delay_new *= 1000; + delay_new /= inf->sample_delay_us; + delay_new++; + inf->aux.delay_hw = (u8)delay_new; + nvi_i2c_slv4_ctrl_wr(inf, (bool)(inf->hw.i2c_slv4_ctrl & BIT_SLV_EN)); /* HW port delay enable */ val = BIT_DELAY_ES_SHADOW; for (i = 0; i < AUX_PORT_MAX; i++) { - if (inf->aux.port[i].nmp.delay_ms) + ap = &inf->aux.port[i]; + if (ap->nmp.delay_ms) val |= (1 << i); } nvi_i2c_mst_delay_ctrl_wr(inf, val); @@ -862,10 +1099,13 @@ static int nvi_aux_delay(struct inv_gyro_state_s *inf, static int nvi_global_delay(struct inv_gyro_state_s *inf) { unsigned long delay_us; - unsigned long delay_min; - unsigned char val; + unsigned long delay_us_old; + unsigned long fs_hz; + u8 dlpf; + u8 smplrt_div; int i; - int err = 0; + int err; + int err_t = 0; /* find the fastest polling of all the devices */ delay_us = -1; @@ -885,45 +1125,59 @@ static int nvi_global_delay(struct inv_gyro_state_s *inf) } if (delay_us == -1) delay_us = NVI_DELAY_DEFAULT; /* default if nothing found */ - /* find what the fastest the external devices will let MPU go */ - delay_min = 0; - for (i = 0; i < AUX_PORT_MAX; i++) { - if (inf->aux.port[i].enable) { - if (inf->aux.port[i].nmp.delay_ms > delay_min) - delay_min = inf->aux.port[i].nmp.delay_ms; - } - } - if (!delay_min) - /* default if nothing found */ - delay_min = inf->chip_config.min_delay_us; - else - delay_min *= 1000L; /* ms => us */ /* set the limits */ - if (delay_us < delay_min) - delay_us = delay_min; + if (delay_us < inf->chip_config.min_delay_us) + delay_us = inf->chip_config.min_delay_us; if (delay_us > MAX_FIFO_RATE) delay_us = MAX_FIFO_RATE; /* program if new value */ if (delay_us != inf->sample_delay_us) { - dev_dbg(&inf->i2c->dev, "%s %lu\n", __func__, delay_us); + if (inf->aux.dbg) + pr_info("%s %lu\n", __func__, delay_us); + delay_us_old = inf->sample_delay_us; inf->sample_delay_us = delay_us; - inf->irq_dur_us = delay_us; delay_us <<= 1; - for (val = 1; val < ARRAY_SIZE(nvi_lpf_us_tbl); val++) { - if (delay_us < nvi_lpf_us_tbl[val]) + for (dlpf = 0; dlpf < ARRAY_SIZE(nvi_lpf_us_tbl); dlpf++) { + if (delay_us < nvi_lpf_us_tbl[dlpf]) break; } - err |= nvi_config_wr(inf, val); - if (val) - delay_min = 1000; + if (dlpf) + fs_hz = 1000; else - delay_min = 8000; - val = inf->sample_delay_us / delay_min - 1; - err |= nvi_smplrt_div_wr(inf, val); - inf->last_isr_time = get_time_ns(); + fs_hz = 8000; + smplrt_div = inf->sample_delay_us / fs_hz - 1; + dlpf |= (inf->hw.config & 0xF8); + fs_hz = 1000000 / inf->sample_delay_us; + if (inf->sample_delay_us < delay_us_old) { + /* go faster */ + if (inf->chip_type == INV_MPU3050) { + if (inf->mpu_slave != NULL) + inf->mpu_slave->set_lpf(inf, fs_hz); + dlpf |= (inf->hw.config & 0xE7); + } else { + nvi_aux_delay(inf, AUX_PORT_BYPASS, 0); + } + err = nvi_config_wr(inf, dlpf); + if (err < 0) + err_t |= err; + err_t |= nvi_smplrt_div_wr(inf, smplrt_div); + } else { + /* go slower */ + err_t |= nvi_smplrt_div_wr(inf, smplrt_div); + if (inf->chip_type == INV_MPU3050) { + if (inf->mpu_slave != NULL) + inf->mpu_slave->set_lpf(inf, fs_hz); + dlpf |= (inf->hw.config & 0xE7); + err = nvi_config_wr(inf, dlpf); + } else { + err = nvi_config_wr(inf, dlpf); + nvi_aux_delay(inf, AUX_PORT_BYPASS, 0); + } + if (err < 0) + err_t |= err; + } } - nvi_aux_delay(inf, AUX_PORT_BYPASS, 0); - return err; + return err_t; } static void nvi_aux_dbg(struct inv_gyro_state_s *inf, char *tag, int val) @@ -931,7 +1185,7 @@ static void nvi_aux_dbg(struct inv_gyro_state_s *inf, char *tag, int val) struct nvi_mpu_port *n; struct aux_port *p; struct aux_ports *a; - unsigned char data[4]; + u8 data[4]; int i; if (!inf->aux.dbg) @@ -941,20 +1195,22 @@ static void nvi_aux_dbg(struct inv_gyro_state_s *inf, char *tag, int val) for (i = 0; i < AUX_PORT_MAX; i++) { inv_i2c_read(inf, (REG_I2C_SLV0_ADDR + (i * 3)), 3, data); inv_i2c_read(inf, (REG_I2C_SLV0_DO + i), 1, &data[3]); - pr_info("PT=%d AD=%x RG=%x CL=%x DO=%x\n", + /* HW = hardware */ + pr_info("HW: P%d AD=%x RG=%x CL=%x DO=%x\n", i, data[0], data[1], data[2], data[3]); n = &inf->aux.port[i].nmp; - pr_info("PT=%d AD=%x RG=%x CL=%x DO=%x MS=%u US=%lu SB=%x\n", + /* NS = nmp structure */ + pr_info("NS: P%d AD=%x RG=%x CL=%x DO=%x MS=%u US=%lu SB=%x\n", i, n->addr, n->reg, n->ctrl, n->data_out, n->delay_ms, n->delay_us, n->shutdown_bypass); p = &inf->aux.port[i]; - pr_info("PT=%d OF=%u EN=%x FE=%x HE=%x HD=%x HV=%x NS=%lld\n", - i, p->ext_data_offset, p->enable, p->fifo_en, p->hw_en, - p->hw_do, p->hw_valid, p->delay_ns); + /* PS = port structure */ + pr_info("PS: P%d OF=%u EN=%x FE=%x HD=%x\n", i, + p->ext_data_offset, p->enable, p->fifo_en, p->hw_do); } a = &inf->aux; - pr_info("EN=%x GE=%x MD=%x GD=%lu DN=%u BE=%x BL=%d SB=%d\n", - a->enable, (inf->hw.user_ctrl & BIT_I2C_MST_EN), + pr_info("AUX: EN=%x GE=%x MD=%x GD=%lu DN=%u BE=%x BL=%d MX=%d\n", + a->enable, (bool)(inf->hw.user_ctrl & BIT_I2C_MST_EN), (inf->hw.i2c_slv4_ctrl & BITS_I2C_MST_DLY), inf->sample_delay_us, a->ext_data_n, (inf->hw.int_pin_cfg & BIT_BYPASS_EN), a->bypass_lock, @@ -975,27 +1231,24 @@ static void nvi_aux_read(struct inv_gyro_state_s *inf) (!(inf->hw.user_ctrl & BIT_I2C_MST_EN))) return; - timestamp1 = get_time_ns(); + timestamp1 = nvi_ts_ns(); err = inv_i2c_read(inf, REG_EXT_SENS_DATA_00, inf->aux.ext_data_n, (unsigned char *)&inf->aux.ext_data); if (err) return; - timestamp2 = get_time_ns(); + timestamp2 = nvi_ts_ns(); timestamp1 = timestamp1 + ((timestamp2 - timestamp1) / 2); for (i = 0; i < AUX_PORT_SPECIAL; i++) { ap = &inf->aux.port[i]; - if ((ap->nmp.addr & BIT_I2C_READ) && + if ((inf->hw.i2c_slv_ctrl[i] & BIT_SLV_EN) && (!ap->fifo_en) && + (ap->nmp.addr & BIT_I2C_READ) && (ap->nmp.handler != NULL)) { - if ((unsigned long)(timestamp2 - ap->delay_ns) - >= (ap->nmp.delay_us * 1000)) { - ap->delay_ns = timestamp2; - p = &inf->aux.ext_data[ap->ext_data_offset]; - len = ap->nmp.ctrl & BITS_I2C_SLV_CTRL_LEN; - ap->nmp.handler(p, len, timestamp1, - ap->nmp.ext_driver); - } + p = &inf->aux.ext_data[ap->ext_data_offset]; + len = ap->nmp.ctrl & BITS_I2C_SLV_CTRL_LEN; + ap->nmp.handler(p, len, timestamp1, + ap->nmp.ext_driver); } } } @@ -1007,8 +1260,8 @@ static void nvi_aux_ext_data_offset(struct inv_gyro_state_s *inf) offset = 0; for (i = 0; i < AUX_PORT_SPECIAL; i++) { - if ((inf->aux.port[i].hw_en) && (inf->aux.port[i].nmp.addr & - BIT_I2C_READ)) { + if ((inf->hw.i2c_slv_ctrl[i] & BIT_SLV_EN) && + (inf->aux.port[i].nmp.addr & BIT_I2C_READ)) { inf->aux.port[i].ext_data_offset = offset; offset += (inf->aux.port[i].nmp.ctrl & BITS_I2C_SLV_CTRL_LEN); @@ -1023,26 +1276,12 @@ static void nvi_aux_ext_data_offset(struct inv_gyro_state_s *inf) return; } -static int nvi_aux_port_do(struct inv_gyro_state_s *inf, - int port, u8 data_out) -{ - unsigned char reg; - int err; - - if (port == AUX_PORT_SPECIAL) - reg = REG_I2C_SLV4_DO; - else - reg = (REG_I2C_SLV0_DO + port); - err = inv_i2c_single_write(inf, reg, data_out); - return err; -} - static int nvi_aux_port_data_out(struct inv_gyro_state_s *inf, int port, u8 data_out) { int err; - err = nvi_aux_port_do(inf, port, data_out); + err = nvi_i2c_slv_do_wr(inf, port, data_out); if (!err) { inf->aux.port[port].nmp.data_out = data_out; inf->aux.port[port].hw_do = true; @@ -1058,11 +1297,9 @@ static int nvi_aux_port_wr(struct inv_gyro_state_s *inf, int port) int err; ap = &inf->aux.port[port]; - err = inv_i2c_single_write(inf, (REG_I2C_SLV0_ADDR + (port * 3)), - ap->nmp.addr); - err |= inv_i2c_single_write(inf, (REG_I2C_SLV0_REG + (port * 3)), - ap->nmp.reg); - err |= nvi_aux_port_do(inf, port, ap->nmp.data_out); + err = nvi_i2c_slv_addr_wr(inf, port, ap->nmp.addr); + err |= nvi_i2c_slv_reg_wr(inf, port, ap->nmp.reg); + err |= nvi_i2c_slv_do_wr(inf, port, ap->nmp.data_out); return err; } @@ -1070,47 +1307,45 @@ static int nvi_aux_port_en(struct inv_gyro_state_s *inf, int port, bool en) { struct aux_port *ap; - unsigned char reg; - unsigned char val; + u8 val; int err = 0; inf->aux.ext_data_n = 0; ap = &inf->aux.port[port]; - if ((!ap->hw_valid) && en) { + if ((!(inf->hw.i2c_slv_addr[port])) && en) { err = nvi_aux_port_wr(inf, port); - if (!err) { - ap->hw_valid = true; + if (!err) ap->hw_do = true; - } } if ((!ap->hw_do) && en) nvi_aux_port_data_out(inf, port, ap->nmp.data_out); if (port == AUX_PORT_SPECIAL) { err = nvi_i2c_slv4_ctrl_wr(inf, en); } else { - reg = (REG_I2C_SLV0_CTRL + (port * 3)); if (en) val = (ap->nmp.ctrl | BIT_SLV_EN); else val = 0; - err = inv_i2c_single_write(inf, reg, val); + err = nvi_i2c_slv_ctrl_wr(inf, port, val); } - if (!err) { - ap->hw_en = en; + if (err > 0) { nvi_aux_ext_data_offset(inf); + err = 0; } return err; } +static int nvi_reset(struct inv_gyro_state_s *inf, + bool reset_fifo, bool reset_i2c); + static int nvi_aux_enable(struct inv_gyro_state_s *inf, bool enable) { bool en; unsigned int i; - int err; + int err = 0; if (inf->hw.int_pin_cfg & BIT_BYPASS_EN) enable = false; - en = false; if (enable) { /* global enable is honored only if a port is enabled */ @@ -1120,29 +1355,33 @@ static int nvi_aux_enable(struct inv_gyro_state_s *inf, bool enable) break; } } - if (en == (inf->hw.user_ctrl & BIT_I2C_MST_EN)) { + if (en == (bool)(inf->hw.user_ctrl & BIT_I2C_MST_EN)) /* if already on then just update delays */ nvi_global_delay(inf); - } } inf->aux.enable = en; - if ((inf->hw.user_ctrl & BIT_I2C_MST_EN) == en) + if ((bool)(inf->hw.user_ctrl & BIT_I2C_MST_EN) == en) { + if (inf->aux.reset_fifo) + nvi_reset(inf, true, false); return 0; + } if (en) { for (i = 0; i < AUX_PORT_MAX; i++) { if (inf->aux.port[i].enable) err |= nvi_aux_port_en(inf, i, true); } - nvi_motion_detect_enable(inf, 0); } else { for (i = 0; i < AUX_PORT_MAX; i++) { - if (inf->aux.port[i].hw_valid) + if (inf->hw.i2c_slv_addr[i]) nvi_aux_port_en(inf, i, false); } } - err = nvi_global_delay(inf); - err |= nvi_user_ctrl_en_wr(inf, true, en); + err |= nvi_global_delay(inf); + if (inf->aux.reset_fifo) + err |= nvi_reset(inf, true, false); + else + err |= nvi_user_ctrl_en_wr(inf, true, en); return err; } @@ -1154,7 +1393,11 @@ static int nvi_aux_port_enable(struct inv_gyro_state_s *inf, ap = &inf->aux.port[port]; ap->enable = enable; - ap->fifo_en = false; + if ((!enable) || (!(ap->nmp.addr & BIT_I2C_READ))) + fifo_enable = false; + if (ap->fifo_en != fifo_enable) + inf->aux.reset_fifo = true; + ap->fifo_en = fifo_enable; if (enable && (inf->hw.int_pin_cfg & BIT_BYPASS_EN)) return 0; @@ -1166,58 +1409,51 @@ static int nvi_aux_port_enable(struct inv_gyro_state_s *inf, static int nvi_reset(struct inv_gyro_state_s *inf, bool reset_fifo, bool reset_i2c) { + u8 val; unsigned long flags; - unsigned char val; - int i; int err; + dev_dbg(&inf->i2c->dev, "%s FIFO=%x I2C=%x\n", + __func__, reset_fifo, reset_i2c); err = nvi_int_enable_wr(inf, false); val = 0; if (reset_i2c) { + inf->aux.reset_i2c = false; /* nvi_aux_bypass_enable(inf, false)? */ err |= nvi_aux_enable(inf, false); - for (i = 0; i < AUX_PORT_MAX; i++) { - inf->aux.port[i].hw_valid = false; - inf->aux.port[i].hw_en = false; - } - inf->aux.need_reset = false; - val |= BIT_I2C_MST_RST; + val |= inf->reg->i2c_mst_reset; } if (reset_fifo) - val |= BIT_FIFO_RST; - err |= nvi_user_ctrl_en_wr(inf, ~reset_fifo, ~reset_i2c); + val |= inf->reg->fifo_reset; + err |= nvi_user_ctrl_en_wr(inf, !reset_fifo, !reset_i2c); val |= inf->hw.user_ctrl; err |= nvi_user_ctrl_reset_wr(inf, val); - if (reset_fifo) { + if (reset_i2c) + err |= nvi_aux_enable(inf, true); + err |= nvi_user_ctrl_en_wr(inf, true, true); + if (reset_fifo && (inf->hw.user_ctrl & BIT_FIFO_EN)) { spin_lock_irqsave(&inf->time_stamp_lock, flags); kfifo_reset(&inf->trigger.timestamps); spin_unlock_irqrestore(&inf->time_stamp_lock, flags); - inf->last_isr_time = get_time_ns(); + inf->fifo_ts = nvi_ts_ns(); + inf->fifo_reset_3050 = true; } - if (reset_i2c) - err |= nvi_aux_enable(inf, true); - else - err |= nvi_user_ctrl_en_wr(inf, true, true); err |= nvi_int_enable_wr(inf, true); return err; } static int nvi_aux_port_free(struct inv_gyro_state_s *inf, int port) { - bool hw_valid; - int err = 0; - - hw_valid = inf->aux.port[port].hw_valid; memset(&inf->aux.port[port], 0, sizeof(struct aux_port)); - if (hw_valid) { + if (inf->hw.i2c_slv_addr[port]) { nvi_aux_port_wr(inf, port); nvi_aux_port_en(inf, port, false); nvi_aux_enable(inf, false); nvi_aux_enable(inf, true); if (port != AUX_PORT_SPECIAL) - inf->aux.need_reset = true; + inf->aux.reset_i2c = true; } - return err; + return 0; } static int nvi_aux_port_alloc(struct inv_gyro_state_s *inf, @@ -1225,7 +1461,7 @@ static int nvi_aux_port_alloc(struct inv_gyro_state_s *inf, { int i; - if (inf->aux.need_reset) + if (inf->aux.reset_i2c) nvi_reset(inf, false, true); if (port < 0) { for (i = 0; i < AUX_PORT_SPECIAL; i++) { @@ -1248,7 +1484,7 @@ static int nvi_aux_port_alloc(struct inv_gyro_state_s *inf, static int nvi_aux_bypass_enable(struct inv_gyro_state_s *inf, bool enable) { - unsigned char val; + u8 val; int err; if ((bool)(inf->hw.int_pin_cfg & BIT_BYPASS_EN) == enable) @@ -1272,14 +1508,25 @@ static int nvi_aux_bypass_enable(struct inv_gyro_state_s *inf, bool enable) static int nvi_aux_bypass_request(struct inv_gyro_state_s *inf, bool enable) { + s64 ns; + s64 to; int err = 0; if ((bool)(inf->hw.int_pin_cfg & BIT_BYPASS_EN) == enable) { + inf->aux.bypass_timeout_ns = nvi_ts_ns(); inf->aux.bypass_lock++; + if (!inf->aux.bypass_lock) + dev_err(&inf->i2c->dev, "%s rollover ERR\n", __func__); } else { if (inf->aux.bypass_lock) { - err = -EBUSY; - } else { + ns = nvi_ts_ns() - inf->aux.bypass_timeout_ns; + to = inf->chip_config.bypass_timeout_ms * 1000000; + if (ns > to) + inf->aux.bypass_lock = 0; + else + err = -EBUSY; + } + if (!inf->aux.bypass_lock) { err = nvi_aux_bypass_enable(inf, enable); if (err) dev_err(&inf->i2c->dev, "%s ERR=%d\n", @@ -1293,7 +1540,7 @@ static int nvi_aux_bypass_request(struct inv_gyro_state_s *inf, bool enable) static int nvi_aux_bypass_release(struct inv_gyro_state_s *inf) { - int err; + int err = 0; if (inf->aux.bypass_lock) inf->aux.bypass_lock--; @@ -1302,7 +1549,7 @@ static int nvi_aux_bypass_release(struct inv_gyro_state_s *inf) if (err) dev_err(&inf->i2c->dev, "%s ERR=%d\n", __func__, err); } - return 0; + return err; } static int nvi_aux_dev_valid(struct inv_gyro_state_s *inf, @@ -1326,6 +1573,7 @@ static int nvi_aux_dev_valid(struct inv_gyro_state_s *inf, /* enable it */ inf->aux.port[AUX_PORT_SPECIAL].nmp.delay_ms = 0; + inf->aux.port[AUX_PORT_SPECIAL].nmp.delay_us = NVI_DELAY_US_MIN; err = nvi_aux_port_enable(inf, AUX_PORT_SPECIAL, true, false); if (err) { nvi_aux_port_free(inf, AUX_PORT_SPECIAL); @@ -1335,12 +1583,12 @@ static int nvi_aux_dev_valid(struct inv_gyro_state_s *inf, /* now turn off all the other ports for fastest response */ for (i = 0; i < AUX_PORT_SPECIAL; i++) { - if (inf->aux.port[i].hw_valid) + if (inf->hw.i2c_slv_addr[i]) nvi_aux_port_en(inf, i, false); } /* start reading the results */ - for (i = 0; i < AUX_DEV_VALID_READ_MAX; i++) { - mdelay(1); + for (i = 0; i < AUX_DEV_VALID_READ_LOOP_MAX; i++) { + mdelay(AUX_DEV_VALID_READ_DELAY_MS); val = 0; err = inv_i2c_read(inf, REG_I2C_MST_STATUS, 1, &val); if (err) @@ -1352,7 +1600,7 @@ static int nvi_aux_dev_valid(struct inv_gyro_state_s *inf, /* these will restore all previously disabled ports */ nvi_aux_bypass_release(inf); nvi_aux_port_free(inf, AUX_PORT_SPECIAL); - if (i == AUX_DEV_VALID_READ_MAX) + if (i == AUX_DEV_VALID_READ_LOOP_MAX) return -ENODEV; if (val & 0x10) /* NACK */ @@ -1378,7 +1626,7 @@ static int nvi_aux_mpu_call_pre(struct inv_gyro_state_s *inf, int port) if ((port < 0) || (port >= AUX_PORT_SPECIAL)) return -EINVAL; - if (inf->shutdown) + if (inf->shutdown || inf->suspend) return -EPERM; if (!inf->aux.port[port].nmp.addr) @@ -1410,7 +1658,7 @@ int nvi_mpu_dev_valid(struct nvi_mpu_port *nmp, u8 *data) if (inf->aux.dbg) pr_info("%s\n", __func__); } else { - pr_debug("%s\n", __func__); + pr_debug("%s ERR -EAGAIN\n", __func__); return -EAGAIN; } @@ -1420,7 +1668,7 @@ int nvi_mpu_dev_valid(struct nvi_mpu_port *nmp, u8 *data) if ((nmp->addr & BIT_I2C_READ) && (data == NULL)) return -EINVAL; - if (inf->shutdown) + if (inf->shutdown || inf->suspend) return -EPERM; mutex_lock(&inf->mutex); @@ -1443,7 +1691,7 @@ int nvi_mpu_port_alloc(struct nvi_mpu_port *nmp) if (inf->aux.dbg) pr_info("%s\n", __func__); } else { - pr_debug("%s\n", __func__); + pr_debug("%s ERR -EAGAIN\n", __func__); return -EAGAIN; } @@ -1453,7 +1701,7 @@ int nvi_mpu_port_alloc(struct nvi_mpu_port *nmp) if (!(nmp->ctrl & BITS_I2C_SLV_CTRL_LEN)) return -EINVAL; - if (inf->shutdown) + if (inf->shutdown || inf->suspend) return -EPERM; mutex_lock(&inf->mutex); @@ -1475,7 +1723,7 @@ int nvi_mpu_port_free(int port) if (inf->aux.dbg) pr_info("%s port %d\n", __func__, port); } else { - pr_debug("%s port %d\n", __func__, port); + pr_debug("%s port %d ERR -EAGAIN\n", __func__, port); return -EAGAIN; } @@ -1501,7 +1749,8 @@ int nvi_mpu_enable(int port, bool enable, bool fifo_enable) if (inf->aux.dbg) pr_info("%s port %d: %x\n", __func__, port, enable); } else { - pr_debug("%s port %d: %x\n", __func__, port, enable); + pr_debug("%s port %d: %x ERR -EAGAIN\n", + __func__, port, enable); return -EAGAIN; } @@ -1527,7 +1776,8 @@ int nvi_mpu_delay_ms(int port, u8 delay_ms) if (inf->aux.dbg) pr_info("%s port %d: %u\n", __func__, port, delay_ms); } else { - pr_debug("%s port %d: %u\n", __func__, port, delay_ms); + pr_debug("%s port %d: %u ERR -EAGAIN\n", + __func__, port, delay_ms); return -EAGAIN; } @@ -1535,7 +1785,7 @@ int nvi_mpu_delay_ms(int port, u8 delay_ms) if (err) return err; - if (inf->aux.port[port].hw_en) { + if (inf->hw.i2c_slv_ctrl[port] & BIT_SLV_EN) { err = nvi_aux_delay(inf, port, delay_ms); nvi_global_delay(inf); } else { @@ -1556,7 +1806,8 @@ int nvi_mpu_delay_us(int port, unsigned long delay_us) if (inf->aux.dbg) pr_info("%s port %d: %lu\n", __func__, port, delay_us); } else { - pr_debug("%s port %d: %lu\n", __func__, port, delay_us); + pr_debug("%s port %d: %lu ERR -EAGAIN\n", + __func__, port, delay_us); return -EAGAIN; } @@ -1565,7 +1816,7 @@ int nvi_mpu_delay_us(int port, unsigned long delay_us) return err; inf->aux.port[port].nmp.delay_us = delay_us; - if (inf->aux.port[port].hw_en) + if (inf->hw.i2c_slv_ctrl[port] & BIT_SLV_EN) err = nvi_global_delay(inf); err = nvi_aux_mpu_call_post(inf, "nvi_mpu_delay_us err: ", err); return err; @@ -1575,7 +1826,7 @@ EXPORT_SYMBOL(nvi_mpu_delay_us); int nvi_mpu_data_out(int port, u8 data_out) { struct inv_gyro_state_s *inf; - int err = 0; + int err; inf = inf_local; if (inf == NULL) @@ -1585,7 +1836,7 @@ int nvi_mpu_data_out(int port, u8 data_out) if (err) return err; - if (inf->aux.port[port].hw_en) { + if (inf->hw.i2c_slv_ctrl[port] & BIT_SLV_EN) { err = nvi_aux_port_data_out(inf, port, data_out); } else { inf->aux.port[port].nmp.data_out = data_out; @@ -1608,11 +1859,11 @@ int nvi_mpu_bypass_request(bool enable) if (inf->aux.dbg) pr_info("%s\n", __func__); } else { - pr_debug("%s\n", __func__); + pr_debug("%s ERR -EAGAIN\n", __func__); return -EAGAIN; } - if (inf->shutdown) + if (inf->shutdown || inf->suspend) return -EPERM; mutex_lock(&inf->mutex); @@ -1627,7 +1878,6 @@ EXPORT_SYMBOL(nvi_mpu_bypass_request); int nvi_mpu_bypass_release(void) { struct inv_gyro_state_s *inf; - int err; inf = inf_local; if (inf != NULL) { @@ -1635,24 +1885,24 @@ int nvi_mpu_bypass_release(void) pr_info("%s\n", __func__); } else { pr_debug("%s\n", __func__); - return -EAGAIN; + return 0; } - if (inf->shutdown) - return -EPERM; + if (inf->shutdown || inf->suspend) + return 0; mutex_lock(&inf->mutex); nvi_pm(inf, NVI_PM_ON); - err = nvi_aux_bypass_release(inf); + nvi_aux_bypass_release(inf); nvi_pm(inf, NVI_PM_AUTO); - err = nvi_aux_mpu_call_post(inf, "nvi_mpu_bypass_release err: ", err); - return err; + nvi_aux_mpu_call_post(inf, "nvi_mpu_bypass_release", 0); + return 0; } EXPORT_SYMBOL(nvi_mpu_bypass_release); -static int nvi_gyro_enable(struct inv_gyro_state_s *inf, - unsigned char enable, unsigned char fifo_enable) +int nvi_gyro_enable(struct inv_gyro_state_s *inf, + unsigned char enable, unsigned char fifo_enable) { unsigned char enable_old; unsigned char fifo_enable_old; @@ -1670,7 +1920,6 @@ static int nvi_gyro_enable(struct inv_gyro_state_s *inf, inf->chip_config.gyro_fsr); if (err < 0) err_t |= err; - nvi_motion_detect_enable(inf, 0); } nvi_global_delay(inf); } @@ -1680,12 +1929,16 @@ static int nvi_gyro_enable(struct inv_gyro_state_s *inf, inf->chip_config.gyro_enable = enable_old; inf->chip_config.gyro_fifo_enable = fifo_enable_old; } + if (inf->chip_config.gyro_enable) + inf->chip_config.temp_enable |= NVI_TEMP_GYRO; + else + inf->chip_config.temp_enable &= ~NVI_TEMP_GYRO; err_t |= nvi_pm(inf, NVI_PM_AUTO); return err_t; } -static int nvi_accl_enable(struct inv_gyro_state_s *inf, - unsigned char enable, unsigned char fifo_enable) +int nvi_accl_enable(struct inv_gyro_state_s *inf, + unsigned char enable, unsigned char fifo_enable) { unsigned char enable_old; unsigned char fifo_enable_old; @@ -1698,20 +1951,36 @@ static int nvi_accl_enable(struct inv_gyro_state_s *inf, inf->chip_config.accl_enable = enable; err_t = nvi_pm(inf, NVI_PM_ON); if (enable != enable_old) { - if (enable) { - err = nvi_accel_config_wr(inf, + if (inf->chip_type == INV_MPU3050) { + if (inf->mpu_slave != NULL) { + if (enable) { + inf->mpu_slave->resume(inf); + inf->mpu_slave->set_fs(inf, + inf->chip_config.accl_fsr); + } else { + inf->mpu_slave->suspend(inf); + } + } + } else { + if (enable) { + err = nvi_accel_config_wr(inf, inf->chip_config.accl_fsr, 0); - if (err < 0) - err_t |= err; + if (err < 0) + err_t |= err; + } } nvi_global_delay(inf); } if (fifo_enable_old != fifo_enable) - err_t = nvi_reset(inf, true, false); + err_t |= nvi_reset(inf, true, false); if (err_t) { inf->chip_config.accl_enable = enable_old; inf->chip_config.accl_fifo_enable = fifo_enable_old; } + if (inf->chip_config.accl_enable) + inf->chip_config.temp_enable |= NVI_TEMP_ACCL; + else + inf->chip_config.temp_enable &= ~NVI_TEMP_ACCL; err_t |= nvi_pm(inf, NVI_PM_AUTO); return err_t; } @@ -1732,15 +2001,11 @@ static ssize_t nvi_gyro_enable_store(struct device *dev, return -EINVAL; if (enable > 7) - return -EINVAL; - + enable = 7; mutex_lock(&inf->mutex); - dev_dbg(&inf->i2c->dev, "%s: %x\n", __func__, enable); if (enable != inf->chip_config.gyro_enable) { - if (enable) - fifo_enable = inf->chip_config.gyro_fifo_enable; - else - fifo_enable = 0; + dev_dbg(&inf->i2c->dev, "%s: %x\n", __func__, enable); + fifo_enable = inf->chip_config.gyro_fifo_enable & enable; err = nvi_gyro_enable(inf, enable, fifo_enable); } mutex_unlock(&inf->mutex); @@ -1775,16 +2040,14 @@ ssize_t nvi_gyro_fifo_enable_store(struct device *dev, if (err) return -EINVAL; + if (fifo_enable > 7) + fifo_enable = 7; mutex_lock(&inf->mutex); - dev_dbg(&inf->i2c->dev, "%s: %x\n", __func__, fifo_enable); - enable = inf->chip_config.gyro_enable; - if (fifo_enable) { - fifo_enable = 1; - if (!enable) - enable = 7; - } - if (fifo_enable != inf->chip_config.gyro_fifo_enable) + if (fifo_enable != inf->chip_config.gyro_fifo_enable) { + dev_dbg(&inf->i2c->dev, "%s: %x\n", __func__, fifo_enable); + enable = inf->chip_config.gyro_enable | fifo_enable; err = nvi_gyro_enable(inf, enable, fifo_enable); + } mutex_unlock(&inf->mutex); if (err) { dev_err(&inf->i2c->dev, "%s: %x ERR=%d\n", @@ -1817,11 +2080,11 @@ static ssize_t inv_gyro_delay_store(struct device *dev, if (err) return err; - mutex_lock(&inf->mutex); - dev_dbg(&inf->i2c->dev, "%s: %lu\n", __func__, gyro_delay_us); if (gyro_delay_us < NVI_INPUT_GYRO_DELAY_US_MIN) gyro_delay_us = NVI_INPUT_GYRO_DELAY_US_MIN; + mutex_lock(&inf->mutex); if (gyro_delay_us != inf->chip_config.gyro_delay_us) { + dev_dbg(&inf->i2c->dev, "%s: %lu\n", __func__, gyro_delay_us); gyro_delay_us_old = inf->chip_config.gyro_delay_us; inf->chip_config.gyro_delay_us = gyro_delay_us; if (inf->chip_config.gyro_enable) { @@ -1841,6 +2104,18 @@ static ssize_t inv_gyro_delay_store(struct device *dev, return count; } +static ssize_t nvi_gyro_delay_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct inv_gyro_state_s *inf; + + inf = dev_get_drvdata(dev); + if (inf->chip_config.gyro_enable) + return sprintf(buf, "%lu\n", inf->sample_delay_us); + + return sprintf(buf, "%d\n", NVI_INPUT_GYRO_DELAY_US_MIN); +} + static ssize_t nvi_gyro_resolution_store(struct device *dev, struct device_attribute *attr, const char *buf, size_t count) @@ -1852,7 +2127,7 @@ static ssize_t nvi_gyro_resolution_store(struct device *dev, if (kstrtouint(buf, 10, &resolution)) return -EINVAL; - dev_dbg(&inf->i2c->dev, "%s %u", __func__, resolution); + dev_dbg(&inf->i2c->dev, "%s %u\n", __func__, resolution); inf->chip_config.gyro_resolution = resolution; return count; } @@ -1872,18 +2147,6 @@ static ssize_t nvi_gyro_resolution_show(struct device *dev, return sprintf(buf, "%u\n", resolution); } -static ssize_t nvi_gyro_delay_show(struct device *dev, - struct device_attribute *attr, char *buf) -{ - struct inv_gyro_state_s *inf; - - inf = dev_get_drvdata(dev); - if (inf->chip_config.gyro_enable) - return sprintf(buf, "%lu\n", inf->sample_delay_us); - - return sprintf(buf, "%d\n", NVI_INPUT_GYRO_DELAY_US_MIN); -} - static ssize_t nvi_gyro_max_range_store(struct device *dev, struct device_attribute *attr, const char *buf, size_t count) @@ -1901,12 +2164,11 @@ static ssize_t nvi_gyro_max_range_store(struct device *dev, return -EINVAL; mutex_lock(&inf->mutex); - dev_dbg(&inf->i2c->dev, "%s: %x\n", __func__, fsr); if (fsr != inf->chip_config.gyro_fsr) { + dev_dbg(&inf->i2c->dev, "%s: %x\n", __func__, fsr); if (inf->chip_config.gyro_enable) { err = nvi_gyro_config_wr(inf, fsr); - if (err > 0) - /* reset fifo to purge old data */ + if ((err > 0) && (inf->hw.fifo_en & BITS_GYRO_OUT)) nvi_reset(inf, true, false); } if (err >= 0) @@ -1949,15 +2211,11 @@ static ssize_t nvi_accl_enable_store(struct device *dev, return -EINVAL; if (enable > 7) - return -EINVAL; - + enable = 7; mutex_lock(&inf->mutex); - dev_dbg(&inf->i2c->dev, "%s: %x\n", __func__, enable); if (enable != inf->chip_config.accl_enable) { - if (enable) - fifo_enable = inf->chip_config.accl_fifo_enable; - else - fifo_enable = 0; + dev_dbg(&inf->i2c->dev, "%s: %x\n", __func__, enable); + fifo_enable = inf->chip_config.accl_fifo_enable & enable; err = nvi_accl_enable(inf, enable, fifo_enable); } mutex_unlock(&inf->mutex); @@ -1970,8 +2228,8 @@ static ssize_t nvi_accl_enable_store(struct device *dev, return count; } -ssize_t nvi_accl_enable_show(struct device *dev, - struct device_attribute *attr, char *buf) +static ssize_t nvi_accl_enable_show(struct device *dev, + struct device_attribute *attr, char *buf) { struct inv_gyro_state_s *inf = dev_get_drvdata(dev); @@ -1992,16 +2250,14 @@ static ssize_t nvi_accl_fifo_enable_store(struct device *dev, if (err) return -EINVAL; + if (fifo_enable > 7) + fifo_enable = 7; mutex_lock(&inf->mutex); - dev_dbg(&inf->i2c->dev, "%s: %x\n", __func__, fifo_enable); - enable = inf->chip_config.accl_enable; - if (fifo_enable) { - fifo_enable = 1; - if (!enable) - enable = 7; - } - if (fifo_enable != inf->chip_config.accl_fifo_enable) + if (fifo_enable != inf->chip_config.accl_fifo_enable) { + dev_dbg(&inf->i2c->dev, "%s: %x\n", __func__, fifo_enable); + enable = inf->chip_config.accl_enable | fifo_enable; err = nvi_accl_enable(inf, enable, fifo_enable); + } mutex_unlock(&inf->mutex); if (err) { dev_err(&inf->i2c->dev, "%s: %x ERR=%d\n", @@ -2012,8 +2268,9 @@ static ssize_t nvi_accl_fifo_enable_store(struct device *dev, return count; } -ssize_t nvi_accl_fifo_enable_show(struct device *dev, - struct device_attribute *attr, char *buf) +static ssize_t nvi_accl_fifo_enable_show(struct device *dev, + struct device_attribute *attr, + char *buf) { struct inv_gyro_state_s *inf = dev_get_drvdata(dev); @@ -2034,14 +2291,16 @@ static ssize_t nvi_accl_delay_store(struct device *dev, if (err) return err; - mutex_lock(&inf->mutex); - dev_dbg(&inf->i2c->dev, "%s: %lu\n", __func__, accl_delay_us); if (accl_delay_us < NVI_INPUT_ACCL_DELAY_US_MIN) accl_delay_us = NVI_INPUT_ACCL_DELAY_US_MIN; + mutex_lock(&inf->mutex); if (accl_delay_us != inf->chip_config.accl_delay_us) { + dev_dbg(&inf->i2c->dev, "%s: %lu\n", __func__, accl_delay_us); accl_delay_us_old = inf->chip_config.accl_delay_us; inf->chip_config.accl_delay_us = accl_delay_us; if (inf->chip_config.accl_enable) { + if (inf->hw.pwr_mgmt_1 & inf->reg->cycle) + nvi_pm(inf, NVI_PM_ON); err = nvi_global_delay(inf); if (err) inf->chip_config.accl_delay_us = @@ -2083,7 +2342,7 @@ static ssize_t nvi_accl_resolution_store(struct device *dev, if (kstrtouint(buf, 10, &resolution)) return -EINVAL; - dev_dbg(&inf->i2c->dev, "%s %u", __func__, resolution); + dev_dbg(&inf->i2c->dev, "%s %u\n", __func__, resolution); inf->chip_config.accl_resolution = resolution; return count; } @@ -2120,12 +2379,21 @@ static ssize_t nvi_accl_max_range_store(struct device *dev, return -EINVAL; mutex_lock(&inf->mutex); - dev_dbg(&inf->i2c->dev, "%s: %x\n", __func__, fsr); if (fsr != inf->chip_config.accl_fsr) { + dev_dbg(&inf->i2c->dev, "%s: %x\n", __func__, fsr); if (inf->chip_config.accl_enable) { - err = nvi_accel_config_wr(inf, fsr, 0); - if (err > 0) - /* reset fifo to purge old data */ + if (inf->hw.pwr_mgmt_1 & inf->reg->cycle) + nvi_pm(inf, NVI_PM_ON); + if (inf->chip_type == INV_MPU3050) { + if (inf->mpu_slave != NULL) { + inf->mpu_slave->set_fs(inf, fsr); + err = 1; + } + } else { + err = nvi_accel_config_wr(inf, fsr, 0); + } + if ((err > 0) && (inf->hw.fifo_en & + inf->reg->accl_fifo_en)) nvi_reset(inf, true, false); nvi_pm(inf, NVI_PM_AUTO); } @@ -2167,8 +2435,8 @@ static ssize_t nvi_lpa_delay_enable_store(struct device *dev, if (err) return err; - mutex_lock(&inf->mutex); dev_dbg(&inf->i2c->dev, "%s: %lu\n", __func__, lpa_delay_us); + mutex_lock(&inf->mutex); inf->chip_config.lpa_delay_us = lpa_delay_us; err = nvi_pm(inf, NVI_PM_AUTO); mutex_unlock(&inf->mutex); @@ -2187,6 +2455,49 @@ static ssize_t nvi_lpa_delay_enable_show(struct device *dev, return sprintf(buf, "%lu\n", inf->chip_config.lpa_delay_us); } +static ssize_t nvi_mot_enable_store(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t count) +{ + struct inv_gyro_state_s *inf; + unsigned char mot_enable; + int err; + + inf = dev_get_drvdata(dev); + err = kstrtou8(buf, 10, &mot_enable); + if (err) + return -EINVAL; + + if (mot_enable > NVI_MOT_DBG) + return -EINVAL; + + dev_dbg(&inf->i2c->dev, "%s: %u\n", __func__, mot_enable); + inf->chip_config.mot_enable = mot_enable; + if (!mot_enable) { + mutex_lock(&inf->mutex); + nvi_pm(inf, NVI_PM_ON); + err = nvi_motion_detect_enable(inf, 0); + err |= nvi_pm(inf, NVI_PM_AUTO); + mutex_unlock(&inf->mutex); + if (err) { + dev_err(&inf->i2c->dev, "%s: %u ERR=%d\n", + __func__, mot_enable, err); + return err; + } + } + + return count; +} + +static ssize_t nvi_mot_enable_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct inv_gyro_state_s *inf = dev_get_drvdata(dev); + + return sprintf(buf, "%x (0=dis 1=en 2=dbg)\n", + inf->chip_config.mot_enable); +} + static ssize_t nvi_motion_thr_store(struct device *dev, struct device_attribute *attr, const char *buf, size_t count) @@ -2201,10 +2512,15 @@ static ssize_t nvi_motion_thr_store(struct device *dev, return -EINVAL; mutex_lock(&inf->mutex); - dev_dbg(&inf->i2c->dev, "%s: %u\n", __func__, mot_thr); - nvi_pm(inf, NVI_PM_ON); - err = nvi_motion_detect_enable(inf, mot_thr); - err |= nvi_pm(inf, NVI_PM_AUTO); + if (((!inf->chip_config.gyro_enable) && (!inf->aux.enable) && + inf->chip_config.mot_enable)) { + dev_dbg(&inf->i2c->dev, "%s: %u\n", __func__, mot_thr); + if (inf->chip_config.mot_enable == NVI_MOT_DBG) + pr_info("%s: %u\n", __func__, mot_thr); + nvi_pm(inf, NVI_PM_ON); + err = nvi_motion_detect_enable(inf, mot_thr); + err |= nvi_pm(inf, NVI_PM_AUTO); + } mutex_unlock(&inf->mutex); if (err) { dev_err(&inf->i2c->dev, "%s: %u ERR=%d\n", @@ -2222,7 +2538,7 @@ static ssize_t nvi_motion_thr_show(struct device *dev, unsigned char mot_thr; inf = dev_get_drvdata(dev); - if (inf->mot_enable) + if ((inf->hw.accl_config & 0x07) == 0x07) mot_thr = inf->hw.mot_thr; else mot_thr = 0; @@ -2307,6 +2623,95 @@ static ssize_t nvi_motion_count_show(struct device *dev, return sprintf(buf, "%u\n", inf->chip_config.mot_cnt); } +static ssize_t nvi_bypass_timeout_ms_store(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t count) +{ + struct inv_gyro_state_s *inf; + unsigned int bypass_timeout_ms; + int err; + + inf = dev_get_drvdata(dev); + err = kstrtouint(buf, 10, &bypass_timeout_ms); + if (err) + return -EINVAL; + + dev_dbg(&inf->i2c->dev, "%s: %u\n", __func__, bypass_timeout_ms); + inf->chip_config.bypass_timeout_ms = bypass_timeout_ms; + return count; +} + +static ssize_t nvi_bypass_timeout_ms_show(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct inv_gyro_state_s *inf = dev_get_drvdata(dev); + + return sprintf(buf, "%u\n", inf->chip_config.bypass_timeout_ms); +} + +static ssize_t nvi_min_delay_us_store(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t count) +{ + struct inv_gyro_state_s *inf; + unsigned long min_delay_us; + int err; + + inf = dev_get_drvdata(dev); + err = kstrtoul(buf, 10, &min_delay_us); + if (err) + return -EINVAL; + + dev_dbg(&inf->i2c->dev, "%s: %lu\n", __func__, min_delay_us); + inf->chip_config.min_delay_us = min_delay_us; + return count; +} + +static ssize_t nvi_min_delay_us_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct inv_gyro_state_s *inf = dev_get_drvdata(dev); + + return sprintf(buf, "%lu\n", inf->chip_config.min_delay_us); +} + +static ssize_t nvi_fifo_thr_store(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t count) +{ + struct inv_gyro_state_s *inf; + unsigned int fifo_thr; + int err; + + inf = dev_get_drvdata(dev); + err = kstrtouint(buf, 10, &fifo_thr); + if (err) + return -EINVAL; + + dev_dbg(&inf->i2c->dev, "%s: %u\n", __func__, fifo_thr); + mutex_lock(&inf->mutex); + inf->chip_config.fifo_thr = fifo_thr; + err = nvi_int_enable_wr(inf, true); + mutex_unlock(&inf->mutex); + if (err) { + dev_err(&inf->i2c->dev, "%s: %u ERR=%d\n", + __func__, fifo_thr, err); + return err; + } + + return count; +} + +static ssize_t nvi_fifo_thr_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct inv_gyro_state_s *inf = dev_get_drvdata(dev); + + return sprintf(buf, "%u 0=batch_mode 1=disable %u=limit\n", + inf->chip_config.fifo_thr, inf->hal.fifo_size); +} + static ssize_t nvi_enable_store(struct device *dev, struct device_attribute *attr, const char *buf, size_t count) @@ -2321,10 +2726,10 @@ static ssize_t nvi_enable_store(struct device *dev, return -EINVAL; mutex_lock(&inf->mutex); - dev_dbg(&inf->i2c->dev, "%s: %u\n", __func__, enable); if (enable) enable = 1; if (enable != inf->chip_config.enable) { + dev_dbg(&inf->i2c->dev, "%s: %u\n", __func__, enable); inf->chip_config.enable = enable; err = nvi_pm(inf, NVI_PM_AUTO); } @@ -2335,8 +2740,8 @@ static ssize_t nvi_enable_store(struct device *dev, return count; } -ssize_t nvi_enable_show(struct device *dev, - struct device_attribute *attr, char *buf) +static ssize_t nvi_enable_show(struct device *dev, + struct device_attribute *attr, char *buf) { struct inv_gyro_state_s *inf = dev_get_drvdata(dev); @@ -2369,7 +2774,7 @@ static ssize_t inv_raw_gyro_show(struct device *dev, (signed short)(be16_to_cpup((short *)&data[0])), (signed short)(be16_to_cpup((short *)&data[2])), (signed short)(be16_to_cpup((short *)&data[4])), - get_time_ns()); + nvi_ts_ns()); } /** @@ -2380,8 +2785,9 @@ static ssize_t inv_raw_accl_show(struct device *dev, { struct inv_gyro_state_s *st; struct inv_reg_map_s *reg; + u8 data[6]; + s16 out[3]; int result; - unsigned char data[6]; st = dev_get_drvdata(dev); reg = st->reg; @@ -2389,19 +2795,117 @@ static ssize_t inv_raw_accl_show(struct device *dev, return -EPERM; result = inv_i2c_read(st, reg->raw_accl, 6, data); - if (result) { - printk(KERN_ERR "Could not read raw registers.\n"); + if (result) return result; + + if (st->chip_type == INV_MPU3050) { + if (st->mpu_slave != NULL) { + if (0 == st->mpu_slave->get_mode(st)) + return -EINVAL; + + st->mpu_slave->combine_data(data, out); + } else { + memcpy(out, data, sizeof(data)); + } + return sprintf(buf, "%d %d %d %lld\n", + out[0], out[1], out[2], nvi_ts_ns()); } return sprintf(buf, "%d %d %d %lld\n", - ((signed short)(be16_to_cpup((short *)&data[0]))* - st->chip_info.multi), - ((signed short)(be16_to_cpup((short *)&data[2]))* - st->chip_info.multi), - ((signed short)(be16_to_cpup((short *)&data[4]))* - st->chip_info.multi), - get_time_ns()); + ((signed short)(be16_to_cpup((short *)&data[0])) * + st->chip_info.multi), + ((signed short)(be16_to_cpup((short *)&data[2])) * + st->chip_info.multi), + ((signed short)(be16_to_cpup((short *)&data[4])) * + st->chip_info.multi), + nvi_ts_ns()); +} + +static ssize_t nvi_temp_enable_store(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t count) +{ + struct inv_gyro_state_s *inf; + unsigned char enable; + int err; + + inf = dev_get_drvdata(dev); + err = kstrtou8(buf, 10, &enable); + if (err) + return -EINVAL; + + if (enable) + enable = NVI_TEMP_EN; + mutex_lock(&inf->mutex); + if (enable != (inf->chip_config.temp_enable & NVI_TEMP_EN)) { + dev_dbg(&inf->i2c->dev, "%s: %x\n", __func__, enable); + if (enable) + inf->chip_config.temp_enable |= NVI_TEMP_EN; + else + inf->chip_config.temp_enable &= ~NVI_TEMP_EN; + err = nvi_pm(inf, NVI_PM_AUTO); + } + mutex_unlock(&inf->mutex); + if (err) { + dev_err(&inf->i2c->dev, "%s: %x ERR=%d\n", + __func__, enable, err); + return err; + } + + return count; +} + +ssize_t nvi_temp_enable_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct inv_gyro_state_s *inf = dev_get_drvdata(dev); + + return sprintf(buf, "%u\n", inf->chip_config.temp_enable); +} + +ssize_t nvi_temp_fifo_enable_store(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t count) +{ + struct inv_gyro_state_s *inf; + unsigned char fifo_enable; + unsigned char fifo_enable_old; + int err; + + inf = dev_get_drvdata(dev); + err = kstrtou8(buf, 10, &fifo_enable); + if (err) + return -EINVAL; + + if (fifo_enable) + fifo_enable = 1; + dev_dbg(&inf->i2c->dev, "%s: %x\n", __func__, fifo_enable); + mutex_lock(&inf->mutex); + if (fifo_enable != inf->chip_config.temp_fifo_enable) { + fifo_enable_old = inf->chip_config.temp_fifo_enable; + inf->chip_config.temp_fifo_enable = fifo_enable; + err = nvi_pm(inf, NVI_PM_ON); + err |= nvi_reset(inf, true, false); + err |= nvi_pm(inf, NVI_PM_AUTO); + if (err) + inf->chip_config.temp_fifo_enable = fifo_enable_old; + } + mutex_unlock(&inf->mutex); + if (err) { + dev_err(&inf->i2c->dev, "%s: %x ERR=%d\n", + __func__, fifo_enable, err); + return err; + } + + return count; +} + +ssize_t nvi_temp_fifo_enable_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct inv_gyro_state_s *inf = dev_get_drvdata(dev); + + return sprintf(buf, "%u\n", inf->chip_config.temp_fifo_enable); } /** @@ -2516,7 +3020,7 @@ static ssize_t inv_gyro_orientation_show(struct device *dev, /** * inv_accl_matrix_show() - show orientation matrix */ -ssize_t inv_accl_matrix_show(struct device *dev, +static ssize_t inv_accl_matrix_show(struct device *dev, struct device_attribute *attr, char *buf) { struct inv_gyro_state_s *st = dev_get_drvdata(dev); @@ -2531,6 +3035,45 @@ ssize_t inv_accl_matrix_show(struct device *dev, } /** + * inv_self_test_show() - self test result. 0 for fail; 1 for success. + * calling this function will trigger self test + * and return test result. + */ +static ssize_t inv_self_test_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + int result; + int bias[3]; + struct inv_gyro_state_s *st = dev_get_drvdata(dev); + + if (INV_MPU3050 == st->chip_type) { + bias[0] = bias[1] = bias[2] = 0; + result = 0; + } else { + result = inv_hw_self_test(st, bias); + } + return sprintf(buf, "%d, %d, %d, %d\n", + bias[0], bias[1], bias[2], result); +} + +/** + * inv_get_accl_bias_show() - show accl bias value + */ +static ssize_t inv_get_accl_bias_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + int result; + int bias[3]; + struct inv_gyro_state_s *st = dev_get_drvdata(dev); + + result = inv_get_accl_bias(st, bias); + if (result) + return -EINVAL; + + return sprintf(buf, "%d, %d, %d\n", bias[0], bias[1], bias[2]); +} + +/** * inv_key_show() - calling this function will show the key * */ @@ -2548,17 +3091,53 @@ static ssize_t inv_key_show(struct device *dev, struct device_attribute *attr, key[13], key[14], key[15]); } +/** + * OBSOLETE + */ +static ssize_t inv_power_state_store(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t count) +{ + struct inv_gyro_state_s *st; + unsigned long power_state; + + st = dev_get_drvdata(dev); + if (kstrtoul(buf, 10, &power_state)) + return -EINVAL; + + if (power_state) + st->chip_config.is_asleep = 0; + else + st->chip_config.is_asleep = 1; + return count; +} + +/** + * OBSOLETE + */ +static ssize_t inv_power_state_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct inv_gyro_state_s *st = dev_get_drvdata(dev); + + if (st->chip_config.is_asleep) + return sprintf(buf, "0\n"); + + else + return sprintf(buf, "1\n"); +} + #if DEBUG_SYSFS_INTERFACE static ssize_t nvi_dbg_i2c_addr_store(struct device *dev, struct device_attribute *attr, const char *buf, size_t count) { struct inv_gyro_state_s *inf; - unsigned char dbg_i2c_addr; + u16 dbg_i2c_addr; int err; inf = dev_get_drvdata(dev); - err = kstrtou8(buf, 16, &dbg_i2c_addr); + err = kstrtou16(buf, 16, &dbg_i2c_addr); if (err) return -EINVAL; @@ -2569,12 +3148,17 @@ static ssize_t nvi_dbg_i2c_addr_store(struct device *dev, static ssize_t nvi_dbg_i2c_addr_show(struct device *dev, struct device_attribute *attr, char *buf) { - struct inv_gyro_state_s *st = dev_get_drvdata(dev); - ssize_t bytes_printed = 0; + struct inv_gyro_state_s *inf; + ssize_t ret = 0; + u16 dbg_i2c_addr; - bytes_printed += sprintf(buf + bytes_printed, - "%#2x\n", st->dbg_i2c_addr); - return bytes_printed; + inf = dev_get_drvdata(dev); + if (inf->dbg_i2c_addr) + dbg_i2c_addr = inf->dbg_i2c_addr; + else + dbg_i2c_addr = inf->i2c->addr; + ret += sprintf(buf + ret, "%#2x\n", dbg_i2c_addr); + return ret; } static ssize_t nvi_dbg_reg_store(struct device *dev, @@ -2582,7 +3166,7 @@ static ssize_t nvi_dbg_reg_store(struct device *dev, const char *buf, size_t count) { struct inv_gyro_state_s *inf; - unsigned char dbg_reg; + u8 dbg_reg; int err; inf = dev_get_drvdata(dev); @@ -2597,11 +3181,12 @@ static ssize_t nvi_dbg_reg_store(struct device *dev, static ssize_t nvi_dbg_reg_show(struct device *dev, struct device_attribute *attr, char *buf) { - struct inv_gyro_state_s *st = dev_get_drvdata(dev); - ssize_t bytes_printed = 0; + struct inv_gyro_state_s *inf; + ssize_t ret = 0; - bytes_printed += sprintf(buf + bytes_printed, "%#2x\n", st->dbg_reg); - return bytes_printed; + inf = dev_get_drvdata(dev); + ret += sprintf(buf + ret, "%#2x\n", inf->dbg_reg); + return ret; } static ssize_t nvi_dbg_dat_store(struct device *dev, @@ -2609,8 +3194,8 @@ static ssize_t nvi_dbg_dat_store(struct device *dev, const char *buf, size_t count) { struct inv_gyro_state_s *inf; - unsigned short dbg_i2c_addr; - unsigned char dbg_dat; + u16 dbg_i2c_addr; + u8 dbg_dat; int err; inf = dev_get_drvdata(dev); @@ -2618,14 +3203,12 @@ static ssize_t nvi_dbg_dat_store(struct device *dev, if (err) return -EINVAL; - if (inf->dbg_i2c_addr) { - err = inv_i2c_single_write_base(inf, inf->dbg_i2c_addr, - inf->dbg_reg, dbg_dat); + if (inf->dbg_i2c_addr) dbg_i2c_addr = inf->dbg_i2c_addr; - } else { - err = inv_i2c_single_write(inf, inf->dbg_reg, dbg_dat); + else dbg_i2c_addr = inf->i2c->addr; - } + err = inv_i2c_single_write_base(inf, dbg_i2c_addr, + inf->dbg_reg, dbg_dat); pr_info("%s dev=%x reg=%x data=%x err=%d\n", __func__, dbg_i2c_addr, inf->dbg_reg, dbg_dat, err); return count; @@ -2635,25 +3218,23 @@ static ssize_t nvi_dbg_dat_show(struct device *dev, struct device_attribute *attr, char *buf) { struct inv_gyro_state_s *inf; - unsigned short dbg_i2c_addr; - unsigned char data; - ssize_t bytes_printed = 0; + ssize_t ret = 0; + u16 dbg_i2c_addr; + u8 dbg_dat = 0; + int err; inf = dev_get_drvdata(dev); - if (inf->dbg_i2c_addr) { - inv_i2c_read_base(inf, inf->dbg_i2c_addr, - inf->dbg_reg, 1, &data); + if (inf->dbg_i2c_addr) dbg_i2c_addr = inf->dbg_i2c_addr; - } else { - inv_i2c_read(inf, inf->dbg_reg, 1, &data); + else dbg_i2c_addr = inf->i2c->addr; - } - bytes_printed += sprintf(buf + bytes_printed, "%#2x:%#2x=%#2x\n", - dbg_i2c_addr, inf->dbg_reg, data); - return bytes_printed; + err = inv_i2c_read_base(inf, dbg_i2c_addr, inf->dbg_reg, 1, &dbg_dat); + ret += sprintf(buf + ret, "%s dev=%x reg=%x data=%x err=%d\n", + __func__, dbg_i2c_addr, inf->dbg_reg, dbg_dat, err); + return ret; } -static ssize_t nvi_aux_dbg_store(struct device *dev, +static ssize_t nvi_dbg_aux_store(struct device *dev, struct device_attribute *attr, const char *buf, size_t count) { @@ -2675,363 +3256,532 @@ static ssize_t nvi_aux_dbg_store(struct device *dev, return count; } -static ssize_t nvi_aux_dbg_show(struct device *dev, +static ssize_t nvi_dbg_aux_show(struct device *dev, struct device_attribute *attr, char *buf) { struct inv_gyro_state_s *inf = dev_get_drvdata(dev); return sprintf(buf, "%x\n", inf->aux.dbg); } +#endif /* DEBUG_SYSFS_INTERFACE */ -static ssize_t nvi_mot_dbg_store(struct device *dev, - struct device_attribute *attr, - const char *buf, size_t count) -{ - struct inv_gyro_state_s *inf; - unsigned int mot_dbg; - int err; - inf = dev_get_drvdata(dev); - err = kstrtouint(buf, 10, &mot_dbg); - if (err) - return err; +static u16 nvi_report_accl(struct inv_gyro_state_s *inf, u8 *data) +{ + u16 val[3]; + u16 buf_index; - if (mot_dbg) - inf->mot_dbg = true; - else - inf->mot_dbg = false; - return count; + if (inf->chip_type == INV_MPU3050) { + if (inf->mpu_slave != NULL) + inf->mpu_slave->combine_data(data, val); + } else { + val[0] = ((data[0] << 8) | data[1]) * inf->chip_info.multi; + val[1] = ((data[2] << 8) | data[3]) * inf->chip_info.multi; + val[2] = ((data[4] << 8) | data[5]) * inf->chip_info.multi; + } + buf_index = 0; + if (!(inf->hw.pwr_mgmt_2 & BIT_STBY_XA)) { + input_report_rel(inf->idev, REL_RX, val[0]); + buf_index += 2; + } + if (!(inf->hw.pwr_mgmt_2 & BIT_STBY_YA)) { + input_report_rel(inf->idev, REL_RY, val[1]); + buf_index += 2; + } + if (!(inf->hw.pwr_mgmt_2 & BIT_STBY_ZA)) { + input_report_rel(inf->idev, REL_RZ, val[2]); + buf_index += 2; + } + return buf_index; } -static ssize_t nvi_mot_dbg_show(struct device *dev, - struct device_attribute *attr, char *buf) +static void nvi_report_temp(struct inv_gyro_state_s *inf, u8 *data, s64 ts) { - struct inv_gyro_state_s *inf = dev_get_drvdata(dev); - - return sprintf(buf, "%x\n", inf->mot_dbg); + mutex_lock(&inf->mutex_temp); + inf->temp_val = (data[0] << 8) | data[1]; + inf->temp_ts = ts; + mutex_unlock(&inf->mutex_temp); } -#endif /* DEBUG_SYSFS_INTERFACE */ -static ssize_t nvi_min_delay_us_store(struct device *dev, - struct device_attribute *attr, - const char *buf, size_t count) +static u16 nvi_report_gyro(struct inv_gyro_state_s *inf, + u8 *data, u8 mask, s64 ts) { - struct inv_gyro_state_s *inf; - unsigned long min_delay_us; - int err; + u16 val; + u16 buf_index; + bool report; - inf = dev_get_drvdata(dev); - err = kstrtoul(buf, 10, &min_delay_us); - if (err) - return -EINVAL; + if (ts < inf->gyro_start_ts) + report = false; + else + report = true; + buf_index = 0; + if (mask & 4) { + if (report && (!(inf->hw.pwr_mgmt_2 & BIT_STBY_XG))) { + val = (data[buf_index] << 8) | data[buf_index + 1]; + input_report_rel(inf->idev, REL_X, val); + } + buf_index += 2; + } + if (mask & 2) { + if (report && (!(inf->hw.pwr_mgmt_2 & BIT_STBY_YG))) { + val = (data[buf_index] << 8) | data[buf_index + 1]; + input_report_rel(inf->idev, REL_Y, val); + } + buf_index += 2; + } + if (mask & 1) { + if (report && (!(inf->hw.pwr_mgmt_2 & BIT_STBY_ZG))) { + val = (data[buf_index] << 8) | data[buf_index + 1]; + input_report_rel(inf->idev, REL_Z, val); + } + buf_index += 2; + } + return buf_index; +} - dev_dbg(&inf->i2c->dev, "%s: %lu\n", __func__, min_delay_us); - inf->chip_config.min_delay_us = min_delay_us; - return count; +static void nvi_sync(struct inv_gyro_state_s *inf, s64 ts) +{ + input_report_rel(inf->idev, REL_MISC, (unsigned int)(ts >> 32)); + input_report_rel(inf->idev, REL_WHEEL, + (unsigned int)(ts & 0xffffffff)); + input_sync(inf->idev); } -static ssize_t nvi_min_delay_us_show(struct device *dev, - struct device_attribute *attr, char *buf) +static int nvi_accl_read(struct inv_gyro_state_s *inf) { - struct inv_gyro_state_s *inf = dev_get_drvdata(dev); + u8 data[6]; + int err; - return sprintf(buf, "%lu\n", inf->chip_config.min_delay_us); + err = inv_i2c_read(inf, inf->reg->raw_accl, 6, data); + if (!err) + err = nvi_report_accl(inf, data); + return err; } +static u16 nvi_fifo_read_accl(struct inv_gyro_state_s *inf, + u16 buf_index) +{ + if (inf->hw.fifo_en & inf->reg->accl_fifo_en) { + nvi_report_accl(inf, &inf->buf[buf_index]); +#if DEBUG_FIFO_DATA_SPEW + pr_info("%s %x %x %x\n", __func__, + (inf->buf[buf_index+0] << 8) | inf->buf[buf_index+1], + (inf->buf[buf_index+2] << 8) | inf->buf[buf_index+3], + (inf->buf[buf_index+4] << 8) | inf->buf[buf_index+5]); +#endif /* DEBUG_FIFO_DATA_SPEW */ + buf_index += 6; + } + return buf_index; +} -static void inv_report_gyro_accl(struct inv_gyro_state_s *st, s64 t, - unsigned char *data) +static u16 nvi_fifo_read_gyro(struct inv_gyro_state_s *inf, + u16 buf_index, s64 ts) { - short x, y, z; - int ind; - struct inv_chip_config_s *conf; + u8 mask; - conf = &st->chip_config; - ind = 0; - if (conf->accl_fifo_enable) { - x = ((data[ind] << 8)|data[ind + 1])*st->chip_info.multi; - y = ((data[ind + 2] << 8)|data[ind + 3])*st->chip_info.multi; - z = ((data[ind + 4] << 8)|data[ind + 5])*st->chip_info.multi; - if (conf->accl_fifo_enable) { - /*it is possible that accl disabled when dmp is on*/ - input_report_rel(st->idev, REL_RX, x); - input_report_rel(st->idev, REL_RY, y); - input_report_rel(st->idev, REL_RZ, z); - } - ind += 6; - } - if (conf->gyro_fifo_enable) { - x = (data[ind] << 8) | data[ind + 1]; - y = (data[ind + 2] << 8) | data[ind + 3]; - z = (data[ind + 4] << 8) | data[ind + 5]; - if (conf->gyro_fifo_enable) { - /*it is possible that gyro disabled when dmp is on*/ - input_report_rel(st->idev, REL_X, x); - input_report_rel(st->idev, REL_Y, y); - input_report_rel(st->idev, REL_Z, z); - } - ind += 6; + if (inf->hw.fifo_en & BIT_TEMP_FIFO_EN) { + nvi_report_temp(inf, &inf->buf[buf_index], ts); +#if DEBUG_FIFO_DATA_SPEW + pr_info("%s %x\n", __func__, + (inf->buf[buf_index+0] << 8) | inf->buf[buf_index+1]); +#endif /* DEBUG_FIFO_DATA_SPEW */ + buf_index += 2; } - if (conf->accl_fifo_enable | conf->gyro_fifo_enable) { - input_report_rel(st->idev, REL_MISC, (unsigned int)(t >> 32)); - input_report_rel(st->idev, REL_WHEEL, - (unsigned int)(t & 0xffffffff)); - input_sync(st->idev); + mask = inf->hw.fifo_en; + mask &= (BIT_GYRO_XOUT | BIT_GYRO_YOUT | BIT_GYRO_ZOUT); + mask >>= 4; + if (mask) { +#if DEBUG_FIFO_DATA_SPEW + pr_info("%s %x %x %x\n", __func__, + (inf->buf[buf_index+0] << 8) | inf->buf[buf_index+1], + (inf->buf[buf_index+2] << 8) | inf->buf[buf_index+3], + (inf->buf[buf_index+4] << 8) | inf->buf[buf_index+5]); +#endif /* DEBUG_FIFO_DATA_SPEW */ + buf_index += nvi_report_gyro(inf, &inf->buf[buf_index], + mask, ts); } + return buf_index; } -/** - * inv_read_fifo() - Transfer data from FIFO to ring buffer. - */ -static irqreturn_t inv_read_fifo(int irq, void *dev_id) +static irqreturn_t nvi_irq_thread(int irq, void *dev_id) { - struct inv_gyro_state_s *st; - unsigned char bytes_per_datum; - const unsigned short fifo_thresh = 500; - int result; - unsigned char data[16]; - unsigned short fifo_count; + struct inv_gyro_state_s *inf; + struct aux_port *ap; + u8 mask; + u16 fifo_count = 0; + u16 fifo_sample_size; + u16 fifo_rd_n; + u16 fifo_align; + u16 buf_index; + s64 ts; + s64 ts_irq; + s64 delay; + bool sync; + unsigned int ts_len; + unsigned int sample_count; unsigned int copied; - s64 timestamp; - struct inv_reg_map_s *reg; - - st = (struct inv_gyro_state_s *)dev_id; - reg = st->reg; + unsigned int len; + int i; + int err; - timestamp = get_time_ns(); - if ((!(st->hw.fifo_en & BIT_TEMP_FIFO_EN)) && - st->chip_config.gyro_enable) { - result = inv_i2c_read(st, st->reg->temperature, 2, data); - if (!result) { - mutex_lock(&st->mutex_temp); - st->temp_val = (data[0] << 8) | data[1]; - st->temp_ts = timestamp; - mutex_unlock(&st->mutex_temp); + inf = (struct inv_gyro_state_s *)dev_id; + if (inf->mot_cnt) + inf->mot_cnt--; + /* if only accelermeter data */ + if ((inf->hw.pwr_mgmt_1 & inf->reg->cycle) || (inf->hw.int_enable & + BIT_MOT_EN)) { + if (inf->hw.int_enable & BIT_MOT_EN) { + inf->mot_cnt = inf->chip_config.mot_cnt; + nvi_motion_detect_enable(inf, 0); + nvi_int_enable_wr(inf, true); + if (inf->chip_config.mot_enable == NVI_MOT_DBG) + pr_info("%s motion detect off\n", __func__); } + ts = nvi_ts_ns(); + err = nvi_accl_read(inf); + if (err < 0) + goto nvi_irq_thread_exit; + + nvi_sync(inf, ts); + if (inf->mot_cnt && (inf->chip_config.mot_enable == + NVI_MOT_DBG)) + pr_info("%s SENDING MOTION DETECT DATA\n", __func__); + if (((inf->hw.accl_config & 0x07) == 0x07) && (!inf->mot_cnt)) + nvi_int_enable_wr(inf, true); + goto nvi_irq_thread_exit; + } + + /* handle FIFO disabled data */ + sync = false; + ts = nvi_ts_ns(); + if (inf->chip_config.accl_enable && (!(inf->hw.fifo_en & + inf->reg->accl_fifo_en))) { + err = nvi_accl_read(inf); + if (err > 0) + sync = true; + } + if (inf->chip_config.temp_enable && + (!(inf->hw.fifo_en & BIT_TEMP_FIFO_EN))) { + err = inv_i2c_read(inf, inf->reg->temperature, 2, inf->buf); + if (!err) + nvi_report_temp(inf, inf->buf, ts); } - if (st->mot_cnt) - st->mot_cnt--; - if (st->lpa_enable || (st->hw.int_enable & BIT_MOT_EN)) { - if (st->hw.int_enable & BIT_MOT_EN) { - st->mot_cnt = st->chip_config.mot_cnt; - st->mot_enable = false; - nvi_int_enable_wr(st, true); - if (st->mot_dbg) - pr_info("%s motion detect off", __func__); + mask = (BIT_GYRO_XOUT | BIT_GYRO_YOUT | BIT_GYRO_ZOUT); + mask &= ~inf->hw.fifo_en; + mask >>= 4; + if (inf->chip_config.gyro_enable && mask) { + buf_index = 0; + err = 0; + if (mask & 4) { + err = inv_i2c_read(inf, inf->reg->raw_gyro, + 2, &inf->buf[buf_index]); + buf_index = 2; + } + if (mask & 2) { + err |= inv_i2c_read(inf, inf->reg->raw_gyro + 2, + 2, &inf->buf[buf_index]); + buf_index += 2; + } + if (mask & 1) + err |= inv_i2c_read(inf, inf->reg->raw_gyro + 4, + 2, &inf->buf[buf_index]); + if (!err) { + buf_index = nvi_report_gyro(inf, inf->buf, mask, ts); + if (buf_index) + sync = true; } - result = inv_i2c_read(st, reg->raw_accl, 6, data); - if (result) - goto end_session; - - inv_report_gyro_accl(st, timestamp, data); - if (st->mot_enable && st->mot_dbg) - pr_info("%s SENDING MOTION DETECT DATA", __func__); - if (st->mot_enable && (!st->mot_cnt)) - nvi_int_enable_wr(st, true); - goto end_session; } + if (sync) + nvi_sync(inf, ts); + nvi_aux_read(inf); + if (!inf->fifo_sample_size) + goto nvi_irq_thread_exit; - bytes_per_datum = (st->chip_config.accl_fifo_enable + - st->chip_config.gyro_fifo_enable)*BYTES_PER_SENSOR; - fifo_count = 0; - if (bytes_per_datum != 0) { - result = inv_i2c_read(st, reg->fifo_count_h, 2, data); - if (result) - goto end_session; - - fifo_count = (data[0] << 8) + data[1]; - if (fifo_count < bytes_per_datum) - goto end_session; - - if (fifo_count%2) - goto flush_fifo; - - if (fifo_count > fifo_thresh) - goto flush_fifo; + /* handle FIFO enabled data */ + fifo_sample_size = inf->fifo_sample_size; + if (!fifo_sample_size) + goto nvi_irq_thread_exit; - /* Timestamp mismatch. */ - if (kfifo_len(&st->trigger.timestamps) < (fifo_count / - bytes_per_datum)) - goto flush_fifo; - - if (kfifo_len(&st->trigger.timestamps) > (fifo_count / - bytes_per_datum + - TIME_STAMP_TOR)) - goto flush_fifo; + /* must get IRQ timestamps first for timestamp best-fit algorithm */ + ts_len = kfifo_len(&inf->trigger.timestamps); + err = inv_i2c_read(inf, inf->reg->fifo_count_h, 2, inf->buf); + if (err) + goto nvi_irq_thread_exit; + + fifo_count = be16_to_cpup((__be16 *)(&inf->buf)); + /* FIFO threshold */ + if (inf->chip_config.fifo_thr > fifo_sample_size) { + if (fifo_count > inf->chip_config.fifo_thr) { + dev_dbg(&inf->i2c->dev, "FIFO threshold exceeded\n"); + goto nvi_irq_thread_exit_reset; + } } - if (bytes_per_datum == 0) { - result = kfifo_to_user(&st->trigger.timestamps, - ×tamp, sizeof(timestamp), &copied); - if (result) - goto flush_fifo; + fifo_align = fifo_count % fifo_sample_size; + if (fifo_count < fifo_sample_size + fifo_align) + goto nvi_irq_thread_exit; + + if (inf->chip_type == INV_MPU3050) { + /* FIFO HW BUG WAR: + * The MPU3050 will fire an IRQ on incomplete sampling of data + * to the FIFO causing misalignment of data. The WAR is to + * simply wait for the next IRQ when this misalignment problem + * usually works itself out with the next data sample. + * The safety net is a FIFO reset, should the problem not work + * itself out by the time the FIFO threshold is reached. + */ + if (inf->fifo_reset_3050) { + if (fifo_align) + goto nvi_irq_thread_exit; + } else { + if (fifo_align != 2) + goto nvi_irq_thread_exit; + } } - while ((bytes_per_datum != 0) && (fifo_count >= bytes_per_datum)) { - result = inv_i2c_read(st, reg->fifo_r_w, bytes_per_datum, - data); - if (result) - goto flush_fifo; + ts = inf->fifo_ts; + delay = inf->sample_delay_us * 1000; + sample_count = (fifo_count / fifo_sample_size); + /* Ideally sample_count >= ts_len. If not, pull excess timestamps */ + if (sample_count < ts_len) { + len = ts_len - sample_count; + for (i = 0; i < len; i++) { + err = kfifo_to_user(&inf->trigger.timestamps, + &ts_irq, sizeof(ts_irq), &copied); + if (err) + goto nvi_irq_thread_exit_reset; - result = kfifo_to_user(&st->trigger.timestamps, - ×tamp, sizeof(timestamp), &copied); - if (result) - goto flush_fifo; + ts_len--; + } + ts = ts_irq + delay; + dev_dbg(&inf->i2c->dev, "%s SYNC FIFO to TS %lld\n", + __func__, ts); + } +#if DEBUG_FIFO_DATA_SPEW + pr_info("fifo_count=%u sample_size=%u fifo_align=%u sample_count=%u\n", + fifo_count, fifo_sample_size, fifo_align, sample_count); +#endif /* DEBUG_FIFO_DATA_SPEW */ + fifo_rd_n = 0; + buf_index = 0; + while (sample_count) { + if (buf_index >= fifo_rd_n) { + fifo_rd_n = (ARRAY_SIZE(inf->buf) - fifo_align) / + fifo_sample_size; + if (sample_count < fifo_rd_n) + fifo_rd_n = sample_count; + fifo_rd_n *= fifo_sample_size; + fifo_rd_n += fifo_align; + if (inf->chip_type == INV_MPU3050) + fifo_rd_n -= 2; /* FIFO_FOOTER */ + err = inv_i2c_read(inf, inf->reg->fifo_r_w, + fifo_rd_n, inf->buf); + if (err) + goto nvi_irq_thread_exit; - inv_report_gyro_accl(st, timestamp, data); - fifo_count -= bytes_per_datum; - } + buf_index = fifo_align; + if (inf->chip_type == INV_MPU3050) { + if (inf->fifo_reset_3050) { + inf->fifo_reset_3050 = false; + fifo_align += 2; + } + } + } + if (ts_len) { + len = ts_len; + for (i = 0; i < len; i++) { + err = kfifo_out_peek(&inf->trigger.timestamps, + &ts_irq, 1); + if (err != 1) + goto nvi_irq_thread_exit_reset; + + if (ts < (ts_irq - delay)) + break; - nvi_aux_read(st); + err = kfifo_to_user(&inf->trigger.timestamps, + &ts_irq, sizeof(ts_irq), + &copied); + if (err) + goto nvi_irq_thread_exit_reset; -end_session: + ts_len--; + if (ts < (ts_irq + delay)) { + ts = ts_irq; + break; + } + } + } + if (ts != ts_irq) + dev_dbg(&inf->i2c->dev, "%s NOSYNC TS=%lld IRQ=%lld\n", + __func__, ts, ts_irq); + if (inf->chip_type == INV_MPU3050) { + buf_index = nvi_fifo_read_gyro(inf, buf_index, ts); + buf_index = nvi_fifo_read_accl(inf, buf_index); + buf_index += 2; /* FIFO_FOOTER */ + } else { + buf_index = nvi_fifo_read_accl(inf, buf_index); + buf_index = nvi_fifo_read_gyro(inf, buf_index, ts); + } + nvi_sync(inf, ts); + for (i = 0; i < AUX_PORT_SPECIAL; i++) { + ap = &inf->aux.port[i]; + if ((inf->hw.i2c_slv_ctrl[i] & BIT_SLV_EN) && + (inf->hw.i2c_slv_addr[i] & BIT_I2C_READ) && + ap->fifo_en) { + len = ap->nmp.ctrl & BITS_I2C_SLV_CTRL_LEN; + if (ap->nmp.handler != NULL) + ap->nmp.handler(&inf->buf[buf_index], + len, ts, + ap->nmp.ext_driver); + buf_index += len; + } + } + ts += delay; + sample_count--; + } + inf->fifo_ts = ts; +nvi_irq_thread_exit: return IRQ_HANDLED; -flush_fifo: - /* Flush HW and SW FIFOs. */ - nvi_reset(st, true, false); +nvi_irq_thread_exit_reset: + dev_dbg(&inf->i2c->dev, "%s fifo_count=%u fifo_sample_size=%u\n", + __func__, fifo_count, fifo_sample_size); + nvi_reset(inf, true, false); return IRQ_HANDLED; } -/** - * inv_irq_handler() - Cache a timestamp at each data ready interrupt. - */ -static irqreturn_t inv_irq_handler(int irq, void *dev_id) +static irqreturn_t nvi_irq_handler(int irq, void *dev_id) { - struct inv_gyro_state_s *st; + struct inv_gyro_state_s *inf; long long timestamp; - int result, catch_up; - unsigned int time_since_last_irq; - - st = (struct inv_gyro_state_s *)dev_id; - timestamp = get_time_ns(); - time_since_last_irq = ((unsigned int)(timestamp - st->last_isr_time)) / - ONE_K_HZ; - spin_lock(&st->time_stamp_lock); - catch_up = 0; - while ((time_since_last_irq > st->irq_dur_us*2) && - (catch_up < MAX_CATCH_UP) && (!st->lpa_enable)) { - st->last_isr_time += st->irq_dur_us * ONE_K_HZ; - result = kfifo_in(&st->trigger.timestamps, - &st->last_isr_time, 1); - time_since_last_irq = ((unsigned int)(timestamp - - st->last_isr_time)) / ONE_K_HZ; - catch_up++; - } - result = kfifo_in(&st->trigger.timestamps, ×tamp, 1); - st->last_isr_time = timestamp; - spin_unlock(&st->time_stamp_lock); - return IRQ_WAKE_THREAD; -} - -static int inv_pm(struct inv_gyro_state_s *inf, int pm_req) -{ - int err; - if (inf->nvi) { - err = nvi_pm(inf, pm_req); - } else { - if ((pm_req > NVI_PM_OFF) || (pm_req == NVI_PM_AUTO)) - err = set_power_mpu3050(inf, 1); - else - err = set_power_mpu3050(inf, 0); + inf = (struct inv_gyro_state_s *)dev_id; + spin_lock(&inf->time_stamp_lock); + if (inf->hw.user_ctrl & BIT_FIFO_EN) { + timestamp = nvi_ts_ns(); + kfifo_in(&inf->trigger.timestamps, ×tamp, 1); } - return err; + spin_unlock(&inf->time_stamp_lock); + return IRQ_WAKE_THREAD; } #ifdef CONFIG_PM -static int inv_suspend(struct device *dev) +static int nvi_suspend(struct device *dev) { - int result; - struct inv_gyro_state_s *st = dev_get_drvdata(dev); + struct inv_gyro_state_s *inf; + int err; - result = inv_pm(st, NVI_PM_OFF_FORCE); - if (result) + inf = dev_get_drvdata(dev); + inf->suspend = true; + err = nvi_pm(inf, NVI_PM_OFF_FORCE); + if (err) dev_err(dev, "%s ERR\n", __func__); - return result; + dev_info(dev, "%s done\n", __func__); + return 0; } -static int inv_resume(struct device *dev) +static int nvi_resume(struct device *dev) { - int result; - struct inv_gyro_state_s *st = dev_get_drvdata(dev); + struct inv_gyro_state_s *inf; - result = inv_pm(st, NVI_PM_AUTO); - if (result) - dev_err(dev, "%s ERR\n", __func__); - return result; + inf = dev_get_drvdata(dev); + inf->suspend = false; + dev_info(dev, "%s done\n", __func__); + return 0; } -static const struct dev_pm_ops inv_pm_ops = { - .suspend = inv_suspend, - .resume = inv_resume, +static const struct dev_pm_ops nvi_pm_ops = { + .suspend = nvi_suspend, + .resume = nvi_resume, }; -#endif +#endif /* CONFIG_PM */ -static DEVICE_ATTR(gyro_enable, S_IRUGO | S_IWUSR | S_IWOTH, +static DEVICE_ATTR(gyro_enable, S_IRUGO | S_IWUSR | S_IWGRP, nvi_gyro_enable_show, nvi_gyro_enable_store); -static DEVICE_ATTR(gyro_fifo_enable, S_IRUGO | S_IWUSR | S_IWOTH, +static DEVICE_ATTR(gyro_fifo_enable, S_IRUGO | S_IWUSR | S_IWGRP, nvi_gyro_fifo_enable_show, nvi_gyro_fifo_enable_store); -static DEVICE_ATTR(gyro_delay, S_IRUGO | S_IWUSR | S_IWOTH, +static DEVICE_ATTR(gyro_delay, S_IRUGO | S_IWUSR | S_IWGRP, nvi_gyro_delay_show, inv_gyro_delay_store); -static DEVICE_ATTR(gyro_resolution, S_IRUGO | S_IWUSR | S_IWOTH, +static DEVICE_ATTR(gyro_resolution, S_IRUGO | S_IWUSR | S_IWGRP, nvi_gyro_resolution_show, nvi_gyro_resolution_store); -static DEVICE_ATTR(gyro_max_range, S_IRUGO | S_IWUSR | S_IWOTH, +static DEVICE_ATTR(gyro_max_range, S_IRUGO | S_IWUSR | S_IWGRP, nvi_gyro_max_range_show, nvi_gyro_max_range_store); static DEVICE_ATTR(gyro_orientation, S_IRUGO, inv_gyro_orientation_show, NULL); static DEVICE_ATTR(raw_gyro, S_IRUGO, inv_raw_gyro_show, NULL); -static DEVICE_ATTR(accl_enable, S_IRUGO | S_IWUSR | S_IWOTH, +static DEVICE_ATTR(accl_enable, S_IRUGO | S_IWUSR | S_IWGRP, nvi_accl_enable_show, nvi_accl_enable_store); -static DEVICE_ATTR(accl_fifo_enable, S_IRUGO | S_IWUSR, +static DEVICE_ATTR(accl_fifo_enable, S_IRUGO | S_IWUSR | S_IWGRP, nvi_accl_fifo_enable_show, nvi_accl_fifo_enable_store); -static DEVICE_ATTR(accl_delay, S_IRUGO | S_IWUSR | S_IWOTH, +static DEVICE_ATTR(accl_delay, S_IRUGO | S_IWUSR | S_IWGRP, nvi_accl_delay_show, nvi_accl_delay_store); -static DEVICE_ATTR(accl_resolution, S_IRUGO | S_IWUSR | S_IWOTH, +static DEVICE_ATTR(accl_resolution, S_IRUGO | S_IWUSR | S_IWGRP, nvi_accl_resolution_show, nvi_accl_resolution_store); -static DEVICE_ATTR(accl_max_range, S_IRUGO | S_IWUSR | S_IWOTH, +static DEVICE_ATTR(accl_max_range, S_IRUGO | S_IWUSR | S_IWGRP, nvi_accl_max_range_show, nvi_accl_max_range_store); static DEVICE_ATTR(accl_orientation, S_IRUGO, inv_accl_matrix_show, NULL); static DEVICE_ATTR(raw_accl, S_IRUGO, inv_raw_accl_show, NULL); -static DEVICE_ATTR(lpa_delay, S_IRUGO | S_IWUSR | S_IWOTH, +static DEVICE_ATTR(accl_bias, S_IRUGO, + inv_get_accl_bias_show, NULL); +static DEVICE_ATTR(lpa_delay, S_IRUGO | S_IWUSR | S_IWGRP, nvi_lpa_delay_enable_show, nvi_lpa_delay_enable_store); -static DEVICE_ATTR(motion_threshold, S_IRUGO | S_IWUSR | S_IWOTH, +static DEVICE_ATTR(motion_enable, S_IRUGO | S_IWUSR | S_IWGRP, + nvi_mot_enable_show, nvi_mot_enable_store); +static DEVICE_ATTR(motion_threshold, S_IRUGO | S_IWUSR | S_IWGRP, nvi_motion_thr_show, nvi_motion_thr_store); -static DEVICE_ATTR(motion_duration, S_IRUGO | S_IWUSR | S_IWOTH, +static DEVICE_ATTR(motion_duration, S_IRUGO | S_IWUSR | S_IWGRP, nvi_motion_dur_show, nvi_motion_dur_store); -static DEVICE_ATTR(motion_count, S_IRUGO | S_IWUSR | S_IWOTH, +static DEVICE_ATTR(motion_count, S_IRUGO | S_IWUSR | S_IWGRP, nvi_motion_count_show, nvi_motion_count_store); -static DEVICE_ATTR(motion_control, S_IRUGO | S_IWUSR | S_IWOTH, +static DEVICE_ATTR(motion_control, S_IRUGO | S_IWUSR | S_IWGRP, nvi_motion_ctrl_show, nvi_motion_ctrl_store); +static DEVICE_ATTR(temp_enable, S_IRUGO | S_IWUSR | S_IWGRP, + nvi_temp_enable_show, nvi_temp_enable_store); +static DEVICE_ATTR(temp_fifo_enable, S_IRUGO | S_IWUSR | S_IWGRP, + nvi_temp_fifo_enable_show, nvi_temp_fifo_enable_store); static DEVICE_ATTR(temp_scale, S_IRUGO, inv_temp_scale_show, NULL); static DEVICE_ATTR(temp_offset, S_IRUGO, inv_temp_offset_show, NULL); static DEVICE_ATTR(temperature, S_IRUGO, inv_temperature_show, NULL); -static DEVICE_ATTR(enable, S_IRUGO | S_IWUSR | S_IWOTH, +static DEVICE_ATTR(bypass_timeout_ms, S_IRUGO | S_IWUSR | S_IWGRP, + nvi_bypass_timeout_ms_show, nvi_bypass_timeout_ms_store); +static DEVICE_ATTR(min_delay_us, S_IRUGO | S_IWUSR | S_IWGRP, + nvi_min_delay_us_show, nvi_min_delay_us_store); +static DEVICE_ATTR(fifo_threshold, S_IRUGO | S_IWUSR | S_IWGRP, + nvi_fifo_thr_show, nvi_fifo_thr_store); +static DEVICE_ATTR(enable, S_IRUGO | S_IWUSR | S_IWGRP, nvi_enable_show, nvi_enable_store); -static DEVICE_ATTR(dbg_reg, S_IRUGO | S_IWUSR | S_IWOTH, +static DEVICE_ATTR(self_test, S_IRUGO, + inv_self_test_show, NULL); +static DEVICE_ATTR(dbg_reg, S_IRUGO | S_IWUSR | S_IWGRP, nvi_dbg_reg_show, nvi_dbg_reg_store); -static DEVICE_ATTR(dbg_dat, S_IRUGO | S_IWUSR | S_IWOTH, +static DEVICE_ATTR(dbg_dat, S_IRUGO | S_IWUSR | S_IWGRP, nvi_dbg_dat_show, nvi_dbg_dat_store); -static DEVICE_ATTR(dbg_i2c_addr, S_IRUGO | S_IWUSR | S_IWOTH, +static DEVICE_ATTR(dbg_i2c_addr, S_IRUGO | S_IWUSR | S_IWGRP, nvi_dbg_i2c_addr_show, nvi_dbg_i2c_addr_store); -static DEVICE_ATTR(aux_dbg, S_IRUGO | S_IWUSR | S_IWOTH, - nvi_aux_dbg_show, nvi_aux_dbg_store); -static DEVICE_ATTR(mot_dbg, S_IRUGO | S_IWUSR | S_IWOTH, - nvi_mot_dbg_show, nvi_mot_dbg_store); -static DEVICE_ATTR(key, S_IRUGO | S_IWUSR, - inv_key_show, inv_key_store); +static DEVICE_ATTR(dbg_aux, S_IRUGO | S_IWUSR | S_IWGRP, + nvi_dbg_aux_show, nvi_dbg_aux_store); static DEVICE_ATTR(reg_dump, S_IRUGO, inv_reg_dump_show, NULL); -static DEVICE_ATTR(min_delay_us, S_IRUGO | S_IWUSR | S_IWOTH, - nvi_min_delay_us_show, nvi_min_delay_us_store); +static DEVICE_ATTR(power_state, S_IRUGO | S_IWUSR | S_IWGRP, + inv_power_state_show, inv_power_state_store); +static DEVICE_ATTR(key, S_IRUGO | S_IWUSR | S_IWGRP, + inv_key_show, inv_key_store); static struct device_attribute *inv_attributes[] = { + &dev_attr_accl_enable, + &dev_attr_accl_fifo_enable, + &dev_attr_accl_delay, + &dev_attr_accl_max_range, + &dev_attr_accl_orientation, + &dev_attr_raw_accl, + &dev_attr_gyro_enable, + &dev_attr_gyro_fifo_enable, + &dev_attr_gyro_max_range, &dev_attr_gyro_delay, &dev_attr_gyro_orientation, &dev_attr_raw_gyro, @@ -3039,6 +3789,9 @@ static struct device_attribute *inv_attributes[] = { &dev_attr_temp_offset, &dev_attr_temperature, &dev_attr_reg_dump, + &dev_attr_self_test, + &dev_attr_enable, + &dev_attr_power_state, &dev_attr_key, #if DEBUG_SYSFS_INTERFACE &dev_attr_dbg_reg, @@ -3049,108 +3802,32 @@ static struct device_attribute *inv_attributes[] = { }; static struct device_attribute *inv_mpu6050_attributes[] = { - &dev_attr_gyro_enable, - &dev_attr_gyro_fifo_enable, - &dev_attr_gyro_max_range, &dev_attr_gyro_resolution, - &dev_attr_accl_enable, - &dev_attr_accl_fifo_enable, - &dev_attr_accl_delay, - &dev_attr_accl_max_range, + &dev_attr_temp_enable, + &dev_attr_temp_fifo_enable, &dev_attr_accl_resolution, - &dev_attr_accl_orientation, - &dev_attr_raw_accl, + &dev_attr_accl_bias, &dev_attr_lpa_delay, + &dev_attr_motion_enable, &dev_attr_motion_threshold, &dev_attr_motion_duration, &dev_attr_motion_count, &dev_attr_motion_control, - &dev_attr_enable, -#if DEBUG_SYSFS_INTERFACE + &dev_attr_bypass_timeout_ms, &dev_attr_min_delay_us, - &dev_attr_aux_dbg, - &dev_attr_mot_dbg, + &dev_attr_fifo_threshold, +#if DEBUG_SYSFS_INTERFACE + &dev_attr_dbg_aux, #endif /* DEBUG_SYSFS_INTERFACE */ NULL }; -static int inv_check_chip_type(struct inv_gyro_state_s *st, - const struct i2c_device_id *id) -{ - struct inv_reg_map_s *reg; - int result = 0; - - reg = st->reg; - st->mpu_slave = NULL; - if (!strcmp(id->name, "itg3500")) { - st->chip_type = INV_ITG3500; - st->nvi = false; - } else if (!strcmp(id->name, "mpu3050")) { - st->chip_type = INV_MPU3050; - inv_setup_reg_mpu3050(reg); - st->nvi = false; - } else if (!strcmp(id->name, "mpu6050")) { - st->chip_type = INV_MPU6050; - st->nvi = true; - } else if (!strcmp(id->name, "mpu9150")) { - st->chip_type = INV_MPU9150; - st->nvi = true; - } - if (SECONDARY_SLAVE_TYPE_ACCEL == st->plat_data.sec_slave_type) { - if (st->plat_data.sec_slave_id == ACCEL_ID_KXTF9) - inv_register_kxtf9_slave(st); - } - if (st->nvi) { - nvi_pm_init(st); - } else { - nvi_vreg_init(st); - /*reset register to power up default*/ - result = inv_i2c_single_write(st, reg->pwr_mgmt_1, 0); - result = inv_i2c_single_write(st, reg->pwr_mgmt_1, BIT_RESET); - if (!result) - mdelay(POWER_UP_TIME); - } - return result; -} - -/** - * inv_init_config() - Initialize hardware, disable FIFO. - * @st: Device driver instance. - * Initial configuration: - * FSR: +/- 2000DPS - * DLPF: 42Hz - * FIFO rate: 50Hz - * Clock source: Gyro PLL - */ -static int inv_init_config(struct inv_gyro_state_s *st) -{ - st->chip_config.min_delay_us = MIN_FIFO_RATE; - st->chip_config.lpf = INV_FILTER_42HZ; - st->chip_config.gyro_enable = 0; - st->chip_config.gyro_fifo_enable = 0; - st->chip_config.gyro_fsr = INV_FSR_2000DPS; - st->chip_config.accl_enable = 0; - st->chip_config.accl_fifo_enable = 0; - st->chip_config.accl_fsr = INV_FS_02G; - st->chip_config.mot_dur = 1; - st->chip_config.mot_ctrl = 1; - st->chip_config.mot_cnt = 10; - st->irq_dur_us = 20 * ONE_K_HZ; - st->chip_config.fifo_rate = 50; - st->chip_config.enable = 0; - st->chip_config.dmp_on = 0; - st->chip_config.firmware_loaded = 0; - st->chip_config.prog_start_addr = DMP_START_ADDR; - return 0; -} - static void inv_input_close(struct input_dev *d) { - struct inv_gyro_state_s *st; + struct inv_gyro_state_s *inf; - st = input_get_drvdata(d); - inv_pm(st, NVI_PM_OFF_FORCE); - nvi_vreg_exit(st); + inf = input_get_drvdata(d); + nvi_pm_exit(inf); } /** @@ -3290,13 +3967,8 @@ static int create_sysfs_interfaces(struct inv_gyro_state_s *st) if (result < 0) goto exit_remove_device_attributes; - if (INV_MPU3050 == st->chip_type) { - result = inv_mpu3050_create_sysfs(st); - if (result) - goto exit_remove_bin_file; - + if (INV_MPU3050 == st->chip_type) return 0; - } result = create_device_attributes(st->inv_dev, inv_mpu6050_attributes); if (result < 0) @@ -3325,14 +3997,138 @@ static void remove_sysfs_interfaces(struct inv_gyro_state_s *st) sysfs_remove_bin_file(&st->inv_dev->kobj, &dmp_firmware); if ((INV_ITG3500 != st->chip_type) && (INV_MPU3050 != st->chip_type)) remove_device_attributes(st->inv_dev, inv_mpu6050_attributes); - if (INV_MPU3050 == st->chip_type) - inv_mpu3050_remove_sysfs(st); device_destroy(st->inv_class, inv_device_dev_t); class_destroy(st->inv_class); st->inv_dev = NULL; st->inv_class = NULL; } +static void nvi_init_config(struct inv_gyro_state_s *inf) +{ + inf->chip_config.bypass_timeout_ms = NVI_BYPASS_TIMEOUT_MS; + inf->chip_config.min_delay_us = NVI_DELAY_US_MIN; + inf->chip_config.fifo_thr = 1; + inf->chip_config.temp_fifo_enable = 1; + inf->chip_config.lpf = INV_FILTER_42HZ; + inf->chip_config.gyro_enable = 0; + inf->chip_config.gyro_fifo_enable = 0; + inf->chip_config.gyro_fsr = INV_FSR_2000DPS; + inf->chip_config.gyro_start_delay_ns = 100000000; + inf->chip_config.accl_enable = 0; + inf->chip_config.accl_fifo_enable = 0; + inf->chip_config.accl_fsr = INV_FS_02G; + inf->chip_config.mot_enable = NVI_MOT_DIS; + inf->chip_config.mot_dur = 1; + inf->chip_config.mot_ctrl = 1; + inf->chip_config.mot_cnt = 10; + inf->chip_config.enable = 0; + inf->chip_config.prog_start_addr = DMP_START_ADDR; +} + +static int nvi_dev_init(struct inv_gyro_state_s *inf, + const struct i2c_device_id *id) +{ + u8 dev_id; + u8 val; + int err = 0; + + dev_id = 0; + if (!strcmp(id->name, "itg3500")) { + inf->chip_type = INV_ITG3500; + } else if (!strcmp(id->name, "mpu3050")) { + inf->chip_type = INV_MPU3050; + inv_setup_reg_mpu3050(inf->reg); + } else if (!strcmp(id->name, "mpu6050")) { + inf->chip_type = INV_MPU6050; + dev_id = MPU6050_ID; + } else if (!strcmp(id->name, "mpu9150")) { + inf->chip_type = INV_MPU6050; + dev_id = MPU6050_ID; + } else if (!strcmp(id->name, "mpu6500")) { + inf->chip_type = INV_MPU6500; + dev_id = MPU6500_ID; + } else if (!strcmp(id->name, "mpu9250")) { + inf->chip_type = INV_MPU6500; + dev_id = MPU9250_ID; + } else if (!strcmp(id->name, "mpu6xxx")) { + inf->chip_type = INV_MPU6050; + dev_id = 0xFF; + } else { + return -ENODEV; + } + + nvi_pm_init(inf); + if (dev_id) { + err = inv_i2c_read(inf, inf->reg->who_am_i, 1, &val); + if (err) { + dev_err(&inf->i2c->dev, "%s I2C ID READ ERR\n", + __func__); + if (dev_id == 0xFF) { + dev_err(&inf->i2c->dev, "%s AUTO ID FAILED\n", + __func__); + return -EPERM; + } + } else { + if ((dev_id != 0xFF) && (dev_id != val)) + dev_err(&inf->i2c->dev, "%s %s_ID %x != %x\n", + __func__, id->name, dev_id, val); + switch (val) { + case MPU6050_ID: + inf->chip_type = INV_MPU6050; + break; + + case MPU6500_ID: + inf->chip_type = INV_MPU6500; + break; + + case MPU9250_ID: + inf->chip_type = INV_MPU6500; + break; + + default: + dev_err(&inf->i2c->dev, "%s ERR: NO ID %x\n", + __func__, val); + } + } + } + + inf->hw_s = (struct inv_hw_s *)(hw_info + inf->chip_type); + dev_dbg(&inf->i2c->dev, "%s: BRD_CFG=%s ID=%x USING: %s\n", + __func__, id->name, val, inf->hw_s->name); + nvi_init_config(inf); + switch (inf->chip_type) { + case INV_ITG3500: + inf->hal.fifo_size = NVI_FIFO_SIZE_3050; + break; + + case INV_MPU3050: + inf->hal.fifo_size = NVI_FIFO_SIZE_3050; + err = inv_init_config_mpu3050(inf); + break; + + case INV_MPU6050: + inf->hal.fifo_size = NVI_FIFO_SIZE_6050; + inf->hal.lpa_tbl = &nvi_lpa_delay_us_tbl_6050[0]; + inf->hal.lpa_tbl_n = ARRAY_SIZE(nvi_lpa_delay_us_tbl_6050); + err = inv_get_silicon_rev_mpu6050(inf); + break; + + case INV_MPU6500: + inf->hal.fifo_size = NVI_FIFO_SIZE_6500; + inf->hal.lpa_tbl = &nvi_lpa_delay_us_tbl_6500[0]; + inf->hal.lpa_tbl_n = ARRAY_SIZE(nvi_lpa_delay_us_tbl_6500); + err = inv_get_silicon_rev_mpu6500(inf); + break; + + default: + err = -ENODEV; + break; + } + + nvi_pm(inf, NVI_PM_OFF); + return err; +} + static void nvi_shutdown(struct i2c_client *client) { struct inv_gyro_state_s *inf; @@ -3340,12 +4136,10 @@ static void nvi_shutdown(struct i2c_client *client) inf = i2c_get_clientdata(client); if (inf != NULL) { - if (inf->nvi) { - for (i = 0; i < AUX_PORT_SPECIAL; i++) { - if (inf->aux.port[i].nmp.shutdown_bypass) { - nvi_aux_bypass_enable(inf, true); - break; - } + for (i = 0; i < AUX_PORT_SPECIAL; i++) { + if (inf->aux.port[i].nmp.shutdown_bypass) { + nvi_aux_bypass_enable(inf, true); + break; } } inf->shutdown = true; @@ -3367,15 +4161,10 @@ static int nvi_remove(struct i2c_client *client) nvi_shutdown(client); inf = i2c_get_clientdata(client); if (inf != NULL) { - if (inf->nvi) { - nvi_pm_exit(inf); - } else { - inv_pm(inf, NVI_PM_OFF_FORCE); - nvi_vreg_exit(inf); - } + nvi_pm_exit(inf); kfree(inf); } - dev_info(&client->dev, "Gyro module removed.\n"); + dev_info(&client->dev, "%s\n", __func__); return 0; } @@ -3407,38 +4196,10 @@ static int nvi_probe(struct i2c_client *client, st->plat_data = *(struct mpu_platform_data *)dev_get_platdata(&client->dev); - /* power is turned on inside check chip type*/ - result = inv_check_chip_type(st, id); + result = nvi_dev_init(st, id); if (result) goto out_free; - st->hw_s = (struct inv_hw_s *)(hw_info + st->chip_type); - if (INV_MPU3050 == st->chip_type) - result = inv_init_config_mpu3050(st); - else - result = inv_init_config(st); - if (result) { - dev_err(&client->adapter->dev, - "Could not initialize device.\n"); - goto out_free; - } - - if (INV_ITG3500 != st->chip_type && INV_MPU3050 != st->chip_type) { - result = inv_get_silicon_rev_mpu6050(st); - if (result) { - dev_err(&client->adapter->dev, - "%s get silicon error.\n", st->hw_s->name); - goto out_free; - } - } - - result = inv_pm(st, NVI_PM_OFF); - if (result) { - dev_err(&client->adapter->dev, - "%s could not be turned off.\n", st->hw_s->name); - goto out_free; - } - mutex_init(&st->mutex); mutex_init(&st->mutex_temp); INIT_KFIFO(st->trigger.timestamps); @@ -3453,16 +4214,10 @@ static int nvi_probe(struct i2c_client *client, } st->trigger.irq = client->irq; - if (INV_MPU3050 == st->chip_type) - result = request_threaded_irq(client->irq, inv_irq_handler, - inv_read_fifo_mpu3050, - IRQF_TRIGGER_RISING | - IRQF_SHARED, "inv_irq", st); - else - result = request_threaded_irq(client->irq, inv_irq_handler, - inv_read_fifo, - IRQF_TRIGGER_RISING | - IRQF_SHARED, "inv_irq", st); + result = request_threaded_irq(client->irq, + nvi_irq_handler, nvi_irq_thread, + IRQF_TRIGGER_RISING | IRQF_SHARED, + "inv_irq", st); if (result) goto out_close_sysfs; @@ -3474,7 +4229,7 @@ static int nvi_probe(struct i2c_client *client, } inf_local = st; - dev_info(&client->adapter->dev, "%s is ready to go!\n", st->hw_s->name); + dev_info(&client->adapter->dev, "%s is ready to go\n", st->hw_s->name); return 0; out_close_sysfs: @@ -3482,12 +4237,7 @@ out_close_sysfs: out_free_kfifo: kfifo_free(&st->trigger.timestamps); out_free: - if (st->nvi) { - nvi_pm_exit(st); - } else { - inv_pm(st, NVI_PM_OFF_FORCE); - nvi_vreg_exit(st); - } + nvi_pm_exit(st); kfree(st); out_no_free: dev_err(&client->adapter->dev, "%s failed %d\n", __func__, result); @@ -3499,26 +4249,29 @@ static unsigned short normal_i2c[] = { I2C_CLIENT_END }; /* device id table is used to identify what device can be * supported by this driver */ -static struct i2c_device_id inv_mod_id[] = { - {"itg3500", 0}, - {"mpu3050", 0}, - {"mpu6050", 0}, - {"mpu9150", 0}, +static struct i2c_device_id nvi_mpu_id[] = { + {"itg3500", INV_ITG3500}, + {"mpu3050", INV_MPU3050}, + {"mpu6050", INV_MPU6050}, + {"mpu9150", INV_MPU9150}, + {"mpu6500", INV_MPU6500}, + {"mpu9250", INV_MPU9250}, + {"mpu6xxx", INV_MPU6XXX}, {} }; -MODULE_DEVICE_TABLE(i2c, inv_mod_id); +MODULE_DEVICE_TABLE(i2c, nvi_mpu_id); static struct i2c_driver inv_mod_driver = { .class = I2C_CLASS_HWMON, .probe = nvi_probe, .remove = nvi_remove, - .id_table = inv_mod_id, + .id_table = nvi_mpu_id, .driver = { .owner = THIS_MODULE, .name = "inv_dev", #ifdef CONFIG_PM - .pm = &inv_pm_ops, + .pm = &nvi_pm_ops, #endif }, .address_list = normal_i2c, @@ -3545,7 +4298,7 @@ static void __exit inv_mod_exit(void) module_init(inv_mod_init); module_exit(inv_mod_exit); -MODULE_AUTHOR("Invensense Corporation"); +MODULE_AUTHOR("NVIDIA Corporation"); MODULE_DESCRIPTION("Invensense device driver"); MODULE_LICENSE("GPL"); MODULE_ALIAS("inv_dev"); |