diff --git a/drivers/iio/imu/inv_icm20602/inv_icm20602_bsp.c b/drivers/iio/imu/inv_icm20602/inv_icm20602_bsp.c
index bfff285457c5..9968e44d91d9 100644
--- a/drivers/iio/imu/inv_icm20602/inv_icm20602_bsp.c
+++ b/drivers/iio/imu/inv_icm20602/inv_icm20602_bsp.c
@@ -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)
@@ -109,38 +109,31 @@ int icm20602_bulk_read(struct inv_icm20602_state *st,
 	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);
+		1, tmp_buf, size);
 		if (result) {
 			pr_err("mpu read reg %u failed, rc %d\n",
-				reg, result);
+			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;
+		result += i2c_smbus_read_i2c_block_data(st->client,
+		reg, size, tmp_buf);
 #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;
+		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;
+		msg[1].addr = st->client->addr;
+		msg[1].flags = I2C_M_RD;
+		msg[1].len = size;
+		msg[1].buf = tmp_buf;
+		i2c_transfer(st->client->adapter, msg, ARRAY_SIZE(msg));
 #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;
diff --git a/drivers/iio/imu/inv_icm20602/inv_icm20602_core.c b/drivers/iio/imu/inv_icm20602/inv_icm20602_core.c
index 0f7fc9200424..15df4479a5e1 100644
--- a/drivers/iio/imu/inv_icm20602/inv_icm20602_core.c
+++ b/drivers/iio/imu/inv_icm20602/inv_icm20602_core.c
@@ -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;
 
 }
diff --git a/drivers/iio/imu/inv_icm20602/inv_icm20602_iio.h b/drivers/iio/imu/inv_icm20602/inv_icm20602_iio.h
index 943fc1e6ee28..9ea5ae5ce96c 100644
--- a/drivers/iio/imu/inv_icm20602/inv_icm20602_iio.h
+++ b/drivers/iio/imu/inv_icm20602/inv_icm20602_iio.h
@@ -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);
diff --git a/drivers/iio/imu/inv_icm20602/inv_icm20602_ring.c b/drivers/iio/imu/inv_icm20602/inv_icm20602_ring.c
index b0f93be33340..de02656478ff 100644
--- a/drivers/iio/imu/inv_icm20602/inv_icm20602_ring.c
+++ b/drivers/iio/imu/inv_icm20602/inv_icm20602_ring.c
@@ -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;
-		}
-
-		for (i = 0; i < package_count; i++) {
-			memcpy((char *)(&st->data_push[i].raw_data),
-				st->buf, ICM20602_PACKAGE_SIZE);
-			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;
-		}
+	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;
 	}
-//end_session:
+	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;
+		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:
 	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;