From: Zorro Liu <lyx@rock-chips.com>
Date: Mon, 6 Jun 2016 07:42:49 +0000 (+0800)
Subject: staging, iio, mpu: repack mpu driver's communicate interface
X-Git-Tag: firefly_0821_release~2488
X-Git-Url: http://demsky.eecs.uci.edu/git/?a=commitdiff_plain;h=2185d7c78adf77c8fdf5627c3ed2702f7fb82f71;p=firefly-linux-kernel-4.4.55.git

staging, iio, mpu: repack mpu driver's communicate interface

Change-Id: Ideac72c0227d10305d3efde7d038bb25a20197bd
Signed-off-by: Zorro Liu <lyx@rock-chips.com>
---

diff --git a/drivers/staging/iio/imu/inv_mpu/Makefile b/drivers/staging/iio/imu/inv_mpu/Makefile
index 393ecad47b84..2524eb60bdb9 100644
--- a/drivers/staging/iio/imu/inv_mpu/Makefile
+++ b/drivers/staging/iio/imu/inv_mpu/Makefile
@@ -4,7 +4,8 @@
 
 obj-$(CONFIG_INV_MPU_IIO) += inv-mpu-iio.o
 
-inv-mpu-iio-objs := inv_mpu_core.o
+inv-mpu-iio-objs := inv_mpu_i2c.o
+inv-mpu-iio-objs += inv_mpu_core.o
 inv-mpu-iio-objs += inv_mpu_ring.o
 inv-mpu-iio-objs += inv_mpu_trigger.o
 inv-mpu-iio-objs += inv_mpu_misc.o
@@ -25,6 +26,7 @@ CFLAGS_inv_mpu_misc.o		+= -Idrivers/iio -DINV_KERNEL_3_10
 CFLAGS_inv_mpu3050_iio.o	+= -Idrivers/iio -DINV_KERNEL_3_10
 CFLAGS_dmpDefaultMPU6050.o	+= -Idrivers/iio -DINV_KERNEL_3_10
 else
+CFLAGS_inv_mpu_i2c.o       += -Idrivers/staging/iio
 CFLAGS_inv_mpu_core.o      += -Idrivers/staging/iio
 CFLAGS_inv_mpu_ring.o      += -Idrivers/staging/iio
 CFLAGS_inv_mpu_trigger.o   += -Idrivers/staging/iio
diff --git a/drivers/staging/iio/imu/inv_mpu/inv_mpu3050_iio.c b/drivers/staging/iio/imu/inv_mpu/inv_mpu3050_iio.c
index 370c77f55540..65eb96675e5c 100644
--- a/drivers/staging/iio/imu/inv_mpu/inv_mpu3050_iio.c
+++ b/drivers/staging/iio/imu/inv_mpu/inv_mpu3050_iio.c
@@ -55,7 +55,7 @@ int set_3050_bypass(struct inv_mpu_iio_s *st, bool enable)
 	u8 b;
 
 	reg = &st->reg;
-	result = inv_i2c_read(st, reg->user_ctrl, 1, &b);
+	result = inv_plat_read(st, reg->user_ctrl, 1, &b);
 	if (result)
 		return result;
 	if (((b & BIT_3050_AUX_IF_EN) == 0) && enable)
@@ -65,7 +65,7 @@ int set_3050_bypass(struct inv_mpu_iio_s *st, bool enable)
 	b &= ~BIT_3050_AUX_IF_EN;
 	if (!enable) {
 		b |= BIT_3050_AUX_IF_EN;
-		result = inv_i2c_single_write(st, reg->user_ctrl, b);
+		result = inv_plat_single_write(st, reg->user_ctrl, b);
 		return result;
 	} else {
 		/* Coming out of I2C is tricky due to several erratta.  Do not
@@ -78,7 +78,7 @@ int set_3050_bypass(struct inv_mpu_iio_s *st, bool enable)
 		*
 		* 0x00 is broadcast.  0x7F is unlikely to be used by any aux.
 		*/
-		result = inv_i2c_single_write(st, REG_3050_SLAVE_ADDR,
+		result = inv_plat_single_write(st, REG_3050_SLAVE_ADDR,
 						MPU3050_BOGUS_ADDR);
 		if (result)
 			return result;
@@ -87,7 +87,7 @@ int set_3050_bypass(struct inv_mpu_iio_s *st, bool enable)
 		*    bypass mode:
 		*/
 		usleep_range(MPU3050_NACK_MIN_TIME, MPU3050_NACK_MAX_TIME);
-		result = inv_i2c_single_write(st, reg->user_ctrl, b);
+		result = inv_plat_single_write(st, reg->user_ctrl, b);
 		if (result)
 			return result;
 		/*
@@ -96,12 +96,12 @@ int set_3050_bypass(struct inv_mpu_iio_s *st, bool enable)
 		*/
 		msleep(MPU3050_ONE_MPU_TIME);
 
-		result = inv_i2c_single_write(st, REG_3050_SLAVE_ADDR,
+		result = inv_plat_single_write(st, REG_3050_SLAVE_ADDR,
 			st->plat_data.secondary_i2c_addr);
 		if (result)
 			return result;
 
-		result = inv_i2c_single_write(st, reg->user_ctrl,
+		result = inv_plat_single_write(st, reg->user_ctrl,
 				(b | BIT_3050_AUX_IF_RST));
 		if (result)
 			return result;
@@ -135,19 +135,19 @@ int inv_switch_3050_gyro_engine(struct inv_mpu_iio_s *st, bool en)
 	if (en) {
 		data = INV_CLK_PLL;
 		p = (BITS_3050_POWER1 | data);
-		result = inv_i2c_single_write(st, reg->pwr_mgmt_1, p);
+		result = inv_plat_single_write(st, reg->pwr_mgmt_1, p);
 		if (result)
 			return result;
 		p = (BITS_3050_POWER2 | data);
-		result = inv_i2c_single_write(st, reg->pwr_mgmt_1, p);
+		result = inv_plat_single_write(st, reg->pwr_mgmt_1, p);
 		if (result)
 			return result;
 		p = data;
-		result = inv_i2c_single_write(st, reg->pwr_mgmt_1, p);
+		result = inv_plat_single_write(st, reg->pwr_mgmt_1, p);
 		msleep(SENSOR_UP_TIME);
 	} else {
 		p = BITS_3050_GYRO_STANDBY;
-		result = inv_i2c_single_write(st, reg->pwr_mgmt_1, p);
+		result = inv_plat_single_write(st, reg->pwr_mgmt_1, p);
 	}
 
 	return result;
@@ -185,26 +185,26 @@ int inv_init_config_mpu3050(struct iio_dev *indio_dev)
 	if (st->chip_config.is_asleep)
 		return -EPERM;
 	/*reading AUX VDDIO register */
-	result = inv_i2c_read(st, REG_3050_AUX_VDDIO, 1, &data);
+	result = inv_plat_read(st, REG_3050_AUX_VDDIO, 1, &data);
 	if (result)
 		return result;
 	data &= ~BIT_3050_VDDIO;
 	if (st->plat_data.level_shifter)
 		data |= BIT_3050_VDDIO;
-	result = inv_i2c_single_write(st, REG_3050_AUX_VDDIO, data);
+	result = inv_plat_single_write(st, REG_3050_AUX_VDDIO, data);
 	if (result)
 		return result;
 
 	reg = &st->reg;
 	/*2000dps full scale range*/
-	result = inv_i2c_single_write(st, reg->lpf,
+	result = inv_plat_single_write(st, reg->lpf,
 				(INV_FSR_2000DPS << GYRO_CONFIG_FSR_SHIFT)
 				| INV_FILTER_42HZ);
 	if (result)
 		return result;
 	st->chip_config.fsr = INV_FSR_2000DPS;
 	st->chip_config.lpf = INV_FILTER_42HZ;
-	result = inv_i2c_single_write(st, reg->sample_rate_div,
+	result = inv_plat_single_write(st, reg->sample_rate_div,
 					ONE_K_HZ/INIT_FIFO_RATE - 1);
 	if (result)
 		return result;
@@ -250,22 +250,22 @@ int set_power_mpu3050(struct inv_mpu_iio_s *st, bool power_on)
 	}
 	if (st->chip_config.gyro_enable) {
 		p = (BITS_3050_POWER1 | INV_CLK_PLL);
-		result = inv_i2c_single_write(st, reg->pwr_mgmt_1, data | p);
+		result = inv_plat_single_write(st, reg->pwr_mgmt_1, data | p);
 		if (result)
 			return result;
 
 		p = (BITS_3050_POWER2 | INV_CLK_PLL);
-		result = inv_i2c_single_write(st, reg->pwr_mgmt_1, data | p);
+		result = inv_plat_single_write(st, reg->pwr_mgmt_1, data | p);
 		if (result)
 			return result;
 
 		p = INV_CLK_PLL;
-		result = inv_i2c_single_write(st, reg->pwr_mgmt_1, data | p);
+		result = inv_plat_single_write(st, reg->pwr_mgmt_1, data | p);
 		if (result)
 			return result;
 	} else {
 		data |= (BITS_3050_GYRO_STANDBY | INV_CLK_INTERNAL);
-		result = inv_i2c_single_write(st, reg->pwr_mgmt_1, data);
+		result = inv_plat_single_write(st, reg->pwr_mgmt_1, data);
 		if (result)
 			return result;
 	}
diff --git a/drivers/staging/iio/imu/inv_mpu/inv_mpu_core.c b/drivers/staging/iio/imu/inv_mpu/inv_mpu_core.c
index f10fce4658ed..1e28497d6b2c 100644
--- a/drivers/staging/iio/imu/inv_mpu/inv_mpu_core.c
+++ b/drivers/staging/iio/imu/inv_mpu/inv_mpu_core.c
@@ -100,93 +100,6 @@ static void inv_setup_reg(struct inv_reg_map_s *reg)
 	reg->prgm_strt_addrh	= REG_PRGM_STRT_ADDRH;
 };
 
-/**
- *  inv_i2c_read() - Read one or more bytes from the device registers.
- *  @st:	Device driver instance.
- *  @reg:	First device register to be read from.
- *  @length:	Number of bytes to read.
- *  @data:	Data read from device.
- *  NOTE:This is not re-implementation of i2c_smbus_read because i2c
- *       address could be specified in this case. We could have two different
- *       i2c address due to secondary i2c interface.
- */
-int inv_i2c_read_base(struct inv_mpu_iio_s *st, u16 i2c_addr,
-	u8 reg, u16 length, u8 *data)
-{
-	struct i2c_msg msgs[2];
-	int res;
-
-	if (!data)
-		return -EINVAL;
-
-	msgs[0].addr = i2c_addr;
-	msgs[0].flags = 0;	/* write */
-	msgs[0].buf = &reg;
-	msgs[0].len = 1;
-	/* msgs[0].scl_rate = 200*1000; */
-
-	msgs[1].addr = i2c_addr;
-	msgs[1].flags = I2C_M_RD;
-	msgs[1].buf = data;
-	msgs[1].len = length;
-	/* msgs[1].scl_rate = 200*1000; */
-
-	res = i2c_transfer(st->sl_handle, msgs, 2);
-
-	if (res < 2) {
-		if (res >= 0)
-			res = -EIO;
-	} else
-		res = 0;
-
-	INV_I2C_INC_MPUWRITE(3);
-	INV_I2C_INC_MPUREAD(length);
-	{
-		char *read = 0;
-		pr_debug("%s RD%02X%02X%02X -> %s%s\n", st->hw->name,
-			 i2c_addr, reg, length,
-			 wr_pr_debug_begin(data, length, read),
-			 wr_pr_debug_end(read));
-	}
-	return res;
-}
-
-/**
- *  inv_i2c_single_write() - Write a byte to a device register.
- *  @st:	Device driver instance.
- *  @reg:	Device register to be written to.
- *  @data:	Byte to write to device.
- *  NOTE:This is not re-implementation of i2c_smbus_write because i2c
- *       address could be specified in this case. We could have two different
- *       i2c address due to secondary i2c interface.
- */
-int inv_i2c_single_write_base(struct inv_mpu_iio_s *st,
-	u16 i2c_addr, u8 reg, u8 data)
-{
-	u8 tmp[2];
-	struct i2c_msg msg;
-	int res;
-	tmp[0] = reg;
-	tmp[1] = data;
-
-	msg.addr = i2c_addr;
-	msg.flags = 0;	/* write */
-	msg.buf = tmp;
-	msg.len = 2;
-	/* msg.scl_rate = 200*1000; */
-
-	pr_debug("%s WR%02X%02X%02X\n", st->hw->name, i2c_addr, reg, data);
-	INV_I2C_INC_MPUWRITE(3);
-
-	res = i2c_transfer(st->sl_handle, &msg, 1);
-	if (res < 1) {
-		if (res == 0)
-			res = -EIO;
-		return res;
-	} else
-		return 0;
-}
-
 static int inv_switch_engine(struct inv_mpu_iio_s *st, bool en, u32 mask)
 {
 	struct inv_reg_map_s *reg;
@@ -197,7 +110,7 @@ static int inv_switch_engine(struct inv_mpu_iio_s *st, bool en, u32 mask)
 	   clock source be switched to gyro. Otherwise, it must be set to
 	   internal clock */
 	if (BIT_PWR_GYRO_STBY == mask) {
-		result = inv_i2c_read(st, reg->pwr_mgmt_1, 1, &mgmt_1);
+		result = inv_plat_read(st, reg->pwr_mgmt_1, 1, &mgmt_1);
 		if (result)
 			return result;
 
@@ -208,20 +121,20 @@ static int inv_switch_engine(struct inv_mpu_iio_s *st, bool en, u32 mask)
 		/* turning off gyro requires switch to internal clock first.
 		   Then turn off gyro engine */
 		mgmt_1 |= INV_CLK_INTERNAL;
-		result = inv_i2c_single_write(st, reg->pwr_mgmt_1,
+		result = inv_plat_single_write(st, reg->pwr_mgmt_1,
 						mgmt_1);
 		if (result)
 			return result;
 	}
 
-	result = inv_i2c_read(st, reg->pwr_mgmt_2, 1, &data);
+	result = inv_plat_read(st, reg->pwr_mgmt_2, 1, &data);
 	if (result)
 		return result;
 	if (en)
 		data &= (~mask);
 	else
 		data |= mask;
-	result = inv_i2c_single_write(st, reg->pwr_mgmt_2, data);
+	result = inv_plat_single_write(st, reg->pwr_mgmt_2, data);
 	if (result)
 		return result;
 
@@ -230,7 +143,7 @@ static int inv_switch_engine(struct inv_mpu_iio_s *st, bool en, u32 mask)
 		msleep(SENSOR_UP_TIME);
 		/* after gyro is on & stable, switch internal clock to PLL */
 		mgmt_1 |= INV_CLK_PLL;
-		result = inv_i2c_single_write(st, reg->pwr_mgmt_1,
+		result = inv_plat_single_write(st, reg->pwr_mgmt_1,
 						mgmt_1);
 		if (result)
 			return result;
@@ -255,7 +168,7 @@ static int inv_lpa_freq(struct inv_mpu_iio_s *st, int lpa_freq)
 
 	if (INV_MPU6500 == st->chip_type) {
 		d = mpu6500_lpa_mapping[lpa_freq];
-		result = inv_i2c_single_write(st, REG_6500_LP_ACCEL_ODR, d);
+		result = inv_plat_single_write(st, REG_6500_LP_ACCEL_ODR, d);
 		if (result)
 			return result;
 	}
@@ -277,7 +190,7 @@ static int set_power_itg(struct inv_mpu_iio_s *st, bool power_on)
 		data = 0;
 	else
 		data = BIT_SLEEP;
-	result = inv_i2c_single_write(st, reg->pwr_mgmt_1, data);
+	result = inv_plat_single_write(st, reg->pwr_mgmt_1, data);
 	if (result)
 		return result;
 
@@ -312,23 +225,23 @@ static int inv_init_config(struct iio_dev *indio_dev)
 
 #if 0
 	/* set int latch en */
-	result = inv_i2c_single_write(st, REG_INT_PIN_CFG, 0x20);
+	result = inv_plat_single_write(st, REG_INT_PIN_CFG, 0x20);
 	if (result)
 		return result;
 #endif
-	result = inv_i2c_single_write(st, reg->gyro_config,
+	result = inv_plat_single_write(st, reg->gyro_config,
 		INV_FSR_2000DPS << GYRO_CONFIG_FSR_SHIFT);
 	if (result)
 		return result;
 
 	st->chip_config.fsr = INV_FSR_2000DPS;
 
-	result = inv_i2c_single_write(st, reg->lpf, INV_FILTER_42HZ);
+	result = inv_plat_single_write(st, reg->lpf, INV_FILTER_42HZ);
 	if (result)
 		return result;
 	st->chip_config.lpf = INV_FILTER_42HZ;
 
-	result = inv_i2c_single_write(st, reg->sample_rate_div,
+	result = inv_plat_single_write(st, reg->sample_rate_div,
 					ONE_K_HZ / INIT_FIFO_RATE - 1);
 	if (result)
 		return result;
@@ -341,7 +254,7 @@ static int inv_init_config(struct iio_dev *indio_dev)
 	st->self_test.threshold = INIT_ST_THRESHOLD;
 	if (INV_ITG3500 != st->chip_type) {
 		st->chip_config.accl_fs = INV_FS_02G;
-		result = inv_i2c_single_write(st, reg->accl_config,
+		result = inv_plat_single_write(st, reg->accl_config,
 			(INV_FS_02G << ACCL_CONFIG_FSR_SHIFT));
 		if (result)
 			return result;
@@ -352,13 +265,13 @@ static int inv_init_config(struct iio_dev *indio_dev)
 		st->smd.delay     = MPU_INIT_SMD_DELAY_THLD;
 		st->smd.delay2    = MPU_INIT_SMD_DELAY2_THLD;
 
-		result = inv_i2c_single_write(st, REG_ACCEL_MOT_DUR,
+		result = inv_plat_single_write(st, REG_ACCEL_MOT_DUR,
 						INIT_MOT_DUR);
 		if (result)
 			return result;
 		st->mot_int.mot_dur = INIT_MOT_DUR;
 
-		result = inv_i2c_single_write(st, REG_ACCEL_MOT_THR,
+		result = inv_plat_single_write(st, REG_ACCEL_MOT_THR,
 						INIT_MOT_THR);
 		if (result)
 			return result;
@@ -533,10 +446,10 @@ static int inv_write_fsr(struct inv_mpu_iio_s *st, int fsr)
 		return 0;
 
 	if (INV_MPU3050 == st->chip_type)
-		result = inv_i2c_single_write(st, reg->lpf,
+		result = inv_plat_single_write(st, reg->lpf,
 			(fsr << GYRO_CONFIG_FSR_SHIFT) | st->chip_config.lpf);
 	else
-		result = inv_i2c_single_write(st, reg->gyro_config,
+		result = inv_plat_single_write(st, reg->gyro_config,
 			fsr << GYRO_CONFIG_FSR_SHIFT);
 
 	if (result)
@@ -562,7 +475,7 @@ static int inv_write_accel_fs(struct inv_mpu_iio_s *st, int fs)
 	if (INV_MPU3050 == st->chip_type)
 		result = st->mpu_slave->set_fs(st, fs);
 	else
-		result = inv_i2c_single_write(st, reg->accl_config,
+		result = inv_plat_single_write(st, reg->accl_config,
 				(fs << ACCL_CONFIG_FSR_SHIFT));
 	if (result)
 		return result;
@@ -585,7 +498,7 @@ static int inv_write_compass_scale(struct inv_mpu_iio_s  *st, int data)
 	if (st->compass_scale == en)
 		return 0;
 	d = (DATA_AKM_MODE_SM | (st->compass_scale << AKM8963_SCALE_SHIFT));
-	result = inv_i2c_single_write(st, REG_I2C_SLV1_DO, d);
+	result = inv_plat_single_write(st, REG_I2C_SLV1_DO, d);
 	if (result)
 		return result;
 	st->compass_scale = en;
@@ -701,7 +614,7 @@ static ssize_t inv_reg_dump_show(struct device *dev,
 		if (ii == st->reg.fifo_r_w)
 			data = 0;
 		else
-			inv_i2c_read(st, ii, 1, &data);
+			inv_plat_read(st, ii, 1, &data);
 		bytes_printed += sprintf(buf + bytes_printed, "%#2x: %#2x\n",
 					 ii, data);
 	}
@@ -1126,7 +1039,7 @@ static ssize_t inv_temperature_show(struct device *dev,
 		mutex_unlock(&indio_dev->mlock);
 		return result;
 	}
-	result = inv_i2c_read(st, reg->temperature, 2, data);
+	result = inv_plat_read(st, reg->temperature, 2, data);
 	if (!st->chip_config.enable)
 		result |= st->set_power_state(st, false);
 	mutex_unlock(&indio_dev->mlock);
@@ -1280,7 +1193,7 @@ static ssize_t inv_attr_store(struct device *dev,
 					| BIT_ACCEL_INTEL_MODE;
 			else
 				d = 0;
-			result = inv_i2c_single_write(st,
+			result = inv_plat_single_write(st,
 						REG_6500_ACCEL_INTEL_CTRL, d);
 			if (result)
 				goto attr_store_fail;
@@ -1298,7 +1211,7 @@ static ssize_t inv_attr_store(struct device *dev,
 		}
 		d = (u8)(data >> MPU6XXX_MOTION_THRESH_SHIFT);
 		data = (d << MPU6XXX_MOTION_THRESH_SHIFT);
-		result = inv_i2c_single_write(st, REG_ACCEL_MOT_THR, d);
+		result = inv_plat_single_write(st, REG_ACCEL_MOT_THR, d);
 		if (result)
 			goto attr_store_fail;
 		st->mot_int.mot_thr = data;
@@ -1385,7 +1298,7 @@ static ssize_t inv_reg_write_store(struct device *dev,
 		return -EINVAL;
 	wreg = temp;
 
-	result = inv_i2c_single_write(st, wreg, wval);
+	result = inv_plat_single_write(st, wreg, wval);
 	if (result)
 		return result;
 
@@ -1623,6 +1536,16 @@ static const struct iio_info mpu_info = {
 	.attrs = &inv_attribute_group,
 };
 
+void inv_set_iio_info(struct inv_mpu_iio_s *st, struct iio_dev *indio_dev)
+{
+	indio_dev->channels = inv_mpu_channels;
+	indio_dev->num_channels = st->num_channels;
+
+	indio_dev->info = &mpu_info;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+	indio_dev->currentmode = INDIO_DIRECT_MODE;
+}
+
 /**
  *  inv_setup_compass() - Configure compass.
  */
@@ -1632,38 +1555,38 @@ static int inv_setup_compass(struct inv_mpu_iio_s *st)
 	u8 data[4];
 
 	if (INV_MPU6050 == st->chip_type) {
-		result = inv_i2c_read(st, REG_YGOFFS_TC, 1, data);
+		result = inv_plat_read(st, REG_YGOFFS_TC, 1, data);
 		if (result)
 			return result;
 		data[0] &= ~BIT_I2C_MST_VDDIO;
 		if (st->plat_data.level_shifter)
 			data[0] |= BIT_I2C_MST_VDDIO;
 		/*set up VDDIO register */
-		result = inv_i2c_single_write(st, REG_YGOFFS_TC, data[0]);
+		result = inv_plat_single_write(st, REG_YGOFFS_TC, data[0]);
 		if (result)
 			return result;
 	}
 	/* set to bypass mode */
-	result = inv_i2c_single_write(st, REG_INT_PIN_CFG,
+	result = inv_plat_single_write(st, REG_INT_PIN_CFG,
 				st->plat_data.int_config | BIT_BYPASS_EN);
 	if (result)
 		return result;
 	/*read secondary i2c ID register */
-	result = inv_secondary_read(REG_AKM_ID, 1, data);
+	result = inv_secondary_read(st, REG_AKM_ID, 1, data);
 	if (result)
 		return result;
 	if (data[0] != DATA_AKM_ID)
 		return -ENXIO;
 	/*set AKM to Fuse ROM access mode */
-	result = inv_secondary_write(REG_AKM_MODE, DATA_AKM_MODE_FR);
+	result = inv_secondary_write(st, REG_AKM_MODE, DATA_AKM_MODE_FR);
 	if (result)
 		return result;
-	result = inv_secondary_read(REG_AKM_SENSITIVITY, THREE_AXIS,
+	result = inv_secondary_read(st, REG_AKM_SENSITIVITY, THREE_AXIS,
 					st->chip_info.compass_sens);
 	if (result)
 		return result;
 	/*revert to power down mode */
-	result = inv_secondary_write(REG_AKM_MODE, DATA_AKM_MODE_PD);
+	result = inv_secondary_write(st, REG_AKM_MODE, DATA_AKM_MODE_PD);
 	if (result)
 		return result;
 	pr_debug("%s senx=%d, seny=%d, senz=%d\n",
@@ -1672,41 +1595,41 @@ static int inv_setup_compass(struct inv_mpu_iio_s *st)
 		 st->chip_info.compass_sens[1],
 		 st->chip_info.compass_sens[2]);
 	/*restore to non-bypass mode */
-	result = inv_i2c_single_write(st, REG_INT_PIN_CFG,
+	result = inv_plat_single_write(st, REG_INT_PIN_CFG,
 			st->plat_data.int_config);
 	if (result)
 		return result;
 
 	/*setup master mode and master clock and ES bit*/
-	result = inv_i2c_single_write(st, REG_I2C_MST_CTRL, BIT_WAIT_FOR_ES);
+	result = inv_plat_single_write(st, REG_I2C_MST_CTRL, BIT_WAIT_FOR_ES);
 	if (result)
 		return result;
 	/* slave 0 is used to read data from compass */
 	/*read mode */
-	result = inv_i2c_single_write(st, REG_I2C_SLV0_ADDR, BIT_I2C_READ|
+	result = inv_plat_single_write(st, REG_I2C_SLV0_ADDR, BIT_I2C_READ|
 		st->plat_data.secondary_i2c_addr);
 	if (result)
 		return result;
 	/* AKM status register address is 2 */
-	result = inv_i2c_single_write(st, REG_I2C_SLV0_REG, REG_AKM_STATUS);
+	result = inv_plat_single_write(st, REG_I2C_SLV0_REG, REG_AKM_STATUS);
 	if (result)
 		return result;
 	/* slave 0 is enabled at the beginning, read 8 bytes from here */
-	result = inv_i2c_single_write(st, REG_I2C_SLV0_CTRL, BIT_SLV_EN |
+	result = inv_plat_single_write(st, REG_I2C_SLV0_CTRL, BIT_SLV_EN |
 				NUM_BYTES_COMPASS_SLAVE);
 	if (result)
 		return result;
 	/*slave 1 is used for AKM mode change only*/
-	result = inv_i2c_single_write(st, REG_I2C_SLV1_ADDR,
+	result = inv_plat_single_write(st, REG_I2C_SLV1_ADDR,
 		st->plat_data.secondary_i2c_addr);
 	if (result)
 		return result;
 	/* AKM mode register address is 0x0A */
-	result = inv_i2c_single_write(st, REG_I2C_SLV1_REG, REG_AKM_MODE);
+	result = inv_plat_single_write(st, REG_I2C_SLV1_REG, REG_AKM_MODE);
 	if (result)
 		return result;
 	/* slave 1 is enabled, byte length is 1 */
-	result = inv_i2c_single_write(st, REG_I2C_SLV1_CTRL, BIT_SLV_EN | 1);
+	result = inv_plat_single_write(st, REG_I2C_SLV1_CTRL, BIT_SLV_EN | 1);
 	if (result)
 		return result;
 	/* output data for slave 1 is fixed, single measure mode*/
@@ -1725,11 +1648,11 @@ static int inv_setup_compass(struct inv_mpu_iio_s *st)
 		data[0] = DATA_AKM_MODE_SM |
 			  (st->compass_scale << AKM8963_SCALE_SHIFT);
 	}
-	result = inv_i2c_single_write(st, REG_I2C_SLV1_DO, data[0]);
+	result = inv_plat_single_write(st, REG_I2C_SLV1_DO, data[0]);
 	if (result)
 		return result;
 	/* slave 0 and 1 timer action is enabled every sample*/
-	result = inv_i2c_single_write(st, REG_I2C_MST_DELAY_CTRL,
+	result = inv_plat_single_write(st, REG_I2C_MST_DELAY_CTRL,
 				BIT_SLV0_DLY_EN | BIT_SLV1_DLY_EN);
 	return result;
 }
@@ -1761,7 +1684,7 @@ static int inv_detect_6xxx(struct inv_mpu_iio_s *st)
 	int result;
 	u8 d;
 
-	result = inv_i2c_read(st, REG_WHOAMI, 1, &d);
+	result = inv_plat_read(st, REG_WHOAMI, 1, &d);
 	if (result)
 		return result;
 	if ((d == MPU6500_ID) || (d == MPU6515_ID)) {
@@ -1777,28 +1700,27 @@ static int inv_detect_6xxx(struct inv_mpu_iio_s *st)
 /**
  *  inv_check_chip_type() - check and setup chip type.
  */
-static int inv_check_chip_type(struct inv_mpu_iio_s *st,
-		const struct i2c_device_id *id)
+int inv_check_chip_type(struct inv_mpu_iio_s *st, const char *name)
 {
 	struct inv_reg_map_s *reg;
 	int result;
 	int t_ind;
 
-	if (!strcmp(id->name, "itg3500"))
+	if (!strcmp(name, "itg3500"))
 		st->chip_type = INV_ITG3500;
-	else if (!strcmp(id->name, "mpu3050"))
+	else if (!strcmp(name, "mpu3050"))
 		st->chip_type = INV_MPU3050;
-	else if (!strcmp(id->name, "mpu6050"))
+	else if (!strcmp(name, "mpu6050"))
 		st->chip_type = INV_MPU6050;
-	else if (!strcmp(id->name, "mpu9150"))
+	else if (!strcmp(name, "mpu9150"))
 		st->chip_type = INV_MPU6050;
-	else if (!strcmp(id->name, "mpu6500"))
+	else if (!strcmp(name, "mpu6500"))
 		st->chip_type = INV_MPU6500;
-	else if (!strcmp(id->name, "mpu9250"))
+	else if (!strcmp(name, "mpu9250"))
 		st->chip_type = INV_MPU6500;
-	else if (!strcmp(id->name, "mpu6xxx"))
+	else if (!strcmp(name, "mpu6xxx"))
 		st->chip_type = INV_MPU6050;
-	else if (!strcmp(id->name, "mpu6515"))
+	else if (!strcmp(name, "mpu6515"))
 		st->chip_type = INV_MPU6500;
 	else
 		return -EPERM;
@@ -1808,7 +1730,7 @@ static int inv_check_chip_type(struct inv_mpu_iio_s *st,
 	reg = &st->reg;
 	st->setup_reg(reg);
 	/* reset to make sure previous state are not there */
-	result = inv_i2c_single_write(st, reg->pwr_mgmt_1, BIT_H_RESET);
+	result = inv_plat_single_write(st, reg->pwr_mgmt_1, BIT_H_RESET);
 	if (result)
 		return result;
 	msleep(POWER_UP_TIME);
@@ -1821,7 +1743,7 @@ static int inv_check_chip_type(struct inv_mpu_iio_s *st,
 	if (result)
 		return result;
 
-	if (!strcmp(id->name, "mpu6xxx")) {
+	if (!strcmp(name, "mpu6xxx")) {
 		/* for MPU6500, reading register need more time */
 		msleep(POWER_UP_TIME);
 		result = inv_detect_6xxx(st);
@@ -1941,7 +1863,7 @@ static const struct bin_attribute dmp_firmware = {
 	.write = inv_dmp_firmware_write,
 };
 
-static int inv_create_dmp_sysfs(struct iio_dev *ind)
+int inv_create_dmp_sysfs(struct iio_dev *ind)
 {
 	int result;
 	result = sysfs_create_bin_file(&ind->dev.kobj, &dmp_firmware);
@@ -1949,322 +1871,6 @@ static int inv_create_dmp_sysfs(struct iio_dev *ind)
 	return result;
 }
 
-#include <linux/gpio.h>
-#include <linux/of_gpio.h>
-#include <linux/of.h>
-
-static struct mpu_platform_data mpu_data = {
-	.int_config  = 0x00,
-	.level_shifter = 0,
-	.orientation = {
-			0,  1,  0,
-			-1,  0,  0,
-			0,  0, -1,
-	},
-	/*
-	.key = {
-		221,  22, 205,   7, 217, 186, 151, 55,
-		206, 254,  35, 144, 225, 102,  47, 50,
-	},
-	*/
-};
-
-static int of_inv_parse_platform_data(struct i2c_client *client,
-				      struct mpu_platform_data *pdata)
-{
-	struct device_node *np = client->dev.of_node;
-	unsigned long irq_flags;
-	int irq_pin;
-	int gpio_pin;
-
-	gpio_pin = of_get_named_gpio_flags(np, "irq-gpio", 0, (enum of_gpio_flags *)&irq_flags);
-	gpio_request(gpio_pin, "mpu6500");
-	irq_pin = gpio_to_irq(gpio_pin);
-	client->irq = irq_pin;
-	i2c_set_clientdata(client, &mpu_data);
-
-	pr_info("%s: %s, %x, %x\n", __func__, client->name, client->addr, client->irq);
-
-	return 0;
-}
-
-/**
- *  inv_mpu_probe() - probe function.
- */
-static int inv_mpu_probe(struct i2c_client *client,
-	const struct i2c_device_id *id)
-{
-	struct inv_mpu_iio_s *st;
-	struct iio_dev *indio_dev;
-	int result;
-
-	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
-		result = -ENOSYS;
-		pr_err("I2c function error\n");
-		goto out_no_free;
-	}
-#ifdef INV_KERNEL_3_10
-    indio_dev = iio_device_alloc(sizeof(*st));
-#else
-	indio_dev = iio_allocate_device(sizeof(*st));
-#endif
-	if (indio_dev == NULL) {
-		pr_err("memory allocation failed\n");
-		result =  -ENOMEM;
-		goto out_no_free;
-	}
-	st = iio_priv(indio_dev);
-	st->client = client;
-	st->sl_handle = client->adapter;
-	st->i2c_addr = client->addr;
-	if (client->dev.of_node) {
-		result = of_inv_parse_platform_data(client, &st->plat_data);
-		if (result)
-			goto out_free;
-		st->plat_data = mpu_data;
-		pr_info("secondary_i2c_addr=%x\n", st->plat_data.secondary_i2c_addr);
-	} else
-		st->plat_data =
-			*(struct mpu_platform_data *)dev_get_platdata(&client->dev);
-	/* power is turned on inside check chip type*/
-	result = inv_check_chip_type(st, id);
-	if (result)
-		goto out_free;
-
-	result = st->init_config(indio_dev);
-	if (result) {
-		dev_err(&client->adapter->dev,
-			"Could not initialize device.\n");
-		goto out_free;
-	}
-	result = st->set_power_state(st, false);
-	if (result) {
-		dev_err(&client->adapter->dev,
-			"%s could not be turned off.\n", st->hw->name);
-		goto out_free;
-	}
-
-	/* Make state variables available to all _show and _store functions. */
-	i2c_set_clientdata(client, indio_dev);
-	indio_dev->dev.parent = &client->dev;
-	if (!strcmp(id->name, "mpu6xxx"))
-		indio_dev->name = st->name;
-	else
-		indio_dev->name = id->name;
-	indio_dev->channels = inv_mpu_channels;
-	indio_dev->num_channels = st->num_channels;
-
-	indio_dev->info = &mpu_info;
-	indio_dev->modes = INDIO_DIRECT_MODE;
-	indio_dev->currentmode = INDIO_DIRECT_MODE;
-
-	result = inv_mpu_configure_ring(indio_dev);
-	if (result) {
-		pr_err("configure ring buffer fail\n");
-		goto out_free;
-	}
-	st->irq = client->irq;
-	result = inv_mpu_probe_trigger(indio_dev);
-	if (result) {
-		pr_err("trigger probe fail\n");
-		goto out_unreg_ring;
-	}
-
-	/* Tell the i2c counter, we have an IRQ */
-	INV_I2C_SETIRQ(IRQ_MPU, client->irq);
-
-	result = iio_device_register(indio_dev);
-	if (result) {
-		pr_err("IIO device register fail\n");
-		goto out_remove_trigger;
-	}
-
-	if (INV_MPU6050 == st->chip_type ||
-	    INV_MPU6500 == st->chip_type) {
-		result = inv_create_dmp_sysfs(indio_dev);
-		if (result) {
-			pr_err("create dmp sysfs failed\n");
-			goto out_unreg_iio;
-		}
-	}
-
-	INIT_KFIFO(st->timestamps);
-	spin_lock_init(&st->time_stamp_lock);
-	dev_info(&client->dev, "%s is ready to go!\n",
-					indio_dev->name);
-
-	return 0;
-out_unreg_iio:
-	iio_device_unregister(indio_dev);
-out_remove_trigger:
-	if (indio_dev->modes & INDIO_BUFFER_TRIGGERED)
-		inv_mpu_remove_trigger(indio_dev);
-out_unreg_ring:
-	inv_mpu_unconfigure_ring(indio_dev);
-out_free:
-#ifdef INV_KERNEL_3_10
-    iio_device_free(indio_dev);
-#else
-	iio_free_device(indio_dev);
-#endif
-out_no_free:
-	dev_err(&client->adapter->dev, "%s failed %d\n", __func__, result);
-
-	return -EIO;
-}
-
-static void inv_mpu_shutdown(struct i2c_client *client)
-{
-	struct iio_dev *indio_dev = i2c_get_clientdata(client);
-	struct inv_mpu_iio_s *st = iio_priv(indio_dev);
-	struct inv_reg_map_s *reg;
-	int result;
-
-	reg = &st->reg;
-	dev_dbg(&client->adapter->dev, "Shutting down %s...\n", st->hw->name);
-
-	/* reset to make sure previous state are not there */
-	result = inv_i2c_single_write(st, reg->pwr_mgmt_1, BIT_H_RESET);
-	if (result)
-		dev_err(&client->adapter->dev, "Failed to reset %s\n",
-			st->hw->name);
-	msleep(POWER_UP_TIME);
-	/* turn off power to ensure gyro engine is off */
-	result = st->set_power_state(st, false);
-	if (result)
-		dev_err(&client->adapter->dev, "Failed to turn off %s\n",
-			st->hw->name);
-}
-
-/**
- *  inv_mpu_remove() - remove function.
- */
-static int inv_mpu_remove(struct i2c_client *client)
-{
-	struct iio_dev *indio_dev = i2c_get_clientdata(client);
-	struct inv_mpu_iio_s *st = iio_priv(indio_dev);
-	kfifo_free(&st->timestamps);
-	iio_device_unregister(indio_dev);
-	if (indio_dev->modes & INDIO_BUFFER_TRIGGERED)
-		inv_mpu_remove_trigger(indio_dev);
-	inv_mpu_unconfigure_ring(indio_dev);
-#ifdef INV_KERNEL_3_10
-    iio_device_free(indio_dev);
-#else
-	iio_free_device(indio_dev);
-#endif
-
-	dev_info(&client->adapter->dev, "inv-mpu-iio module removed.\n");
-
-	return 0;
-}
-
-#ifdef CONFIG_PM
-static int inv_mpu_resume(struct device *dev)
-{
-	struct inv_mpu_iio_s *st =
-			iio_priv(i2c_get_clientdata(to_i2c_client(dev)));
-	pr_debug("%s inv_mpu_resume\n", st->hw->name);
-	return st->set_power_state(st, true);
-}
-
-static int inv_mpu_suspend(struct device *dev)
-{
-	struct iio_dev *indio_dev = i2c_get_clientdata(to_i2c_client(dev));
-	struct inv_mpu_iio_s *st = iio_priv(indio_dev);
-	int result;
-
-	pr_debug("%s inv_mpu_suspend\n", st->hw->name);
-	mutex_lock(&indio_dev->mlock);
-	result = 0;
-	if ((!st->chip_config.dmp_on) ||
-		(!st->chip_config.enable) ||
-		(!st->chip_config.dmp_event_int_on))
-		result = st->set_power_state(st, false);
-	mutex_unlock(&indio_dev->mlock);
-
-	return result;
-}
-static const struct dev_pm_ops inv_mpu_pmops = {
-	SET_SYSTEM_SLEEP_PM_OPS(inv_mpu_suspend, inv_mpu_resume)
-};
-#define INV_MPU_PMOPS (&inv_mpu_pmops)
-#else
-#define INV_MPU_PMOPS NULL
-#endif /* CONFIG_PM */
-
-static const u16 normal_i2c[] = { I2C_CLIENT_END };
-/* device id table is used to identify what device can be
- * supported by this driver
- */
-static const struct i2c_device_id inv_mpu_id[] = {
-	{"itg3500", INV_ITG3500},
-	{"mpu3050", INV_MPU3050},
-	{"mpu6050", INV_MPU6050},
-	{"mpu9150", INV_MPU9150},
-	{"mpu6500", INV_MPU6500},
-	{"mpu9250", INV_MPU9250},
-	{"mpu6xxx", INV_MPU6XXX},
-	{"mpu6515", INV_MPU6515},
-	{}
-};
-
-MODULE_DEVICE_TABLE(i2c, inv_mpu_id);
-
-static const struct of_device_id inv_mpu_of_match[] = {
-	{ .compatible = "invensense,itg3500", },
-	{ .compatible = "invensense,mpu3050", },
-	{ .compatible = "invensense,mpu6050", },
-	{ .compatible = "invensense,mpu9150", },
-	{ .compatible = "invensense,mpu6500", },
-	{ .compatible = "invensense,mpu9250", },
-	{ .compatible = "invensense,mpu6xxx", },
-	{ .compatible = "invensense,mpu9350", },
-	{ .compatible = "invensense,mpu6515", },
-	{}
-};
-
-MODULE_DEVICE_TABLE(of, inv_mpu_of_match);
-
-static struct i2c_driver inv_mpu_driver = {
-	.class = I2C_CLASS_HWMON,
-	.probe		=	inv_mpu_probe,
-	.remove		=	inv_mpu_remove,
-	.shutdown	=	inv_mpu_shutdown,
-	.id_table	=	inv_mpu_id,
-	.driver = {
-		.owner	=	THIS_MODULE,
-		.name	=	"inv-mpu-iio",
-		.pm     =       INV_MPU_PMOPS,
-		.of_match_table = of_match_ptr(inv_mpu_of_match),
-	},
-	.address_list = normal_i2c,
-};
-
-static int __init inv_mpu_init(void)
-{
-	int result = i2c_add_driver(&inv_mpu_driver);
-    pr_info("%s:%d\n", __func__, __LINE__);
-	if (result) {
-		pr_err("failed\n");
-		return result;
-	}
-	return 0;
-}
-
-static void __exit inv_mpu_exit(void)
-{
-	i2c_del_driver(&inv_mpu_driver);
-}
-
-module_init(inv_mpu_init);
-module_exit(inv_mpu_exit);
-
-MODULE_AUTHOR("Invensense Corporation");
-MODULE_DESCRIPTION("Invensense device driver");
-MODULE_LICENSE("GPL");
-MODULE_ALIAS("inv-mpu-iio");
-
 /**
  *  @}
  */
diff --git a/drivers/staging/iio/imu/inv_mpu/inv_mpu_i2c.c b/drivers/staging/iio/imu/inv_mpu/inv_mpu_i2c.c
new file mode 100644
index 000000000000..c572dd3de753
--- /dev/null
+++ b/drivers/staging/iio/imu/inv_mpu/inv_mpu_i2c.c
@@ -0,0 +1,631 @@
+/*
+* Copyright (C) 2012 Invensense, Inc.
+*
+* This software is licensed under the terms of the GNU General Public
+* License version 2, as published by the Free Software Foundation, and
+* may be copied, distributed, and modified under those terms.
+*
+* 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.
+*
+*/
+
+/**
+ *  @addtogroup  DRIVERS
+ *  @brief       Hardware drivers.
+ *
+ *  @{
+ *      @file    inv_mpu_core.c
+ *      @brief   A sysfs device driver for Invensense devices
+ *      @details This driver currently works for the
+ *               MPU3050/MPU6050/MPU9150/MPU6500/MPU9250 devices.
+ */
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/i2c.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/miscdevice.h>
+#include <linux/spinlock.h>
+
+#include "inv_mpu_iio.h"
+#ifdef INV_KERNEL_3_10
+#include <linux/iio/sysfs.h>
+#else
+#include "sysfs.h"
+#endif
+#include "inv_counters.h"
+
+/**
+ *  inv_i2c_read_base() - Read one or more bytes from the device registers.
+ *  @st:	Device driver instance.
+ *  @reg:	First device register to be read from.
+ *  @length:	Number of bytes to read.
+ *  @data:	Data read from device.
+ *  NOTE:This is not re-implementation of i2c_smbus_read because i2c
+ *       address could be specified in this case. We could have two different
+ *       i2c address due to secondary i2c interface.
+ */
+int inv_i2c_read_base(struct inv_mpu_iio_s *st, u16 i2c_addr,
+	u8 reg, u16 length, u8 *data)
+{
+	struct i2c_msg msgs[2];
+	int res;
+
+	if (!data)
+		return -EINVAL;
+
+	msgs[0].addr = i2c_addr;
+	msgs[0].flags = 0;	/* write */
+	msgs[0].buf = &reg;
+	msgs[0].len = 1;
+	/* msgs[0].scl_rate = 200*1000; */
+
+	msgs[1].addr = i2c_addr;
+	msgs[1].flags = I2C_M_RD;
+	msgs[1].buf = data;
+	msgs[1].len = length;
+	/* msgs[1].scl_rate = 200*1000; */
+
+	res = i2c_transfer(st->sl_handle, msgs, 2);
+
+	if (res < 2) {
+		if (res >= 0)
+			res = -EIO;
+	} else
+		res = 0;
+
+	INV_I2C_INC_MPUWRITE(3);
+	INV_I2C_INC_MPUREAD(length);
+	{
+		char *read = 0;
+		pr_debug("%s RD%02X%02X%02X -> %s%s\n", st->hw->name,
+			 i2c_addr, reg, length,
+			 wr_pr_debug_begin(data, length, read),
+			 wr_pr_debug_end(read));
+	}
+	return res;
+}
+
+/**
+ *  inv_i2c_single_write_base() - Write a byte to a device register.
+ *  @st:	Device driver instance.
+ *  @reg:	Device register to be written to.
+ *  @data:	Byte to write to device.
+ *  NOTE:This is not re-implementation of i2c_smbus_write because i2c
+ *       address could be specified in this case. We could have two different
+ *       i2c address due to secondary i2c interface.
+ */
+int inv_i2c_single_write_base(struct inv_mpu_iio_s *st,
+	u16 i2c_addr, u8 reg, u8 data)
+{
+	u8 tmp[2];
+	struct i2c_msg msg;
+	int res;
+	tmp[0] = reg;
+	tmp[1] = data;
+
+	msg.addr = i2c_addr;
+	msg.flags = 0;	/* write */
+	msg.buf = tmp;
+	msg.len = 2;
+	/* msg.scl_rate = 200*1000; */
+
+	pr_debug("%s WR%02X%02X%02X\n", st->hw->name, i2c_addr, reg, data);
+	INV_I2C_INC_MPUWRITE(3);
+
+	res = i2c_transfer(st->sl_handle, &msg, 1);
+	if (res < 1) {
+		if (res == 0)
+			res = -EIO;
+		return res;
+	} else
+		return 0;
+}
+
+int inv_plat_single_write(struct inv_mpu_iio_s *st, u8 reg, u8 data)
+{
+	return inv_i2c_single_write_base(st, st->i2c_addr, reg, data);
+}
+
+int inv_plat_read(struct inv_mpu_iio_s *st, u8 reg, int len, u8 *data)
+{
+	return inv_i2c_read_base(st, st->i2c_addr, reg, len, data);
+}
+
+int inv_secondary_read(struct inv_mpu_iio_s *st, u8 reg, int len, u8 *data)
+{
+	return inv_i2c_read_base(st, st->plat_data.secondary_i2c_addr, reg, len, data);
+}
+
+int inv_secondary_write(struct inv_mpu_iio_s *st, u8 reg, u8 data)
+{
+	return inv_i2c_single_write_base(st, st->plat_data.secondary_i2c_addr, reg, data);
+}
+
+int mpu_memory_write(struct inv_mpu_iio_s *st, u8 mpu_addr, u16 mem_addr,
+		     u32 len, u8 const *data)
+{
+	u8 bank[2];
+	u8 addr[2];
+	u8 buf[513];
+
+	struct i2c_msg msg;
+	int res;
+
+	if (!data || !st)
+		return -EINVAL;
+
+	if (len >= (sizeof(buf) - 1))
+		return -ENOMEM;
+
+	bank[0] = REG_BANK_SEL;
+	bank[1] = mem_addr >> 8;
+
+	addr[0] = REG_MEM_START_ADDR;
+	addr[1] = mem_addr & 0xFF;
+
+	buf[0] = REG_MEM_RW;
+	memcpy(buf + 1, data, len);
+
+	/* write message */
+	msg.addr = mpu_addr;
+	msg.flags = 0;
+	msg.buf = bank;
+	msg.len = sizeof(bank);
+	/* msg.scl_rate = 200*1000; */
+	INV_I2C_INC_MPUWRITE(3);
+	res = i2c_transfer(st->sl_handle, &msg, 1);
+	if (res < 1) {
+		if (res >= 0)
+			res = -EIO;
+		return res;
+	}
+
+	msg.addr = mpu_addr;
+	msg.flags = 0;
+	msg.buf = addr;
+	msg.len = sizeof(addr);
+	/* msg.scl_rate = 200*1000; */
+	INV_I2C_INC_MPUWRITE(3);
+	res = i2c_transfer(st->sl_handle, &msg, 1);
+	if (res < 1) {
+		if (res >= 0)
+			res = -EIO;
+		return res;
+	}
+
+	msg.addr = mpu_addr;
+	msg.flags = 0;
+	msg.buf = (u8 *)buf;
+	msg.len = len + 1;
+	/* msg.scl_rate = 200*1000; */
+	INV_I2C_INC_MPUWRITE(2+len);
+	res = i2c_transfer(st->sl_handle, &msg, 1);
+	if (res < 1) {
+		if (res >= 0)
+			res = -EIO;
+		return res;
+	}
+
+	{
+		char *write = 0;
+		pr_debug("%s WM%02X%02X%02X%s%s - %d\n", st->hw->name,
+			 mpu_addr, bank[1], addr[1],
+			 wr_pr_debug_begin(data, len, write),
+			 wr_pr_debug_end(write),
+			 len);
+	}
+	return 0;
+}
+
+int mpu_memory_read(struct inv_mpu_iio_s *st, u8 mpu_addr, u16 mem_addr,
+		    u32 len, u8 *data)
+{
+	u8 bank[2];
+	u8 addr[2];
+	u8 buf;
+
+	struct i2c_msg msg;
+	int res;
+
+	if (!data || !st)
+		return -EINVAL;
+
+	bank[0] = REG_BANK_SEL;
+	bank[1] = mem_addr >> 8;
+
+	addr[0] = REG_MEM_START_ADDR;
+	addr[1] = mem_addr & 0xFF;
+
+	buf = REG_MEM_RW;
+
+	/* write message */
+	msg.addr = mpu_addr;
+	msg.flags = 0;
+	msg.buf = bank;
+	msg.len = sizeof(bank);
+	/* msg.scl_rate = 200*1000; */
+	INV_I2C_INC_MPUWRITE(3);
+	res = i2c_transfer(st->sl_handle, &msg, 1);
+	if (res < 1) {
+		if (res >= 0)
+			res = -EIO;
+		return res;
+	}
+
+	msg.addr = mpu_addr;
+	msg.flags = 0;
+	msg.buf = addr;
+	msg.len = sizeof(addr);
+	/* msg.scl_rate = 200*1000; */
+	INV_I2C_INC_MPUWRITE(3);
+	res = i2c_transfer(st->sl_handle, &msg, 1);
+	if (res < 1) {
+		if (res >= 0)
+			res = -EIO;
+		return res;
+	}
+
+	msg.addr = mpu_addr;
+	msg.flags = 0;
+	msg.buf = &buf;
+	msg.len = 1;
+	/* msg.scl_rate = 200*1000; */
+	INV_I2C_INC_MPUWRITE(3);
+	res = i2c_transfer(st->sl_handle, &msg, 1);
+	if (res < 1) {
+		if (res >= 0)
+			res = -EIO;
+		return res;
+	}
+
+	msg.addr = mpu_addr;
+	msg.flags = I2C_M_RD;
+	msg.buf = data;
+	msg.len = len;
+	/* msg.scl_rate = 200*1000; */
+	INV_I2C_INC_MPUREAD(len);
+	res = i2c_transfer(st->sl_handle, &msg, 1);
+	if (res < 1) {
+		if (res >= 0)
+			res = -EIO;
+		return res;
+	}
+	{
+		char *read = 0;
+		pr_debug("%s RM%02X%02X%02X%02X - %s%s\n", st->hw->name,
+			 mpu_addr, bank[1], addr[1], len,
+			 wr_pr_debug_begin(data, len, read),
+			 wr_pr_debug_end(read));
+	}
+
+	return 0;
+}
+
+#include <linux/gpio.h>
+#include <linux/of_gpio.h>
+#include <linux/of.h>
+
+static struct mpu_platform_data mpu_data = {
+	.int_config  = 0x00,
+	.level_shifter = 0,
+	.orientation = {
+			0,  1,  0,
+			-1,  0,  0,
+			0,  0, -1,
+	},
+/*
+	.key = {
+		221,  22, 205,   7, 217, 186, 151, 55,
+		206, 254,  35, 144, 225, 102,  47, 50,
+	},
+*/
+};
+
+static int of_inv_parse_platform_data(struct i2c_client *client,
+				      struct mpu_platform_data *pdata)
+{
+	struct device_node *np = client->dev.of_node;
+	unsigned long irq_flags;
+	int irq_pin;
+	int gpio_pin;
+
+	gpio_pin = of_get_named_gpio_flags(np, "irq-gpio", 0, (enum of_gpio_flags *)&irq_flags);
+	gpio_request(gpio_pin, "mpu6500");
+	irq_pin = gpio_to_irq(gpio_pin);
+	client->irq = irq_pin;
+	i2c_set_clientdata(client, &mpu_data);
+
+	pr_info("%s: %s, %x, %x\n", __func__, client->name, client->addr, client->irq);
+
+	return 0;
+}
+
+/**
+ *  inv_mpu_probe() - probe function.
+ */
+static int inv_mpu_probe(struct i2c_client *client,
+	const struct i2c_device_id *id)
+{
+	struct inv_mpu_iio_s *st;
+	struct iio_dev *indio_dev;
+	int result;
+
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+		result = -ENOSYS;
+		pr_err("I2c function error\n");
+		goto out_no_free;
+	}
+#ifdef INV_KERNEL_3_10
+    indio_dev = iio_device_alloc(sizeof(*st));
+#else
+	indio_dev = iio_allocate_device(sizeof(*st));
+#endif
+	if (indio_dev == NULL) {
+		pr_err("memory allocation failed\n");
+		result =  -ENOMEM;
+		goto out_no_free;
+	}
+	st = iio_priv(indio_dev);
+	st->client = client;
+	st->sl_handle = client->adapter;
+	st->i2c_addr = client->addr;
+	if (client->dev.of_node) {
+		result = of_inv_parse_platform_data(client, &st->plat_data);
+		if (result)
+			goto out_free;
+		st->plat_data = mpu_data;
+		pr_info("secondary_i2c_addr=%x\n", st->plat_data.secondary_i2c_addr);
+	} else
+		st->plat_data =
+			*(struct mpu_platform_data *)dev_get_platdata(&client->dev);
+	/* power is turned on inside check chip type*/
+	result = inv_check_chip_type(st, id->name);
+	if (result)
+		goto out_free;
+
+	result = st->init_config(indio_dev);
+	if (result) {
+		dev_err(&client->adapter->dev,
+			"Could not initialize device.\n");
+		goto out_free;
+	}
+	result = st->set_power_state(st, false);
+	if (result) {
+		dev_err(&client->adapter->dev,
+			"%s could not be turned off.\n", st->hw->name);
+		goto out_free;
+	}
+
+	/* Make state variables available to all _show and _store functions. */
+	i2c_set_clientdata(client, indio_dev);
+	indio_dev->dev.parent = &client->dev;
+	if (!strcmp(id->name, "mpu6xxx"))
+		indio_dev->name = st->name;
+	else
+		indio_dev->name = id->name;
+
+	inv_set_iio_info(st, indio_dev);
+
+	result = inv_mpu_configure_ring(indio_dev);
+	if (result) {
+		pr_err("configure ring buffer fail\n");
+		goto out_free;
+	}
+	st->irq = client->irq;
+	result = inv_mpu_probe_trigger(indio_dev);
+	if (result) {
+		pr_err("trigger probe fail\n");
+		goto out_unreg_ring;
+	}
+
+	/* Tell the i2c counter, we have an IRQ */
+	INV_I2C_SETIRQ(IRQ_MPU, client->irq);
+
+	result = iio_device_register(indio_dev);
+	if (result) {
+		pr_err("IIO device register fail\n");
+		goto out_remove_trigger;
+	}
+
+	if (INV_MPU6050 == st->chip_type ||
+	    INV_MPU6500 == st->chip_type) {
+		result = inv_create_dmp_sysfs(indio_dev);
+		if (result) {
+			pr_err("create dmp sysfs failed\n");
+			goto out_unreg_iio;
+		}
+	}
+
+	INIT_KFIFO(st->timestamps);
+	spin_lock_init(&st->time_stamp_lock);
+	dev_info(&client->dev, "%s is ready to go!\n",
+					indio_dev->name);
+
+	return 0;
+out_unreg_iio:
+	iio_device_unregister(indio_dev);
+out_remove_trigger:
+	if (indio_dev->modes & INDIO_BUFFER_TRIGGERED)
+		inv_mpu_remove_trigger(indio_dev);
+out_unreg_ring:
+	inv_mpu_unconfigure_ring(indio_dev);
+out_free:
+#ifdef INV_KERNEL_3_10
+    iio_device_free(indio_dev);
+#else
+	iio_free_device(indio_dev);
+#endif
+out_no_free:
+	dev_err(&client->adapter->dev, "%s failed %d\n", __func__, result);
+
+	return -EIO;
+}
+
+static void inv_mpu_shutdown(struct i2c_client *client)
+{
+	struct iio_dev *indio_dev = i2c_get_clientdata(client);
+	struct inv_mpu_iio_s *st = iio_priv(indio_dev);
+	struct inv_reg_map_s *reg;
+	int result;
+
+	reg = &st->reg;
+	dev_dbg(&client->adapter->dev, "Shutting down %s...\n", st->hw->name);
+
+	/* reset to make sure previous state are not there */
+	result = inv_plat_single_write(st, reg->pwr_mgmt_1, BIT_H_RESET);
+	if (result)
+		dev_err(&client->adapter->dev, "Failed to reset %s\n",
+			st->hw->name);
+	msleep(POWER_UP_TIME);
+	/* turn off power to ensure gyro engine is off */
+	result = st->set_power_state(st, false);
+	if (result)
+		dev_err(&client->adapter->dev, "Failed to turn off %s\n",
+			st->hw->name);
+}
+
+/**
+ *  inv_mpu_remove() - remove function.
+ */
+static int inv_mpu_remove(struct i2c_client *client)
+{
+	struct iio_dev *indio_dev = i2c_get_clientdata(client);
+	struct inv_mpu_iio_s *st = iio_priv(indio_dev);
+	kfifo_free(&st->timestamps);
+	iio_device_unregister(indio_dev);
+	if (indio_dev->modes & INDIO_BUFFER_TRIGGERED)
+		inv_mpu_remove_trigger(indio_dev);
+	inv_mpu_unconfigure_ring(indio_dev);
+#ifdef INV_KERNEL_3_10
+    iio_device_free(indio_dev);
+#else
+	iio_free_device(indio_dev);
+#endif
+
+	dev_info(&client->adapter->dev, "inv-mpu-iio module removed.\n");
+
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int inv_mpu_resume(struct device *dev)
+{
+	struct inv_mpu_iio_s *st =
+			iio_priv(i2c_get_clientdata(to_i2c_client(dev)));
+	pr_debug("%s inv_mpu_resume\n", st->hw->name);
+	return st->set_power_state(st, true);
+}
+
+static int inv_mpu_suspend(struct device *dev)
+{
+	struct iio_dev *indio_dev = i2c_get_clientdata(to_i2c_client(dev));
+	struct inv_mpu_iio_s *st = iio_priv(indio_dev);
+	int result;
+
+	pr_debug("%s inv_mpu_suspend\n", st->hw->name);
+	mutex_lock(&indio_dev->mlock);
+	result = 0;
+	if ((!st->chip_config.dmp_on) ||
+		(!st->chip_config.enable) ||
+		(!st->chip_config.dmp_event_int_on))
+		result = st->set_power_state(st, false);
+	mutex_unlock(&indio_dev->mlock);
+
+	return result;
+}
+static const struct dev_pm_ops inv_mpu_pmops = {
+	SET_SYSTEM_SLEEP_PM_OPS(inv_mpu_suspend, inv_mpu_resume)
+};
+#define INV_MPU_PMOPS (&inv_mpu_pmops)
+#else
+#define INV_MPU_PMOPS NULL
+#endif /* CONFIG_PM */
+
+static const u16 normal_i2c[] = { I2C_CLIENT_END };
+/* device id table is used to identify what device can be
+ * supported by this driver
+ */
+static const struct i2c_device_id inv_mpu_id[] = {
+	{"itg3500", INV_ITG3500},
+	{"mpu3050", INV_MPU3050},
+	{"mpu6050", INV_MPU6050},
+	{"mpu9150", INV_MPU9150},
+	{"mpu6500", INV_MPU6500},
+	{"mpu9250", INV_MPU9250},
+	{"mpu6xxx", INV_MPU6XXX},
+	{"mpu6515", INV_MPU6515},
+	{}
+};
+
+MODULE_DEVICE_TABLE(i2c, inv_mpu_id);
+
+static const struct of_device_id inv_mpu_of_match[] = {
+	{ .compatible = "invensense,itg3500", },
+	{ .compatible = "invensense,mpu3050", },
+	{ .compatible = "invensense,mpu6050", },
+	{ .compatible = "invensense,mpu9150", },
+	{ .compatible = "invensense,mpu6500", },
+	{ .compatible = "invensense,mpu9250", },
+	{ .compatible = "invensense,mpu6xxx", },
+	{ .compatible = "invensense,mpu9350", },
+	{ .compatible = "invensense,mpu6515", },
+	{}
+};
+
+MODULE_DEVICE_TABLE(of, inv_mpu_of_match);
+
+static struct i2c_driver inv_mpu_driver = {
+	.class = I2C_CLASS_HWMON,
+	.probe		=	inv_mpu_probe,
+	.remove		=	inv_mpu_remove,
+	.shutdown	=	inv_mpu_shutdown,
+	.id_table	=	inv_mpu_id,
+	.driver = {
+		.owner	=	THIS_MODULE,
+		.name	=	"inv-mpu-iio",
+		.pm     =       INV_MPU_PMOPS,
+		.of_match_table = of_match_ptr(inv_mpu_of_match),
+	},
+	.address_list = normal_i2c,
+};
+
+static int __init inv_mpu_init(void)
+{
+	int result = i2c_add_driver(&inv_mpu_driver);
+    pr_info("%s:%d\n", __func__, __LINE__);
+	if (result) {
+		pr_err("failed\n");
+		return result;
+	}
+	return 0;
+}
+
+static void __exit inv_mpu_exit(void)
+{
+	i2c_del_driver(&inv_mpu_driver);
+}
+
+module_init(inv_mpu_init);
+module_exit(inv_mpu_exit);
+
+MODULE_AUTHOR("Invensense Corporation");
+MODULE_DESCRIPTION("Invensense device driver");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("inv-mpu-iio");
+
+/**
+ *  @}
+ */
diff --git a/drivers/staging/iio/imu/inv_mpu/inv_mpu_iio.h b/drivers/staging/iio/imu/inv_mpu/inv_mpu_iio.h
index 18b32e8fe8ad..13544818a705 100644
--- a/drivers/staging/iio/imu/inv_mpu/inv_mpu_iio.h
+++ b/drivers/staging/iio/imu/inv_mpu/inv_mpu_iio.h
@@ -868,10 +868,13 @@ int inv_set_tap_threshold_dmp(struct inv_mpu_iio_s *st,
 int inv_set_min_taps_dmp(struct inv_mpu_iio_s *st, u16 min_taps);
 int  inv_set_tap_time_dmp(struct inv_mpu_iio_s *st, u16 time);
 int inv_enable_tap_dmp(struct inv_mpu_iio_s *st, bool on);
-int inv_i2c_read_base(struct inv_mpu_iio_s *st, u16 i2c_addr,
-	u8 reg, u16 length, u8 *data);
-int inv_i2c_single_write_base(struct inv_mpu_iio_s *st,
-	u16 i2c_addr, u8 reg, u8 data);
+int inv_plat_read(struct inv_mpu_iio_s *st, u8 reg, int len, u8 *data);
+int inv_plat_single_write(struct inv_mpu_iio_s *st, u8 reg, u8 data);
+int inv_secondary_read(struct inv_mpu_iio_s *st, u8 reg, int len, u8 *data);
+int inv_secondary_write(struct inv_mpu_iio_s *st, u8 reg, u8 data);
+int inv_check_chip_type(struct inv_mpu_iio_s *st, const char *name);
+int inv_create_dmp_sysfs(struct iio_dev *ind);
+void inv_set_iio_info(struct inv_mpu_iio_s *st, struct iio_dev *indio_dev);
 int inv_do_test(struct inv_mpu_iio_s *st, int self_test_flag,
 		int *gyro_result, int *accl_result);
 int inv_hw_self_test(struct inv_mpu_iio_s *st);
@@ -896,15 +899,5 @@ char *wr_pr_debug_end(char *string);
 #define mem_w(a, b, c) \
 	mpu_memory_write(st, st->i2c_addr, a, b, c)
 #define mem_w_key(key, b, c) mpu_memory_write_unaligned(st, key, b, c)
-#define inv_i2c_read(st, reg, len, data) \
-	inv_i2c_read_base(st, st->i2c_addr, reg, len, data)
-#define inv_i2c_single_write(st, reg, data) \
-	inv_i2c_single_write_base(st, st->i2c_addr, reg, data)
-#define inv_secondary_read(reg, len, data) \
-	inv_i2c_read_base(st, st->plat_data.secondary_i2c_addr, reg, len, data)
-#define inv_secondary_write(reg, data) \
-	inv_i2c_single_write_base(st, st->plat_data.secondary_i2c_addr, \
-		reg, data)
-
 #endif  /* #ifndef _INV_MPU_IIO_H_ */
 
diff --git a/drivers/staging/iio/imu/inv_mpu/inv_mpu_misc.c b/drivers/staging/iio/imu/inv_mpu/inv_mpu_misc.c
index 0774e185b29d..02e2070ffce2 100644
--- a/drivers/staging/iio/imu/inv_mpu/inv_mpu_misc.c
+++ b/drivers/staging/iio/imu/inv_mpu/inv_mpu_misc.c
@@ -293,166 +293,6 @@ char *wr_pr_debug_end(char *string)
 	return "";
 }
 
-int mpu_memory_write(struct inv_mpu_iio_s *st, u8 mpu_addr, u16 mem_addr,
-		     u32 len, u8 const *data)
-{
-	u8 bank[2];
-	u8 addr[2];
-	u8 buf[513];
-
-	struct i2c_msg msg;
-	int res;
-
-	if (!data || !st)
-		return -EINVAL;
-
-	if (len >= (sizeof(buf) - 1))
-		return -ENOMEM;
-
-	bank[0] = REG_BANK_SEL;
-	bank[1] = mem_addr >> 8;
-
-	addr[0] = REG_MEM_START_ADDR;
-	addr[1] = mem_addr & 0xFF;
-
-	buf[0] = REG_MEM_RW;
-	memcpy(buf + 1, data, len);
-
-	/* write message */
-	msg.addr = mpu_addr;
-	msg.flags = 0;
-	msg.buf = bank;
-	msg.len = sizeof(bank);
-	/* msg.scl_rate = 200*1000; */
-	INV_I2C_INC_MPUWRITE(3);
-	res = i2c_transfer(st->sl_handle, &msg, 1);
-	if (res < 1) {
-		if (res >= 0)
-			res = -EIO;
-		return res;
-	}
-
-	msg.addr = mpu_addr;
-	msg.flags = 0;
-	msg.buf = addr;
-	msg.len = sizeof(addr);
-	/* msg.scl_rate = 200*1000; */
-	INV_I2C_INC_MPUWRITE(3);
-	res = i2c_transfer(st->sl_handle, &msg, 1);
-	if (res < 1) {
-		if (res >= 0)
-			res = -EIO;
-		return res;
-	}
-
-	msg.addr = mpu_addr;
-	msg.flags = 0;
-	msg.buf = (u8 *)buf;
-	msg.len = len + 1;
-	/* msg.scl_rate = 200*1000; */
-	INV_I2C_INC_MPUWRITE(2+len);
-	res = i2c_transfer(st->sl_handle, &msg, 1);
-	if (res < 1) {
-		if (res >= 0)
-			res = -EIO;
-		return res;
-	}
-
-	{
-		char *write = 0;
-		pr_debug("%s WM%02X%02X%02X%s%s - %d\n", st->hw->name,
-			 mpu_addr, bank[1], addr[1],
-			 wr_pr_debug_begin(data, len, write),
-			 wr_pr_debug_end(write),
-			 len);
-	}
-	return 0;
-}
-
-int mpu_memory_read(struct inv_mpu_iio_s *st, u8 mpu_addr, u16 mem_addr,
-		    u32 len, u8 *data)
-{
-	u8 bank[2];
-	u8 addr[2];
-	u8 buf;
-
-	struct i2c_msg msg;
-	int res;
-
-	if (!data || !st)
-		return -EINVAL;
-
-	bank[0] = REG_BANK_SEL;
-	bank[1] = mem_addr >> 8;
-
-	addr[0] = REG_MEM_START_ADDR;
-	addr[1] = mem_addr & 0xFF;
-
-	buf = REG_MEM_RW;
-
-	/* write message */
-	msg.addr = mpu_addr;
-	msg.flags = 0;
-	msg.buf = bank;
-	msg.len = sizeof(bank);
-	/* msg.scl_rate = 200*1000; */
-	INV_I2C_INC_MPUWRITE(3);
-	res = i2c_transfer(st->sl_handle, &msg, 1);
-	if (res < 1) {
-		if (res >= 0)
-			res = -EIO;
-		return res;
-	}
-
-	msg.addr = mpu_addr;
-	msg.flags = 0;
-	msg.buf = addr;
-	msg.len = sizeof(addr);
-	/* msg.scl_rate = 200*1000; */
-	INV_I2C_INC_MPUWRITE(3);
-	res = i2c_transfer(st->sl_handle, &msg, 1);
-	if (res < 1) {
-		if (res >= 0)
-			res = -EIO;
-		return res;
-	}
-
-	msg.addr = mpu_addr;
-	msg.flags = 0;
-	msg.buf = &buf;
-	msg.len = 1;
-	/* msg.scl_rate = 200*1000; */
-	INV_I2C_INC_MPUWRITE(3);
-	res = i2c_transfer(st->sl_handle, &msg, 1);
-	if (res < 1) {
-		if (res >= 0)
-			res = -EIO;
-		return res;
-	}
-
-	msg.addr = mpu_addr;
-	msg.flags = I2C_M_RD;
-	msg.buf = data;
-	msg.len = len;
-	/* msg.scl_rate = 200*1000; */
-	INV_I2C_INC_MPUREAD(len);
-	res = i2c_transfer(st->sl_handle, &msg, 1);
-	if (res < 1) {
-		if (res >= 0)
-			res = -EIO;
-		return res;
-	}
-	{
-		char *read = 0;
-		pr_debug("%s RM%02X%02X%02X%02X - %s%s\n", st->hw->name,
-			 mpu_addr, bank[1], addr[1], len,
-			 wr_pr_debug_begin(data, len, read),
-			 wr_pr_debug_end(read));
-	}
-
-	return 0;
-}
-
 int mpu_memory_write_unaligned(struct inv_mpu_iio_s *st, u16 key, int len,
 								u8 const *d)
 {
@@ -500,7 +340,7 @@ int inv_get_silicon_rev_mpu6500(struct inv_mpu_iio_s *st)
 	int result;
 	u8 whoami, sw_rev;
 
-	result = inv_i2c_read(st, REG_WHOAMI, 1, &whoami);
+	result = inv_plat_read(st, REG_WHOAMI, 1, &whoami);
 	if (result)
 		return result;
 	if (whoami != MPU6500_ID && whoami != MPU9250_ID &&
@@ -545,7 +385,7 @@ int inv_get_silicon_rev_mpu6050(struct inv_mpu_iio_s *st)
 	struct inv_chip_info_s *chip_info = &st->chip_info;
 	reg = &st->reg;
 
-	result = inv_i2c_read(st, REG_PRODUCT_ID, 1, &prod_ver);
+	result = inv_plat_read(st, REG_PRODUCT_ID, 1, &prod_ver);
 	if (result)
 		return result;
 	prod_ver &= 0xf;
@@ -557,11 +397,11 @@ int inv_get_silicon_rev_mpu6050(struct inv_mpu_iio_s *st)
 		return result;
 	prod_rev >>= 2;
 	/* clean the prefetch and cfg user bank bits */
-	result = inv_i2c_single_write(st, reg->bank_sel, 0);
+	result = inv_plat_single_write(st, reg->bank_sel, 0);
 	if (result)
 		return result;
 	/* get the software-product version, read from XA_OFFS_L */
-	result = inv_i2c_read(st, REG_XA_OFFS_L_TC,
+	result = inv_plat_read(st, REG_XA_OFFS_L_TC,
 				SOFT_PROD_VER_BYTES, regs);
 	if (result)
 		return result;
@@ -622,7 +462,7 @@ static int read_accel_hw_self_test_prod_shift(struct inv_mpu_iio_s *st,
 	for (i = 0; i < 3; i++)
 		st_prod[i] = 0;
 
-	result = inv_i2c_read(st, REG_ST_GCT_X, ARRAY_SIZE(regs), regs);
+	result = inv_plat_read(st, REG_ST_GCT_X, ARRAY_SIZE(regs), regs);
 
 	if (result)
 		return result;
@@ -724,7 +564,7 @@ static int inv_check_3500_gyro_self_test(struct inv_mpu_iio_s *st,
 
 	for (i = 0; i < 3; i++)
 		gst[i] = st_avg[i] - reg_avg[i];
-	result = inv_i2c_read(st, REG_3500_OTP, THREE_AXIS, st_code);
+	result = inv_plat_read(st, REG_3500_OTP, THREE_AXIS, st_code);
 	if (result)
 		return result;
 	gst_otp[0] = 0;
@@ -779,7 +619,7 @@ static int inv_check_6050_gyro_self_test(struct inv_mpu_iio_s *st,
 		return 0;
 
 	ret_val = 0;
-	result = inv_i2c_read(st, REG_ST_GCT_X, 3, regs);
+	result = inv_plat_read(st, REG_ST_GCT_X, 3, regs);
 	if (result)
 		return result;
 	regs[X] &= 0x1f;
@@ -836,7 +676,7 @@ static int inv_check_6500_gyro_self_test(struct inv_mpu_iio_s *st,
 	int st_shift_prod[3], st_shift_cust[3], i;
 
 	ret_val = 0;
-	result = inv_i2c_read(st, REG_6500_XG_ST_DATA, 3, regs);
+	result = inv_plat_read(st, REG_6500_XG_ST_DATA, 3, regs);
 	if (result)
 		return result;
 	pr_debug("%s self_test gyro shift_code - %02x %02x %02x\n",
@@ -906,7 +746,7 @@ static int inv_check_6500_accel_self_test(struct inv_mpu_iio_s *st,
 				 / DEF_ST_6500_ACCEL_FS_MG) * DEF_ST_PRECISION)
 
 	ret_val = 0;
-	result = inv_i2c_read(st, REG_6500_XA_ST_DATA, 3, regs);
+	result = inv_plat_read(st, REG_6500_XA_ST_DATA, 3, regs);
 	if (result)
 		return result;
 	pr_debug("%s self_test accel shift_code - %02x %02x %02x\n",
@@ -968,42 +808,42 @@ int inv_do_test(struct inv_mpu_iio_s *st, int self_test_flag,
 	else
 		packet_size = BYTES_PER_SENSOR;
 
-	result = inv_i2c_single_write(st, reg->int_enable, 0);
+	result = inv_plat_single_write(st, reg->int_enable, 0);
 	if (result)
 		return result;
 	/* disable the sensor output to FIFO */
-	result = inv_i2c_single_write(st, reg->fifo_en, 0);
+	result = inv_plat_single_write(st, reg->fifo_en, 0);
 	if (result)
 		return result;
 	/* disable fifo reading */
-	result = inv_i2c_single_write(st, reg->user_ctrl, 0);
+	result = inv_plat_single_write(st, reg->user_ctrl, 0);
 	if (result)
 		return result;
 	/* clear FIFO */
-	result = inv_i2c_single_write(st, reg->user_ctrl, BIT_FIFO_RST);
+	result = inv_plat_single_write(st, reg->user_ctrl, BIT_FIFO_RST);
 	if (result)
 		return result;
 	/* setup parameters */
-	result = inv_i2c_single_write(st, reg->lpf, INV_FILTER_98HZ);
+	result = inv_plat_single_write(st, reg->lpf, INV_FILTER_98HZ);
 	if (result)
 		return result;
 
 	if (INV_MPU6500 == st->chip_type) {
 		/* config accel LPF register for MPU6500 */
-		result = inv_i2c_single_write(st, REG_6500_ACCEL_CONFIG2,
+		result = inv_plat_single_write(st, REG_6500_ACCEL_CONFIG2,
 						DEF_ST_MPU6500_ACCEL_LPF |
 						BIT_FIFO_SIZE_1K);
 		if (result)
 			return result;
 	}
 
-	result = inv_i2c_single_write(st, reg->sample_rate_div,
+	result = inv_plat_single_write(st, reg->sample_rate_div,
 			DEF_SELFTEST_SAMPLE_RATE);
 	if (result)
 		return result;
 	/* wait for the sampling rate change to stabilize */
 	mdelay(INV_MPU_SAMPLE_RATE_CHANGE_STABLE);
-	result = inv_i2c_single_write(st, reg->gyro_config,
+	result = inv_plat_single_write(st, reg->gyro_config,
 		self_test_flag | DEF_SELFTEST_GYRO_FS);
 	if (result)
 		return result;
@@ -1013,7 +853,7 @@ int inv_do_test(struct inv_mpu_iio_s *st, int self_test_flag,
 		else
 			d = DEF_SELFTEST_ACCEL_FS;
 		d |= self_test_flag;
-		result = inv_i2c_single_write(st, reg->accl_config, d);
+		result = inv_plat_single_write(st, reg->accl_config, d);
 		if (result)
 			return result;
 	}
@@ -1026,7 +866,7 @@ int inv_do_test(struct inv_mpu_iio_s *st, int self_test_flag,
 	}
 
 	/* enable FIFO reading */
-	result = inv_i2c_single_write(st, reg->user_ctrl, BIT_FIFO_EN);
+	result = inv_plat_single_write(st, reg->user_ctrl, BIT_FIFO_EN);
 	if (result)
 		return result;
 	/* enable sensor output to FIFO */
@@ -1040,15 +880,15 @@ int inv_do_test(struct inv_mpu_iio_s *st, int self_test_flag,
 	}
 	s = 0;
 	while (s < st->self_test.samples) {
-		result = inv_i2c_single_write(st, reg->fifo_en, d);
+		result = inv_plat_single_write(st, reg->fifo_en, d);
 		if (result)
 			return result;
 		mdelay(DEF_GYRO_WAIT_TIME);
-		result = inv_i2c_single_write(st, reg->fifo_en, 0);
+		result = inv_plat_single_write(st, reg->fifo_en, 0);
 		if (result)
 			return result;
 
-		result = inv_i2c_read(st, reg->fifo_count_h,
+		result = inv_plat_read(st, reg->fifo_count_h,
 					FIFO_COUNT_BYTE, data);
 		if (result)
 			return result;
@@ -1059,7 +899,7 @@ int inv_do_test(struct inv_mpu_iio_s *st, int self_test_flag,
 		i = 0;
 		while ((i < packet_count) && (s < st->self_test.samples)) {
 			short vals[3];
-			result = inv_i2c_read(st, reg->fifo_r_w,
+			result = inv_plat_read(st, reg->fifo_r_w,
 				packet_size, data);
 			if (result)
 				return result;
@@ -1112,13 +952,13 @@ void inv_recover_setting(struct inv_mpu_iio_s *st)
 	int data;
 
 	reg = &st->reg;
-	inv_i2c_single_write(st, reg->gyro_config,
+	inv_plat_single_write(st, reg->gyro_config,
 			     st->chip_config.fsr << GYRO_CONFIG_FSR_SHIFT);
-	inv_i2c_single_write(st, reg->lpf, st->chip_config.lpf);
+	inv_plat_single_write(st, reg->lpf, st->chip_config.lpf);
 	data = ONE_K_HZ/st->chip_config.new_fifo_rate - 1;
-	inv_i2c_single_write(st, reg->sample_rate_div, data);
+	inv_plat_single_write(st, reg->sample_rate_div, data);
 	if (INV_ITG3500 != st->chip_type) {
-		inv_i2c_single_write(st, reg->accl_config,
+		inv_plat_single_write(st, reg->accl_config,
 				     (st->chip_config.accl_fs <<
 				     ACCL_CONFIG_FSR_SHIFT));
 	}
@@ -1137,30 +977,30 @@ static int inv_check_compass_self_test(struct inv_mpu_iio_s *st)
 	sens = st->chip_info.compass_sens;
 
 	/* set to bypass mode */
-	result = inv_i2c_single_write(st, REG_INT_PIN_CFG,
+	result = inv_plat_single_write(st, REG_INT_PIN_CFG,
 				st->plat_data.int_config | BIT_BYPASS_EN);
 	if (result) {
-		result = inv_i2c_single_write(st, REG_INT_PIN_CFG,
+		result = inv_plat_single_write(st, REG_INT_PIN_CFG,
 				st->plat_data.int_config);
 		return result;
 	}
 	/* set to power down mode */
-	result = inv_secondary_write(REG_AKM_MODE, DATA_AKM_MODE_PD);
+	result = inv_secondary_write(st, REG_AKM_MODE, DATA_AKM_MODE_PD);
 	if (result)
 		goto AKM_fail;
 
 	/* write 1 to ASTC register */
-	result = inv_secondary_write(REG_AKM_ST_CTRL, DATA_AKM_SELF_TEST);
+	result = inv_secondary_write(st, REG_AKM_ST_CTRL, DATA_AKM_SELF_TEST);
 	if (result)
 		goto AKM_fail;
 	/* set self test mode */
-	result = inv_secondary_write(REG_AKM_MODE, DATA_AKM_MODE_ST);
+	result = inv_secondary_write(st, REG_AKM_MODE, DATA_AKM_MODE_ST);
 	if (result)
 		goto AKM_fail;
 	counter = DEF_ST_COMPASS_TRY_TIMES;
 	while (counter > 0) {
 		usleep_range(DEF_ST_COMPASS_WAIT_MIN, DEF_ST_COMPASS_WAIT_MAX);
-		result = inv_secondary_read(REG_AKM_STATUS, 1, data);
+		result = inv_secondary_read(st, REG_AKM_STATUS, 1, data);
 		if (result)
 			goto AKM_fail;
 		if ((data[0] & DATA_AKM_DRDY) == 0)
@@ -1172,7 +1012,7 @@ static int inv_check_compass_self_test(struct inv_mpu_iio_s *st)
 		result = -EINVAL;
 		goto AKM_fail;
 	}
-	result = inv_secondary_read(REG_AKM_MEASURE_DATA,
+	result = inv_secondary_read(st, REG_AKM_MEASURE_DATA,
 					BYTES_PER_SENSOR, data);
 	if (result)
 		goto AKM_fail;
@@ -1184,7 +1024,7 @@ static int inv_check_compass_self_test(struct inv_mpu_iio_s *st)
 	y = ((y * (sens[1] + 128)) >> 8);
 	z = ((z * (sens[2] + 128)) >> 8);
 	if (COMPASS_ID_AK8963 == st->plat_data.sec_slave_id) {
-		result = inv_secondary_read(REG_AKM8963_CNTL1, 1, &cntl);
+		result = inv_secondary_read(st, REG_AKM8963_CNTL1, 1, &cntl);
 		if (result)
 			goto AKM_fail;
 		if (0 == (cntl & DATA_AKM8963_BIT)) {
@@ -1203,11 +1043,11 @@ static int inv_check_compass_self_test(struct inv_mpu_iio_s *st)
 	result = 0;
 AKM_fail:
 	/*write 0 to ASTC register */
-	result |= inv_secondary_write(REG_AKM_ST_CTRL, 0);
+	result |= inv_secondary_write(st, REG_AKM_ST_CTRL, 0);
 	/*set to power down mode */
-	result |= inv_secondary_write(REG_AKM_MODE, DATA_AKM_MODE_PD);
+	result |= inv_secondary_write(st, REG_AKM_MODE, DATA_AKM_MODE_PD);
 	/*restore to non-bypass mode */
-	result |= inv_i2c_single_write(st, REG_INT_PIN_CFG,
+	result |= inv_plat_single_write(st, REG_INT_PIN_CFG,
 			st->plat_data.int_config);
 	return result;
 }
@@ -2078,11 +1918,11 @@ ssize_t inv_dmp_firmware_write(struct file *fp, struct kobject *kobj,
 	if (result)
 		goto firmware_write_fail;
 
-	result = inv_i2c_single_write(st, reg->prgm_strt_addrh,
+	result = inv_plat_single_write(st, reg->prgm_strt_addrh,
 	st->chip_config.prog_start_addr >> 8);
 	if (result)
 		goto firmware_write_fail;
-	result = inv_i2c_single_write(st, reg->prgm_strt_addrh + 1,
+	result = inv_plat_single_write(st, reg->prgm_strt_addrh + 1,
 	st->chip_config.prog_start_addr & 0xff);
 	if (result)
 		goto firmware_write_fail;
diff --git a/drivers/staging/iio/imu/inv_mpu/inv_mpu_ring.c b/drivers/staging/iio/imu/inv_mpu/inv_mpu_ring.c
index 82d4d87bba11..624b0fb62fa0 100644
--- a/drivers/staging/iio/imu/inv_mpu/inv_mpu_ring.c
+++ b/drivers/staging/iio/imu/inv_mpu/inv_mpu_ring.c
@@ -64,15 +64,15 @@ static int reset_fifo_mpu3050(struct iio_dev *indio_dev)
 	reg = &st->reg;
 
 	/* disable interrupt */
-	result = inv_i2c_single_write(st, reg->int_enable,
+	result = inv_plat_single_write(st, reg->int_enable,
 				st->plat_data.int_config);
 	if (result)
 		return result;
 	/* disable the sensor output to FIFO */
-	result = inv_i2c_single_write(st, reg->fifo_en, 0);
+	result = inv_plat_single_write(st, reg->fifo_en, 0);
 	if (result)
 		goto reset_fifo_fail;
-	result = inv_i2c_read(st, reg->user_ctrl, 1, &user_ctrl);
+	result = inv_plat_read(st, reg->user_ctrl, 1, &user_ctrl);
 	if (result)
 		goto reset_fifo_fail;
 	/* disable fifo reading */
@@ -80,18 +80,18 @@ static int reset_fifo_mpu3050(struct iio_dev *indio_dev)
 	st->chip_config.has_footer = 0;
 	/* reset fifo */
 	val = (BIT_3050_FIFO_RST | user_ctrl);
-	result = inv_i2c_single_write(st, reg->user_ctrl, val);
+	result = inv_plat_single_write(st, reg->user_ctrl, val);
 	if (result)
 		goto reset_fifo_fail;
 	st->last_isr_time = get_time_ns();
 	if (st->chip_config.dmp_on) {
 		/* enable interrupt when DMP is done */
-		result = inv_i2c_single_write(st, reg->int_enable,
+		result = inv_plat_single_write(st, reg->int_enable,
 				st->plat_data.int_config | BIT_DMP_INT_EN);
 		if (result)
 			return result;
 
-		result = inv_i2c_single_write(st, reg->user_ctrl,
+		result = inv_plat_single_write(st, reg->user_ctrl,
 			BIT_FIFO_EN|user_ctrl);
 		if (result)
 			return result;
@@ -99,13 +99,13 @@ static int reset_fifo_mpu3050(struct iio_dev *indio_dev)
 		/* enable interrupt */
 		if (st->chip_config.accl_fifo_enable ||
 		    st->chip_config.gyro_fifo_enable) {
-			result = inv_i2c_single_write(st, reg->int_enable,
+			result = inv_plat_single_write(st, reg->int_enable,
 				st->plat_data.int_config | BIT_DATA_RDY_EN);
 			if (result)
 				return result;
 		}
 		/* enable FIFO reading and I2C master interface*/
-		result = inv_i2c_single_write(st, reg->user_ctrl,
+		result = inv_plat_single_write(st, reg->user_ctrl,
 			BIT_FIFO_EN | user_ctrl);
 		if (result)
 			return result;
@@ -115,7 +115,7 @@ static int reset_fifo_mpu3050(struct iio_dev *indio_dev)
 			val |= BITS_3050_ACCL_OUT;
 		if (st->chip_config.gyro_fifo_enable)
 			val |= BITS_GYRO_OUT;
-		result = inv_i2c_single_write(st, reg->fifo_en, val);
+		result = inv_plat_single_write(st, reg->fifo_en, val);
 		if (result)
 			return result;
 	}
@@ -126,7 +126,7 @@ reset_fifo_fail:
 		val = BIT_DMP_INT_EN;
 	else
 		val = BIT_DATA_RDY_EN;
-	inv_i2c_single_write(st, reg->int_enable,
+	inv_plat_single_write(st, reg->int_enable,
 			     st->plat_data.int_config | val);
 	pr_err("reset fifo failed\n");
 
@@ -156,10 +156,10 @@ static int inv_set_lpf(struct inv_mpu_iio_s *st, int rate)
 			if (result)
 				return result;
 		}
-		result = inv_i2c_single_write(st, reg->lpf, data |
+		result = inv_plat_single_write(st, reg->lpf, data |
 			(st->chip_config.fsr << GYRO_CONFIG_FSR_SHIFT));
 	} else {
-		result = inv_i2c_single_write(st, reg->lpf, data);
+		result = inv_plat_single_write(st, reg->lpf, data);
 	}
 	if (result)
 		return result;
@@ -181,7 +181,7 @@ static int set_fifo_rate_reg(struct inv_mpu_iio_s *st)
 	reg = &st->reg;
 	fifo_rate = st->chip_config.new_fifo_rate;
 	data = ONE_K_HZ / fifo_rate - 1;
-	result = inv_i2c_single_write(st, reg->sample_rate_div, data);
+	result = inv_plat_single_write(st, reg->sample_rate_div, data);
 	if (result)
 		return result;
 	result = inv_set_lpf(st, fifo_rate);
@@ -202,7 +202,7 @@ static int inv_lpa_mode(struct inv_mpu_iio_s *st, int lpa_mode)
 	struct inv_reg_map_s *reg;
 
 	reg = &st->reg;
-	result = inv_i2c_read(st, reg->pwr_mgmt_1, 1, &d);
+	result = inv_plat_read(st, reg->pwr_mgmt_1, 1, &d);
 	if (result)
 		return result;
 	if (lpa_mode)
@@ -210,7 +210,7 @@ static int inv_lpa_mode(struct inv_mpu_iio_s *st, int lpa_mode)
 	else
 		d &= ~BIT_CYCLE;
 
-	result = inv_i2c_single_write(st, reg->pwr_mgmt_1, d);
+	result = inv_plat_single_write(st, reg->pwr_mgmt_1, d);
 	if (result)
 		return result;
 	if (INV_MPU6500 == st->chip_type) {
@@ -218,7 +218,7 @@ static int inv_lpa_mode(struct inv_mpu_iio_s *st, int lpa_mode)
 			d = BIT_ACCEL_FCHOCIE_B;
 		else
 			d = 0;
-		result = inv_i2c_single_write(st, REG_6500_ACCEL_CONFIG2, d);
+		result = inv_plat_single_write(st, REG_6500_ACCEL_CONFIG2, d);
 		if (result)
 			return result;
 	}
@@ -246,17 +246,17 @@ static int reset_fifo_itg(struct iio_dev *indio_dev)
 		}
 	}
 	/* disable interrupt */
-	result = inv_i2c_single_write(st, reg->int_enable, 0);
+	result = inv_plat_single_write(st, reg->int_enable, 0);
 	if (result) {
 		pr_err("int_enable write failed\n");
 		return result;
 	}
 	/* disable the sensor output to FIFO */
-	result = inv_i2c_single_write(st, reg->fifo_en, 0);
+	result = inv_plat_single_write(st, reg->fifo_en, 0);
 	if (result)
 		goto reset_fifo_fail;
 	/* disable fifo reading */
-	result = inv_i2c_single_write(st, reg->user_ctrl, 0);
+	result = inv_plat_single_write(st, reg->user_ctrl, 0);
 	if (result)
 		goto reset_fifo_fail;
 	int_word = 0;
@@ -267,13 +267,13 @@ static int reset_fifo_itg(struct iio_dev *indio_dev)
 
 	if (st->chip_config.dmp_on) {
 		val = (BIT_FIFO_RST | BIT_DMP_RST);
-		result = inv_i2c_single_write(st, reg->user_ctrl, val);
+		result = inv_plat_single_write(st, reg->user_ctrl, val);
 		if (result)
 			goto reset_fifo_fail;
 		st->last_isr_time = get_time_ns();
 		if (st->chip_config.dmp_int_on) {
 			int_word |= BIT_DMP_INT_EN;
-			result = inv_i2c_single_write(st, reg->int_enable,
+			result = inv_plat_single_write(st, reg->int_enable,
 							int_word);
 			if (result)
 				return result;
@@ -282,7 +282,7 @@ static int reset_fifo_itg(struct iio_dev *indio_dev)
 		if (st->chip_config.compass_enable &
 			(!st->chip_config.dmp_event_int_on))
 			val |= BIT_I2C_MST_EN;
-		result = inv_i2c_single_write(st, reg->user_ctrl, val);
+		result = inv_plat_single_write(st, reg->user_ctrl, val);
 		if (result)
 			goto reset_fifo_fail;
 
@@ -295,7 +295,7 @@ static int reset_fifo_itg(struct iio_dev *indio_dev)
 				st->chip_config.dmp_output_rate);
 			if (data > 0)
 				data -= 1;
-			result = inv_i2c_single_write(st, REG_I2C_SLV4_CTRL,
+			result = inv_plat_single_write(st, REG_I2C_SLV4_CTRL,
 							data);
 			if (result)
 				return result;
@@ -315,7 +315,7 @@ static int reset_fifo_itg(struct iio_dev *indio_dev)
 	} else {
 		/* reset FIFO and possibly reset I2C*/
 		val = BIT_FIFO_RST;
-		result = inv_i2c_single_write(st, reg->user_ctrl, val);
+		result = inv_plat_single_write(st, reg->user_ctrl, val);
 		if (result)
 			goto reset_fifo_fail;
 		st->last_isr_time = get_time_ns();
@@ -325,14 +325,14 @@ static int reset_fifo_itg(struct iio_dev *indio_dev)
 		    st->chip_config.compass_enable) {
 			int_word |= BIT_DATA_RDY_EN;
 		}
-		result = inv_i2c_single_write(st, reg->int_enable, int_word);
+		result = inv_plat_single_write(st, reg->int_enable, int_word);
 		if (result)
 			return result;
 		/* enable FIFO reading and I2C master interface*/
 		val = BIT_FIFO_EN;
 		if (st->chip_config.compass_enable)
 			val |= BIT_I2C_MST_EN;
-		result = inv_i2c_single_write(st, reg->user_ctrl, val);
+		result = inv_plat_single_write(st, reg->user_ctrl, val);
 		if (result)
 			goto reset_fifo_fail;
 		if (st->chip_config.compass_enable) {
@@ -342,7 +342,7 @@ static int reset_fifo_itg(struct iio_dev *indio_dev)
 				st->chip_config.new_fifo_rate / ONE_K_HZ;
 			if (data > 0)
 				data -= 1;
-			result = inv_i2c_single_write(st, REG_I2C_SLV4_CTRL,
+			result = inv_plat_single_write(st, REG_I2C_SLV4_CTRL,
 							data);
 			if (result)
 				return result;
@@ -353,7 +353,7 @@ static int reset_fifo_itg(struct iio_dev *indio_dev)
 			val |= BITS_GYRO_OUT;
 		if (st->chip_config.accl_fifo_enable)
 			val |= BIT_ACCEL_OUT;
-		result = inv_i2c_single_write(st, reg->fifo_en, val);
+		result = inv_plat_single_write(st, reg->fifo_en, val);
 		if (result)
 			goto reset_fifo_fail;
 	}
@@ -369,7 +369,7 @@ reset_fifo_fail:
 		val = BIT_DMP_INT_EN;
 	else
 		val = BIT_DATA_RDY_EN;
-	inv_i2c_single_write(st, reg->int_enable, val);
+	inv_plat_single_write(st, reg->int_enable, val);
 	pr_err("reset fifo failed\n");
 
 	return result;
@@ -465,17 +465,17 @@ static int set_inv_enable(struct iio_dev *indio_dev,
 			if (result)
 				return result;
 		}
-		result = inv_i2c_single_write(st, reg->fifo_en, 0);
+		result = inv_plat_single_write(st, reg->fifo_en, 0);
 		if (result)
 			return result;
 		/* disable fifo reading */
 		if (INV_MPU3050 != st->chip_type) {
-			result = inv_i2c_single_write(st, reg->int_enable, 0);
+			result = inv_plat_single_write(st, reg->int_enable, 0);
 			if (result)
 				return result;
-			result = inv_i2c_single_write(st, reg->user_ctrl, 0);
+			result = inv_plat_single_write(st, reg->user_ctrl, 0);
 		} else {
-			result = inv_i2c_single_write(st, reg->int_enable,
+			result = inv_plat_single_write(st, reg->int_enable,
 				st->plat_data.int_config);
 		}
 		if (result)
@@ -628,7 +628,7 @@ irqreturn_t inv_read_fifo_mpu3050(int irq, void *dev_id)
 
 	fifo_count = 0;
 	if (byte_read != 0) {
-		result = inv_i2c_read(st, reg->fifo_count_h,
+		result = inv_plat_read(st, reg->fifo_count_h,
 				FIFO_COUNT_BYTE, data);
 		if (result)
 			goto end_session;
@@ -656,7 +656,7 @@ irqreturn_t inv_read_fifo_mpu3050(int irq, void *dev_id)
 		}
 	}
 	while ((bytes_per_datum != 0) && (fifo_count >= byte_read)) {
-		result = inv_i2c_read(st, reg->fifo_r_w, byte_read, data);
+		result = inv_plat_read(st, reg->fifo_r_w, byte_read, data);
 		if (result)
 			goto flush_fifo;
 
@@ -758,7 +758,7 @@ static int inv_report_gyro_accl_compass(struct iio_dev *indio_dev,
 			compass_divider = st->compass_divider;
 		if (compass_divider <= st->compass_counter) {
 			/*read from external sensor data register */
-			result = inv_i2c_read(st, REG_EXT_SENS_DATA_00,
+			result = inv_plat_read(st, REG_EXT_SENS_DATA_00,
 					      NUM_BYTES_COMPASS_SLAVE, d);
 			/* d[7] is status 2 register */
 			/*for AKM8975, bit 2 and 3 should be all be zero*/
@@ -818,7 +818,7 @@ static void inv_process_motion(struct inv_mpu_iio_s *st)
 	u8 data[1];
 
 	/* motion interrupt */
-	result = inv_i2c_read(st, REG_INT_STATUS, 1, data);
+	result = inv_plat_read(st, REG_INT_STATUS, 1, data);
 	if (result)
 		return;
 
@@ -891,7 +891,7 @@ irqreturn_t inv_read_fifo(int irq, void *dev_id)
 		inv_process_motion(st);
 	if (st->chip_config.dmp_on && st->chip_config.smd_enable) {
 		/* dmp interrupt status */
-		result = inv_i2c_read(st, REG_DMP_INT_STATUS, 1, data);
+		result = inv_plat_read(st, REG_DMP_INT_STATUS, 1, data);
 		if (!result)
 			if (data[0] & SMD_INT_ON) {
 				sysfs_notify(&indio_dev->dev.kobj, NULL,
@@ -900,7 +900,7 @@ irqreturn_t inv_read_fifo(int irq, void *dev_id)
 			}
 	}
 	if (st->chip_config.lpa_mode) {
-		result = inv_i2c_read(st, reg->raw_accl,
+		result = inv_plat_read(st, reg->raw_accl,
 				      BYTES_PER_SENSOR, data);
 		if (result)
 			goto end_session;
@@ -911,7 +911,7 @@ irqreturn_t inv_read_fifo(int irq, void *dev_id)
 	bytes_per_datum = get_bytes_per_datum(st);
 	fifo_count = 0;
 	if (bytes_per_datum != 0) {
-		result = inv_i2c_read(st, reg->fifo_count_h,
+		result = inv_plat_read(st, reg->fifo_count_h,
 				FIFO_COUNT_BYTE, data);
 		if (result)
 			goto end_session;
@@ -948,7 +948,7 @@ irqreturn_t inv_read_fifo(int irq, void *dev_id)
 	}
 	tmp = (s8 *)buf;
 	while ((bytes_per_datum != 0) && (fifo_count >= bytes_per_datum)) {
-		result = inv_i2c_read(st, reg->fifo_r_w, bytes_per_datum,
+		result = inv_plat_read(st, reg->fifo_r_w, bytes_per_datum,
 			data);
 		if (result)
 			goto flush_fifo;
diff --git a/drivers/staging/iio/imu/inv_mpu/inv_slave_bma250.c b/drivers/staging/iio/imu/inv_mpu/inv_slave_bma250.c
index bd84f637af52..1452f9c0b5d1 100644
--- a/drivers/staging/iio/imu/inv_mpu/inv_slave_bma250.c
+++ b/drivers/staging/iio/imu/inv_mpu/inv_slave_bma250.c
@@ -127,7 +127,7 @@ static int bma250_set_bandwidth(struct inv_mpu_iio_s *st, u8 bw)
 		return res;
 	data &= BMA250_BW_REG_MASK;
 	data |= bandwidth;
-	res = inv_secondary_write(BMA250_BW_SEL_REG, data);
+	res = inv_secondary_write(st, BMA250_BW_SEL_REG, data);
 	return res;
 }
 
@@ -156,7 +156,7 @@ static int bma250_set_range(struct inv_mpu_iio_s *st, u8 range)
 		return res;
 	orig &= BMA250_RANGE_MASK;
 	data |= orig;
-	res = inv_secondary_write(BMA250_RANGE_SEL_REG, data);
+	res = inv_secondary_write(st, BMA250_RANGE_SEL_REG, data);
 	if (res)
 		return res;
 	bma_static_property.range = range;
@@ -182,7 +182,7 @@ static int setup_slave_bma250(struct inv_mpu_iio_s *st)
 		return result;
 	/*AUX(accel), slave address is set inside set_3050_bypass*/
 	/* bma250 x axis LSB register address is 2 */
-	result = inv_i2c_single_write(st, REG_3050_AUX_BST_ADDR,
+	result = inv_plat_single_write(st, REG_3050_AUX_BST_ADDR,
 					BMA250_X_AXIS_LSB_REG);
 
 	return result;
@@ -209,7 +209,7 @@ static int bma250_set_mode(struct inv_mpu_iio_s *st, u8 mode)
 	default:
 		return -EINVAL;
 	}
-	res = inv_secondary_write(BMA250_MODE_CTRL_REG, data);
+	res = inv_secondary_write(st, BMA250_MODE_CTRL_REG, data);
 	if (res)
 		return res;
 	bma_static_property.mode = mode;