Donate to e Foundation | Murena handsets with /e/OS | Own a part of Murena! Learn more

Commit 672dfcfa authored by zhaochen's avatar zhaochen
Browse files

icm20602 driver: fix waterlevel bug and add self test feature 



Icm20602 waterlevel calculate has some errors to fix and the IMU
needs self test when we use it on the first time.

Change-Id: I7eb848ac73e7500002dabc292d108576dfab3ad3
Signed-off-by: default avatarzhaochen <zhaochen@codeaurora.org>
parent 7c75be09

drivers/iio/imu/inv_icm20602/inv_icm20602_bsp.c

+99 −29
Original line number Diff line number Diff line
@@ -43,7 +43,7 @@ 


static struct inv_icm20602_reg_map reg_set_20602;



static int icm20602_init_reg_map(void)

int icm20602_init_reg_map(void)

{

	struct struct_XG_OFFS_TC_H *reg_map = &(reg_set_20602.XG_OFFS_TC_H);
@@ -92,26 +92,53 @@ static int icm20602_init_reg_map(void) 
	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, buf, size);

		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 = i2c_smbus_read_i2c_block_data(st->client,

						reg, size, buf);

		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;
@@ -122,6 +149,7 @@ static int icm20602_write_reg(struct inv_icm20602_state *st, 
{

	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);
@@ -133,8 +161,19 @@ static int icm20602_write_reg(struct inv_icm20602_state *st, 
			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;
@@ -144,28 +183,44 @@ static int icm20602_read_reg(struct inv_icm20602_state *st, 
				uint8_t reg, uint8_t *val)

{

	int result = MPU_SUCCESS;

	char txbuf[2] = {0x0, 0x0};

	char rxbuf[2] = {0x0, 0x0};

	char txbuf[1] = {0x0};

	char rxbuf[1] = {0x0};

	struct i2c_msg msg[2];



	*val = rxbuf[0];

	if (st->interface == ICM20602_SPI) {

		txbuf[0] = ICM20602_READ_REG(reg);

		result = spi_write_then_read(st->spi,

						&txbuf[0], 1, &rxbuf[0], 1);

						&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[0]);

						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;

}
@@ -267,7 +322,7 @@ int icm20602_start_fifo(struct inv_icm20602_state *st) 
		}



		/* enable interrupt, need to test */

		reg_set_20602.INT_ENABLE.reg_u.REG.FIFO_OFLOW_EN = 0x0;

		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)) {
@@ -303,30 +358,33 @@ static int icm20602_stop_fifo(struct inv_icm20602_state *st) 
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)/14) {

		(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.FIFO_WM_TH =

				config->fifo_waterlevel >> 8;

	reg_set_20602.FIFO_WM_TH2.reg_u.REG.FIFO_WM_TH =

				config->fifo_waterlevel & 0xff;

	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);



	if (icm20602_write_reg_simple(st, reg_set_20602.FIFO_WM_TH1) &&

		icm20602_write_reg_simple(st, reg_set_20602.FIFO_WM_TH2))

		return -MPU_FAIL;

	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;

}
@@ -398,7 +456,7 @@ static int icm20602_do_test_acc(struct inv_icm20602_state *st, 
		acc->Y += real_data->ACCEL_YOUT;

		acc->Z += real_data->ACCEL_ZOUT;

		/* sample rate is 1kHz*/

		usleep(1000);

		usleep_range(1000, 1001);

	}

	acc->X /= SELFTEST_COUNT;

	acc->X *= ST_PRECISION;
@@ -420,7 +478,7 @@ static int icm20602_do_test_acc(struct inv_icm20602_state *st, 
		acc_st->Y += real_data->ACCEL_YOUT;

		acc_st->Z += real_data->ACCEL_ZOUT;

		/* sample rate is 1kHz */

		usleep(1000);

		usleep_range(1000, 1001);

	}

	acc_st->X /= SELFTEST_COUNT;

	acc_st->X *= ST_PRECISION;
@@ -447,7 +505,7 @@ static int icm20602_do_test_gyro(struct inv_icm20602_state *st, 
		gyro->Y += real_data->GYRO_YOUT;

		gyro->Z += real_data->GYRO_ZOUT;

		/* sample rate is 1kHz*/

		usleep(1000);

		usleep_range(1000, 1001);

	}

	gyro->X /= SELFTEST_COUNT;

	gyro->X *= ST_PRECISION;
@@ -469,7 +527,7 @@ static int icm20602_do_test_gyro(struct inv_icm20602_state *st, 
		gyro_st->Y += real_data->GYRO_YOUT;

		gyro_st->Z += real_data->GYRO_ZOUT;

		/* sample rate is 1kHz */

		usleep(1000);

		usleep_range(1000, 1001);

	}

	gyro_st->X /= SELFTEST_COUNT;

	gyro_st->X *= ST_PRECISION;
@@ -499,7 +557,6 @@ static bool icm20602_check_acc_selftest(struct inv_icm20602_state *st, 


	if (st_otp.X & st_otp.Y & st_otp.Z == 0) {

		otp_value_zero = true;

		dev_dbginfo("self test code is zero\n");

	}



	st_shift_cust.X = acc_st->X - acc->X;
@@ -562,7 +619,6 @@ static int icm20602_check_gyro_selftest(struct inv_icm20602_state *st, 


	if (st_otp.X & st_otp.Y & st_otp.Z == 0) {

		otp_value_zero = true;

		dev_dbginfo("self test code is zero\n");

	}



	st_shift_cust.X = gyro_st->X - gyro->X;
@@ -767,9 +823,6 @@ int icm20602_init_device(struct inv_icm20602_state *st) 
		return -MPU_FAIL;

	}



	/* clear the cached reg cfg info */

	icm20602_init_reg_map();



	/* 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);
@@ -825,12 +878,28 @@ int icm20602_init_device(struct inv_icm20602_state *st) 
	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);
@@ -840,6 +909,7 @@ int icm20602_detect(struct inv_icm20602_state *st) 
		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);
@@ -848,11 +918,10 @@ int icm20602_detect(struct inv_icm20602_state *st) 
	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.reg_u.reg, &val);

		icm20602_read_reg(st, reg_set_20602.WHO_AM_I.address, &val);

		if (val == ICM20602_WHO_AM_I)

			break;

		retry++;
@@ -864,6 +933,7 @@ int icm20602_detect(struct inv_icm20602_state *st) 
	} else {

		dev_dbginfo("detect mpu ok,whoami reg 0x%x\n", val);

	}

	icm20602_rw_test(st);



	return result;

}

drivers/iio/imu/inv_icm20602/inv_icm20602_core.c

+46 −11
Original line number Diff line number Diff line
@@ -34,12 +34,31 @@ int icm20602_debug_enable = 1; 
static ssize_t inv_icm20602_init_show(struct device *dev,

	struct device_attribute *attr, char *buf)

{

	return count;

	return MPU_SUCCESS;

}



static int inv_icm20602_def_config(struct inv_icm20602_state *st)

{

	struct icm20602_user_config *config = st->config;



	config->user_fps_in_ms = 10;

	config->gyro_lpf = INV_ICM20602_GYRO_LFP_92HZ;

	config->gyro_fsr = ICM20602_GYRO_FSR_1000DPS;

	config->acc_lpf = ICM20602_ACCLFP_99;

	config->acc_fsr = ICM20602_ACC_FSR_4G;

	config->gyro_accel_sample_rate = ICM20602_SAMPLE_RATE_100HZ;

	config->fifo_enabled = true;

}



static int inv_icm20602_load_config(struct inv_icm20602_state *st)

{

	struct icm20602_chip_config *config = st->config;

	struct icm20602_user_config *config = st->config;



	if (config->user_fps_in_ms == 0)

		inv_icm20602_def_config(st);

	config->fifo_enabled = true;



	return MPU_SUCCESS;

}



static ssize_t inv_icm20602_init_store(struct device *dev,
@@ -53,6 +72,7 @@ static ssize_t inv_icm20602_init_store(struct device *dev, 
	inv_icm20602_load_config(st);

	result |= icm20602_detect(st);

	result |= icm20602_init_device(st);

	icm20602_start_fifo(st);

	if (result)

		dev_dbgerr("inv_select_config_store failed\n");
@@ -395,14 +415,31 @@ static int of_populate_icm20602_dt(struct inv_icm20602_state *st) 
	/* use client device irq */

	st->gpio = of_get_named_gpio(st->client->dev.of_node,

			"invn,icm20602-irq", 0);

	result = gpio_is_valid(st->gpio);

	if (!result) {

		dev_dbgerr("gpio_is_valid %d failed\n", st->gpio);

		return -MPU_FAIL;

	}



	result = gpio_request(st->gpio, "icm20602-irq");

	if (result) {

		dev_dbgerr("gpio request %d failed\n", st->gpio);

		dev_dbgerr("gpio_request failed\n");

		return -MPU_FAIL;

	}



	result = gpio_direction_input(st->gpio);

	if (result) {

		dev_dbgerr("gpio_direction_input failed\n");

		return -MPU_FAIL;

	}



	st->client->irq = gpio_to_irq(st->gpio);

	if (st->client->irq < 0) {

		dev_dbgerr("gpio_to_irq failed\n");

		return -MPU_FAIL;

	}



	return result;

	return MPU_SUCCESS;

}



/*
@@ -419,7 +456,6 @@ static int inv_icm20602_probe(struct i2c_client *client, 
{

	struct inv_icm20602_state *st;

	struct iio_dev *indio_dev;

	struct inv_icm20602_platform_data *pdata;

	int result = MPU_SUCCESS;



	indio_dev = devm_iio_device_alloc(&client->dev, sizeof(*st));
@@ -430,21 +466,20 @@ static int inv_icm20602_probe(struct i2c_client *client, 
	}

	st = iio_priv(indio_dev);

	st->client = client;

	st->fifo_cnt_threshold = 140;

	st->interface = ICM20602_I2C;



	dev_dbginfo("i2c address is %x\n", client->addr);

	result = of_populate_icm20602_dt(st);

	if (result)

		dev_dbgerr("populate dt failed\n");



	st->config = kmalloc(sizeof(struct icm20602_user_config), GFP_ATOMIC);



	pdata =

	(struct inv_icm20602_platform_data *)dev_get_platdata(&client->dev);

	if (pdata)

		st->plat_data = *pdata;

	memset(st->config, 0, sizeof(struct icm20602_user_config));

	icm20602_init_reg_map();



	i2c_set_clientdata(&client->dev, indio_dev);



	dev_set_drvdata(&client->dev, indio_dev);

	indio_dev->dev.parent = &client->dev;

	indio_dev->name = ICM20602_DEV_NAME;

	indio_dev->channels = inv_icm20602_channels;

drivers/iio/imu/inv_icm20602/inv_icm20602_iio.h

+2 −4
Original line number Diff line number Diff line
@@ -219,7 +219,6 @@ struct inv_icm20602_state { 
	spinlock_t time_stamp_lock;

	struct spi_device *spi;

	struct i2c_client *client;

	struct inv_icm20602_platform_data plat_data;

	u8 fifo_packet_size;

	int fifo_cnt_threshold;

	char *buf;
@@ -283,11 +282,10 @@ int inv_icm20602_validate_trigger(struct iio_dev *indio_dev, 
int icm20602_read_raw(struct inv_icm20602_state *st,

		struct struct_icm20602_real_data *real_data, u8 type);





int icm20602_init_reg_map(void);

int icm20602_init_device(struct inv_icm20602_state *st);

int icm20602_detect(struct inv_icm20602_state *st);

int icm20602_read_fifo(struct inv_icm20602_state *st,

	void *buf, const int size);





int icm20602_start_fifo(struct inv_icm20602_state *st);

#endif

drivers/iio/imu/inv_icm20602/inv_icm20602_ring.c

+6 −5
Original line number Diff line number Diff line
@@ -24,6 +24,7 @@ 
#include <linux/poll.h>

#include <linux/spi/spi.h>

#include "inv_icm20602_iio.h"

#include <linux/i2c.h>



#define combine_8_to_16(upper, lower)  ((_upper << 8) | _lower)
@@ -57,6 +58,7 @@ irqreturn_t inv_icm20602_irq_handler(int irq, void *p) 
	s64 timestamp;



	timestamp = iio_get_time_ns();



	kfifo_in_spinlocked(&st->timestamps, &timestamp, 1,

				&st->time_stamp_lock);
@@ -67,7 +69,7 @@ static int inv_icm20602_read_data(struct iio_dev *indio_dev) 
{

	int result = MPU_SUCCESS;

	struct inv_icm20602_state *st = iio_priv(indio_dev);

	struct icm20602_user_config *config = NULL;

	struct icm20602_user_config *config = st->config;

	int package_count;

	char *buf = st->buf;

	struct struct_icm20602_data *data_push = st->data_push;
@@ -77,13 +79,13 @@ static int inv_icm20602_read_data(struct iio_dev *indio_dev) 
	if (!st)

		return -MPU_FAIL;

	package_count = config->fifo_waterlevel / ICM20602_PACKAGE_SIZE;

	config = st->config;

	mutex_lock(&indio_dev->mlock);

	if (config->fifo_enabled) {

		result = icm20602_read_fifo(st,

				st->buf, config->fifo_waterlevel);

		if (result) {

			dev_dbgerr("icm20602 read fifo failed\n");

		if (result != config->fifo_waterlevel) {

			dev_dbgerr("icm20602 read fifo failed, result = %d\n",

				result);

			goto flush_fifo;

		}
@@ -100,7 +102,6 @@ static int inv_icm20602_read_data(struct iio_dev *indio_dev) 
			st->buf += ICM20602_PACKAGE_SIZE;

		}

	}



end_session:

	mutex_unlock(&indio_dev->mlock);

	iio_trigger_notify_done(indio_dev->trig);

drivers/iio/imu/inv_icm20602/inv_icm20602_trigger.c

+9 −5
Original line number Diff line number Diff line
@@ -15,6 +15,7 @@ 
#include <linux/sched/rt.h>

#include <linux/delay.h>

#include "inv_icm20602_iio.h"

#include <linux/i2c.h>



static const struct iio_trigger_ops inv_icm20602_trigger_ops = {

	.owner = THIS_MODULE,
@@ -46,17 +47,20 @@ int inv_icm20602_probe_trigger(struct iio_dev *indio_dev) 


	ret = request_irq(st->client->irq, &iio_trigger_generic_data_rdy_poll,

				IRQF_TRIGGER_RISING,

				"inv_mpu",

				"inv_icm20602",

				st->trig);

	if (ret)

	if (ret) {

		dev_dbgerr("request_irq\n");

		goto error_free_trig;



	st->trig->dev.parent = &st->spi->dev;

	}

	st->trig->dev.parent = &st->client->dev;

	st->trig->ops = &inv_icm20602_trigger_ops;

	iio_trigger_set_drvdata(st->trig, indio_dev);

	ret = iio_trigger_register(st->trig);

	if (ret)

	if (ret) {

		dev_dbgerr("iio_trigger_register\n");

		goto error_free_irq;

	}

	indio_dev->trig = st->trig;



	return 0;