drivers/iio/imu/inv_mpu/iam20680/inv_mpu_setup_20680.c - kernel/msm-4.9 - Gitiles

 /*
 * Copyright (C) 2017-2018 InvenSense, Inc.
 *
 * This software is licensed under the terms of the GNU General Public
 * License version 2, as published by the Free Software Foundation, and
 * may be copied, distributed, and modified under those terms.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 */
 #define pr_fmt(fmt) "inv_mpu: " fmt
 #include "../inv_mpu_iio.h"

 /* set LN mode for gyro regardless of conditions */
 #define USE_GYRO_LN_MODE

 static int inv_calc_engine_dur(struct inv_engine_info *ei)
 {
 	if (!ei->running_rate)
 		return -EINVAL;
 	ei->dur = ei->base_time / ei->orig_rate;
 	ei->dur *= ei->divider;

 	return 0;
 }

 static int inv_turn_on_fifo(struct inv_mpu_state *st)
 {
 	u8 int_en, fifo_en, mode, user;
 	int r;

 	r = inv_plat_single_write(st, REG_FIFO_EN, 0);
 	if (r)
 		return r;
 	r = inv_plat_single_write(st, REG_USER_CTRL, BIT_FIFO_RST);
 	if (r)
 		return r;
 	fifo_en = 0;
 	int_en = 0;

 	if (st->gesture_only_on && (!st->batch.timeout)) {
 		st->gesture_int_count = WOM_DELAY_THRESHOLD;
 		int_en |= BIT_WOM_ALL_INT_EN;
 	}
 #ifdef TIMER_BASED_BATCHING
 	if (st->chip_config.eis_enable)
 		int_en |= BIT_FSYNC_INT_EN;
 	if (!st->batch_timeout) {
 		int_en |= BIT_DATA_RDY_EN;
 	}
 #else
 	if (st->batch.timeout) {
 		if(!st->batch.fifo_wm_th)
 			int_en = BIT_DATA_RDY_EN;
 	} else {
 		int_en = BIT_DATA_RDY_EN;
 		if (st->chip_config.eis_enable)
 			int_en |= BIT_FSYNC_INT_EN;
 	}
 #endif
 	if (st->sensor[SENSOR_GYRO].on)
 		fifo_en |= BITS_GYRO_FIFO_EN;

 	if (st->sensor[SENSOR_ACCEL].on)
 		fifo_en |= BIT_ACCEL_FIFO_EN;
 	r = inv_plat_single_write(st, REG_FIFO_EN, fifo_en);
 	if (r)
 		return r;
 	st->int_en = int_en;
 	r = inv_plat_single_write(st, REG_INT_ENABLE, int_en);
 	if (r)
 		return r;
 	if (st->gesture_only_on && (!st->batch.timeout)) {
 		mode = BIT_ACCEL_INTEL_EN | BIT_ACCEL_INTEL_MODE;
 	} else {
 		mode = 0;
 	}
 	r = inv_plat_single_write(st, REG_ACCEL_INTEL_CTRL, mode);
 #ifdef SENSOR_DATA_FROM_REGISTERS
 	user = 0;
 #else
 	user = BIT_FIFO_EN;
 #endif
 	r = inv_plat_single_write(st, REG_USER_CTRL, user | st->i2c_dis);
 #ifdef TIMER_BASED_BATCHING
 	if (fifo_en && st->batch_timeout) {
 		if (st->is_batch_timer_running)
 			hrtimer_cancel(&st ->hr_batch_timer);
 		st->is_batch_timer_running = true;
 		hrtimer_start(&st ->hr_batch_timer,
 			ns_to_ktime(st->batch_timeout), HRTIMER_MODE_REL);
 	} else {
 		if (st->is_batch_timer_running)
 			hrtimer_cancel(&st ->hr_batch_timer);
 		st->is_batch_timer_running = false;
 	}
 #endif

 	return r;
 }

 /*
  *  inv_reset_fifo() - Reset FIFO related registers.
  */
 int inv_reset_fifo(struct inv_mpu_state *st, bool turn_off)
 {
 	int r, i;
 	struct inv_timestamp_algo *ts_algo = &st->ts_algo;
 	int dur_ms;

 	r = inv_turn_on_fifo(st);
 	if (r)
 		return r;

 	ts_algo->last_run_time = get_time_ns();
 	ts_algo->reset_ts = ts_algo->last_run_time;
 	if (st->mode_1k_on)
 		ts_algo->first_sample = MODE_1K_INIT_SAMPLE;
 	else
 		ts_algo->first_sample = 1;

 	dur_ms = st->smplrt_div + 1;
 	if ((ts_algo->first_sample * dur_ms) < FIRST_SAMPLE_BUF_MS)
 		ts_algo->first_sample = FIRST_SAMPLE_BUF_MS / dur_ms;
 	if (ts_algo->first_sample == 0)
 		ts_algo->first_sample = 1;

 	st->last_temp_comp_time = ts_algo->last_run_time;
 	st->left_over_size = 0;
 	for (i = 0; i < SENSOR_NUM_MAX; i++) {
 		st->sensor[i].calib_flag = 0;
 		st->sensor[i].sample_calib = 0;
 		st->sensor[i].time_calib = ts_algo->last_run_time;
 	}

 	ts_algo->calib_counter = 0;

 	return 0;
 }

 static int inv_turn_on_engine(struct inv_mpu_state *st)
 {
 	u8 v, w;
 	int r;
 	unsigned int wait_ms;

 	if (st->chip_config.gyro_enable | st->chip_config.accel_enable) {
 		w = 0;
 		if (!st->chip_config.gyro_enable)
 			w |= BIT_PWR_GYRO_STBY;
 		if (!st->chip_config.accel_enable)
 			w |= BIT_PWR_ACCEL_STBY;
 	} else if (st->chip_config.compass_enable) {
 		w = BIT_PWR_GYRO_STBY;
 	} else {
 		w = (BIT_PWR_GYRO_STBY | BIT_PWR_ACCEL_STBY);
 	}

 	r = inv_plat_read(st, REG_PWR_MGMT_2, 1, &v);
 	if (r)
 		return r;
 	r = inv_plat_single_write(st, REG_PWR_MGMT_2, w);
 	if (r)
 		return r;

 	wait_ms = 0;
 	if (st->chip_config.gyro_enable
 		&& (v & BIT_PWR_GYRO_STBY)) {
 		wait_ms = INV_IAM20680_GYRO_START_TIME;
 	}
 	if (st->chip_config.accel_enable
 		&& (v & BIT_PWR_ACCEL_STBY)) {
 		if (INV_IAM20680_ACCEL_START_TIME > wait_ms)
 			wait_ms = INV_IAM20680_ACCEL_START_TIME;
 	}
 	if (wait_ms)
 		msleep(wait_ms);

 	if (st->chip_config.has_compass) {
 		if (st->chip_config.compass_enable)
 			r = st->slave_compass->resume(st);
 		else
 			r = st->slave_compass->suspend(st);
 		if (r)
 			return r;
 	}

 	return 0;
 }

 static int inv_setup_dmp_rate(struct inv_mpu_state *st)
 {
 	int i;

 	for (i = 0; i < SENSOR_NUM_MAX; i++) {
 		if (st->sensor[i].on) {
 			st->cntl |= st->sensor[i].output;
 			st->sensor[i].dur =
 				st->eng_info[st->sensor[i].engine_base].dur;
 			st->sensor[i].div = 1;
 		}
 	}

 	return 0;
 }

 /*
  *  inv_set_lpf() - set low pass filer based on fifo rate.
  */
 static int inv_set_lpf(struct inv_mpu_state *st, int rate)
 {
 	const short hz[] = {188, 98, 42, 20, 10, 5};
 	const int   d[] = {INV_FILTER_188HZ, INV_FILTER_98HZ,
 			INV_FILTER_42HZ, INV_FILTER_20HZ,
 			INV_FILTER_10HZ, INV_FILTER_5HZ};
 	int i, h, data, result;

 #ifdef USE_GYRO_LN_MODE
 	if (1) {
 #else
 	if (st->chip_config.eis_enable || st->ois.en || st->mode_1k_on) {
 #endif
 		h = (rate >> 1);
 		i = 0;
 		while ((h < hz[i]) && (i < ARRAY_SIZE(d) - 1))
 			i++;
 		data = d[i];
 		data |= EXT_SYNC_SET;
 		result = inv_plat_single_write(st, REG_CONFIG, data);
 		if (result)
 			return result;

 		st->chip_config.lpf = data;
 		result = inv_plat_single_write(st, REG_LP_MODE_CTRL, 0);
 	} else {
 		result = inv_plat_single_write(st, REG_LP_MODE_CTRL,
 							BIT_GYRO_CYCLE_EN);
 		if (result)
 			return result;
 		data = 0;
 		result = inv_plat_single_write(st, REG_CONFIG, data | 3);
 	}

 	return result;
 }

 static int inv_set_div(struct inv_mpu_state *st, int a_d, int g_d)
 {
 	int result, div;

 	if (st->chip_config.gyro_enable)
 		div = g_d;
 	else
 		div = a_d;
 	if (st->chip_config.eis_enable)
 		div = 0;

 	st->smplrt_div = div;
 	pr_debug("div= %d\n", div);
 	result = inv_plat_single_write(st, REG_SAMPLE_RATE_DIV, div);

 	return result;
 }

 // 20680 does not support batching
 static int inv_set_batch(struct inv_mpu_state *st)
 {
 #ifdef TIMER_BASED_BATCHING
 	u64 timeout;
 	int required_fifo_size;

 #ifdef CONFIG_ENABLE_IAM_ACC_GYRO_BUFFERING
 	st->batch.timeout = 100;
 #endif
 	if (st->batch.timeout) {
 		required_fifo_size = st->batch.timeout * st->eng_info[ENGINE_GYRO].running_rate
 					* st->batch.pk_size / 1000;
 		if (required_fifo_size > MAX_BATCH_FIFO_SIZE) {
 			required_fifo_size = MAX_BATCH_FIFO_SIZE;
 			timeout = (required_fifo_size / st->batch.pk_size) * (1000 / st->eng_info[ENGINE_GYRO].running_rate);
 		} else {
 			timeout = st->batch.timeout;
 		}
 	} else {
 		timeout = 1000 / st->eng_info[ENGINE_GYRO].running_rate;
 	}
 	if (timeout <= 1000 / st->eng_info[ENGINE_GYRO].running_rate)
 		st->batch_timeout = 0;
 	else
 		st->batch_timeout = timeout * 1000000; // ms to ns
 #endif
 	st->batch.fifo_wm_th = 0;

 	return 0;
 }

 static int inv_set_rate(struct inv_mpu_state *st)
 {
 	int g_d, a_d, result, i;

 	result = inv_setup_dmp_rate(st);
 	if (result)
 		return result;

 	g_d = st->eng_info[ENGINE_GYRO].divider - 1;
 	a_d = st->eng_info[ENGINE_ACCEL].divider - 1;
 	result = inv_set_div(st, a_d, g_d);
 	if (result)
 		return result;
 	result = inv_set_lpf(st, st->eng_info[ENGINE_GYRO].running_rate);
 	if (result)
 		return result;
 	// set ADLPF at this point not to change after accel is enabled
 	result = inv_set_accel_config2(st, false);
 	st->batch.pk_size = 0;
 	for (i = 0; i < SENSOR_NUM_MAX; i++) {
 		if (st->sensor[i].on)
 			st->batch.pk_size +=  st->sensor[i].sample_size;
 	}

 	inv_set_batch(st);

 	return result;
 }

 static int inv_determine_engine(struct inv_mpu_state *st)
 {
 	int i;
 	bool a_en, g_en;
 	int accel_rate, gyro_rate;

 	a_en = false;
 	g_en = false;
 	gyro_rate = MPU_INIT_SENSOR_RATE;
 	accel_rate = MPU_INIT_SENSOR_RATE;
 	/* loop the streaming sensors to see which engine needs to be turned on
 		*/
 	for (i = 0; i < SENSOR_NUM_MAX; i++) {
 		if (st->sensor[i].on) {
 			a_en |= st->sensor[i].a_en;
 			g_en |= st->sensor[i].g_en;
 		}
 	}

 	if (st->chip_config.eis_enable) {
 		g_en = true;
 		st->eis.frame_count = 0;
 		st->eis.fsync_delay = 0;
 		st->eis.gyro_counter = 0;
 		st->eis.voting_count = 0;
 		st->eis.voting_count_sub = 0;
 		gyro_rate = BASE_SAMPLE_RATE;
 	} else {
 		st->eis.eis_triggered = false;
 		st->eis.prev_state = false;
 	}

 	accel_rate = st->sensor[SENSOR_ACCEL].rate;
 	gyro_rate  = max(gyro_rate, st->sensor[SENSOR_GYRO].rate);

 	st->ts_algo.clock_base = ENGINE_ACCEL;

 	if (g_en) {
 		/* gyro engine needs to be fastest */
 		if (a_en)
 			gyro_rate = max(gyro_rate, accel_rate);
 		accel_rate = gyro_rate;
 		st->ts_algo.clock_base = ENGINE_GYRO;
 	} else if (a_en) {
 		/* accel engine needs to be fastest if gyro engine is off */
 		gyro_rate = accel_rate;
 		st->ts_algo.clock_base = ENGINE_ACCEL;
 	}

 	st->eng_info[ENGINE_GYRO].running_rate = gyro_rate;
 	st->eng_info[ENGINE_ACCEL].running_rate = accel_rate;
 	if ((gyro_rate >= BASE_SAMPLE_RATE) ||
 					(accel_rate >= BASE_SAMPLE_RATE))
 		st->mode_1k_on = true;
 	else
 		st->mode_1k_on = false;
 	/* engine divider for pressure and compass is set later */
 	if (st->chip_config.eis_enable || st->mode_1k_on) {
 		st->eng_info[ENGINE_GYRO].divider = 1;
 		st->eng_info[ENGINE_ACCEL].divider = 1;
 		// need to update rate and div for 1khz mode
 		for ( i = 0 ; i < SENSOR_L_NUM_MAX ; i++ ) {
 			if (st->sensor_l[i].on) {
 				st->sensor_l[i].counter = 0;
 				if (st->sensor_l[i].rate)
 					st->sensor_l[i].div =
 						BASE_SAMPLE_RATE
 						/ st->sensor_l[i].rate;
 				else
 					st->sensor_l[i].div = 0xffff;
 			}
 		}
 	} else {
 		st->eng_info[ENGINE_GYRO].divider = BASE_SAMPLE_RATE /
 			st->eng_info[ENGINE_GYRO].running_rate;
 		st->eng_info[ENGINE_ACCEL].divider = BASE_SAMPLE_RATE /
 			st->eng_info[ENGINE_ACCEL].running_rate;
 	}

 	for ( i = 0 ; i < SENSOR_L_NUM_MAX ; i++ )
 		st->sensor_l[i].counter = 0;

 	inv_calc_engine_dur(&st->eng_info[ENGINE_GYRO]);
 	inv_calc_engine_dur(&st->eng_info[ENGINE_ACCEL]);

 	pr_debug("gen: %d aen: %d grate: %d arate: %d\n",
 				g_en, a_en, gyro_rate, accel_rate);

 	st->chip_config.gyro_enable = g_en;
 	st->chip_config.accel_enable = a_en;

 	return 0;
 }

 /*
  *  set_inv_enable() - enable function.
  */
 int set_inv_enable(struct iio_dev *indio_dev)
 {
 	int result;
 	struct inv_mpu_state *st = iio_priv(indio_dev);

 	result = inv_switch_power_in_lp(st, true);
 	if (result)
 		return result;
 	inv_stop_interrupt(st);
 	inv_determine_engine(st);
 	result = inv_set_rate(st);
 	if (result) {
 		pr_err("inv_set_rate error\n");
 		return result;
 	}
 	result = inv_turn_on_engine(st);
 	if (result) {
 		pr_err("inv_turn_on_engine error\n");
 		return result;
 	}
 	result = inv_reset_fifo(st, false);
 	if (result)
 		return result;
 	result = inv_switch_power_in_lp(st, false);
 	if ((!st->chip_config.gyro_enable) &&
 		(!st->chip_config.accel_enable)) {
 		inv_set_power(st, false);
 		return 0;
 	}

 	return result;
 }
 /* dummy function for 20608D */
 int inv_enable_pedometer_interrupt(struct inv_mpu_state *st, bool en)
 {
 	return 0;
 }
 int inv_dmp_read(struct inv_mpu_state *st, int off, int size, u8 *buf)
 {
 	return 0;
 }
 int inv_firmware_load(struct inv_mpu_state *st)
 {
 	return 0;
 }
	/*
	* Copyright (C) 2017-2018 InvenSense, Inc.
	*
	* This software is licensed under the terms of the GNU General Public
	* License version 2, as published by the Free Software Foundation, and
	* may be copied, distributed, and modified under those terms.
	*
	* This program is distributed in the hope that it will be useful,
	* but WITHOUT ANY WARRANTY; without even the implied warranty of
	* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
	* GNU General Public License for more details.
	*/
	#define pr_fmt(fmt) "inv_mpu: " fmt
	#include "../inv_mpu_iio.h"

	/* set LN mode for gyro regardless of conditions */
	#define USE_GYRO_LN_MODE

	static int inv_calc_engine_dur(struct inv_engine_info *ei)
	{
	if (!ei->running_rate)
	return -EINVAL;
	ei->dur = ei->base_time / ei->orig_rate;
	ei->dur *= ei->divider;

	return 0;
	}

	static int inv_turn_on_fifo(struct inv_mpu_state *st)
	{
	u8 int_en, fifo_en, mode, user;
	int r;

	r = inv_plat_single_write(st, REG_FIFO_EN, 0);
	if (r)
	return r;
	r = inv_plat_single_write(st, REG_USER_CTRL, BIT_FIFO_RST);
	if (r)
	return r;
	fifo_en = 0;
	int_en = 0;

	if (st->gesture_only_on && (!st->batch.timeout)) {
	st->gesture_int_count = WOM_DELAY_THRESHOLD;
	int_en \|= BIT_WOM_ALL_INT_EN;
	}
	#ifdef TIMER_BASED_BATCHING
	if (st->chip_config.eis_enable)
	int_en \|= BIT_FSYNC_INT_EN;
	if (!st->batch_timeout) {
	int_en \|= BIT_DATA_RDY_EN;
	}
	#else
	if (st->batch.timeout) {
	if(!st->batch.fifo_wm_th)
	int_en = BIT_DATA_RDY_EN;
	} else {
	int_en = BIT_DATA_RDY_EN;
	if (st->chip_config.eis_enable)
	int_en \|= BIT_FSYNC_INT_EN;
	}
	#endif
	if (st->sensor[SENSOR_GYRO].on)
	fifo_en \|= BITS_GYRO_FIFO_EN;

	if (st->sensor[SENSOR_ACCEL].on)
	fifo_en \|= BIT_ACCEL_FIFO_EN;
	r = inv_plat_single_write(st, REG_FIFO_EN, fifo_en);
	if (r)
	return r;
	st->int_en = int_en;
	r = inv_plat_single_write(st, REG_INT_ENABLE, int_en);
	if (r)
	return r;
	if (st->gesture_only_on && (!st->batch.timeout)) {
	mode = BIT_ACCEL_INTEL_EN \| BIT_ACCEL_INTEL_MODE;
	} else {
	mode = 0;
	}
	r = inv_plat_single_write(st, REG_ACCEL_INTEL_CTRL, mode);
	#ifdef SENSOR_DATA_FROM_REGISTERS
	user = 0;
	#else
	user = BIT_FIFO_EN;
	#endif
	r = inv_plat_single_write(st, REG_USER_CTRL, user \| st->i2c_dis);
	#ifdef TIMER_BASED_BATCHING
	if (fifo_en && st->batch_timeout) {
	if (st->is_batch_timer_running)
	hrtimer_cancel(&st ->hr_batch_timer);
	st->is_batch_timer_running = true;
	hrtimer_start(&st ->hr_batch_timer,
	ns_to_ktime(st->batch_timeout), HRTIMER_MODE_REL);
	} else {
	if (st->is_batch_timer_running)
	hrtimer_cancel(&st ->hr_batch_timer);
	st->is_batch_timer_running = false;
	}
	#endif

	return r;
	}

	/*
	* inv_reset_fifo() - Reset FIFO related registers.
	*/
	int inv_reset_fifo(struct inv_mpu_state *st, bool turn_off)
	{
	int r, i;
	struct inv_timestamp_algo *ts_algo = &st->ts_algo;
	int dur_ms;

	r = inv_turn_on_fifo(st);
	if (r)
	return r;

	ts_algo->last_run_time = get_time_ns();
	ts_algo->reset_ts = ts_algo->last_run_time;
	if (st->mode_1k_on)
	ts_algo->first_sample = MODE_1K_INIT_SAMPLE;
	else
	ts_algo->first_sample = 1;

	dur_ms = st->smplrt_div + 1;
	if ((ts_algo->first_sample * dur_ms) < FIRST_SAMPLE_BUF_MS)
	ts_algo->first_sample = FIRST_SAMPLE_BUF_MS / dur_ms;
	if (ts_algo->first_sample == 0)
	ts_algo->first_sample = 1;

	st->last_temp_comp_time = ts_algo->last_run_time;
	st->left_over_size = 0;
	for (i = 0; i < SENSOR_NUM_MAX; i++) {
	st->sensor[i].calib_flag = 0;
	st->sensor[i].sample_calib = 0;
	st->sensor[i].time_calib = ts_algo->last_run_time;
	}

	ts_algo->calib_counter = 0;

	return 0;
	}

	static int inv_turn_on_engine(struct inv_mpu_state *st)
	{
	u8 v, w;
	int r;
	unsigned int wait_ms;

	if (st->chip_config.gyro_enable \| st->chip_config.accel_enable) {
	w = 0;
	if (!st->chip_config.gyro_enable)
	w \|= BIT_PWR_GYRO_STBY;
	if (!st->chip_config.accel_enable)
	w \|= BIT_PWR_ACCEL_STBY;
	} else if (st->chip_config.compass_enable) {
	w = BIT_PWR_GYRO_STBY;
	} else {
	w = (BIT_PWR_GYRO_STBY \| BIT_PWR_ACCEL_STBY);
	}

	r = inv_plat_read(st, REG_PWR_MGMT_2, 1, &v);
	if (r)
	return r;
	r = inv_plat_single_write(st, REG_PWR_MGMT_2, w);
	if (r)
	return r;

	wait_ms = 0;
	if (st->chip_config.gyro_enable
	&& (v & BIT_PWR_GYRO_STBY)) {
	wait_ms = INV_IAM20680_GYRO_START_TIME;
	}
	if (st->chip_config.accel_enable
	&& (v & BIT_PWR_ACCEL_STBY)) {
	if (INV_IAM20680_ACCEL_START_TIME > wait_ms)
	wait_ms = INV_IAM20680_ACCEL_START_TIME;
	}
	if (wait_ms)
	msleep(wait_ms);

	if (st->chip_config.has_compass) {
	if (st->chip_config.compass_enable)
	r = st->slave_compass->resume(st);
	else
	r = st->slave_compass->suspend(st);
	if (r)
	return r;
	}

	return 0;
	}

	static int inv_setup_dmp_rate(struct inv_mpu_state *st)
	{
	int i;

	for (i = 0; i < SENSOR_NUM_MAX; i++) {
	if (st->sensor[i].on) {
	st->cntl \|= st->sensor[i].output;
	st->sensor[i].dur =
	st->eng_info[st->sensor[i].engine_base].dur;
	st->sensor[i].div = 1;
	}
	}

	return 0;
	}

	/*
	* inv_set_lpf() - set low pass filer based on fifo rate.
	*/
	static int inv_set_lpf(struct inv_mpu_state *st, int rate)
	{
	const short hz[] = {188, 98, 42, 20, 10, 5};
	const int d[] = {INV_FILTER_188HZ, INV_FILTER_98HZ,
	INV_FILTER_42HZ, INV_FILTER_20HZ,
	INV_FILTER_10HZ, INV_FILTER_5HZ};
	int i, h, data, result;

	#ifdef USE_GYRO_LN_MODE
	if (1) {
	#else
	if (st->chip_config.eis_enable \|\| st->ois.en \|\| st->mode_1k_on) {
	#endif
	h = (rate >> 1);
	i = 0;
	while ((h < hz[i]) && (i < ARRAY_SIZE(d) - 1))
	i++;
	data = d[i];
	data \|= EXT_SYNC_SET;
	result = inv_plat_single_write(st, REG_CONFIG, data);
	if (result)
	return result;

	st->chip_config.lpf = data;
	result = inv_plat_single_write(st, REG_LP_MODE_CTRL, 0);
	} else {
	result = inv_plat_single_write(st, REG_LP_MODE_CTRL,
	BIT_GYRO_CYCLE_EN);
	if (result)
	return result;
	data = 0;
	result = inv_plat_single_write(st, REG_CONFIG, data \| 3);
	}

	return result;
	}

	static int inv_set_div(struct inv_mpu_state *st, int a_d, int g_d)
	{
	int result, div;

	if (st->chip_config.gyro_enable)
	div = g_d;
	else
	div = a_d;
	if (st->chip_config.eis_enable)
	div = 0;

	st->smplrt_div = div;
	pr_debug("div= %d\n", div);
	result = inv_plat_single_write(st, REG_SAMPLE_RATE_DIV, div);

	return result;
	}

	// 20680 does not support batching
	static int inv_set_batch(struct inv_mpu_state *st)
	{
	#ifdef TIMER_BASED_BATCHING
	u64 timeout;
	int required_fifo_size;

	#ifdef CONFIG_ENABLE_IAM_ACC_GYRO_BUFFERING
	st->batch.timeout = 100;
	#endif
	if (st->batch.timeout) {
	required_fifo_size = st->batch.timeout * st->eng_info[ENGINE_GYRO].running_rate
	* st->batch.pk_size / 1000;
	if (required_fifo_size > MAX_BATCH_FIFO_SIZE) {
	required_fifo_size = MAX_BATCH_FIFO_SIZE;
	timeout = (required_fifo_size / st->batch.pk_size) * (1000 / st->eng_info[ENGINE_GYRO].running_rate);
	} else {
	timeout = st->batch.timeout;
	}
	} else {
	timeout = 1000 / st->eng_info[ENGINE_GYRO].running_rate;
	}
	if (timeout <= 1000 / st->eng_info[ENGINE_GYRO].running_rate)
	st->batch_timeout = 0;
	else
	st->batch_timeout = timeout * 1000000; // ms to ns
	#endif
	st->batch.fifo_wm_th = 0;

	return 0;
	}

	static int inv_set_rate(struct inv_mpu_state *st)
	{
	int g_d, a_d, result, i;

	result = inv_setup_dmp_rate(st);
	if (result)
	return result;

	g_d = st->eng_info[ENGINE_GYRO].divider - 1;
	a_d = st->eng_info[ENGINE_ACCEL].divider - 1;
	result = inv_set_div(st, a_d, g_d);
	if (result)
	return result;
	result = inv_set_lpf(st, st->eng_info[ENGINE_GYRO].running_rate);
	if (result)
	return result;
	// set ADLPF at this point not to change after accel is enabled
	result = inv_set_accel_config2(st, false);
	st->batch.pk_size = 0;
	for (i = 0; i < SENSOR_NUM_MAX; i++) {
	if (st->sensor[i].on)
	st->batch.pk_size += st->sensor[i].sample_size;
	}

	inv_set_batch(st);

	return result;
	}

	static int inv_determine_engine(struct inv_mpu_state *st)
	{
	int i;
	bool a_en, g_en;
	int accel_rate, gyro_rate;

	a_en = false;
	g_en = false;
	gyro_rate = MPU_INIT_SENSOR_RATE;
	accel_rate = MPU_INIT_SENSOR_RATE;
	/* loop the streaming sensors to see which engine needs to be turned on
	*/
	for (i = 0; i < SENSOR_NUM_MAX; i++) {
	if (st->sensor[i].on) {
	a_en \|= st->sensor[i].a_en;
	g_en \|= st->sensor[i].g_en;
	}
	}

	if (st->chip_config.eis_enable) {
	g_en = true;
	st->eis.frame_count = 0;
	st->eis.fsync_delay = 0;
	st->eis.gyro_counter = 0;
	st->eis.voting_count = 0;
	st->eis.voting_count_sub = 0;
	gyro_rate = BASE_SAMPLE_RATE;
	} else {
	st->eis.eis_triggered = false;
	st->eis.prev_state = false;
	}

	accel_rate = st->sensor[SENSOR_ACCEL].rate;
	gyro_rate = max(gyro_rate, st->sensor[SENSOR_GYRO].rate);

	st->ts_algo.clock_base = ENGINE_ACCEL;

	if (g_en) {
	/* gyro engine needs to be fastest */
	if (a_en)
	gyro_rate = max(gyro_rate, accel_rate);
	accel_rate = gyro_rate;
	st->ts_algo.clock_base = ENGINE_GYRO;
	} else if (a_en) {
	/* accel engine needs to be fastest if gyro engine is off */
	gyro_rate = accel_rate;
	st->ts_algo.clock_base = ENGINE_ACCEL;
	}

	st->eng_info[ENGINE_GYRO].running_rate = gyro_rate;
	st->eng_info[ENGINE_ACCEL].running_rate = accel_rate;
	if ((gyro_rate >= BASE_SAMPLE_RATE) \|\|
	(accel_rate >= BASE_SAMPLE_RATE))
	st->mode_1k_on = true;
	else
	st->mode_1k_on = false;
	/* engine divider for pressure and compass is set later */
	if (st->chip_config.eis_enable \|\| st->mode_1k_on) {
	st->eng_info[ENGINE_GYRO].divider = 1;
	st->eng_info[ENGINE_ACCEL].divider = 1;
	// need to update rate and div for 1khz mode
	for ( i = 0 ; i < SENSOR_L_NUM_MAX ; i++ ) {
	if (st->sensor_l[i].on) {
	st->sensor_l[i].counter = 0;
	if (st->sensor_l[i].rate)
	st->sensor_l[i].div =
	BASE_SAMPLE_RATE
	/ st->sensor_l[i].rate;
	else
	st->sensor_l[i].div = 0xffff;
	}
	}
	} else {
	st->eng_info[ENGINE_GYRO].divider = BASE_SAMPLE_RATE /
	st->eng_info[ENGINE_GYRO].running_rate;
	st->eng_info[ENGINE_ACCEL].divider = BASE_SAMPLE_RATE /
	st->eng_info[ENGINE_ACCEL].running_rate;
	}

	for ( i = 0 ; i < SENSOR_L_NUM_MAX ; i++ )
	st->sensor_l[i].counter = 0;

	inv_calc_engine_dur(&st->eng_info[ENGINE_GYRO]);
	inv_calc_engine_dur(&st->eng_info[ENGINE_ACCEL]);

	pr_debug("gen: %d aen: %d grate: %d arate: %d\n",
	g_en, a_en, gyro_rate, accel_rate);

	st->chip_config.gyro_enable = g_en;
	st->chip_config.accel_enable = a_en;

	return 0;
	}

	/*
	* set_inv_enable() - enable function.
	*/
	int set_inv_enable(struct iio_dev *indio_dev)
	{
	int result;
	struct inv_mpu_state *st = iio_priv(indio_dev);

	result = inv_switch_power_in_lp(st, true);
	if (result)
	return result;
	inv_stop_interrupt(st);
	inv_determine_engine(st);
	result = inv_set_rate(st);
	if (result) {
	pr_err("inv_set_rate error\n");
	return result;
	}
	result = inv_turn_on_engine(st);
	if (result) {
	pr_err("inv_turn_on_engine error\n");
	return result;
	}
	result = inv_reset_fifo(st, false);
	if (result)
	return result;
	result = inv_switch_power_in_lp(st, false);
	if ((!st->chip_config.gyro_enable) &&
	(!st->chip_config.accel_enable)) {
	inv_set_power(st, false);
	return 0;
	}

	return result;
	}
	/* dummy function for 20608D */
	int inv_enable_pedometer_interrupt(struct inv_mpu_state *st, bool en)
	{
	return 0;
	}
	int inv_dmp_read(struct inv_mpu_state st, int off, int size, u8 buf)
	{
	return 0;
	}
	int inv_firmware_load(struct inv_mpu_state *st)
	{
	return 0;
	}