static int resume_apds9930_avago(struct inv_mpu_state *st)
{
int result, start, bytes;
u8 addr;
u8 d[1];
result = inv_plat_single_write(st, REG_USER_CTRL, st->i2c_dis);
if (result)
return result;
addr = st->plat_data.read_only_i2c_addr;
/* setup the secondary bus to default speed */
result = inv_set_bank(st, BANK_SEL_3);
if (result)
return result;
result = inv_plat_single_write(st, REG_I2C_MST_ODR_CONFIG,
MIN_MST_ODR_CONFIG);
if (result)
return result;
/* clear up the ctrl register to avoid interference of setup */
if (!st->chip_config.compass_enable) {
result = inv_plat_single_write(st, st->slv_reg[0].ctrl, 0);
if (result)
return result;
}
/* enable ALS only and power on */
result = inv_execute_write_secondary(st, 2, addr,
CMD_BYTE | APDS9930_ENABLE_REG,
PON | AEN | PEN | WEN);
if (result)
return result;
if (st->chip_config.compass_enable &&
((st->plat_data.sec_slave_id == COMPASS_ID_AK8975) ||
(st->plat_data.sec_slave_id == COMPASS_ID_AK8963))) {
start = APDS9930_ID_REG;
bytes = ALS_89XX_BYTES;
} else {
start = APDS9930_STATUS_REG;
bytes = ALS_99XX_BYTES;
}
/* dummy read */
result = inv_execute_read_secondary(st, 2, addr,
CMD_WORD | start, 1, d);
result = inv_set_bank(st, BANK_SEL_3);
if (result)
return result;
result = inv_read_secondary(st, 2, addr, CMD_WORD | start, bytes);
if (result)
return result;
result = inv_set_bank(st, BANK_SEL_0);
secondary_resume_state = true;
return result;
}
static int suspend_apds9930_avago(struct inv_mpu_state *st)
{
int result;
u8 addr;
if (!secondary_resume_state)
return 0;
result = inv_plat_single_write(st, REG_USER_CTRL, st->i2c_dis);
if (result)
return result;
addr = st->plat_data.read_only_i2c_addr;
result = inv_set_bank(st, BANK_SEL_3);
if (result)
return result;
result = inv_plat_single_write(st, REG_I2C_MST_ODR_CONFIG,
MIN_MST_ODR_CONFIG);
if (result)
return result;
if (!st->chip_config.compass_enable) {
result = inv_plat_single_write(st, st->slv_reg[0].ctrl, 0);
if (result)
return result;
}
/* disable 9930 */
result = inv_execute_write_secondary(st, 2, addr,
CMD_BYTE | APDS9930_ENABLE_REG, 0);
if (result)
return result;
/* slave 2 is disabled */
result = inv_set_bank(st, BANK_SEL_3);
if (result)
return result;
result = inv_plat_single_write(st, REG_I2C_SLV2_CTRL, 0);
if (result)
return result;
result = inv_set_bank(st, BANK_SEL_0);
secondary_resume_state = false;
return result;
}
static int inv_selftest_read_samples(struct inv_mpu_state *st, enum INV_SENSORS
type, int *sum_result, int *s)
{
u8 w;
u16 fifo_count;
s16 vals[3];
u8 d[BYTES_PER_SENSOR];
int r, i, j, packet_count;
if (SENSOR_GYRO == type)
w = (BIT_MULTI_FIFO_CFG | BIT_GYRO_FIFO_NUM);
else
w = (BIT_MULTI_FIFO_CFG | BIT_ACCEL_FIFO_NUM);
r = inv_plat_single_write(st, REG_FIFO_CFG, w);
if (r)
return r;
r = inv_plat_read(st, REG_FIFO_COUNT_H, FIFO_COUNT_BYTE, d);
if (r)
return r;
fifo_count = be16_to_cpup((__be16 *)(&d[0]));
pr_debug("self_test fifo_count - %d\n", fifo_count);
packet_count = fifo_count / BYTES_PER_SENSOR;
i = 0;
while ((i < packet_count) && (*s < DEF_ST_SAMPLES)) {
r = inv_plat_read(st, REG_FIFO_R_W, BYTES_PER_SENSOR, d);
if (r)
return r;
for (j = 0; j < THREE_AXES; j++) {
vals[j] = (s16)be16_to_cpup((__be16 *)(&d[2 * j]));
sum_result[j] += vals[j];
}
pr_debug("self_test data - %d %+d %+d %+d",
*s, vals[0], vals[1], vals[2]);
(*s)++;
i++;
}
return 0;
}
/*
* inv_do_test() - do the actual test of self testing
*/
static int inv_do_test(struct inv_mpu_state *st, bool self_test_flag,
int *gyro_result, int *accel_result)
{
int result, i, j;
int gyro_s, accel_s;
u8 w;
if (self_test_flag) {
result = inv_set_bank(st, BANK_SEL_2);
if (result)
return result;
w = BIT_GYRO_CTEN | SELFTEST_GYRO_AVGCFG;
result = inv_plat_single_write(st, REG_GYRO_CONFIG_2, w);
if (result)
return result;
w = BIT_ACCEL_CTEN | SELFTEST_ACCEL_DEC3_CFG;
result = inv_plat_single_write(st, REG_ACCEL_CONFIG_2, w);
if (result)
return result;
result = inv_set_bank(st, BANK_SEL_0);
msleep(DEF_ST_STABLE_TIME);
}
for (i = 0; i < THREE_AXES; i++) {
gyro_result[i] = 0;
accel_result[i] = 0;
}
gyro_s = 0;
accel_s = 0;
result = inv_plat_single_write(st, REG_FIFO_EN_2, 0);
if (result)
return result;
while ((gyro_s < DEF_ST_SAMPLES) || (accel_s < DEF_ST_SAMPLES)) {
result = inv_plat_single_write(st, REG_FIFO_RST,
MAX_5_BIT_VALUE);
if (result)
return result;
result = inv_plat_single_write(st, REG_FIFO_RST, 0);
if (result)
return result;
w = (BITS_GYRO_FIFO_EN | BIT_ACCEL_FIFO_EN);
result = inv_plat_single_write(st, REG_FIFO_EN_2, w);
if (result)
return result;
mdelay(SELFTEST_WAIT_TIME);
result = inv_plat_single_write(st, REG_FIFO_EN_2, 0);
if (result)
return result;
result = inv_selftest_read_samples(st, SENSOR_GYRO,
gyro_result, &gyro_s);
if (result)
return result;
result = inv_selftest_read_samples(st, SENSOR_ACCEL,
accel_result, &accel_s);
if (result)
return result;
}
pr_debug("accel=%d, %d, %d\n", accel_result[0], accel_result[1],
accel_result[2]);
for (j = 0; j < THREE_AXES; j++) {
accel_result[j] = accel_result[j] / accel_s;
accel_result[j] *= DEF_ST_PRECISION;
}
pr_debug("gyro=%d, %d, %d\n", gyro_result[0], gyro_result[1],
gyro_result[2]);
for (j = 0; j < THREE_AXES; j++) {
gyro_result[j] = gyro_result[j] / gyro_s;
gyro_result[j] *= DEF_ST_PRECISION;
}
return 0;
}
static int inv_setup_selftest(struct inv_mpu_state *st)
{
int result;
u8 w;
result = inv_set_bank(st, BANK_SEL_0);
if (result)
return result;
/* Save PWR_MGMT_1 value */
result = inv_plat_read(st, REG_PWR_MGMT_1, 1, &saved_regs.pwr_mgmt_1);
if (result)
return result;
/* Wake up */
result = inv_plat_single_write(st, REG_PWR_MGMT_1, BIT_CLK_PLL);
if (result)
return result;
/* Save the current settings */
result = inv_save_setting(st);
if (result)
return result;
/* Stop sensors for now */
result = inv_plat_single_write(st, REG_PWR_MGMT_2,
BIT_PWR_PRESSURE_STBY | BIT_PWR_ACCEL_STBY |
BIT_PWR_GYRO_STBY);
if (result)
return result;
/* Disable interrupts, FIFO and DMP */
result = inv_plat_single_write(st, REG_INT_ENABLE, 0);
if (result)
return result;
result = inv_plat_single_write(st, REG_INT_ENABLE_1, 0);
if (result)
return result;
result = inv_plat_single_write(st, REG_USER_CTRL, st->i2c_dis);
if (result)
return result;
/* Configure FIFO */
result = inv_plat_single_write(st, REG_FIFO_CFG, BIT_MULTI_FIFO_CFG);
if (result)
return result;
w = (BIT_ACCEL_FIFO_SIZE_128 | BIT_GYRO_FIFO_SIZE_128);
result = inv_plat_single_write(st, REG_FIFO_SIZE_0, w);
if (result)
return result;
result = inv_plat_single_write(st, REG_USER_CTRL,
BIT_FIFO_EN | st->i2c_dis);
if (result)
return result;
/* Set cycle mode */
result = inv_plat_single_write(st, REG_LP_CONFIG,
BIT_I2C_MST_CYCLE | BIT_ACCEL_CYCLE | BIT_GYRO_CYCLE);
if (result)
return result;
result = inv_set_bank(st, BANK_SEL_2);
if (result)
return result;
/* Configure FSR and DLPF */
result = inv_plat_single_write(st, REG_GYRO_SMPLRT_DIV,
SELFTEST_GYRO_SMPLRT_DIV);
if (result)
return result;
result = inv_plat_single_write(st, REG_GYRO_CONFIG_1, SELFTEST_GYRO_FS);
if (result)
return result;
result = inv_plat_single_write(st, REG_GYRO_CONFIG_2,
SELFTEST_GYRO_AVGCFG);
if (result)
return result;
result = inv_plat_single_write(st, REG_ACCEL_SMPLRT_DIV_1, 0);
if (result)
return result;
result = inv_plat_single_write(st, REG_ACCEL_SMPLRT_DIV_2,
SELFTEST_ACCEL_SMPLRT_DIV);
if (result)
return result;
result = inv_plat_single_write(st, REG_ACCEL_CONFIG, SELFTEST_ACCEL_FS);
if (result)
return result;
result = inv_plat_single_write(st, REG_ACCEL_CONFIG_2,
SELFTEST_ACCEL_DEC3_CFG);
if (result)
return result;
result = inv_set_bank(st, BANK_SEL_1);
if (result)
return result;
/* Read selftest values */
result = inv_plat_read(st, REG_SELF_TEST1, 1, &st->gyro_st_data[0]);
if (result)
return result;
result = inv_plat_read(st, REG_SELF_TEST2, 1, &st->gyro_st_data[1]);
if (result)
return result;
result = inv_plat_read(st, REG_SELF_TEST3, 1, &st->gyro_st_data[2]);
if (result)
return result;
result = inv_plat_read(st, REG_SELF_TEST4, 1, &st->accel_st_data[0]);
if (result)
return result;
result = inv_plat_read(st, REG_SELF_TEST5, 1, &st->accel_st_data[1]);
if (result)
return result;
result = inv_plat_read(st, REG_SELF_TEST6, 1, &st->accel_st_data[2]);
if (result)
return result;
result = inv_set_bank(st, BANK_SEL_0);
/* Restart sensors */
result = inv_plat_single_write(st, REG_PWR_MGMT_2,
BIT_PWR_PRESSURE_STBY);
if (result)
return result;
msleep(GYRO_ENGINE_UP_TIME);
return result;
}
static int inv_recover_setting(struct inv_mpu_state *st)
{
int result;
result = inv_set_bank(st, BANK_SEL_0);
if (result)
return result;
/* Stop sensors for now */
result = inv_plat_single_write(st, REG_PWR_MGMT_2,
BIT_PWR_PRESSURE_STBY | BIT_PWR_ACCEL_STBY | BIT_PWR_GYRO_STBY);
if (result)
return result;
/* Disable FIFO */
result = inv_plat_single_write(st, REG_USER_CTRL, st->i2c_dis);
if (result)
return result;
result = inv_set_bank(st, BANK_SEL_2);
if (result)
return result;
/* Restore sensor configurations */
result = inv_plat_single_write(st, REG_GYRO_SMPLRT_DIV,
saved_regs.gyro_smplrt_div);
if (result)
return result;
result = inv_plat_single_write(st, REG_GYRO_CONFIG_1,
saved_regs.gyro_config_1);
if (result)
return result;
result = inv_plat_single_write(st, REG_GYRO_CONFIG_2,
saved_regs.gyro_config_2);
if (result)
return result;
result = inv_plat_single_write(st, REG_ACCEL_SMPLRT_DIV_1,
saved_regs.accel_smplrt_div_1);
if (result)
return result;
result = inv_plat_single_write(st, REG_ACCEL_SMPLRT_DIV_2,
saved_regs.accel_smplrt_div_2);
if (result)
return result;
result = inv_plat_single_write(st, REG_ACCEL_CONFIG,
saved_regs.accel_config);
if (result)
return result;
result = inv_plat_single_write(st, REG_ACCEL_CONFIG_2,
saved_regs.accel_config_2);
if (result)
return result;
result = inv_set_bank(st, BANK_SEL_0);
if (result)
return result;
/* Restore FIFO configurations */
result = inv_plat_single_write(st, REG_FIFO_CFG, saved_regs.fifo_cfg);
if (result)
return result;
result = inv_plat_single_write(st, REG_FIFO_SIZE_0,
saved_regs.fifo_size_0);
if (result)
return result;
result = inv_plat_single_write(st, REG_LP_CONFIG, saved_regs.lp_config);
if (result)
return result;
result = inv_plat_single_write(st, REG_INT_ENABLE,
saved_regs.int_enable);
if (result)
return result;
result = inv_plat_single_write(st, REG_INT_ENABLE_1,
saved_regs.int_enable_1);
if (result)
return result;
result = inv_plat_single_write(st, REG_FIFO_EN, saved_regs.fifo_en);
if (result)
return result;
result = inv_plat_single_write(st, REG_FIFO_EN_2, saved_regs.fifo_en_2);
if (result)
return result;
result = inv_plat_single_write(st, REG_FIFO_RST, MAX_5_BIT_VALUE);
if (result)
return result;
result = inv_plat_single_write(st, REG_FIFO_RST, saved_regs.fifo_rst);
if (result)
return result;
/* Reset DMP */
result = inv_plat_single_write(st, REG_USER_CTRL,
(saved_regs.user_ctrl & (~BIT_FIFO_EN)) | BIT_DMP_RST);
if (result)
return result;
msleep(DMP_RESET_TIME);
/* Restart sensors */
result = inv_plat_single_write(st, REG_PWR_MGMT_2,
saved_regs.pwr_mgmt_2);
if (result)
return result;
msleep(GYRO_ENGINE_UP_TIME);
result = inv_plat_single_write(st, REG_USER_CTRL,
saved_regs.user_ctrl);
if (result)
return result;
/* Possibly will set SLEEP bit */
result = inv_plat_single_write(st, REG_PWR_MGMT_1,
saved_regs.pwr_mgmt_1);
if (result)
return result;
/* Allow to load DMP iamge again */
//rocky liu st->chip_config.firmware_loaded = 0;
return result;
}
