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

Commit 201bf295 authored by Ji Xu's avatar Ji Xu
Browse files

icm20602 driver: fix read fifo and timestamp 



Fixed some errors on fifo read interface, and the timestamp also
need to be push to userspace

Change-Id: I765cb6570b62d7fc4cf3af805aa0ca0e024ba6fa
Signed-off-by: default avatarxuji <xuji@codeaurora.org>
parent 8f3515e7

drivers/iio/imu/inv_icm20602/inv_icm20602_bsp.c

+63 −25
Original line number Diff line number Diff line
@@ -95,12 +95,12 @@ int icm20602_init_reg_map(void) 
#define W_FLG	0

#define R_FLG	1

int icm20602_bulk_read(struct inv_icm20602_state *st,

			int reg, char *buf, int size)

			int reg, u8 *buf, int size)

{

	int result = MPU_SUCCESS;

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

	int tmp_size = size;

	int tmp_buf = buf;

	u8 *tmp_buf = buf;

	struct i2c_msg msg[2];



	if (!st || !buf)
@@ -117,12 +117,9 @@ int icm20602_bulk_read(struct inv_icm20602_state *st, 
		}

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

		reg, size, tmp_buf);

#else

		tx_buf[0] = reg;

		msg[0].addr = st->client->addr;
@@ -132,15 +129,11 @@ int icm20602_bulk_read(struct inv_icm20602_state *st, 


		msg[1].addr = st->client->addr;

		msg[1].flags = I2C_M_RD;

			msg[1].len = (tmp_size < 32)?tmp_size:32;

		msg[1].len = size;

		msg[1].buf = tmp_buf;

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

			tmp_size -= 32;

			tmp_buf += tmp_size;

#endif

	}

	}



	return result;

}
@@ -355,6 +348,33 @@ static int icm20602_stop_fifo(struct inv_icm20602_state *st) 
	return MPU_SUCCESS;

}



int icm20602_int_status(struct inv_icm20602_state *st,

	u8 *int_status)

{

	return icm20602_read_reg(st,

		reg_set_20602.INT_STATUS.address, int_status);

}



int icm20602_int_wm_status(struct inv_icm20602_state *st,

	u8 *int_status)

{

	return icm20602_read_reg(st,

		reg_set_20602.FIFO_WM_INT_STATUS.address, int_status);

}



int icm20602_fifo_count(struct inv_icm20602_state *st,

	u16 *fifo_count)

{

	u8 count_h, count_l;



	*fifo_count = 0;

	icm20602_read_reg(st, reg_set_20602.FIFO_COUNTH.address, &count_h);

	icm20602_read_reg(st, reg_set_20602.FIFO_COUNTL.address, &count_l);

	*fifo_count |= (count_h << 8);

	*fifo_count |= count_l;

	return MPU_SUCCESS;

}



static int icm20602_config_waterlevel(struct inv_icm20602_state *st)

{

	struct icm20602_user_config *config = NULL;
@@ -809,6 +829,7 @@ int icm20602_init_device(struct inv_icm20602_state *st) 
	int result = MPU_SUCCESS;

	struct icm20602_user_config *config = NULL;

	int package_count;

	int i;



	config = st->config;

	if (st == NULL || st->config == NULL) {
@@ -860,7 +881,7 @@ int icm20602_init_device(struct inv_icm20602_state *st) 
	/* buffer malloc */

	package_count = config->fifo_waterlevel / ICM20602_PACKAGE_SIZE;



	st->buf = kzalloc(sizeof(config->fifo_waterlevel * 2), GFP_ATOMIC);

	st->buf = kzalloc(config->fifo_waterlevel * 2, GFP_ATOMIC);

	if (!st->buf)

		return -ENOMEM;
@@ -869,9 +890,26 @@ int icm20602_init_device(struct inv_icm20602_state *st) 
	if (!st->data_push)

		return -ENOMEM;



	for (i = 0; i < package_count; i++) {

		st->data_push[i].raw_data =

			kzalloc(ICM20602_PACKAGE_SIZE, GFP_ATOMIC);

	}



	return result;

}



int icm20602_reset_fifo(struct inv_icm20602_state *st)

{

	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;

	return MPU_SUCCESS;

}





void icm20602_rw_test(struct inv_icm20602_state *st)

{

	uint8_t val = 0;

drivers/iio/imu/inv_icm20602/inv_icm20602_core.c

+2 −2
Original line number Diff line number Diff line
@@ -41,12 +41,12 @@ static void inv_icm20602_def_config(struct inv_icm20602_state *st) 
{

	struct icm20602_user_config *config = st->config;



	config->user_fps_in_ms = 10;

	config->user_fps_in_ms = 20;

	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->gyro_accel_sample_rate = ICM20602_SAMPLE_RATE_200HZ;

	config->fifo_enabled = true;



}

drivers/iio/imu/inv_icm20602/inv_icm20602_iio.h

+6 −4
Original line number Diff line number Diff line
@@ -180,7 +180,7 @@ struct icm20602_user_config { 
	uint32_t user_fps_in_ms;



	bool fifo_enabled;

	uint32_t fifo_waterlevel;

	uint16_t fifo_waterlevel;

	struct X_Y_Z wake_on_motion;

};
@@ -257,19 +257,21 @@ struct struct_icm20602_real_data { 


struct struct_icm20602_data {

	s64 timestamps;

	struct struct_icm20602_raw_data raw_data;

	struct struct_icm20602_real_data real_data;

	u8 *raw_data;

};



extern struct iio_trigger *inv_trig;

irqreturn_t inv_icm20602_irq_handler(int irq, void *p);

irqreturn_t inv_icm20602_read_fifo_fn(int irq, void *p);

int inv_icm20602_reset_fifo(struct iio_dev *indio_dev);



int inv_icm20602_probe_trigger(struct iio_dev *indio_dev);

void inv_icm20602_remove_trigger(struct inv_icm20602_state *st);

int inv_icm20602_validate_trigger(struct iio_dev *indio_dev,

				struct iio_trigger *trig);

int icm20602_int_status(struct inv_icm20602_state *st, u8 *int_status);

int icm20602_int_wm_status(struct inv_icm20602_state *st, u8 *int_status);

int icm20602_reset_fifo(struct inv_icm20602_state *st);

int icm20602_fifo_count(struct inv_icm20602_state *st, u16 *fifo_count);



int icm20602_read_raw(struct inv_icm20602_state *st,

		struct struct_icm20602_real_data *real_data, uint32_t type);

drivers/iio/imu/inv_icm20602/inv_icm20602_ring.c

+28 −31
Original line number Diff line number Diff line
@@ -40,15 +40,6 @@ static void inv_clear_kfifo(struct inv_icm20602_state *st) 
	spin_unlock_irqrestore(&st->time_stamp_lock, flags);

}



int inv_icm20602_reset_fifo(struct iio_dev *indio_dev)

{

	int result;

	//u8 d;

	//struct inv_icm20602_state  *st = iio_priv(indio_dev);



	return result;

}



/*

 * inv_icm20602_irq_handler() - Cache a timestamp at each data ready interrupt.

 */
@@ -67,44 +58,50 @@ irqreturn_t inv_icm20602_irq_handler(int irq, void *p) 
	return IRQ_WAKE_THREAD;

}



#define BIT_FIFO_OFLOW_INT			0x10

#define BIT_FIFO_WM_INT				0x40

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 = st->config;

	int package_count;

	//char *buf = st->buf;

	//struct struct_icm20602_data *data_push = st->data_push;

	char *buf = st->buf;

	struct struct_icm20602_data *data_push = st->data_push;

	s64 timestamp;

	u8 int_status, int_wm_status;

	u16 fifo_count;

	int i;



	if (!st)

		return -MPU_FAIL;

	package_count = config->fifo_waterlevel / ICM20602_PACKAGE_SIZE;

	mutex_lock(&indio_dev->mlock);

	if (config->fifo_enabled) {

		result = icm20602_read_fifo(st,

				st->buf, config->fifo_waterlevel);

		if (result != config->fifo_waterlevel) {

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

				result);

			goto flush_fifo;

	icm20602_int_status(st, &int_status);

	if (int_status & BIT_FIFO_OFLOW_INT) {

		icm20602_fifo_count(st, &fifo_count);

		pr_debug("fifo_count = %d\n", fifo_count);

		icm20602_reset_fifo(st);

		goto end_session;

	}



		for (i = 0; i < package_count; i++) {

			memcpy((char *)(&st->data_push[i].raw_data),

				st->buf, ICM20602_PACKAGE_SIZE);

	if (config->fifo_enabled) {

		result = kfifo_out(&st->timestamps,

				&timestamp, 1);

		/* when there is no timestamp, put it as 0 */

		if (result == 0)

		timestamp = 0;

			st->data_push[i].timestamps = timestamp;

			iio_push_to_buffers(indio_dev, st->data_push+i);

			st->buf += ICM20602_PACKAGE_SIZE;

		for (i = 0; i < package_count; i++) {

			result = icm20602_read_fifo(st,

			buf, ICM20602_PACKAGE_SIZE);

			memcpy(st->data_push[i].raw_data,

			buf, ICM20602_PACKAGE_SIZE);

			iio_push_to_buffers_with_timestamp(indio_dev,

			st->data_push[i].raw_data, timestamp);

			buf += ICM20602_PACKAGE_SIZE;

		}

		memset(st->buf, 0, config->fifo_waterlevel);

	}

//end_session:

end_session:

	mutex_unlock(&indio_dev->mlock);

	iio_trigger_notify_done(indio_dev->trig);

	return MPU_SUCCESS;
@@ -112,7 +109,7 @@ static int inv_icm20602_read_data(struct iio_dev *indio_dev) 
flush_fifo:

	/* Flush HW and SW FIFOs. */

	inv_clear_kfifo(st);

	inv_icm20602_reset_fifo(indio_dev);

	icm20602_reset_fifo(st);

	mutex_unlock(&indio_dev->mlock);

	iio_trigger_notify_done(indio_dev->trig);

	return MPU_SUCCESS;