/*
 * Copyright (C) 2012-2017 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"
#ifdef CONFIG_RTC_INTF_ALARM
#include <linux/android_alarm.h>
#endif
#include <linux/export.h>

#ifdef CONFIG_RTC_INTF_ALARM
s64 get_time_ns(void)
{
	struct timespec ts;

	/* get_monotonic_boottime(&ts); */

	/* Workaround for some platform on which monotonic clock and
	 * Android SystemClock has a gap.
	 * Use ktime_to_timespec(alarm_get_elapsed_realtime()) instead of
	 * get_monotonic_boottime() for these platform
	 */

	ts = ktime_to_timespec(alarm_get_elapsed_realtime());

	return timespec_to_ns(&ts);
}
#else
s64 get_time_ns(void)
{
	struct timespec ts;

	get_monotonic_boottime(&ts);

	/* Workaround for some platform on which monotonic clock and
	 * Android SystemClock has a gap.
	 * Use ktime_to_timespec(alarm_get_elapsed_realtime()) instead of
	 * get_monotonic_boottime() for these platform
	 */
	return timespec_to_ns(&ts);
}

#endif

#ifdef ACCEL_BIAS_TEST
int inv_get_3axis_average(s16 src[], s16 dst[], s16 reset)
{
#define BUFFER_SIZE 200
	static s16 buffer[BUFFER_SIZE][3];
	static s16 current_position = 0;
	static s16 ready = 0;
	int sum[3]= {0,};
	int i;

	if(reset){
		current_position = 0;
		ready = 0;
	}
	buffer[current_position][0] = src[0];
	buffer[current_position][1] = src[1];
	buffer[current_position][2] = src[2];
	current_position++;
	if(current_position == BUFFER_SIZE){
		ready = 1;
		current_position = 0;
	}
	if(ready){
		for(i = 0 ; i < BUFFER_SIZE ; i++){
			sum[0] += buffer[i][0];
			sum[1] += buffer[i][1];
			sum[2] += buffer[i][2];
		}
		dst[0] = sum[0]/BUFFER_SIZE;
		dst[1] = sum[1]/BUFFER_SIZE;
		dst[2] = sum[2]/BUFFER_SIZE;
		return 1;
	}
	return 0;
}
#endif

int inv_q30_mult(int a, int b)
{
#define DMP_MULTI_SHIFT                 30
	u64 temp;
	int result;

	temp = ((u64)a) * b;
	result = (int)(temp >> DMP_MULTI_SHIFT);

	return result;
}
#if defined(CONFIG_INV_MPU_IIO_ICM20648) || \
					defined(CONFIG_INV_MPU_IIO_ICM20690)
/* inv_read_secondary(): set secondary registers for reading.
   The chip must be set as bank 3 before calling.
 */
int inv_read_secondary(struct inv_mpu_state *st, int ind, int addr,
		       int reg, int len)
{
	int result;

	result = inv_plat_single_write(st, st->slv_reg[ind].addr,
				       INV_MPU_BIT_I2C_READ | addr);
	if (result)
		return result;
	result = inv_plat_single_write(st, st->slv_reg[ind].reg, reg);
	if (result)
		return result;
	result = inv_plat_single_write(st, st->slv_reg[ind].ctrl,
				       INV_MPU_BIT_SLV_EN | len);

	return result;
}

int inv_execute_read_secondary(struct inv_mpu_state *st, int ind, int addr,
			       int reg, int len, u8 *d)
{
	int result;

	inv_set_bank(st, BANK_SEL_3);
	result = inv_read_secondary(st, ind, addr, reg, len);
	if (result)
		return result;
	inv_set_bank(st, BANK_SEL_0);
	result = inv_plat_single_write(st, REG_USER_CTRL, st->i2c_dis |
				       BIT_I2C_MST_EN);
	msleep(SECONDARY_INIT_WAIT);
	result = inv_plat_single_write(st, REG_USER_CTRL, st->i2c_dis);
	if (result)
		return result;
	result = inv_plat_read(st, REG_EXT_SLV_SENS_DATA_00, len, d);

	return result;
}

/* inv_write_secondary(): set secondary registers for writing.
   The chip must be set as bank 3 before calling.
 */
int inv_write_secondary(struct inv_mpu_state *st, int ind, int addr,
			int reg, int v)
{
	int result;

	result = inv_plat_single_write(st, st->slv_reg[ind].addr, addr);
	if (result)
		return result;
	result = inv_plat_single_write(st, st->slv_reg[ind].reg, reg);
	if (result)
		return result;
	result = inv_plat_single_write(st, st->slv_reg[ind].ctrl,
				       INV_MPU_BIT_SLV_EN | 1);

	result = inv_plat_single_write(st, st->slv_reg[ind].d0, v);

	return result;
}

int inv_execute_write_secondary(struct inv_mpu_state *st, int ind, int addr,
				int reg, int v)
{
	int result;

	inv_set_bank(st, BANK_SEL_3);
	result = inv_write_secondary(st, ind, addr, reg, v);
	if (result)
		return result;
	inv_set_bank(st, BANK_SEL_0);
	result = inv_plat_single_write(st, REG_USER_CTRL, st->i2c_dis |
				       BIT_I2C_MST_EN);
	msleep(SECONDARY_INIT_WAIT);
	result = inv_plat_single_write(st, REG_USER_CTRL, st->i2c_dis);

	return result;
}

int inv_set_bank(struct inv_mpu_state *st, u8 bank)
{
#ifdef CONFIG_INV_MPU_IIO_ICM20648
	int r;

	r = inv_plat_single_write(st, REG_BANK_SEL, bank);

	return r;
#else
	return 0;
#endif
}
#endif

#ifdef CONFIG_INV_MPU_IIO_ICM20648
/**
 *  inv_write_cntl() - Write control word to designated address.
 *  @st:	Device driver instance.
 *  @wd:        control word.
 *  @en:	enable/disable.
 *  @cntl:	control address to be written.
 */
int inv_write_cntl(struct inv_mpu_state *st, u16 wd, bool en, int cntl)
{
	int result;
	u8 reg[2], d_out[2];

	result = mem_r(cntl, 2, d_out);
	if (result)
		return result;
	reg[0] = ((wd >> 8) & 0xff);
	reg[1] = (wd & 0xff);
	if (!en) {
		d_out[0] &= ~reg[0];
		d_out[1] &= ~reg[1];
	} else {
		d_out[0] |= reg[0];
		d_out[1] |= reg[1];
	}
	result = mem_w(cntl, 2, d_out);

	return result;
}
#endif

int inv_set_power(struct inv_mpu_state *st, bool power_on)
{
	u8 d;
	int r;

	if ((!power_on) == st->chip_config.is_asleep)
		return 0;

	d = BIT_CLK_PLL;
	if (!power_on)
		d |= BIT_SLEEP;

	r = inv_plat_single_write(st, REG_PWR_MGMT_1, d);
	if (r)
		return r;

	if (power_on)
		usleep_range(REG_UP_TIME_USEC, REG_UP_TIME_USEC);

	st->chip_config.is_asleep = !power_on;

	return 0;
}
EXPORT_SYMBOL_GPL(inv_set_power);

int inv_stop_interrupt(struct inv_mpu_state *st)
{
	int res;
#if defined(CONFIG_INV_MPU_IIO_ICM20648)
	/* disable_irq_wake alone should work already. However,
	   it might need system configuration change. From driver side,
	   we will disable IRQ altogether for non-wakeup sensors. */
	res = inv_plat_read(st, REG_INT_ENABLE, 1, &st->int_en);
	if (res)
		return res;
	res = inv_plat_read(st, REG_INT_ENABLE_2, 1, &st->int_en_2);
	if (res)
		return res;
	res = inv_plat_single_write(st, REG_INT_ENABLE, 0);
	if (res)
		return res;
	res = inv_plat_single_write(st, REG_INT_ENABLE_2, 0);
	if (res)
		return res;
#endif
#if defined(CONFIG_INV_MPU_IIO_ICM20608D)
	res = inv_plat_read(st, REG_INT_ENABLE, 1, &st->int_en);
	if (res)
		return res;
	res = inv_plat_single_write(st, REG_INT_ENABLE, 0);
	if (res)
		return res;
#endif
#if defined(CONFIG_INV_MPU_IIO_ICM20602) \
	|| defined(CONFIG_INV_MPU_IIO_ICM20690) \
	|| defined(CONFIG_INV_MPU_IIO_IAM20680)	
	res = inv_plat_read(st, REG_INT_ENABLE, 1, &st->int_en);
	if (res)
		return res;
	res = inv_plat_single_write(st, REG_INT_ENABLE, 0);
	if (res)
		return res;
#endif
	return 0;
}
int inv_reenable_interrupt(struct inv_mpu_state *st)
{
	int res = 0;
#if defined(CONFIG_INV_MPU_IIO_ICM20648)
	res = inv_plat_single_write(st, REG_INT_ENABLE, st->int_en);
	if (res)
		return res;
	res = inv_plat_single_write(st, REG_INT_ENABLE_2, st->int_en_2);
	if (res)
		return res;
#elif defined(CONFIG_INV_MPU_IIO_ICM20608D)
	res = inv_plat_single_write(st, REG_INT_ENABLE, st->int_en);
	if (res)
		return res;
#endif
#if defined(CONFIG_INV_MPU_IIO_ICM20602) \
	|| defined(CONFIG_INV_MPU_IIO_ICM20690) \
	|| defined(CONFIG_INV_MPU_IIO_IAM20680)	
	res = inv_plat_single_write(st, REG_INT_ENABLE, st->int_en);
	if (res)
		return res;
#endif
	return res;
}

static int inv_lp_en_off_mode(struct inv_mpu_state *st, bool on)
{
	int r;

	if (!st->chip_config.is_asleep)
		return 0;

	r = inv_plat_single_write(st, REG_PWR_MGMT_1, BIT_CLK_PLL);
	st->chip_config.is_asleep = 0;

	return r;
}
#ifdef CONFIG_INV_MPU_IIO_ICM20648
static int inv_lp_en_on_mode(struct inv_mpu_state *st, bool on)
{
	int r = 0;
	u8 w;

	if ((!st->chip_config.is_asleep) &&
	    ((!on) == st->chip_config.lp_en_set))
		return 0;

	w = BIT_CLK_PLL;
	if ((!on) && (!st->eis.eis_triggered))
		w |= BIT_LP_EN;
	r = inv_plat_single_write(st, REG_PWR_MGMT_1, w);
	st->chip_config.is_asleep = 0;
	st->chip_config.lp_en_set = (!on);
	return r;
}
#endif
#if defined(CONFIG_INV_MPU_IIO_ICM20602) \
	|| defined(CONFIG_INV_MPU_IIO_ICM20690) \
	|| defined(CONFIG_INV_MPU_IIO_IAM20680)	
int inv_set_accel_config2(struct inv_mpu_state *st, bool cycle_mode)
{
	int cycle_freq[] = {275, 192, 111, 59};
	int cont_freq[] = {219, 219, 99, 45, 22, 11, 6};
	int i, r, rate;
	u8 v;

	v = 0;
#ifdef CONFIG_INV_MPU_IIO_ICM20690
	v |= BIT_FIFO_SIZE_1K;
#endif
	if (cycle_mode) {
		rate = (st->eng_info[ENGINE_ACCEL].running_rate << 1);
		i = ARRAY_SIZE(cycle_freq) - 1;
		while (i > 0) {
			if (rate < cycle_freq[i]) {
				break;
			}
			i--;
		}
		r = inv_plat_single_write(st, REG_ACCEL_CONFIG_2, v |
								(i << 4) | 7);
		if (r)
			return r;
	} else {
		rate = (st->eng_info[ENGINE_ACCEL].running_rate >> 1);
		for (i = 1; i < ARRAY_SIZE(cont_freq); i++) {
			if (rate >= cont_freq[i])
				break;
		}
		if (i > 6)
			i = 6;
		r = inv_plat_single_write(st, REG_ACCEL_CONFIG_2, v | i);
		if (r)
			return r;
	}

	return 0;
}
static int inv_lp_en_on_mode(struct inv_mpu_state *st, bool on)
{
	int r = 0;
	u8 w;
	bool cond_check;

	if ((!st->chip_config.is_asleep) &&
					((!on) == st->chip_config.lp_en_set))
		return 0;
	cond_check = (!on) && st->cycle_on;

	w = BIT_CLK_PLL;
	r = inv_plat_single_write(st, REG_PWR_MGMT_1, w);
	if (cond_check) {
		w |= BIT_LP_EN;
		inv_set_accel_config2(st, true);
		st->chip_config.lp_en_set = true;
		r = inv_plat_single_write(st, REG_PWR_MGMT_1, w);
	} else {
		inv_set_accel_config2(st, false);
#ifdef CONFIG_INV_MPU_IIO_ICM20690
		r = inv_plat_single_write(st, REG_PWR_MGMT_1, w | BIT_SLEEP);
		if (r)
			return r;
#endif
		st->chip_config.lp_en_set = false;
		r = inv_plat_single_write(st, REG_PWR_MGMT_1, w);
		msleep(10);
	}
	st->chip_config.is_asleep = 0;

	return r;
}
#endif
#ifdef CONFIG_INV_MPU_IIO_ICM20608D
static int inv_set_accel_config2(struct inv_mpu_state *st)
{
	int cont_freq[] = {219, 219, 99, 45, 22, 11, 6};
	int dec2_cfg = 0;
	int i, r, rate;

	rate = (st->eng_info[ENGINE_ACCEL].running_rate << 1);
	i = 0;
	if (!st->chip_config.eis_enable){
		while ((rate < cont_freq[i]) && (i < ARRAY_SIZE(cont_freq) - 1))
			i++;
		dec2_cfg = 2<<4; //4x
	}
	r = inv_plat_single_write(st, REG_ACCEL_CONFIG_2, i | dec2_cfg);
	if (r)
		return r;
	return 0;
}
static int inv_lp_en_on_mode(struct inv_mpu_state *st, bool on)
{
	int r = 0;
	u8 w;

	w = BIT_CLK_PLL;
	if ((!on) && (!st->chip_config.eis_enable))
		w |= BIT_LP_EN;
	inv_set_accel_config2(st);
	r = inv_plat_single_write(st, REG_PWR_MGMT_1, w);

	return r;
}
#endif
int inv_switch_power_in_lp(struct inv_mpu_state *st, bool on)
{
	int r;

	if (st->chip_config.lp_en_mode_off)
		r = inv_lp_en_off_mode(st, on);
	else
		r = inv_lp_en_on_mode(st, on);

	return r;
}
EXPORT_SYMBOL_GPL(inv_switch_power_in_lp);

int write_be16_to_mem(struct inv_mpu_state *st, u16 data, int addr)
{
	u8 d[2];

	d[0] = (data >> 8) & 0xff;
	d[1] = data & 0xff;

	return mem_w(addr, sizeof(d), d);
}

int write_be32_to_mem(struct inv_mpu_state *st, u32 data, int addr)
{
	cpu_to_be32s(&data);
	return mem_w(addr, sizeof(data), (u8 *)&data);
}

int read_be16_from_mem(struct inv_mpu_state *st, u16 *o, int addr)
{
	int result;
	u8 d[2];

	result = mem_r(addr, 2, (u8 *) &d);
	*o = d[0] << 8 | d[1];

	return result;
}

int read_be32_from_mem(struct inv_mpu_state *st, u32 *o, int addr)
{
	int result;
	u32 d = 0;

	result = mem_r(addr, 4, (u8 *) &d);
	*o = be32_to_cpup((__be32 *)(&d));

	return result;
}

int be32_to_int(u8 *d)
{
	return (d[0] << 24) | (d[1] << 16) | (d[2] << 8) | d[3];
}

u32 inv_get_cntr_diff(u32 curr_counter, u32 prev)
{
	u32 diff;

	if (curr_counter > prev)
		diff = curr_counter - prev;
	else
		diff = 0xffffffff - prev + curr_counter + 1;

	return diff;
}

int inv_write_2bytes(struct inv_mpu_state *st, int addr, int data)
{
	u8 d[2];

	if (data < 0 || data > USHRT_MAX)
		return -EINVAL;

	d[0] = (u8) ((data >> 8) & 0xff);
	d[1] = (u8) (data & 0xff);

	return mem_w(addr, ARRAY_SIZE(d), d);
}



int inv_process_eis(struct inv_mpu_state *st, u16 delay)
{
	int tmp1, tmp2, tmp3;

	switch (st->eis.voting_state) {
	case 0:
		st->eis.gyro_counter_s[0] = st->eis.gyro_counter;
		st->eis.fsync_delay_s[0] = delay - st->eis.fsync_delay;
		st->eis.voting_count = 1;
		st->eis.voting_count_sub = 0;
		st->eis.voting_state = 1;
		break;
	case 1:
		if (abs(st->eis.gyro_counter_s[0] -
						st->eis.gyro_counter) <= 1) {
			st->eis.voting_count++;
		} else {
			st->eis.gyro_counter_s[2] = st->eis.gyro_counter;
			st->eis.voting_count_sub++;
			st->eis.voting_state = 2;
		}
		if (st->eis.voting_count > 5)
			st->eis.voting_state = 3;
		break;
	case 2:
		tmp1 = abs(st->eis.gyro_counter_s[0] - st->eis.gyro_counter);
		tmp2 = abs(st->eis.gyro_counter_s[2] - st->eis.gyro_counter);

		if ((tmp1 < tmp2) && (tmp1 <= 1))
			st->eis.voting_count++;
		else
			st->eis.voting_count_sub++;
		if (st->eis.voting_count > 5) {
			st->eis.voting_state = 3;
			st->eis.voting_count = 0;
			st->eis.voting_count_sub = 0;
		}

		if (st->eis.voting_count_sub > 5) {
			st->eis.gyro_counter_s[0] = st->eis.gyro_counter;
			st->eis.fsync_delay_s[0] = delay - st->eis.fsync_delay;
			st->eis.voting_state = 1;
			st->eis.voting_count = 1;
			st->eis.voting_count_sub = 0;
		}
		break;
	case 3:
		tmp1 = abs(st->eis.gyro_counter_s[0] - st->eis.gyro_counter);
		if (tmp1 == 1) {
			st->eis.gyro_counter_s[1] = st->eis.gyro_counter;
			st->eis.fsync_delay_s[1] = delay - st->eis.fsync_delay;
			st->eis.voting_state = 4;
			st->eis.voting_count_sub = 1;
			st->eis.voting_count = 1;
		}
		break;
	case 4:
		if (st->eis.gyro_counter == st->eis.gyro_counter_s[0]) {
			tmp1 = delay - st->eis.fsync_delay;
			tmp2 = abs(tmp1 - st->eis.fsync_delay_s[0]);
			if (tmp2 < 3) {
				st->eis.voting_count++;
			} else {
				st->eis.fsync_delay_s[2] = tmp1;
				st->eis.voting_count_sub = 1;
				st->eis.voting_state = 5;
			}
			if (st->eis.voting_count > 5) {
				st->eis.voting_count = 1;
				st->eis.voting_state = 6;
			}
		}
		break;
	case 5:
		if (st->eis.gyro_counter == st->eis.gyro_counter_s[0]) {
			tmp1 = delay - st->eis.fsync_delay;

			tmp2 = abs(tmp1 - st->eis.fsync_delay_s[0]);
			tmp3 = abs(tmp1 - st->eis.fsync_delay_s[2]);
			if ((tmp2 < tmp3) && (tmp2 < 3))
				st->eis.voting_count++;
			else
				st->eis.voting_count_sub++;
			if ((st->eis.voting_count > 5) &&
					(st->eis.voting_count_sub
					< st->eis.voting_count)) {
				st->eis.voting_state = 6;
				st->eis.voting_count = 1;
			} else if (st->eis.voting_count_sub > 5) {
				st->eis.fsync_delay_s[0] = tmp1;
				st->eis.voting_state = 4;
				st->eis.voting_count = 1;
			}

		}
		break;
	case 6:
		if (st->eis.gyro_counter == st->eis.gyro_counter_s[1]) {
			tmp1 = delay - st->eis.fsync_delay;
			tmp2 = abs(tmp1 - st->eis.fsync_delay_s[1]);
			if (tmp2 < 3) {
				st->eis.voting_count++;
			} else {
				st->eis.fsync_delay_s[2] = tmp1;
				st->eis.voting_count_sub = 1;
				st->eis.voting_count = 1;
				st->eis.voting_state = 7;
			}
			if (st->eis.voting_count > 5)
				st->eis.voting_state = 8;
		}
		break;
	case 7:
		if (st->eis.gyro_counter == st->eis.gyro_counter_s[1]) {
			tmp1 = delay - st->eis.fsync_delay;

			tmp2 = abs(tmp1 - st->eis.fsync_delay_s[1]);
			tmp3 = abs(tmp1 - st->eis.fsync_delay_s[2]);
			if ((tmp2 < tmp3) && (tmp2 < 3))
				st->eis.voting_count++;
			else
				st->eis.voting_count_sub++;
			if ((st->eis.voting_count > 5) &&
					(st->eis.voting_count_sub
					< st->eis.voting_count)) {
				st->eis.voting_state = 8;
			} else if (st->eis.voting_count_sub > 5) {
				st->eis.fsync_delay_s[1] = tmp1;
				st->eis.voting_state = 6;
				st->eis.voting_count = 1;
			}

		}
		break;
	default:
		break;
	}

	pr_debug("de= %d gc= %d\n", delay, st->eis.gyro_counter);
	st->eis.fsync_delay = delay;
	st->eis.gyro_counter = 0;

	pr_debug("state=%d g1= %d d1= %d g2= %d d2= %d\n",
			st->eis.voting_state,
			st->eis.gyro_counter_s[0],
			st->eis.fsync_delay_s[0],
			st->eis.gyro_counter_s[1],
			st->eis.fsync_delay_s[1]);

	return 0;
}

int inv_rate_convert(struct inv_mpu_state *st, int ind, int data)
{
	int t, out, out1, out2;
	int base_freq;

	if (data <= MPU_DEFAULT_DMP_FREQ)
		base_freq = MPU_DEFAULT_DMP_FREQ;
	else
		base_freq = BASE_SAMPLE_RATE;

	t = base_freq / data;
	if (!t)
		t = 1;
	out1 = base_freq / (t + 1);
	out2 = base_freq / t;
	if ((data - out1) * INV_ODR_BUFFER_MULTI < data)
		out = out1;
	else
		out = out2;

	return out;
}

static void inv_check_wake_non_wake(struct inv_mpu_state *st,
			enum SENSOR_L wake, enum SENSOR_L non_wake)
{
	int tmp_rate;

	if (!st->sensor_l[wake].on && !st->sensor_l[non_wake].on)
		return;

	tmp_rate = MPU_INIT_SENSOR_RATE;
	if (st->sensor_l[wake].on)
		tmp_rate = st->sensor_l[wake].rate;
	if (st->sensor_l[non_wake].on)
		tmp_rate = max(tmp_rate, st->sensor_l[non_wake].rate);
	st->sensor_l[wake].rate = tmp_rate;
	st->sensor_l[non_wake].rate = tmp_rate;
}

static void inv_check_wake_non_wake_divider(struct inv_mpu_state *st,
			enum SENSOR_L wake, enum SENSOR_L non_wake)
{
	if (st->sensor_l[wake].on && st->sensor_l[non_wake].on)
		st->sensor_l[non_wake].div = 0xffff;

}

#if defined(CONFIG_INV_MPU_IIO_ICM20602) \
	|| defined(CONFIG_INV_MPU_IIO_ICM20690) \
	|| defined(CONFIG_INV_MPU_IIO_IAM20680)	
int inv_check_sensor_on(struct inv_mpu_state *st)
{
	int i, max_rate;
	enum SENSOR_L wake[] = {SENSOR_L_GYRO_WAKE, SENSOR_L_ACCEL_WAKE,
					SENSOR_L_MAG_WAKE};
	enum SENSOR_L non_wake[] = {SENSOR_L_GYRO, SENSOR_L_ACCEL,
					SENSOR_L_MAG};

	st->sensor_l[SENSOR_L_GESTURE_ACCEL].rate = GESTURE_ACCEL_RATE;
	for (i = 0; i < SENSOR_NUM_MAX; i++)
		st->sensor[i].on = false;
	for (i = 0; i < SENSOR_NUM_MAX; i++)
		st->sensor[i].rate = MPU_INIT_SENSOR_RATE;

	if ((st->step_detector_l_on
			|| st->step_detector_wake_l_on
			|| st->step_counter_l_on
			|| st->step_counter_wake_l_on
			|| st->chip_config.pick_up_enable
			|| st->chip_config.tilt_enable)
			&& (!st->sensor_l[SENSOR_L_ACCEL].on)
			&& (!st->sensor_l[SENSOR_L_ACCEL_WAKE].on))
		st->sensor_l[SENSOR_L_GESTURE_ACCEL].on = true;
	else
		st->sensor_l[SENSOR_L_GESTURE_ACCEL].on = false;


	st->chip_config.wake_on = false;
	for (i = 0; i < SENSOR_L_NUM_MAX; i++) {
		if (st->sensor_l[i].on && st->sensor_l[i].rate) {
			st->sensor[st->sensor_l[i].base].on = true;
			st->chip_config.wake_on |= st->sensor_l[i].wake_on;
		}
	}
	if (st->sensor_l[SENSOR_L_GESTURE_ACCEL].on &&
				(!st->sensor[SENSOR_GYRO].on) &&
				(!st->sensor[SENSOR_COMPASS].on))
		st->gesture_only_on = true;
	else
		st->gesture_only_on = false;

	for (i = 0; i < SENSOR_L_NUM_MAX; i++) {
		if (st->sensor_l[i].on) {
			st->sensor[st->sensor_l[i].base].rate =
			    max(st->sensor[st->sensor_l[i].base].rate,
							st->sensor_l[i].rate);
		}
	}
	max_rate = MPU_INIT_SENSOR_RATE;
	if (st->chip_config.eis_enable) {
		max_rate = ESI_GYRO_RATE;
		st->sensor_l[SENSOR_L_EIS_GYRO].rate = ESI_GYRO_RATE;
	}

	for (i = 0; i < SENSOR_NUM_MAX; i++) {
		if (st->sensor[i].on) {
			max_rate = max(max_rate, st->sensor[i].rate);
		}
	}
	for (i = 0; i < SENSOR_NUM_MAX; i++) {
		if (st->sensor[i].on) {
			st->sensor[i].rate = max_rate;
		}
	}
	for (i = 0; i < ARRAY_SIZE(wake); i++)
		inv_check_wake_non_wake(st, wake[i], non_wake[i]);

	for (i = 0; i < SENSOR_L_NUM_MAX; i++) {
		if (st->sensor_l[i].on) {
			if (st->sensor_l[i].rate)
				st->sensor_l[i].div =
				    st->sensor[st->sensor_l[i].base].rate
							/ st->sensor_l[i].rate;
			else
				st->sensor_l[i].div = 0xffff;
			pr_debug("sensor= %d, div= %d\n",
						i, st->sensor_l[i].div);
		}
	}
	for (i = 0; i < ARRAY_SIZE(wake); i++)
		inv_check_wake_non_wake_divider(st, wake[i], non_wake[i]);

	if (st->step_detector_wake_l_on ||
			st->step_counter_wake_l_on ||
			st->chip_config.pick_up_enable ||
			st->chip_config.tilt_enable)
		st->chip_config.wake_on = true;

	return 0;
}
#else
static void inv_do_check_sensor_on(struct inv_mpu_state *st,
				enum SENSOR_L *wake,
				enum SENSOR_L *non_wake, int sensor_size)
{
	int i;

	for (i = 0; i < SENSOR_NUM_MAX; i++)
		st->sensor[i].on = false;

	for (i = 0; i < SENSOR_NUM_MAX; i++)
		st->sensor[i].rate = MPU_INIT_SENSOR_RATE;

	st->chip_config.wake_on = false;
	for (i = 0; i < SENSOR_L_NUM_MAX; i++) {
		if (st->sensor_l[i].on && st->sensor_l[i].rate) {
			st->sensor[st->sensor_l[i].base].on = true;
			st->chip_config.wake_on |= st->sensor_l[i].wake_on;
		}
	}

	for (i = 0; i < SENSOR_L_NUM_MAX; i++) {
		if (st->sensor_l[i].on) {
			st->sensor[st->sensor_l[i].base].rate =
			    max(st->sensor[st->sensor_l[i].base].rate,
				st->sensor_l[i].rate);
		}
	}
	for (i = 0; i < sensor_size; i++)
		inv_check_wake_non_wake(st, wake[i], non_wake[i]);

	for (i = 0; i < SENSOR_L_NUM_MAX; i++) {
		if (st->sensor_l[i].on) {
			if (st->sensor_l[i].rate)
				st->sensor_l[i].div =
				    st->sensor[st->sensor_l[i].base].rate
				    / st->sensor_l[i].rate;
			else
				st->sensor_l[i].div = 0xffff;
		}
	}
	for (i = 0; i < sensor_size; i++)
		inv_check_wake_non_wake_divider(st, wake[i], non_wake[i]);

	if (st->step_detector_wake_l_on ||
			st->step_counter_wake_l_on ||
			st->chip_config.pick_up_enable ||
			st->chip_config.tilt_enable ||
			st->smd.on)
		st->chip_config.wake_on = true;

}
#endif

#if defined(CONFIG_INV_MPU_IIO_ICM20608D)
int inv_check_sensor_on(struct inv_mpu_state *st)
{
	enum SENSOR_L wake[] = {SENSOR_L_GYRO_WAKE, SENSOR_L_ACCEL_WAKE,
				SENSOR_L_SIXQ_WAKE, SENSOR_L_PEDQ_WAKE,
				SENSOR_L_GYRO_CAL_WAKE};
	enum SENSOR_L non_wake[] = {SENSOR_L_GYRO, SENSOR_L_ACCEL,
				SENSOR_L_SIXQ, SENSOR_L_PEDQ,
				SENSOR_L_GYRO_CAL};

	inv_do_check_sensor_on(st, wake, non_wake, ARRAY_SIZE(wake));

	return 0;
}
#endif

#if defined(CONFIG_INV_MPU_IIO_ICM20648)
int inv_check_sensor_on(struct inv_mpu_state *st)
{
	enum SENSOR_L wake[] = {SENSOR_L_GYRO_WAKE, SENSOR_L_ACCEL_WAKE,
				SENSOR_L_MAG_WAKE, SENSOR_L_ALS_WAKE,
				SENSOR_L_SIXQ_WAKE, SENSOR_L_PEDQ_WAKE,
				SENSOR_L_NINEQ_WAKE, SENSOR_L_GEOMAG_WAKE,
				SENSOR_L_PRESSURE_WAKE,
				SENSOR_L_GYRO_CAL_WAKE,
				SENSOR_L_MAG_CAL_WAKE};
	enum SENSOR_L non_wake[] = {SENSOR_L_GYRO, SENSOR_L_ACCEL,
					SENSOR_L_MAG, SENSOR_L_ALS,
					SENSOR_L_SIXQ, SENSOR_L_PEDQ,
					SENSOR_L_NINEQ, SENSOR_L_GEOMAG,
					SENSOR_L_PRESSURE,
					SENSOR_L_GYRO_CAL,
					SENSOR_L_MAG_CAL};

	inv_do_check_sensor_on(st, wake, non_wake, ARRAY_SIZE(wake));

	return 0;
}
#endif

#ifdef CONFIG_PM_SLEEP
int inv_mpu_suspend(struct iio_dev *indio_dev)
{
	struct inv_mpu_state *st = iio_priv(indio_dev);

	/* add code according to different request Start */
	dev_info(st->dev, "%s suspend\n", st->hw->name);
	mutex_lock(&indio_dev->mlock);

	st->resume_state = false;
	if (st->chip_config.wake_on) {
		enable_irq_wake(st->irq);
	} else {
		inv_stop_interrupt(st);
	}

	mutex_unlock(&indio_dev->mlock);

	return 0;
}
EXPORT_SYMBOL_GPL(inv_mpu_suspend);

/*
 * inv_mpu_complete(): complete method for this driver.
 *    This method can be modified according to the request of different
 *    customers. It basically undo everything suspend is doing
 *    and recover the chip to what it was before suspend. We use complete to
 *    make sure that alarm clock resume is finished. If we use resume, the
 *    alarm clock may not resume yet and get incorrect clock reading.
 */
void inv_mpu_complete(struct iio_dev *indio_dev)
{
	struct inv_mpu_state *st = iio_priv(indio_dev);

	dev_info(st->dev, "%s resume\n", st->hw->name);
	if (st->resume_state)
		return;

	mutex_lock(&indio_dev->mlock);

	if (!st->chip_config.wake_on) {
		inv_reenable_interrupt(st);
	} else {
		disable_irq_wake(st->irq);
	}
	/*	resume state is used to synchronize read_fifo such that it won't
		proceed unless resume is finished. */
	st->resume_state = true;
	/*	resume flag is indicating that current clock reading is from resume,
		it has up to 1 second drift and should do proper processing */
	st->ts_algo.resume_flag  = true;
	mutex_unlock(&indio_dev->mlock);
	wake_up_interruptible(&st->wait_queue);

	return;
}
EXPORT_SYMBOL_GPL(inv_mpu_complete);
#endif