/*
 * Copyright (c) 2018, The Linux Foundation. All rights reserved.
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 and
 * only version 2 as published by the Free Software Foundation.
 *
 * 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.
 */

#include <linux/module.h>
#include <linux/init.h>
#include <linux/slab.h>
#include <linux/err.h>
#include <linux/delay.h>
#include <linux/sysfs.h>
#include <linux/jiffies.h>
#include <linux/irq.h>
#include <linux/interrupt.h>
#include <linux/kfifo.h>
#include <linux/poll.h>
#include <linux/i2c.h>
#include <linux/spi/spi.h>
#include "inv_icm20602_bsp.h"
#include "inv_icm20602_iio.h"

#define icm20602_init_reg_addr(head, tail, reg_map) { \
			enum inv_icm20602_reg_addr i;\
			for (i = head; i <= tail; i++) {\
				reg_map->address = i;\
				reg_map->reg_u.reg = 0x0;\
				reg_map++;\
			} \
}

#define icm20602_write_reg_simple(st, register) \
		icm20602_write_reg(st, \
		register.address, \
		register.reg_u.reg)

static struct inv_icm20602_reg_map reg_set_20602;

int icm20602_init_reg_map(void)
{
	struct struct_XG_OFFS_TC_H *reg_map = &(reg_set_20602.XG_OFFS_TC_H);

	icm20602_init_reg_addr(ADDR_XG_OFFS_TC_H,
			ADDR_XG_OFFS_TC_L, reg_map);

	icm20602_init_reg_addr(ADDR_YG_OFFS_TC_H,
			ADDR_YG_OFFS_TC_L, reg_map);

	icm20602_init_reg_addr(ADDR_ZG_OFFS_TC_H,
			ADDR_ZG_OFFS_TC_L, reg_map);

	icm20602_init_reg_addr(ADDR_SELF_TEST_X_ACCEL,
			ADDR_SELF_TEST_Z_ACCEL, reg_map);

	icm20602_init_reg_addr(ADDR_XG_OFFS_USRH,
			ADDR_LP_MODE_CFG, reg_map);

	icm20602_init_reg_addr(ADDR_ACCEL_WOM_X_THR,
			ADDR_FIFO_EN, reg_map);

	icm20602_init_reg_addr(ADDR_FSYNC_INT,
			ADDR_GYRO_ZOUT_L, reg_map);

	icm20602_init_reg_addr(ADDR_SELF_TEST_X_GYRO,
			ADDR_SELF_TEST_Z_GYRO, reg_map);

	icm20602_init_reg_addr(ADDR_FIFO_WM_TH1,
			ADDR_FIFO_WM_TH2, reg_map);

	icm20602_init_reg_addr(ADDR_SIGNAL_PATH_RESET,
			ADDR_PWR_MGMT_2, reg_map);

	icm20602_init_reg_addr(ADDR_I2C_IF,
			ADDR_I2C_IF, reg_map);

	icm20602_init_reg_addr(ADDR_FIFO_COUNTH,
			ADDR_XA_OFFSET_L, reg_map);

	icm20602_init_reg_addr(ADDR_YA_OFFSET_H,
			ADDR_YA_OFFSET_L, reg_map);

	icm20602_init_reg_addr(ADDR_ZA_OFFSET_H,
			ADDR_ZA_OFFSET_L, reg_map);

	return MPU_SUCCESS;
}

#define W_FLG	0
#define R_FLG	1
int icm20602_bulk_read(struct inv_icm20602_state *st,
			int reg, char *buf, int size)
{
	int result = MPU_SUCCESS;
	char tx_buf[2] = {0x0, 0x0};
	int tmp_size = size;
	int tmp_buf = buf;
	struct i2c_msg msg[2];

	if (!st || !buf)
		return -MPU_FAIL;

	if (st->interface == ICM20602_SPI) {
		tx_buf[0] = ICM20602_READ_REG(reg);
		result = spi_write_then_read(st->spi, &tx_buf[0],
			1, tmp_buf, size);
		if (result) {
			dev_dbgerr("mpu read reg %u failed, rc %d\n",
				reg, result);
			result = -MPU_READ_FAIL;
		}
	} else {
		result = size;
		while (tmp_size > 0) {
#ifdef ICM20602_I2C_SMBUS
			result += i2c_smbus_read_i2c_block_data(st->client,
				reg, (tmp_size < 32)?tmp_size:32, tmp_buf);
			tmp_size -= 32;
			tmp_buf += tmp_size;
#else
			tx_buf[0] = reg;
			msg[0].addr = st->client->addr;
			msg[0].flags = W_FLG;
			msg[0].len = 1;
			msg[0].buf = tx_buf;

			msg[1].addr = st->client->addr;
			msg[1].flags = I2C_M_RD;
			msg[1].len = (tmp_size < 32)?tmp_size:32;
			msg[1].buf = tmp_buf;
			i2c_transfer(st->client->adapter, msg, ARRAY_SIZE(msg));
			tmp_size -= 32;
			tmp_buf += tmp_size;
#endif
		}
	}

	return result;
}

static int icm20602_write_reg(struct inv_icm20602_state *st,
			 uint8_t reg, uint8_t val)
{
	int result = MPU_SUCCESS;
	char txbuf[2] = {0x0, 0x0};
	struct i2c_msg msg[1];

	if (st->interface == ICM20602_SPI) {
		txbuf[0] = ICM20602_WRITE_REG(reg);
		txbuf[1] = val;
		result = spi_write_then_read(st->spi, &txbuf[0], 2, NULL, 0);
		if (result) {
			dev_dbgerr("mpu write reg %u failed, rc %d\n",
						reg, val);
			result = -MPU_READ_FAIL;
		}
	} else if (st->interface == ICM20602_I2C) {
#ifdef ICM20602_I2C_SMBUS
		result = i2c_smbus_write_i2c_block_data(st->client,
						reg, 1, &val);
#else
		txbuf[0] = reg;
		txbuf[1] = val;
		msg[0].addr = st->client->addr;
		msg[0].flags = I2C_M_IGNORE_NAK;
		msg[0].len = 2;
		msg[0].buf = txbuf;

		i2c_transfer(st->client->adapter, msg, ARRAY_SIZE(msg));
#endif
	}

	return result;
}

static int icm20602_read_reg(struct inv_icm20602_state *st,
				uint8_t reg, uint8_t *val)
{
	int result = MPU_SUCCESS;
	char txbuf[1] = {0x0};
	char rxbuf[1] = {0x0};
	struct i2c_msg msg[2];

	if (st->interface == ICM20602_SPI) {
		txbuf[0] = ICM20602_READ_REG(reg);
		result = spi_write_then_read(st->spi,
						&txbuf[0], 1, rxbuf, 1);
		if (result) {
			dev_dbgerr("mpu read reg %u failed, rc %d\n",
						reg, result);
			result = -MPU_READ_FAIL;
		}
	} else if (st->interface == ICM20602_I2C) {
#ifdef ICM20602_I2C_SMBUS
		result = i2c_smbus_read_i2c_block_data(st->client,
						reg, 1, rxbuf);
		if (result != 1) {
			dev_dbgerr("mpu read reg %u failed, rc %d\n",
						reg, result);
			result = -MPU_READ_FAIL;
		}
#else
		txbuf[0] = reg;
		msg[0].addr = st->client->addr;
		msg[0].flags = W_FLG;
		msg[0].len = 1;
		msg[0].buf = txbuf;

		msg[1].addr = st->client->addr;
		msg[1].flags = I2C_M_RD;
		msg[1].len = 1;
		msg[1].buf = rxbuf;

		i2c_transfer(st->client->adapter, msg, ARRAY_SIZE(msg));
#endif
	}
	*val = rxbuf[0];

	return result;
}

#define combine_8_to_16(upper, lower) ((upper << 8) | lower)

int icm20602_read_raw(struct inv_icm20602_state *st,
		struct struct_icm20602_real_data *real_data, u8 type)
{
	struct struct_icm20602_raw_data raw_data;

	if (type & ACCEL != 0) {
		icm20602_read_reg(st,
			reg_set_20602.ACCEL_XOUT_H.address,
			&raw_data.ACCEL_XOUT_H);
		icm20602_read_reg(st,
			reg_set_20602.ACCEL_XOUT_L.address,
			&raw_data.ACCEL_XOUT_L);
		real_data->ACCEL_XOUT =
			combine_8_to_16(raw_data.ACCEL_XOUT_H,
			raw_data.ACCEL_XOUT_L);

		icm20602_read_reg(st,
			reg_set_20602.ACCEL_YOUT_H.address,
			&raw_data.ACCEL_YOUT_H);
		icm20602_read_reg(st,
			reg_set_20602.ACCEL_YOUT_L.address,
			&raw_data.ACCEL_YOUT_L);
		real_data->ACCEL_YOUT =
			combine_8_to_16(raw_data.ACCEL_YOUT_H,
			raw_data.ACCEL_YOUT_L);

		icm20602_read_reg(st,
			reg_set_20602.ACCEL_ZOUT_H.address,
			&raw_data.ACCEL_ZOUT_H);
		icm20602_read_reg(st,
			reg_set_20602.ACCEL_ZOUT_L.address,
			&raw_data.ACCEL_ZOUT_L);
		real_data->ACCEL_ZOUT =
			combine_8_to_16(raw_data.ACCEL_ZOUT_H,
			raw_data.ACCEL_ZOUT_L);
	}

	if (type & GYRO != 0) {
		icm20602_read_reg(st,
			reg_set_20602.GYRO_XOUT_H.address,
			&raw_data.GYRO_XOUT_H);
		icm20602_read_reg(st,
			reg_set_20602.GYRO_XOUT_L.address,
			&raw_data.GYRO_XOUT_L);
		real_data->GYRO_XOUT =
			combine_8_to_16(raw_data.GYRO_XOUT_H,
			raw_data.GYRO_XOUT_L);

		icm20602_read_reg(st,
			reg_set_20602.GYRO_YOUT_H.address,
			&raw_data.GYRO_YOUT_H);
		icm20602_read_reg(st,
			reg_set_20602.GYRO_YOUT_L.address,
			&raw_data.GYRO_YOUT_L);
		real_data->GYRO_YOUT =
			combine_8_to_16(raw_data.GYRO_YOUT_H,
			raw_data.GYRO_YOUT_L);

		icm20602_read_reg(st,
			reg_set_20602.GYRO_ZOUT_H.address,
			&raw_data.GYRO_ZOUT_H);
		icm20602_read_reg(st,
			reg_set_20602.GYRO_ZOUT_L.address,
			&raw_data.GYRO_ZOUT_L);
		real_data->GYRO_ZOUT =
			combine_8_to_16(raw_data.GYRO_ZOUT_H,
			raw_data.GYRO_ZOUT_L);
	}

	return MPU_SUCCESS;
}

int icm20602_read_fifo(struct inv_icm20602_state *st,
					void *buf, const int size)
{
	return icm20602_bulk_read(st,
		reg_set_20602.FIFO_R_W.address, buf, size);
}

int icm20602_start_fifo(struct inv_icm20602_state *st)
{
	struct icm20602_user_config *config = NULL;

	config = st->config;

	/* enable fifo */
	if (config->fifo_enabled) {
		reg_set_20602.USER_CTRL.reg_u.REG.FIFO_EN = 0x1;
		if (icm20602_write_reg_simple(st,
						reg_set_20602.USER_CTRL)) {
			dev_dbgerr("icm20602 start fifo failed\n");
			return -MPU_FAIL;
		}

		/* enable interrupt, need to test */
		reg_set_20602.INT_ENABLE.reg_u.REG.FIFO_OFLOW_EN = 0x1;
		reg_set_20602.INT_ENABLE.reg_u.REG.DATA_RDY_INT_EN = 0x0;
		if (icm20602_write_reg_simple(st,
						reg_set_20602.INT_ENABLE)) {
			dev_dbgerr("icm20602 set FIFO_OFLOW_EN failed\n");
			return -MPU_FAIL;
		}
	}

	return MPU_SUCCESS;
}

static int icm20602_stop_fifo(struct inv_icm20602_state *st)
{
	struct icm20602_user_config *config = NULL;

	config = st->config;
	/* disable fifo */
	if (config->fifo_enabled) {
		reg_set_20602.USER_CTRL.reg_u.REG.FIFO_EN = 0x0;
		reg_set_20602.USER_CTRL.reg_u.REG.FIFO_RST = 0x1;
		if (icm20602_write_reg_simple(st,
				reg_set_20602.USER_CTRL)) {
			reg_set_20602.USER_CTRL.reg_u.REG.FIFO_RST = 0x0;
			dev_dbgerr("icm20602 stop fifo failed\n");
			return -MPU_FAIL;
		}
		reg_set_20602.USER_CTRL.reg_u.REG.FIFO_RST = 0x0;
	}

	return MPU_SUCCESS;
}

static int icm20602_config_waterlevel(struct inv_icm20602_state *st)
{
	struct icm20602_user_config *config = NULL;
	u8 val = 0;

	config = st->config;
	if (config->fifo_enabled != true)
		return MPU_SUCCESS;
	/* config waterlevel as the fps need */
		config->gyro_accel_sample_rate);
	config->fifo_waterlevel = (config->user_fps_in_ms /
				(1000 / config->gyro_accel_sample_rate))
				*ICM20602_PACKAGE_SIZE;

	if (config->fifo_waterlevel > 1023 ||
		config->fifo_waterlevel/50 >
		(1023-config->fifo_waterlevel)/ICM20602_PACKAGE_SIZE) {
		dev_dbgerr("set fifo_waterlevel failed %d\n",
					config->fifo_waterlevel);
		return MPU_FAIL;
	}
	reg_set_20602.FIFO_WM_TH1.reg_u.reg =
				(config->fifo_waterlevel & 0xff00) >> 8;
	reg_set_20602.FIFO_WM_TH2.reg_u.reg =
				(config->fifo_waterlevel & 0x00ff);

	icm20602_write_reg_simple(st, reg_set_20602.FIFO_WM_TH1);
	icm20602_write_reg_simple(st, reg_set_20602.FIFO_WM_TH2);
	icm20602_read_reg(st, reg_set_20602.FIFO_WM_TH1.address, &val);
	icm20602_read_reg(st, reg_set_20602.FIFO_WM_TH2.address, &val);

	return MPU_SUCCESS;
}

static int icm20602_read_ST_code(struct inv_icm20602_state *st)
{
	struct icm20602_user_config *config = NULL;
	int result = 0;

	config = st->config;
	result |= icm20602_read_reg(st, reg_set_20602.SELF_TEST_X_ACCEL.address,
		&(config->acc_self_test.X));
	result |= icm20602_read_reg(st, reg_set_20602.SELF_TEST_Y_ACCEL.address,
		&(config->acc_self_test.Y));
	result |= icm20602_read_reg(st, reg_set_20602.SELF_TEST_Z_ACCEL.address,
		&(config->acc_self_test.Z));

	result |= icm20602_read_reg(st, reg_set_20602.SELF_TEST_X_GYRO.address,
		&(config->gyro_self_test.X));
	result |= icm20602_read_reg(st, reg_set_20602.SELF_TEST_Y_GYRO.address,
		&(config->gyro_self_test.Y));
	result |= icm20602_read_reg(st, reg_set_20602.SELF_TEST_Z_GYRO.address,
		&(config->gyro_self_test.Z));

	return result;
}

static int icm20602_set_self_test(struct inv_icm20602_state *st)
{
	uint8_t raw_data[6] = {0, 0, 0, 0, 0, 0};
	uint8_t selfTest[6];
	float factory_trim[6];
	int result = 0;
	int ii;

	reg_set_20602.SMPLRT_DIV.reg_u.REG.SMPLRT_DIV = 0;
	result |= icm20602_write_reg_simple(st, reg_set_20602.SMPLRT_DIV);

	reg_set_20602.CONFIG.reg_u.REG.DLFP_CFG = INV_ICM20602_GYRO_LFP_92HZ;
	result |= icm20602_write_reg_simple(st, reg_set_20602.CONFIG);

	reg_set_20602.GYRO_CONFIG.reg_u.REG.FCHOICE_B = 0x0;
	reg_set_20602.GYRO_CONFIG.reg_u.REG.FS_SEL = ICM20602_GYRO_FSR_250DPS;
	result |= icm20602_write_reg_simple(st, reg_set_20602.GYRO_CONFIG);

	reg_set_20602.ACCEL_CONFIG2.reg_u.REG.A_DLPF_CFG = ICM20602_ACCLFP_99;
	reg_set_20602.ACCEL_CONFIG2.reg_u.REG.ACCEL_FCHOICE_B = 0X0;
	result |= icm20602_write_reg_simple(st, reg_set_20602.ACCEL_CONFIG2);

	reg_set_20602.ACCEL_CONFIG.reg_u.REG.ACCEL_FS_SEL = ICM20602_ACC_FSR_2G;
	result |= icm20602_write_reg_simple(st, reg_set_20602.ACCEL_CONFIG);

	icm20602_read_ST_code(st);

	return 0;
}

static int icm20602_do_test_acc(struct inv_icm20602_state *st,
	struct X_Y_Z *acc, struct X_Y_Z *acc_st)
{
	struct struct_icm20602_real_data *real_data =
		kmalloc(sizeof(struct inv_icm20602_state), GFP_ATOMIC);
	struct icm20602_user_config *config = st->config;
	int i, j;

	for (i = 0; i < SELFTEST_COUNT; i++) {
		icm20602_read_raw(st, real_data, ACCEL);
		acc->X += real_data->ACCEL_XOUT;
		acc->Y += real_data->ACCEL_YOUT;
		acc->Z += real_data->ACCEL_ZOUT;
		/* sample rate is 1kHz*/
		usleep_range(1000, 1001);
	}
	acc->X /= SELFTEST_COUNT;
	acc->X *= ST_PRECISION;

	acc->Y /= SELFTEST_COUNT;
	acc->Y *= ST_PRECISION;

	acc->Z /= SELFTEST_COUNT;
	acc->Z *= ST_PRECISION;

	reg_set_20602.ACCEL_CONFIG.reg_u.REG.XG_ST = 0x1;
	reg_set_20602.ACCEL_CONFIG.reg_u.REG.YG_ST = 0x1;
	reg_set_20602.ACCEL_CONFIG.reg_u.REG.ZG_ST = 0x1;
	icm20602_write_reg_simple(st, reg_set_20602.ACCEL_CONFIG);

	for (i = 0; i < SELFTEST_COUNT; i++) {
		icm20602_read_raw(st, real_data, ACCEL);
		acc_st->X += real_data->ACCEL_XOUT;
		acc_st->Y += real_data->ACCEL_YOUT;
		acc_st->Z += real_data->ACCEL_ZOUT;
		/* sample rate is 1kHz */
		usleep_range(1000, 1001);
	}
	acc_st->X /= SELFTEST_COUNT;
	acc_st->X *= ST_PRECISION;

	acc_st->Y /= SELFTEST_COUNT;
	acc_st->Y *= ST_PRECISION;

	acc_st->Z /= SELFTEST_COUNT;
	acc_st->Z *= ST_PRECISION;

	return MPU_SUCCESS;
}

static int icm20602_do_test_gyro(struct inv_icm20602_state *st,
	struct X_Y_Z *gyro, struct X_Y_Z *gyro_st)
{
	struct struct_icm20602_real_data *real_data =
		kmalloc(sizeof(struct inv_icm20602_state), GFP_ATOMIC);
	int i, j;

	for (i = 0; i < SELFTEST_COUNT; i++) {
		icm20602_read_raw(st, real_data, GYRO);
		gyro->X += real_data->GYRO_XOUT;
		gyro->Y += real_data->GYRO_YOUT;
		gyro->Z += real_data->GYRO_ZOUT;
		/* sample rate is 1kHz*/
		usleep_range(1000, 1001);
	}
	gyro->X /= SELFTEST_COUNT;
	gyro->X *= ST_PRECISION;

	gyro->Y /= SELFTEST_COUNT;
	gyro->Y *= ST_PRECISION;

	gyro->Z /= SELFTEST_COUNT;
	gyro->Z *= ST_PRECISION;

	reg_set_20602.GYRO_CONFIG.reg_u.REG.XG_ST = 0x1;
	reg_set_20602.GYRO_CONFIG.reg_u.REG.YG_ST = 0x1;
	reg_set_20602.GYRO_CONFIG.reg_u.REG.ZG_ST = 0x1;
	icm20602_write_reg_simple(st, reg_set_20602.GYRO_CONFIG);

	for (i = 0; i < SELFTEST_COUNT; i++) {
		icm20602_read_raw(st, real_data, ACCEL);
		gyro_st->X += real_data->GYRO_XOUT;
		gyro_st->Y += real_data->GYRO_YOUT;
		gyro_st->Z += real_data->GYRO_ZOUT;
		/* sample rate is 1kHz */
		usleep_range(1000, 1001);
	}
	gyro_st->X /= SELFTEST_COUNT;
	gyro_st->X *= ST_PRECISION;

	gyro_st->Y /= SELFTEST_COUNT;
	gyro_st->Y *= ST_PRECISION;

	gyro_st->Z /= SELFTEST_COUNT;
	gyro_st->Z *= ST_PRECISION;

	return MPU_SUCCESS;
}

static bool icm20602_check_acc_selftest(struct inv_icm20602_state *st,
	struct X_Y_Z *acc, struct X_Y_Z *acc_st)
{
	struct X_Y_Z acc_ST_code, st_otp, st_shift_cust;
	bool otp_value_zero = false, test_result = true;

	acc_ST_code.X = st->config->acc_self_test.X;
	acc_ST_code.Y = st->config->acc_self_test.Y;
	acc_ST_code.Z = st->config->acc_self_test.Z;

	st_otp.X = (st_otp.X != 0) ? mpu_st_tb[acc_ST_code.X - 1] : 0;
	st_otp.Y = (st_otp.Y != 0) ? mpu_st_tb[acc_ST_code.Y - 1] : 0;
	st_otp.Z = (st_otp.Z != 0) ? mpu_st_tb[acc_ST_code.Z - 1] : 0;

	if (st_otp.X & st_otp.Y & st_otp.Z == 0) {
		otp_value_zero = true;
	}

	st_shift_cust.X = acc_st->X - acc->X;
	st_shift_cust.Y = acc_st->X - acc->Y;
	st_shift_cust.Z = acc_st->X - acc->Z;
	if (!otp_value_zero) {
		if (
		st_shift_cust.X <
		(st_otp.X * ST_PRECISION * ACC_ST_SHIFT_MIN / 100) ||
		st_shift_cust.Y <
		(st_otp.Y * ST_PRECISION * ACC_ST_SHIFT_MIN / 100) ||
		st_shift_cust.Z <
		(st_otp.Z * ST_PRECISION * ACC_ST_SHIFT_MIN / 100) ||

		st_shift_cust.X >
		(st_otp.X * ST_PRECISION * ACC_ST_SHIFT_MAX / 100) ||
		st_shift_cust.Y >
		(st_otp.Y * ST_PRECISION * ACC_ST_SHIFT_MAX / 100) ||
		st_shift_cust.Z >
		(st_otp.Z * ST_PRECISION * ACC_ST_SHIFT_MAX / 100)
		) {
			test_result = false;
		}
	} else {
		if (
		abs(st_shift_cust.X) <
		(ACC_ST_AL_MIN * 16384 / 1000 * ST_PRECISION) ||
		abs(st_shift_cust.Y) <
		(ACC_ST_AL_MIN * 16384 / 1000 * ST_PRECISION) ||
		abs(st_shift_cust.Z) <
		(ACC_ST_AL_MIN * 16384 / 1000 * ST_PRECISION)  ||

		abs(st_shift_cust.X) >
		(ACC_ST_AL_MAX * 16384 / 1000 * ST_PRECISION) ||
		abs(st_shift_cust.Y) >
		(ACC_ST_AL_MAX * 16384 / 1000 * ST_PRECISION) ||
		abs(st_shift_cust.Z) >
		(ACC_ST_AL_MAX * 16384 / 1000 * ST_PRECISION)
		) {
			test_result = false;
		}
	}

	return test_result;
}

static int icm20602_check_gyro_selftest(struct inv_icm20602_state *st,
	struct X_Y_Z *gyro, struct X_Y_Z *gyro_st)
{
	struct X_Y_Z gyro_ST_code, st_otp, st_shift_cust;
	bool otp_value_zero = false, test_result = true;

	gyro_ST_code.X = st->config->gyro_self_test.X;
	gyro_ST_code.Y = st->config->gyro_self_test.Y;
	gyro_ST_code.Z = st->config->gyro_self_test.Z;

	st_otp.X = (st_otp.X != 0) ? mpu_st_tb[gyro_ST_code.X - 1] : 0;
	st_otp.Y = (st_otp.Y != 0) ? mpu_st_tb[gyro_ST_code.Y - 1] : 0;
	st_otp.Z = (st_otp.Z != 0) ? mpu_st_tb[gyro_ST_code.Z - 1] : 0;

	if (st_otp.X & st_otp.Y & st_otp.Z == 0) {
		otp_value_zero = true;
	}

	st_shift_cust.X = gyro_st->X - gyro->X;
	st_shift_cust.Y = gyro_st->X - gyro->Y;
	st_shift_cust.Z = gyro_st->X - gyro->Z;
	if (!otp_value_zero) {
		if (
		st_shift_cust.X <
		(st_otp.X * ST_PRECISION * GYRO_ST_SHIFT / 100) ||
		st_shift_cust.Y <
		(st_otp.Y * ST_PRECISION * GYRO_ST_SHIFT / 100) ||
		st_shift_cust.Z <
		(st_otp.Z * ST_PRECISION * GYRO_ST_SHIFT / 100)
		) {
			test_result = false;
		}
	} else {
		if (
		abs(st_shift_cust.X) <
		(GYRO_ST_AL * 32768 / 250 * ST_PRECISION) ||
		abs(st_shift_cust.Y) <
		(GYRO_ST_AL * 32768 / 250 * ST_PRECISION) ||
		abs(st_shift_cust.Z) <
		(GYRO_ST_AL * 32768 / 250 * ST_PRECISION)
		) {
			test_result = false;
		}
	}

	if (test_result == true) {
		/* Self Test Pass/Fail Criteria C */
		if (
		abs(st_shift_cust.X) >
		GYRO_OFFSET_MAX * 32768 / 250 * ST_PRECISION ||
		abs(st_shift_cust.Y) >
		GYRO_OFFSET_MAX * 32768 / 250 * ST_PRECISION ||
		abs(st_shift_cust.Z) >
		GYRO_OFFSET_MAX * 32768 / 250 * ST_PRECISION
		) {
			test_result = false;
		}
	}

	return test_result;
}

bool icm20602_self_test(struct inv_icm20602_state *st)
{
	struct X_Y_Z acc, acc_st;
	struct X_Y_Z gyro, gyro_st;
	bool test_result = true;

	icm20602_set_self_test(st);
	icm20602_do_test_acc(st, &acc, &acc_st);
	icm20602_do_test_gyro(st, &gyro, &gyro_st);
	test_result = icm20602_check_acc_selftest(st, &acc, &acc_st);
	test_result = icm20602_check_gyro_selftest(st, &gyro, &gyro_st);

	return test_result;
}

static int icm20602_config_fifo(struct inv_icm20602_state *st)
{
	struct icm20602_user_config *config = NULL;

	config = st->config;
	if (config->fifo_enabled != true)
		return MPU_SUCCESS;

	/*
	 * Set CONFIG.USER_SET_BIT = 0, No reason as datasheet said
	 */
	reg_set_20602.CONFIG.reg_u.REG.USER_SET_BIT = 0x0;
	/*
	 * Set CONFIG.FIFO_MODE = 1,
	 * i.e. when FIFO is full, additional writes will
	 * not be written to FIFO
	 */
	reg_set_20602.CONFIG.reg_u.REG.FIFO_MODE = 0x1;
	if (icm20602_write_reg_simple(st, reg_set_20602.CONFIG))
		return -MPU_FAIL;

	/* reset fifo */
	reg_set_20602.USER_CTRL.reg_u.REG.FIFO_RST = 0x1;
	if (icm20602_write_reg_simple(st, reg_set_20602.USER_CTRL)) {
		reg_set_20602.USER_CTRL.reg_u.REG.FIFO_RST = 0x0;
		return -MPU_FAIL;
	}
	reg_set_20602.USER_CTRL.reg_u.REG.FIFO_RST = 0x0;

	/* Enable FIFO on specified sensors */
	reg_set_20602.FIFO_EN.reg_u.REG.GYRO_FIFO_EN = 0x1;
	reg_set_20602.FIFO_EN.reg_u.REG.ACCEL_FIFO_EN = 0x1;
	if (icm20602_write_reg_simple(st, reg_set_20602.FIFO_EN))
		return -MPU_FAIL;

	if (icm20602_config_waterlevel(st))
		return -MPU_FAIL;

	if (icm20602_start_fifo(st))
		return -MPU_FAIL;

	return MPU_SUCCESS;
}

static int icm20602_initialize_gyro(struct inv_icm20602_state *st)
{
	struct icm20602_user_config *config = NULL;
	int result = MPU_SUCCESS;
	int sample_rate;
	uint8_t fchoice_b;

	if (st == NULL)
		return -MPU_FAIL;

	/*
	 * ICM20602 supports gyro sampling rate up to 32KHz
	 * when fchoice_b != 0x00
	 * In our driver, we supports up to 8KHz
	 * thus always set fchoice_b to 0x00;
	 */
	config = st->config;
	/*
	 * SAPLRT_DIV in ICM20602_REG_SMPLRT_DIV is only used for 1kHz internal
	 * sampling, i.e. fchoice_b in ICM20602_REG_GYRO_CONFIG is 00
	 * and 0 < dlpf_cfg in ICM20602_REG_CONFIG < 7
	 * SAMPLE_RATE=Internal_Sample_Rate / (1 + SMPLRT_DIV)
	 */
	if (config->gyro_accel_sample_rate <= ICM20602_SAMPLE_RATE_1000HZ)
		reg_set_20602.SMPLRT_DIV.reg_u.reg =
		ICM20602_INTERNAL_SAMPLE_RATE_HZ /
		config->gyro_accel_sample_rate - 1;

	result = icm20602_write_reg_simple(st, reg_set_20602.SMPLRT_DIV);

	/* Set gyro&temperature(combine) LPF */
	reg_set_20602.CONFIG.reg_u.REG.DLFP_CFG = config->gyro_lpf;
	result |= icm20602_write_reg_simple(st, reg_set_20602.CONFIG);

	/* Set gyro full scale range */
	reg_set_20602.GYRO_CONFIG.reg_u.REG.FCHOICE_B = 0x0;
	reg_set_20602.GYRO_CONFIG.reg_u.REG.FS_SEL = config->gyro_fsr;
	result |= icm20602_write_reg_simple(st, reg_set_20602.GYRO_CONFIG);

	/* Set Accel full scale range */
	reg_set_20602.ACCEL_CONFIG.reg_u.REG.ACCEL_FS_SEL = config->acc_fsr;
	result |= icm20602_write_reg_simple(st, reg_set_20602.ACCEL_CONFIG);

	/*
	 * Set accel LPF
	 * Support accel sample rate up to 1KHz
	 * thus set accel_fchoice_b to 0x00
	 * The actual accel sample rate is 1KHz/(1+SMPLRT_DIV)
	 */
	reg_set_20602.ACCEL_CONFIG2.reg_u.REG.ACCEL_FCHOICE_B = 0x0;
	reg_set_20602.ACCEL_CONFIG2.reg_u.REG.A_DLPF_CFG = config->acc_lpf;
	result |= icm20602_write_reg_simple(st,
				reg_set_20602.ACCEL_CONFIG2);

	if (result) {
		dev_dbgerr("icm20602 init gyro and accel failed\n");
		return -MPU_FAIL;
	}

	return result;
}

int icm20602_set_power_itg(struct inv_mpu9250_state *st, bool power_on)
{
	int result = MPU_SUCCESS;

	if (power_on) {
		reg_set_20602.PWR_MGMT_1.reg_u.reg = 0;
		result = icm20602_write_reg_simple(st,
					reg_set_20602.PWR_MGMT_1);
	} else {
		reg_set_20602.PWR_MGMT_1.reg_u.REG.SLEEP = 0x1;
		result = icm20602_write_reg_simple(st,
					reg_set_20602.PWR_MGMT_1);
	}
	if (result) {
		dev_dbgerr("set power failed power %d  err %d\n",
					power_on, result);
		return result;
	}

	if (power_on)
		msleep(30);

	return result;
}

int icm20602_init_device(struct inv_icm20602_state *st)
{
	int result = MPU_SUCCESS;
	struct icm20602_user_config *config = NULL;
	int package_count;

	config = st->config;
	if (st == NULL || st->config == NULL) {
		dev_dbgerr("icm20602 validate config failed\n");
		return -MPU_FAIL;
	}

	/* turn on gyro and accel */
	reg_set_20602.PWR_MGMT_2.reg_u.reg = 0x0;
	result |= icm20602_write_reg_simple(st, reg_set_20602.PWR_MGMT_2);
	msleep(30);

	/* disable INT */
	reg_set_20602.INT_ENABLE.reg_u.reg = 0x0;
	result |= icm20602_write_reg_simple(st, reg_set_20602.INT_ENABLE);

	/* disbale FIFO */
	reg_set_20602.FIFO_EN.reg_u.reg = 0x0;
	result |= icm20602_write_reg_simple(st, reg_set_20602.FIFO_EN);

	/* reset FIFO */
	reg_set_20602.USER_CTRL.reg_u.REG.FIFO_RST = 0x1;
	result |= icm20602_write_reg_simple(st, reg_set_20602.USER_CTRL);
	reg_set_20602.USER_CTRL.reg_u.REG.FIFO_RST = 0x0;
	msleep(30);

	/* init gyro and accel */
	if (icm20602_initialize_gyro(st)) {
		dev_dbgerr("icm20602 init device failed\n");
		return -MPU_FAIL;
	}

	/* if FIFO enable, config FIFO */
	if (config->fifo_enabled) {
		if (icm20602_config_fifo(st)) {
			dev_dbgerr("icm20602 init config fifo failed\n");
			return -MPU_FAIL;
		}
	} else {
		/* enable interrupt */
		reg_set_20602.INT_ENABLE.reg_u.REG.DATA_RDY_INT_EN = 0x0;
		if (icm20602_write_reg_simple(st,
				reg_set_20602.INT_ENABLE)) {
			dev_dbgerr("icm20602 set raw rdy failed\n");
			return -MPU_FAIL;
		}
	}

	/* buffer malloc */
	package_count = config->fifo_waterlevel / ICM20602_PACKAGE_SIZE;

	st->buf = kmalloc(config->fifo_waterlevel * 2, GFP_ATOMIC);
	memset(st->buf, 0, config->fifo_waterlevel * 2);

	st->data_push = kmalloc_array(package_count,
		sizeof(struct struct_icm20602_data), GFP_ATOMIC);
	memset(st->data_push,
		0, (sizeof(struct struct_icm20602_data)*package_count));

	return result;
}

int icm20602_rw_test(struct inv_icm20602_state *st)
{
	u8 val = 0;

	reg_set_20602.PWR_MGMT_2.reg_u.REG.STBY_ZG = 0x1;
	icm20602_write_reg_simple(st, reg_set_20602.PWR_MGMT_2);
	reg_set_20602.CONFIG.reg_u.REG.FIFO_MODE = 0x1;
	icm20602_write_reg_simple(st, reg_set_20602.CONFIG);

	icm20602_read_reg(st, reg_set_20602.PWR_MGMT_2.address, &val);
	icm20602_read_reg(st, reg_set_20602.CONFIG.address, &val);

	return 0;
}

int icm20602_detect(struct inv_icm20602_state *st)
{
	int result = MPU_SUCCESS;
	uint8_t retry = 0, val = 0;
	uint8_t usr_ctrl = 0;

	dev_dbginfo("icm20602_detect\n");
	/* reset to make sure previous state are not there */
	reg_set_20602.PWR_MGMT_1.reg_u.REG.DEVICE_RESET = 0x1;
	result = icm20602_write_reg_simple(st, reg_set_20602.PWR_MGMT_1);
	if (result) {
		dev_dbgerr("mpu write reg 0x%x value 0x%x failed\n",
		reg_set_20602.PWR_MGMT_1.reg_u.reg,
		reg_set_20602.PWR_MGMT_1.reg_u.REG.DEVICE_RESET);
		return result;
	}
	reg_set_20602.PWR_MGMT_1.reg_u.REG.DEVICE_RESET = 0x0;

	/* the power up delay */
	msleep(30);

	/* out of sleep */
	result = icm20602_set_power_itg(st, true);
	if (result)
		return result;
	/* get who am i register */
	while (retry < 10) {
		/* get version (expecting 0x12 for the icm20602) */
		icm20602_read_reg(st, reg_set_20602.WHO_AM_I.address, &val);
		if (val == ICM20602_WHO_AM_I)
			break;
		retry++;
	}

	if (val != ICM20602_WHO_AM_I) {
		dev_dbgerr("detect mpu failed,whoami reg 0x%x\n", val);
		result = -MPU_FAIL;
	} else {
		dev_dbginfo("detect mpu ok,whoami reg 0x%x\n", val);
	}
	icm20602_rw_test(st);

	return result;
}