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; }
/* * 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; }
static int inv_save_setting(struct inv_mpu_state *st) { int result; result = inv_set_bank(st, BANK_SEL_0); if (result) return result; /* REG_PWR_MGMT_1 should be saved before this function */ result = inv_plat_read(st, REG_PWR_MGMT_2, 1, &saved_regs.pwr_mgmt_2); if (result) return result; result = inv_plat_read(st, REG_FIFO_CFG, 1, &saved_regs.fifo_cfg); if (result) return result; result = inv_plat_read(st, REG_FIFO_SIZE_0, 1, &saved_regs.fifo_size_0); if (result) return result; result = inv_plat_read(st, REG_USER_CTRL, 1, &saved_regs.user_ctrl); if (result) return result; result = inv_plat_read(st, REG_LP_CONFIG, 1, &saved_regs.lp_config); if (result) return result; result = inv_plat_read(st, REG_INT_ENABLE, 1, &saved_regs.int_enable); if (result) return result; result = inv_plat_read(st, REG_INT_ENABLE_1, 1, &saved_regs.int_enable_1); if (result) return result; result = inv_plat_read(st, REG_FIFO_EN, 1, &saved_regs.fifo_en); if (result) return result; result = inv_plat_read(st, REG_FIFO_EN_2, 1, &saved_regs.fifo_en_2); if (result) return result; result = inv_plat_read(st, REG_FIFO_RST, 1, &saved_regs.fifo_rst); if (result) return result; result = inv_set_bank(st, BANK_SEL_2); if (result) return result; result = inv_plat_read(st, REG_GYRO_SMPLRT_DIV, 1, &saved_regs.gyro_smplrt_div); if (result) return result; result = inv_plat_read(st, REG_GYRO_CONFIG_1, 1, &saved_regs.gyro_config_1); if (result) return result; result = inv_plat_read(st, REG_GYRO_CONFIG_2, 1, &saved_regs.gyro_config_2); if (result) return result; result = inv_plat_read(st, REG_ACCEL_SMPLRT_DIV_1, 1, &saved_regs.accel_smplrt_div_1); if (result) return result; result = inv_plat_read(st, REG_ACCEL_SMPLRT_DIV_2, 1, &saved_regs.accel_smplrt_div_2); if (result) return result; result = inv_plat_read(st, REG_ACCEL_CONFIG, 1, &saved_regs.accel_config); if (result) return result; result = inv_plat_read(st, REG_ACCEL_CONFIG_2, 1, &saved_regs.accel_config_2); if (result) return result; result = inv_set_bank(st, BANK_SEL_0); if (result) return result; inv_show_saved_setting(st); return result; }