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

Commit 52f31593 authored by Linux Build Service Account's avatar Linux Build Service Account Committed by Gerrit - the friendly Code Review server
Browse files

Merge "icm20602 driver: fix waterlevel bug and add self test feature"

parents f199cc04 672dfcfa

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;