Initial file dump from open source releaseHEADmaster

1 files changed, 2078 insertions, 0 deletions

diff --git a/drivers/iio/imu/inv_mpu/inv_mpu_core.c b/drivers/iio/imu/inv_mpu/inv_mpu_core.c
new file mode 100644
index 00000000..e547ab77
--- /dev/null
+++ b/drivers/iio/imu/inv_mpu/inv_mpu_core.c
@@ -0,0 +1,2078 @@
+/*
+* 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 <linux/iio/sysfs.h>
+
+#include "inv_mpu_iio.h"
+#include "inv_counters.h"
+
+#include <linux/regulator/consumer.h>
+#include <mach/regulator.h>
+#include <linux/gpio.h>
+#include <linux/input.h>
+s64 get_time_ns(void)
+{
+	struct timespec ts;
+	ktime_get_ts(&ts);
+	return timespec_to_ns(&ts);
+}
+
+static const short AKM8975_ST_Lower[3] = {-100, -100, -1000};
+static const short AKM8975_ST_Upper[3] = {100, 100, -300};
+
+static const short AKM8972_ST_Lower[3] = {-50, -50, -500};
+static const short AKM8972_ST_Upper[3] = {50, 50, -100};
+
+static const short AKM8963_ST_Lower[3] = {-200, -200, -3200};
+static const short AKM8963_ST_Upper[3] = {200, 200, -800};
+
+static const struct inv_hw_s hw_info[INV_NUM_PARTS] = {
+	{119, "ITG3500"},
+	{ 63, "MPU3050"},
+	{117, "MPU6050"},
+	{118, "MPU9150"},
+	{119, "MPU6500"},
+	{118, "MPU9250"},
+};
+
+static void inv_setup_reg(struct inv_reg_map_s *reg)
+{
+	reg->sample_rate_div	= REG_SAMPLE_RATE_DIV;
+	reg->lpf		= REG_CONFIG;
+	reg->bank_sel		= REG_BANK_SEL;
+	reg->user_ctrl		= REG_USER_CTRL;
+	reg->fifo_en		= REG_FIFO_EN;
+	reg->gyro_config	= REG_GYRO_CONFIG;
+	reg->accl_config	= REG_ACCEL_CONFIG;
+	reg->fifo_count_h	= REG_FIFO_COUNT_H;
+	reg->fifo_r_w		= REG_FIFO_R_W;
+	reg->raw_gyro		= REG_RAW_GYRO;
+	reg->raw_accl		= REG_RAW_ACCEL;
+	reg->temperature	= REG_TEMPERATURE;
+	reg->int_enable		= REG_INT_ENABLE;
+	reg->int_status		= REG_INT_STATUS;
+	reg->pwr_mgmt_1		= REG_PWR_MGMT_1;
+	reg->pwr_mgmt_2		= REG_PWR_MGMT_2;
+	reg->mem_start_addr	= REG_MEM_START_ADDR;
+	reg->mem_r_w		= REG_MEM_RW;
+	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[1].addr = i2c_addr;
+	msgs[1].flags = I2C_M_RD;
+	msgs[1].buf = data;
+	msgs[1].len = length;
+
+	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);
+#if CONFIG_DYNAMIC_DEBUG
+	{
+		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));
+	}
+#endif
+	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;
+
+	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;
+	u8 data, mgmt_1;
+	int result;
+	reg = &st->reg;
+	/* switch clock needs to be careful. Only when gyro is on, can
+	   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);
+		if (result)
+			return result;
+
+		mgmt_1 &= ~BIT_CLK_MASK;
+	}
+
+	if ((BIT_PWR_GYRO_STBY == mask) && (!en)) {
+		/* 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,
+						mgmt_1);
+		if (result)
+			return result;
+	}
+
+	result = inv_i2c_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);
+	if (result)
+		return result;
+
+	if ((BIT_PWR_GYRO_STBY == mask) && en) {
+		/* only gyro on needs sensor up time */
+		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,
+						mgmt_1);
+		if (result)
+			return result;
+	}
+
+	return 0;
+}
+
+/**
+ *  inv_lpa_freq() - store current low power frequency setting.
+ */
+static int inv_lpa_freq(struct inv_mpu_iio_s *st, int lpa_freq)
+{
+	unsigned long result;
+	u8 d;
+	struct inv_reg_map_s *reg;
+	/* this mapping makes 6500 and 6050 setting close */
+	/* 2, 4, 6, 7 corresponds to 0.98, 3.91, 15.63, 31.25 */
+	const u8 mpu6500_lpa_mapping[] = {2, 4, 6, 7};
+
+	if (lpa_freq > MAX_LPA_FREQ_PARAM)
+		return -EINVAL;
+
+	if (INV_MPU6500 == st->chip_type) {
+		d = mpu6500_lpa_mapping[lpa_freq];
+		result = inv_i2c_single_write(st, REG_6500_LP_ACCEL_ODR, d);
+		if (result)
+			return result;
+	} else {
+		reg = &st->reg;
+		result = inv_i2c_read(st, reg->pwr_mgmt_2, 1, &d);
+		if (result)
+			return result;
+		d &= ~BIT_LPA_FREQ;
+		d |= (u8)(lpa_freq << LPA_FREQ_SHIFT);
+		result = inv_i2c_single_write(st, reg->pwr_mgmt_2, d);
+		if (result)
+			return result;
+	}
+	st->chip_config.lpa_freq = lpa_freq;
+
+	return 0;
+}
+
+static int set_power_itg(struct inv_mpu_iio_s *st, bool power_on)
+{
+	struct inv_reg_map_s *reg;
+	u8 data;
+	int result;
+
+	reg = &st->reg;
+	if (power_on)
+		data = 0;
+	else
+		data = BIT_SLEEP;
+	result = inv_i2c_single_write(st, reg->pwr_mgmt_1, data);
+	if (result)
+		return result;
+
+	if (power_on) {
+		msleep(POWER_UP_TIME);
+		result = inv_switch_engine(st, st->chip_config.gyro_enable,
+					BIT_PWR_GYRO_STBY);
+		if (result)
+			return result;
+		result = inv_switch_engine(st, st->chip_config.accl_enable,
+					BIT_PWR_ACCL_STBY);
+		if (result)
+			return result;
+		result = inv_lpa_freq(st, st->chip_config.lpa_freq);
+		if (result)
+			return result;
+	}
+	st->chip_config.is_asleep = !power_on;
+
+	return 0;
+}
+
+/**
+ *  inv_init_config() - Initialize hardware, disable FIFO.
+ *  @indio_dev:	Device driver instance.
+ *  Initial configuration:
+ *  FSR: +/- 2000DPS
+ *  DLPF: 42Hz
+ *  FIFO rate: 50Hz
+ */
+static int inv_init_config(struct iio_dev *indio_dev)
+{
+	struct inv_reg_map_s *reg;
+	int result;
+	struct inv_mpu_iio_s *st = iio_priv(indio_dev);
+
+	if (st->chip_config.is_asleep)
+		return -EPERM;
+
+	reg = &st->reg;
+	result = set_inv_enable(indio_dev, false);
+	if (result)
+		return result;
+
+	result = inv_i2c_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);
+	if (result)
+		return result;
+	st->chip_config.lpf = INV_FILTER_42HZ;
+
+	result = inv_i2c_single_write(st, reg->sample_rate_div,
+					ONE_K_HZ / INIT_FIFO_RATE - 1);
+	if (result)
+		return result;
+	st->chip_config.fifo_rate = INIT_FIFO_RATE;
+	st->irq_dur_ns            = INIT_DUR_TIME;
+	st->chip_config.prog_start_addr = DMP_START_ADDR;
+	st->chip_config.gyro_enable = 1;
+	st->chip_config.gyro_fifo_enable = 1;
+	st->chip_config.dmp_output_rate = INIT_DMP_OUTPUT_RATE;
+	if (INV_ITG3500 != st->chip_type) {
+		st->chip_config.accl_enable = 1;
+		st->chip_config.accl_fifo_enable = 1;
+		st->chip_config.accl_fs = INV_FS_02G;
+		result = inv_i2c_single_write(st, reg->accl_config,
+			(INV_FS_02G << ACCL_CONFIG_FSR_SHIFT));
+		if (result)
+			return result;
+		st->tap.time = INIT_TAP_TIME;
+		st->tap.thresh = INIT_TAP_THRESHOLD;
+		st->tap.min_count = INIT_TAP_MIN_COUNT;
+
+		result = inv_i2c_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,
+						INIT_MOT_THR);
+		if (result)
+			return result;
+		st->mot_int.mot_thr = INIT_MOT_THR;
+	}
+
+	return 0;
+}
+
+/**
+ *  inv_compass_scale_show() - show compass scale.
+ */
+static int inv_compass_scale_show(struct inv_mpu_iio_s *st, int *scale)
+{
+	if (COMPASS_ID_AK8975 == st->plat_data.sec_slave_id)
+		*scale = DATA_AKM8975_SCALE;
+	else if (COMPASS_ID_AK8972 == st->plat_data.sec_slave_id)
+		*scale = DATA_AKM8972_SCALE;
+	else if (COMPASS_ID_AK8963 == st->plat_data.sec_slave_id)
+		if (st->compass_scale)
+			*scale = DATA_AKM8963_SCALE1;
+		else
+			*scale = DATA_AKM8963_SCALE0;
+	else
+		return -EINVAL;
+
+	return IIO_VAL_INT;
+}
+
+/**
+ *  mpu_read_raw() - read raw method.
+ */
+static int mpu_read_raw(struct iio_dev *indio_dev,
+			struct iio_chan_spec const *chan,
+			int *val, int *val2, long mask)
+{
+	struct inv_mpu_iio_s  *st = iio_priv(indio_dev);
+	int result;
+	if (st->chip_config.is_asleep)
+		return -EINVAL;
+	switch (mask) {
+	case 0:
+		if (!st->chip_config.enable)
+			return -EPERM;
+		switch (chan->type) {
+		case IIO_ANGL_VEL:
+			if (!st->chip_config.gyro_enable)
+				return -EPERM;
+			*val = st->raw_gyro[chan->channel2 - IIO_MOD_X];
+			return IIO_VAL_INT;
+		case IIO_ACCEL:
+			if (!st->chip_config.accl_enable)
+				return -EPERM;
+			*val = st->raw_accel[chan->channel2 - IIO_MOD_X];
+			return IIO_VAL_INT;
+		case IIO_MAGN:
+			if (!st->chip_config.compass_enable)
+				return -EPERM;
+			*val = st->raw_compass[chan->channel2 - IIO_MOD_X];
+			return IIO_VAL_INT;
+		case IIO_QUATERNION:
+			if (!(st->chip_config.dmp_on
+				&& st->chip_config.quaternion_on))
+				return -EPERM;
+			if (IIO_MOD_R == chan->channel2)
+				*val = st->raw_quaternion[0];
+			else
+				*val = st->raw_quaternion[chan->channel2 -
+							  IIO_MOD_X + 1];
+			return IIO_VAL_INT;
+		default:
+			return -EINVAL;
+		}
+		return -EINVAL;
+	case IIO_CHAN_INFO_SCALE:
+		switch (chan->type) {
+		case IIO_ANGL_VEL:
+		{
+			const s16 gyro_scale[] = {250, 500, 1000, 2000};
+
+			*val = gyro_scale[st->chip_config.fsr];
+
+			return IIO_VAL_INT;
+		}
+		case IIO_ACCEL:
+		{
+			const s16 accel_scale[] = {2, 4, 8, 16};
+			*val = accel_scale[st->chip_config.accl_fs];
+			return IIO_VAL_INT;
+		}
+		case IIO_MAGN:
+			return inv_compass_scale_show(st, val);
+		default:
+			return -EINVAL;
+		}
+	case IIO_CHAN_INFO_CALIBBIAS:
+		/* return bias=0 for both accel and gyro for MPU6500;
+		   self test not supported yet */
+		if (INV_MPU6500 == st->chip_type) {
+			*val = 0;
+			return IIO_VAL_INT;
+		}
+		if (st->chip_config.self_test_run_once == 0) {
+			result = inv_do_test(st, 0,  st->gyro_bias,
+				st->accel_bias);
+			/* Reset Accel and Gyro full scale range
+			   back to default value */
+			inv_recover_setting(st);
+			if (result)
+				return result;
+			st->chip_config.self_test_run_once = 1;
+		}
+
+		switch (chan->type) {
+		case IIO_ANGL_VEL:
+			*val = st->gyro_bias[chan->channel2 - IIO_MOD_X];
+			return IIO_VAL_INT;
+		case IIO_ACCEL:
+			*val = st->accel_bias[chan->channel2 - IIO_MOD_X] *
+					st->chip_info.multi;
+			return IIO_VAL_INT;
+		default:
+			return -EINVAL;
+		}
+	case IIO_CHAN_INFO_OFFSET:
+		switch (chan->type) {
+		case IIO_ACCEL:
+			*val = st->input_accel_bias[chan->channel2 - IIO_MOD_X];
+			return IIO_VAL_INT;
+		default:
+			return -EINVAL;
+		}
+	default:
+		return -EINVAL;
+	}
+}
+
+/**
+ *  inv_write_fsr() - Configure the gyro's scale range.
+ */
+static int inv_write_fsr(struct inv_mpu_iio_s *st, int fsr)
+{
+	struct inv_reg_map_s *reg;
+	int result;
+	reg = &st->reg;
+	if ((fsr < 0) || (fsr > MAX_GYRO_FS_PARAM))
+		return -EINVAL;
+	if (fsr == st->chip_config.fsr)
+		return 0;
+
+	if (INV_MPU3050 == st->chip_type)
+		result = inv_i2c_single_write(st, reg->lpf,
+			(fsr << GYRO_CONFIG_FSR_SHIFT) | st->chip_config.lpf);
+	else
+		result = inv_i2c_single_write(st, reg->gyro_config,
+			fsr << GYRO_CONFIG_FSR_SHIFT);
+
+	if (result)
+		return result;
+	st->chip_config.fsr = fsr;
+
+	return 0;
+}
+
+/**
+ *  inv_write_accel_fs() - Configure the accelerometer's scale range.
+ */
+static int inv_write_accel_fs(struct inv_mpu_iio_s *st, int fs)
+{
+	int result;
+	struct inv_reg_map_s *reg;
+	reg = &st->reg;
+
+	if (fs < 0 || fs > MAX_ACCL_FS_PARAM)
+		return -EINVAL;
+	if (fs == st->chip_config.accl_fs)
+		return 0;
+	if (INV_MPU3050 == st->chip_type)
+		result = st->mpu_slave->set_fs(st, fs);
+	else
+		result = inv_i2c_single_write(st, reg->accl_config,
+				(fs << ACCL_CONFIG_FSR_SHIFT));
+	if (result)
+		return result;
+
+	st->chip_config.accl_fs = fs;
+
+	return 0;
+}
+
+/**
+ *  inv_write_compass_scale() - Configure the compass's scale range.
+ */
+static int inv_write_compass_scale(struct inv_mpu_iio_s  *st, int data)
+{
+	char d, en;
+	int result;
+	if (COMPASS_ID_AK8963 != st->plat_data.sec_slave_id)
+		return 0;
+	en = !!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);
+	if (result)
+		return result;
+	st->compass_scale = en;
+
+	return 0;
+}
+
+static inline int check_enable(struct inv_mpu_iio_s  *st)
+{
+	return st->chip_config.is_asleep | st->chip_config.enable;
+}
+
+static inline int check_dmp_on(struct inv_mpu_iio_s  *st)
+{
+	return (!st->chip_config.is_asleep) &&
+		st->chip_config.enable && st->chip_config.dmp_on;
+}
+
+/**
+ *  mpu_write_raw() - write raw method.
+ */
+static int mpu_write_raw(struct iio_dev *indio_dev,
+			       struct iio_chan_spec const *chan,
+			       int val,
+			       int val2,
+			       long mask) {
+	struct inv_mpu_iio_s  *st = iio_priv(indio_dev);
+	int result;
+	if (check_enable(st))
+		return -EPERM;
+	switch (mask) {
+	case IIO_CHAN_INFO_SCALE:
+		switch (chan->type) {
+		case IIO_ANGL_VEL:
+			return inv_write_fsr(st, val);
+		case IIO_ACCEL:
+			return inv_write_accel_fs(st, val);
+		case IIO_MAGN:
+			return inv_write_compass_scale(st, val);
+		default:
+			return -EINVAL;
+		}
+	case IIO_CHAN_INFO_OFFSET:
+		switch (chan->type) {
+		case IIO_ACCEL:
+			if (!st->chip_config.firmware_loaded)
+				return -EPERM;
+			result = inv_set_accel_bias_dmp(st);
+			if (result)
+				return result;
+			st->input_accel_bias[chan->channel2 - IIO_MOD_X] = val;
+			return 0;
+		default:
+			return -EINVAL;
+		}
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/**
+ *  inv_set_lpf() - set low pass filer based on fifo rate.
+ */
+static int inv_set_lpf(struct inv_mpu_iio_s *st, int rate)
+{
+	const short hz[] = {188, 98, 42, 20, 10, 5};
+	const int   d[] = {INV_FILTER_188HZ, INV_FILTER_98HZ,
+			INV_FILTER_42HZ, INV_FILTER_20HZ,
+			INV_FILTER_10HZ, INV_FILTER_5HZ};
+	int i, h, data, result;
+	struct inv_reg_map_s *reg;
+	reg = &st->reg;
+	h = (rate >> 1);
+	i = 0;
+	while ((h < hz[i]) && (i < ARRAY_SIZE(d) - 1))
+		i++;
+	data = d[i];
+	if (INV_MPU3050 == st->chip_type) {
+		if (st->mpu_slave != NULL) {
+			result = st->mpu_slave->set_lpf(st, rate);
+			if (result)
+				return result;
+		}
+		result = inv_i2c_single_write(st, reg->lpf, data |
+			(st->chip_config.fsr << GYRO_CONFIG_FSR_SHIFT));
+	} else {
+		result = inv_i2c_single_write(st, reg->lpf, data);
+	}
+	if (result)
+		return result;
+	st->chip_config.lpf = data;
+
+	return 0;
+}
+
+/**
+ *  inv_fifo_rate_store() - Set fifo rate.
+ */
+static ssize_t inv_fifo_rate_store(struct device *dev,
+	struct device_attribute *attr, const char *buf, size_t count)
+{
+	u32 fifo_rate;
+	u8 data;
+	int result;
+	struct inv_mpu_iio_s *st = iio_priv(dev_get_drvdata(dev));
+	struct inv_reg_map_s *reg;
+	reg = &st->reg;
+
+	if (check_enable(st))
+		return -EPERM;
+	if (kstrtouint(buf, 10, &fifo_rate))
+		return -EINVAL;
+	if ((fifo_rate < MIN_FIFO_RATE) || (fifo_rate > MAX_FIFO_RATE))
+		return -EINVAL;
+	if (fifo_rate == st->chip_config.fifo_rate)
+		return count;
+	if (st->chip_config.has_compass) {
+		st->compass_divider = COMPASS_RATE_SCALE * fifo_rate /
+					ONE_K_HZ;
+		if (st->compass_divider > 0)
+			st->compass_divider -= 1;
+		st->compass_counter = 0;
+	}
+	data = ONE_K_HZ / fifo_rate - 1;
+	result = inv_i2c_single_write(st, reg->sample_rate_div, data);
+	if (result)
+		return result;
+	st->chip_config.fifo_rate = fifo_rate;
+	result = inv_set_lpf(st, fifo_rate);
+	if (result)
+		return result;
+	st->irq_dur_ns = (data + 1) * NSEC_PER_MSEC;
+
+	return count;
+}
+
+/**
+ *  inv_fifo_rate_show() - Get the current sampling rate.
+ */
+static ssize_t inv_fifo_rate_show(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct inv_mpu_iio_s *st = iio_priv(dev_get_drvdata(dev));
+
+	return sprintf(buf, "%d\n", st->chip_config.fifo_rate);
+}
+
+/**
+ *  inv_power_state_store() - Turn device on/off.
+ */
+static ssize_t inv_power_state_store(struct device *dev,
+	struct device_attribute *attr, const char *buf, size_t count)
+{
+	int result;
+	u32 power_state;
+	struct inv_mpu_iio_s *st = iio_priv(dev_get_drvdata(dev));
+	if (kstrtouint(buf, 10, &power_state))
+		return -EINVAL;
+	if ((!power_state) == st->chip_config.is_asleep)
+		return count;
+	result = st->set_power_state(st, power_state);
+
+	return count;
+}
+
+/**
+ *  inv_reg_dump_show() - Register dump for testing.
+ */
+static ssize_t inv_reg_dump_show(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	int ii;
+	char data;
+	ssize_t bytes_printed = 0;
+	struct inv_mpu_iio_s *st = iio_priv(dev_get_drvdata(dev));
+
+	for (ii = 0; ii < st->hw->num_reg; ii++) {
+		/* don't read fifo r/w register */
+		if (ii == st->reg.fifo_r_w)
+			data = 0;
+		else
+			inv_i2c_read(st, ii, 1, &data);
+		bytes_printed += sprintf(buf + bytes_printed, "%#2x: %#2x\n",
+					 ii, data);
+	}
+
+	return bytes_printed;
+}
+
+int write_be32_key_to_mem(struct inv_mpu_iio_s *st,
+					u32 data, int key)
+{
+	cpu_to_be32s(&data);
+	return mem_w_key(key, sizeof(data), (u8 *)&data);
+}
+
+/**
+ * inv_dmp_attr_store() -  calling this function will store current
+ *                        dmp parameter settings
+ */
+static ssize_t inv_dmp_attr_store(struct device *dev,
+	struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct inv_mpu_iio_s *st = iio_priv(indio_dev);
+	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
+	int result, data;
+	mutex_lock(&indio_dev->mlock);
+	if (st->chip_config.is_asleep | (!st->chip_config.firmware_loaded) |
+				st->chip_config.enable) {
+		result = -EINVAL;
+		goto dmp_attr_store_fail;
+	}
+	result = kstrtoint(buf, 10, &data);
+	if (result)
+		goto dmp_attr_store_fail;
+	switch (this_attr->address) {
+	case ATTR_DMP_PEDOMETER_STEPS:
+		result = write_be32_key_to_mem(st, data, KEY_D_PEDSTD_STEPCTR);
+		if (result)
+			goto dmp_attr_store_fail;
+		break;
+	case ATTR_DMP_PEDOMETER_TIME:
+		result = write_be32_key_to_mem(st, data, KEY_D_PEDSTD_TIMECTR);
+		if (result)
+			goto dmp_attr_store_fail;
+		break;
+	case ATTR_DMP_TAP_THRESHOLD: {
+		const char ax[] = {INV_TAP_AXIS_X, INV_TAP_AXIS_Y,
+							INV_TAP_AXIS_Z};
+		int i;
+		if (data < 0 || data > USHRT_MAX) {
+			result = -EINVAL;
+			goto dmp_attr_store_fail;
+		}
+		for (i = 0; i < ARRAY_SIZE(ax); i++) {
+			result = inv_set_tap_threshold_dmp(st, ax[i], data);
+			if (result)
+				goto dmp_attr_store_fail;
+		}
+		st->tap.thresh = data;
+		break;
+	}
+	case ATTR_DMP_TAP_MIN_COUNT:
+		if (data < 0 || data > USHRT_MAX) {
+			result = -EINVAL;
+			goto dmp_attr_store_fail;
+		}
+		result = inv_set_min_taps_dmp(st, data);
+		if (result)
+			goto dmp_attr_store_fail;
+		st->tap.min_count = data;
+		break;
+	case ATTR_DMP_TAP_ON:
+		result = inv_enable_tap_dmp(st, !!data);
+		if (result)
+			goto dmp_attr_store_fail;
+		st->chip_config.tap_on = !!data;
+		break;
+	case ATTR_DMP_TAP_TIME:
+		if (data < 0 || data > USHRT_MAX) {
+			result = -EINVAL;
+			goto dmp_attr_store_fail;
+		}
+		result = inv_set_tap_time_dmp(st, data);
+		if (result)
+			goto dmp_attr_store_fail;
+		st->tap.time = data;
+		break;
+	case ATTR_DMP_ON:
+		st->chip_config.dmp_on = !!data;
+		break;
+	case ATTR_DMP_INT_ON:
+		st->chip_config.dmp_int_on = !!data;
+		break;
+	case ATTR_DMP_EVENT_INT_ON:
+		result = inv_set_interrupt_on_gesture_event(st, !!data);
+		if (result)
+			goto dmp_attr_store_fail;
+		st->chip_config.dmp_event_int_on = !!data;
+		break;
+	case ATTR_DMP_OUTPUT_RATE:
+		if (data <= 0 || data > USHRT_MAX) {
+			result = -EINVAL;
+			goto dmp_attr_store_fail;
+		}
+		result = inv_set_fifo_rate(st, data);
+		if (result)
+			goto dmp_attr_store_fail;
+		if (st->chip_config.has_compass) {
+			st->compass_dmp_divider = COMPASS_RATE_SCALE * data /
+							ONE_K_HZ;
+			if (st->compass_dmp_divider > 0)
+				st->compass_dmp_divider -= 1;
+			st->compass_counter = 0;
+		}
+		st->chip_config.dmp_output_rate = data;
+		break;
+	case ATTR_DMP_DISPLAY_ORIENTATION_ON:
+		result = inv_set_display_orient_interrupt_dmp(st, !!data);
+		if (result)
+			goto dmp_attr_store_fail;
+		st->chip_config.display_orient_on = !!data;
+		break;
+	default:
+		result = -EINVAL;
+		goto dmp_attr_store_fail;
+	}
+	result = count;
+
+dmp_attr_store_fail:
+	mutex_unlock(&indio_dev->mlock);
+
+	return result;
+}
+
+/**
+ * inv_attr_show() -  calling this function will show current
+ *                        dmp parameters.
+ */
+static ssize_t inv_attr_show(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct inv_mpu_iio_s *st = iio_priv(dev_get_drvdata(dev));
+	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
+	s8 d[4];
+	int result, data;
+	s8 *m;
+
+	switch (this_attr->address) {
+	case ATTR_DMP_PEDOMETER_STEPS:
+		if (!check_dmp_on(st))
+			return -EPERM;
+		result = mpu_memory_read(st, st->i2c_addr,
+			inv_dmp_get_address(KEY_D_PEDSTD_STEPCTR), 4, d);
+		if (result)
+			return result;
+		data = be32_to_cpup((int *)d);
+		return sprintf(buf, "%d\n", data);
+	case ATTR_DMP_PEDOMETER_TIME:
+		if (!check_dmp_on(st))
+			return -EPERM;
+		result = mpu_memory_read(st, st->i2c_addr,
+			inv_dmp_get_address(KEY_D_PEDSTD_TIMECTR), 4, d);
+		if (result)
+			return result;
+		data = be32_to_cpup((int *)d);
+		return sprintf(buf, "%d\n", data * MS_PER_DMP_TICK);
+	case ATTR_DMP_TAP_THRESHOLD:
+		return sprintf(buf, "%d\n", st->tap.thresh);
+	case ATTR_DMP_TAP_MIN_COUNT:
+		return sprintf(buf, "%d\n", st->tap.min_count);
+	case ATTR_DMP_TAP_ON:
+		return sprintf(buf, "%d\n", st->chip_config.tap_on);
+	case ATTR_DMP_TAP_TIME:
+		return sprintf(buf, "%d\n", st->tap.time);
+	case ATTR_DMP_ON:
+		return sprintf(buf, "%d\n", st->chip_config.dmp_on);
+	case ATTR_DMP_INT_ON:
+		return sprintf(buf, "%d\n", st->chip_config.dmp_int_on);
+	case ATTR_DMP_EVENT_INT_ON:
+		return sprintf(buf, "%d\n", st->chip_config.dmp_event_int_on);
+	case ATTR_DMP_OUTPUT_RATE:
+		return sprintf(buf, "%d\n", st->chip_config.dmp_output_rate);
+	case ATTR_DMP_QUATERNION_ON:
+		return sprintf(buf, "%d\n", st->chip_config.quaternion_on);
+	case ATTR_DMP_DISPLAY_ORIENTATION_ON:
+		return sprintf(buf, "%d\n",
+			st->chip_config.display_orient_on);
+	case ATTR_LPA_FREQ:{
+		const char *f[] = {"1.25", "5", "20", "40"};
+		return sprintf(buf, "%s\n", f[st->chip_config.lpa_freq]);
+	}
+	case ATTR_SELF_TEST:
+		if (INV_MPU3050 == st->chip_type)
+			result = 1;
+		else if (INV_MPU6500 == st->chip_type)
+			result = inv_hw_self_test_6500(st);
+		else
+			result = inv_hw_self_test(st);
+		return sprintf(buf, "%d\n", result);
+	case ATTR_GYRO_MATRIX:
+		m = st->plat_data.orientation;
+		return sprintf(buf, "%d,%d,%d,%d,%d,%d,%d,%d,%d\n",
+			m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]);
+	case ATTR_ACCL_MATRIX:
+		if (st->plat_data.sec_slave_type == SECONDARY_SLAVE_TYPE_ACCEL)
+			m = st->plat_data.secondary_orientation;
+		else
+			m = st->plat_data.orientation;
+		return sprintf(buf, "%d,%d,%d,%d,%d,%d,%d,%d,%d\n",
+			m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]);
+	case ATTR_COMPASS_MATRIX:
+		if (st->plat_data.sec_slave_type ==
+				SECONDARY_SLAVE_TYPE_COMPASS)
+			m = st->plat_data.secondary_orientation;
+		else
+			return -ENODEV;
+		return sprintf(buf, "%d,%d,%d,%d,%d,%d,%d,%d,%d\n",
+			m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]);
+	case ATTR_GYRO_ENABLE:
+		return sprintf(buf, "%d\n", st->chip_config.gyro_enable);
+	case ATTR_ACCL_ENABLE:
+		return sprintf(buf, "%d\n", st->chip_config.accl_enable);
+	case ATTR_COMPASS_ENABLE:
+		return sprintf(buf, "%d\n", st->chip_config.compass_enable);
+	case ATTR_POWER_STATE:
+		return sprintf(buf, "%d\n", !st->chip_config.is_asleep);
+	case ATTR_FIRMWARE_LOADED:
+		return sprintf(buf, "%d\n", st->chip_config.firmware_loaded);
+	case ATTR_MOTION_ON:
+		return sprintf(buf, "%d\n", st->mot_int.mot_on);
+	case ATTR_MOTION_DURATION:
+		return sprintf(buf, "%d\n", st->mot_int.mot_dur);
+	case ATTR_MOTION_THRESHOLD:
+		return sprintf(buf, "%d\n", st->mot_int.mot_thr);
+#ifdef CONFIG_INV_TESTING
+	case ATTR_REG_WRITE:
+		return sprintf(buf, "1\n");
+#endif
+	default:
+		return -EPERM;
+	}
+}
+
+/**
+ * inv_dmp_display_orient_show() -  calling this function will
+ *			show orientation This event must use poll.
+ */
+static ssize_t inv_dmp_display_orient_show(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct inv_mpu_iio_s *st = iio_priv(dev_get_drvdata(dev));
+	return sprintf(buf, "%d\n", st->display_orient_data);
+}
+
+/**
+ * inv_accel_motion_show() -  calling this function showes motion interrupt.
+ *                         This event must use poll.
+ */
+static ssize_t inv_accel_motion_show(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	return sprintf(buf, "1\n");
+}
+
+/**
+ * inv_dmp_tap_show() -  calling this function will show tap
+ *                         This event must use poll.
+ */
+static ssize_t inv_dmp_tap_show(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct inv_mpu_iio_s *st = iio_priv(dev_get_drvdata(dev));
+	return sprintf(buf, "%d\n", st->tap_data);
+}
+
+/**
+ *  inv_temperature_show() - Read temperature data directly from registers.
+ */
+static ssize_t inv_temperature_show(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct inv_mpu_iio_s *st = iio_priv(dev_get_drvdata(dev));
+	struct inv_reg_map_s *reg;
+	int result;
+	short temp;
+	long scale_t;
+	u8 data[2];
+	reg = &st->reg;
+
+	if (st->chip_config.is_asleep)
+		return -EPERM;
+	result = inv_i2c_read(st, reg->temperature, 2, data);
+	if (result) {
+		pr_err("Could not read temperature register.\n");
+		return result;
+	}
+	temp = (signed short)(be16_to_cpup((short *)&data[0]));
+
+	if (INV_MPU3050 == st->chip_type)
+		scale_t = MPU3050_TEMP_OFFSET +
+			inv_q30_mult((int)temp << MPU_TEMP_SHIFT,
+				     MPU3050_TEMP_SCALE);
+	else
+		scale_t = MPU6050_TEMP_OFFSET +
+			inv_q30_mult((int)temp << MPU_TEMP_SHIFT,
+				     MPU6050_TEMP_SCALE);
+
+	INV_I2C_INC_TEMPREAD(1);
+
+	return sprintf(buf, "%ld %lld\n", scale_t, get_time_ns());
+}
+
+/**
+ * inv_firmware_loaded() -  calling this function will change
+ *                        firmware load
+ */
+static int inv_firmware_loaded(struct inv_mpu_iio_s *st, int data)
+{
+	if (data)
+		return -EINVAL;
+	st->chip_config.firmware_loaded = 0;
+	st->chip_config.dmp_on = 0;
+	st->chip_config.quaternion_on = 0;
+
+	return 0;
+}
+
+/**
+ * inv_quaternion_on() -  calling this function will store
+ *                                 current quaternion on
+ */
+static int inv_quaternion_on(struct inv_mpu_iio_s *st,
+				 struct iio_buffer *ring, bool en)
+{
+	st->chip_config.quaternion_on = en;
+	if (!en) {
+		clear_bit(INV_MPU_SCAN_QUAT_R, ring->scan_mask);
+		clear_bit(INV_MPU_SCAN_QUAT_X, ring->scan_mask);
+		clear_bit(INV_MPU_SCAN_QUAT_Y, ring->scan_mask);
+		clear_bit(INV_MPU_SCAN_QUAT_Z, ring->scan_mask);
+	}
+
+	return 0;
+}
+
+static int inv_switch_gyro_engine(struct inv_mpu_iio_s *st, bool en)
+{
+	return inv_switch_engine(st, en, BIT_PWR_GYRO_STBY);
+}
+
+static int inv_switch_accl_engine(struct inv_mpu_iio_s *st, bool en)
+{
+	return inv_switch_engine(st, en, BIT_PWR_ACCL_STBY);
+}
+
+/**
+ *  inv_gyro_enable() - Enable/disable gyro.
+ */
+static int inv_gyro_enable(struct inv_mpu_iio_s *st,
+				 struct iio_buffer *ring, bool en)
+{
+	int result;
+	if (en == st->chip_config.gyro_enable)
+		return 0;
+	result = st->switch_gyro_engine(st, en);
+	if (result)
+		return result;
+
+	if (!en) {
+		st->chip_config.gyro_fifo_enable = 0;
+		clear_bit(INV_MPU_SCAN_GYRO_X, ring->scan_mask);
+		clear_bit(INV_MPU_SCAN_GYRO_Y, ring->scan_mask);
+		clear_bit(INV_MPU_SCAN_GYRO_Z, ring->scan_mask);
+	}
+	st->chip_config.gyro_enable = en;
+
+	return 0;
+}
+
+/**
+ *  inv_accl_enable() - Enable/disable accl.
+ */
+static ssize_t inv_accl_enable(struct inv_mpu_iio_s *st,
+				 struct iio_buffer *ring, bool en)
+{
+	int result;
+	if (en == st->chip_config.accl_enable)
+		return 0;
+	result = st->switch_accl_engine(st, en);
+	if (result)
+		return result;
+	st->chip_config.accl_enable = en;
+	if (!en) {
+		st->chip_config.accl_fifo_enable = 0;
+		clear_bit(INV_MPU_SCAN_ACCL_X, ring->scan_mask);
+		clear_bit(INV_MPU_SCAN_ACCL_Y, ring->scan_mask);
+		clear_bit(INV_MPU_SCAN_ACCL_Z, ring->scan_mask);
+	}
+
+	return 0;
+}
+
+/**
+ * inv_compass_enable() -  calling this function will store compass
+ *                         enable
+ */
+static ssize_t inv_compass_enable(struct inv_mpu_iio_s *st,
+				 struct iio_buffer *ring, bool en)
+{
+	if (en == st->chip_config.compass_enable)
+		return 0;
+	st->chip_config.compass_enable = en;
+	if (!en) {
+		st->chip_config.compass_fifo_enable = 0;
+		clear_bit(INV_MPU_SCAN_MAGN_X, ring->scan_mask);
+		clear_bit(INV_MPU_SCAN_MAGN_Y, ring->scan_mask);
+		clear_bit(INV_MPU_SCAN_MAGN_Z, ring->scan_mask);
+	}
+
+	return 0;
+}
+
+/**
+ * inv_attr_store() -  calling this function will store current
+ *                        non-dmp parameter settings
+ */
+static ssize_t inv_attr_store(struct device *dev,
+	struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct inv_mpu_iio_s *st = iio_priv(indio_dev);
+	struct iio_buffer *ring = indio_dev->buffer;
+	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
+	int data;
+	u8  d;
+	int result;
+
+	mutex_lock(&indio_dev->mlock);
+	if (check_enable(st)) {
+		result = -EINVAL;
+		goto attr_store_fail;
+	}
+
+	result = kstrtoint(buf, 10, &data);
+	if (result)
+		goto attr_store_fail;
+	switch (this_attr->address) {
+	case ATTR_GYRO_ENABLE:
+		result = inv_gyro_enable(st, ring, !!data);
+		break;
+	case ATTR_ACCL_ENABLE:
+		result = inv_accl_enable(st, ring, !!data);
+		break;
+	case ATTR_COMPASS_ENABLE:
+		result = inv_compass_enable(st, ring, !!data);
+		break;
+	case ATTR_DMP_QUATERNION_ON:
+		result = inv_quaternion_on(st, ring, !!data);
+		break;
+	case ATTR_LPA_FREQ:
+		result = inv_lpa_freq(st, data);
+		break;
+	case ATTR_FIRMWARE_LOADED:
+		result = inv_firmware_loaded(st, data);
+		break;
+	case ATTR_MOTION_ON:
+		if (INV_MPU6500 == st->chip_type) {
+			if (data)
+				/* enable and put in MPU6500 mode */
+				d = BIT_ACCEL_INTEL_ENABLE
+					| BIT_ACCEL_INTEL_MODE;
+			else
+				d = 0;
+			result = inv_i2c_single_write(st,
+						REG_6500_ACCEL_INTEL_CTRL, d);
+			if (result)
+				goto attr_store_fail;
+		}
+		st->mot_int.mot_on = !!data;
+		st->chip_config.lpa_mode = !!data;
+		break;
+	case ATTR_MOTION_DURATION:
+		if (INV_MPU6500 != st->chip_type) {
+			result = inv_i2c_single_write(st, REG_ACCEL_MOT_DUR,
+					      MPU6050_MOTION_DUR_DEFAULT);
+			if (result)
+				goto attr_store_fail;
+		}
+		st->mot_int.mot_dur = data;
+		break;
+	case ATTR_MOTION_THRESHOLD:
+		if ((data > MPU6XXX_MAX_MOTION_THRESH) || (data < 0)) {
+			result = -EINVAL;
+			goto attr_store_fail;
+		}
+		d = (u8)(data >> MPU6XXX_MOTION_THRESH_SHIFT);
+		data = (d << MPU6XXX_MOTION_THRESH_SHIFT);
+		result = inv_i2c_single_write(st, REG_ACCEL_MOT_THR, d);
+		if (result)
+			goto attr_store_fail;
+		st->mot_int.mot_thr = data;
+		break;
+	default:
+		result = -EINVAL;
+		goto attr_store_fail;
+	};
+	if (result)
+		goto attr_store_fail;
+	result = count;
+
+attr_store_fail:
+	mutex_unlock(&indio_dev->mlock);
+
+	return result;
+}
+
+#ifdef CONFIG_INV_TESTING
+/**
+ * inv_reg_write_store() - register write command for testing.
+ *                         Format: WSRRDD, where RR is the register in hex,
+ *                                         and DD is the data in hex.
+ */
+static ssize_t inv_reg_write_store(struct device *dev,
+	struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct inv_mpu_iio_s *st = iio_priv(indio_dev);
+	u32 result;
+	u8 wreg, wval;
+	int temp;
+	char local_buf[10];
+
+	if ((buf[0] != 'W' && buf[0] != 'w') ||
+	    (buf[1] != 'S' && buf[1] != 's'))
+		return -EINVAL;
+	if (strlen(buf) < 6)
+		return -EINVAL;
+
+	strncpy(local_buf, buf, 7);
+	local_buf[6] = 0;
+	result = sscanf(&local_buf[4], "%x", &temp);
+	if (result == 0)
+		return -EINVAL;
+	wval = temp;
+	local_buf[4] = 0;
+	sscanf(&local_buf[2], "%x", &temp);
+	if (result == 0)
+		return -EINVAL;
+	wreg = temp;
+
+	result = inv_i2c_single_write(st, wreg, wval);
+	if (result)
+		return result;
+
+	return count;
+}
+#endif /* CONFIG_INV_TESTING */
+
+#define INV_MPU_CHAN(_type, _channel2, _index)                \
+	{                                                         \
+		.type = _type,                                        \
+		.modified = 1,                                        \
+		.channel2 = _channel2,                                \
+		.info_mask =  (IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT | \
+				IIO_CHAN_INFO_SCALE_SHARED_BIT),              \
+		.scan_index = _index,                                 \
+		.scan_type  = IIO_ST('s', 16, 16, 0)                  \
+	}
+
+#define INV_ACCL_CHAN(_type, _channel2, _index)                \
+	{                                                         \
+		.type = _type,                                        \
+		.modified = 1,                                        \
+		.channel2 = _channel2,                                \
+		.info_mask =  (IIO_CHAN_INFO_CALIBBIAS_SEPARATE_BIT | \
+				IIO_CHAN_INFO_SCALE_SHARED_BIT |     \
+				IIO_CHAN_INFO_OFFSET_SEPARATE_BIT),  \
+		.scan_index = _index,                                 \
+		.scan_type  = IIO_ST('s', 16, 16, 0)                  \
+	}
+
+#define INV_MPU_QUATERNION_CHAN(_channel2, _index)            \
+	{                                                         \
+		.type = IIO_QUATERNION,                               \
+		.modified = 1,                                        \
+		.channel2 = _channel2,                                \
+		.scan_index = _index,                                 \
+		.scan_type  = IIO_ST('s', 32, 32, 0)                  \
+	}
+
+#define INV_MPU_MAGN_CHAN(_channel2, _index)                  \
+	{                                                         \
+		.type = IIO_MAGN,                                     \
+		.modified = 1,                                        \
+		.channel2 = _channel2,                                \
+		.info_mask =  IIO_CHAN_INFO_SCALE_SHARED_BIT,         \
+		.scan_index = _index,                                 \
+		.scan_type  = IIO_ST('s', 16, 16, 0)                  \
+	}
+
+static const struct iio_chan_spec inv_mpu_channels[] = {
+	IIO_CHAN_SOFT_TIMESTAMP(INV_MPU_SCAN_TIMESTAMP),
+
+	INV_MPU_CHAN(IIO_ANGL_VEL, IIO_MOD_X, INV_MPU_SCAN_GYRO_X),
+	INV_MPU_CHAN(IIO_ANGL_VEL, IIO_MOD_Y, INV_MPU_SCAN_GYRO_Y),
+	INV_MPU_CHAN(IIO_ANGL_VEL, IIO_MOD_Z, INV_MPU_SCAN_GYRO_Z),
+
+	INV_ACCL_CHAN(IIO_ACCEL, IIO_MOD_X, INV_MPU_SCAN_ACCL_X),
+	INV_ACCL_CHAN(IIO_ACCEL, IIO_MOD_Y, INV_MPU_SCAN_ACCL_Y),
+	INV_ACCL_CHAN(IIO_ACCEL, IIO_MOD_Z, INV_MPU_SCAN_ACCL_Z),
+
+	INV_MPU_QUATERNION_CHAN(IIO_MOD_R, INV_MPU_SCAN_QUAT_R),
+	INV_MPU_QUATERNION_CHAN(IIO_MOD_X, INV_MPU_SCAN_QUAT_X),
+	INV_MPU_QUATERNION_CHAN(IIO_MOD_Y, INV_MPU_SCAN_QUAT_Y),
+	INV_MPU_QUATERNION_CHAN(IIO_MOD_Z, INV_MPU_SCAN_QUAT_Z),
+
+	INV_MPU_MAGN_CHAN(IIO_MOD_X, INV_MPU_SCAN_MAGN_X),
+	INV_MPU_MAGN_CHAN(IIO_MOD_Y, INV_MPU_SCAN_MAGN_Y),
+	INV_MPU_MAGN_CHAN(IIO_MOD_Z, INV_MPU_SCAN_MAGN_Z),
+};
+
+/*constant IIO attribute */
+static IIO_CONST_ATTR_SAMP_FREQ_AVAIL("10 20 50 100 200 500");
+static IIO_DEV_ATTR_SAMP_FREQ(S_IRUGO | S_IWUSR, inv_fifo_rate_show,
+	inv_fifo_rate_store);
+static DEVICE_ATTR(temperature, S_IRUGO, inv_temperature_show, NULL);
+static IIO_DEVICE_ATTR(power_state, S_IRUGO | S_IWUSR, inv_attr_show,
+	inv_power_state_store, ATTR_POWER_STATE);
+static IIO_DEVICE_ATTR(firmware_loaded, S_IRUGO | S_IWUSR, inv_attr_show,
+	inv_attr_store, ATTR_FIRMWARE_LOADED);
+static IIO_DEVICE_ATTR(motion_lpa_freq, S_IRUGO | S_IWUSR, inv_attr_show,
+	inv_attr_store, ATTR_LPA_FREQ);
+static IIO_DEVICE_ATTR(motion_lpa_on, S_IRUGO | S_IWUSR, inv_attr_show,
+	inv_attr_store, ATTR_MOTION_ON);
+static IIO_DEVICE_ATTR(motion_lpa_duration, S_IRUGO | S_IWUSR, inv_attr_show,
+	inv_attr_store, ATTR_MOTION_DURATION);
+static IIO_DEVICE_ATTR(motion_lpa_threshold, S_IRUGO | S_IWUSR, inv_attr_show,
+	inv_attr_store, ATTR_MOTION_THRESHOLD);
+static DEVICE_ATTR(reg_dump, S_IRUGO, inv_reg_dump_show, NULL);
+static IIO_DEVICE_ATTR(self_test, S_IRUGO, inv_attr_show, NULL,
+	ATTR_SELF_TEST);
+static IIO_DEVICE_ATTR(gyro_matrix, S_IRUGO, inv_attr_show, NULL,
+	ATTR_GYRO_MATRIX);
+static IIO_DEVICE_ATTR(accl_matrix, S_IRUGO, inv_attr_show, NULL,
+	ATTR_ACCL_MATRIX);
+static IIO_DEVICE_ATTR(compass_matrix, S_IRUGO, inv_attr_show, NULL,
+	ATTR_COMPASS_MATRIX);
+static IIO_DEVICE_ATTR(dmp_on, S_IRUGO | S_IWUSR, inv_attr_show,
+	inv_dmp_attr_store, ATTR_DMP_ON);
+static IIO_DEVICE_ATTR(dmp_int_on, S_IRUGO | S_IWUSR, inv_attr_show,
+	inv_dmp_attr_store, ATTR_DMP_INT_ON);
+static IIO_DEVICE_ATTR(dmp_event_int_on, S_IRUGO | S_IWUSR, inv_attr_show,
+	inv_dmp_attr_store, ATTR_DMP_EVENT_INT_ON);
+static IIO_DEVICE_ATTR(dmp_output_rate, S_IRUGO | S_IWUSR, inv_attr_show,
+	inv_dmp_attr_store, ATTR_DMP_OUTPUT_RATE);
+static IIO_DEVICE_ATTR(quaternion_on, S_IRUGO | S_IWUSR, inv_attr_show,
+	inv_attr_store, ATTR_DMP_QUATERNION_ON);
+static IIO_DEVICE_ATTR(display_orientation_on, S_IRUGO | S_IWUSR,
+	inv_attr_show, inv_dmp_attr_store, ATTR_DMP_DISPLAY_ORIENTATION_ON);
+static IIO_DEVICE_ATTR(tap_on, S_IRUGO | S_IWUSR, inv_attr_show,
+	inv_dmp_attr_store, ATTR_DMP_TAP_ON);
+static IIO_DEVICE_ATTR(tap_time, S_IRUGO | S_IWUSR, inv_attr_show,
+	inv_dmp_attr_store, ATTR_DMP_TAP_TIME);
+static IIO_DEVICE_ATTR(tap_min_count, S_IRUGO | S_IWUSR, inv_attr_show,
+	inv_dmp_attr_store, ATTR_DMP_TAP_MIN_COUNT);
+static IIO_DEVICE_ATTR(tap_threshold, S_IRUGO | S_IWUSR, inv_attr_show,
+	inv_dmp_attr_store, ATTR_DMP_TAP_THRESHOLD);
+static IIO_DEVICE_ATTR(pedometer_time, S_IRUGO | S_IWUSR, inv_attr_show,
+	inv_dmp_attr_store, ATTR_DMP_PEDOMETER_TIME);
+static IIO_DEVICE_ATTR(pedometer_steps, S_IRUGO | S_IWUSR, inv_attr_show,
+	inv_dmp_attr_store, ATTR_DMP_PEDOMETER_STEPS);
+static DEVICE_ATTR(event_tap, S_IRUGO, inv_dmp_tap_show, NULL);
+static DEVICE_ATTR(event_display_orientation, S_IRUGO,
+	inv_dmp_display_orient_show, NULL);
+static DEVICE_ATTR(event_accel_motion, S_IRUGO, inv_accel_motion_show, NULL);
+static IIO_DEVICE_ATTR(gyro_enable, S_IRUGO | S_IWUSR, inv_attr_show,
+	inv_attr_store, ATTR_GYRO_ENABLE);
+static IIO_DEVICE_ATTR(accl_enable, S_IRUGO | S_IWUSR, inv_attr_show,
+	inv_attr_store, ATTR_ACCL_ENABLE);
+static IIO_DEVICE_ATTR(compass_enable, S_IRUGO | S_IWUSR, inv_attr_show,
+	inv_attr_store, ATTR_COMPASS_ENABLE);
+#ifdef CONFIG_INV_TESTING
+static IIO_DEVICE_ATTR(reg_write, S_IRUGO | S_IWUSR, inv_attr_show,
+	inv_reg_write_store, ATTR_REG_WRITE);
+#endif
+
+static const struct attribute *inv_gyro_attributes[] = {
+	&iio_dev_attr_gyro_enable.dev_attr.attr,
+	&dev_attr_temperature.attr,
+	&iio_dev_attr_power_state.dev_attr.attr,
+	&dev_attr_reg_dump.attr,
+	&iio_dev_attr_self_test.dev_attr.attr,
+	&iio_dev_attr_gyro_matrix.dev_attr.attr,
+#ifdef CONFIG_INV_TESTING
+	&iio_dev_attr_reg_write.dev_attr.attr,
+#endif
+	&iio_dev_attr_sampling_frequency.dev_attr.attr,
+	&iio_const_attr_sampling_frequency_available.dev_attr.attr,
+};
+
+static const struct attribute *inv_mpu6050_attributes[] = {
+	&iio_dev_attr_accl_enable.dev_attr.attr,
+	&iio_dev_attr_accl_matrix.dev_attr.attr,
+	&iio_dev_attr_firmware_loaded.dev_attr.attr,
+	&iio_dev_attr_motion_lpa_freq.dev_attr.attr,
+	&iio_dev_attr_motion_lpa_on.dev_attr.attr,
+	&iio_dev_attr_motion_lpa_duration.dev_attr.attr,
+	&iio_dev_attr_motion_lpa_threshold.dev_attr.attr,
+	&iio_dev_attr_dmp_on.dev_attr.attr,
+	&iio_dev_attr_dmp_int_on.dev_attr.attr,
+	&iio_dev_attr_dmp_event_int_on.dev_attr.attr,
+	&iio_dev_attr_dmp_output_rate.dev_attr.attr,
+	&iio_dev_attr_quaternion_on.dev_attr.attr,
+	&iio_dev_attr_display_orientation_on.dev_attr.attr,
+	&iio_dev_attr_tap_on.dev_attr.attr,
+	&iio_dev_attr_tap_time.dev_attr.attr,
+	&iio_dev_attr_tap_min_count.dev_attr.attr,
+	&iio_dev_attr_tap_threshold.dev_attr.attr,
+	&iio_dev_attr_pedometer_time.dev_attr.attr,
+	&iio_dev_attr_pedometer_steps.dev_attr.attr,
+	&dev_attr_event_display_orientation.attr,
+	&dev_attr_event_tap.attr,
+	&dev_attr_event_accel_motion.attr,
+};
+
+static const struct attribute *inv_compass_attributes[] = {
+	&iio_dev_attr_compass_matrix.dev_attr.attr,
+	&iio_dev_attr_compass_enable.dev_attr.attr,
+};
+
+static const struct attribute *inv_mpu3050_attributes[] = {
+	&iio_dev_attr_accl_matrix.dev_attr.attr,
+	&iio_dev_attr_accl_enable.dev_attr.attr,
+};
+
+static struct attribute *inv_attributes[ARRAY_SIZE(inv_gyro_attributes) +
+				ARRAY_SIZE(inv_mpu6050_attributes) +
+				ARRAY_SIZE(inv_compass_attributes) + 1];
+
+static const struct attribute_group inv_attribute_group = {
+	.name = "mpu",
+	.attrs = inv_attributes
+};
+
+static const struct iio_info mpu_info = {
+	.driver_module = THIS_MODULE,
+	.read_raw = &mpu_read_raw,
+	.write_raw = &mpu_write_raw,
+	.attrs = &inv_attribute_group,
+};
+
+/**
+ *  inv_setup_compass() - Configure compass.
+ */
+static int inv_setup_compass(struct inv_mpu_iio_s *st)
+{
+	int result;
+	u8 data[4];
+
+	result = inv_i2c_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]);
+	if (result)
+		return result;
+	/* set to bypass mode */
+	result = inv_i2c_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);
+	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);
+	if (result)
+		return result;
+	result = inv_secondary_read(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);
+	if (result)
+		return result;
+	pr_debug("%s senx=%d, seny=%d, senz=%d\n",
+		 st->hw->name,
+		 st->chip_info.compass_sens[0],
+		 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,
+			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);
+	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|
+		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);
+	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 |
+				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,
+		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);
+	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);
+	if (result)
+		return result;
+	/* output data for slave 1 is fixed, single measure mode*/
+	st->compass_scale = 1;
+	if (COMPASS_ID_AK8975 == st->plat_data.sec_slave_id) {
+		st->compass_st_upper = AKM8975_ST_Upper;
+		st->compass_st_lower = AKM8975_ST_Lower;
+		data[0] = DATA_AKM_MODE_SM;
+	} else if (COMPASS_ID_AK8972 == st->plat_data.sec_slave_id) {
+		st->compass_st_upper = AKM8972_ST_Upper;
+		st->compass_st_lower = AKM8972_ST_Lower;
+		data[0] = DATA_AKM_MODE_SM;
+	} else if (COMPASS_ID_AK8963 == st->plat_data.sec_slave_id) {
+		st->compass_st_upper = AKM8963_ST_Upper;
+		st->compass_st_lower = AKM8963_ST_Lower;
+		data[0] = DATA_AKM_MODE_SM |
+			  (st->compass_scale << AKM8963_SCALE_SHIFT);
+	}
+	result = inv_i2c_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,
+				BIT_SLV0_DLY_EN | BIT_SLV1_DLY_EN);
+	return result;
+}
+
+static void inv_setup_func_ptr(struct inv_mpu_iio_s *st)
+{
+	if (st->chip_type == INV_MPU3050) {
+		st->set_power_state    = set_power_mpu3050;
+		st->switch_gyro_engine = inv_switch_3050_gyro_engine;
+		st->switch_accl_engine = inv_switch_3050_accl_engine;
+		st->init_config        = inv_init_config_mpu3050;
+		st->setup_reg          = inv_setup_reg_mpu3050;
+	} else {
+		st->set_power_state    = set_power_itg;
+		st->switch_gyro_engine = inv_switch_gyro_engine;
+		st->switch_accl_engine = inv_switch_accl_engine;
+		st->init_config        = inv_init_config;
+		st->setup_reg          = inv_setup_reg;
+		/*MPU6XXX special functions */
+		st->compass_en         = inv_compass_enable;
+		st->quaternion_en      = inv_quaternion_on;
+		st->gyro_en            = inv_gyro_enable;
+		st->accl_en            = inv_accl_enable;
+	}
+}
+
+static int inv_detect_6xxx(struct inv_mpu_iio_s *st)
+{
+	int result;
+	u8 d;
+
+	result = inv_i2c_read(st, REG_WHOAMI, 1, &d);
+	if (result)
+		return result;
+	if (d == MPU6500_ID) {
+		st->chip_type = INV_MPU6500;
+		strcpy(st->name, "mpu6500");
+	} else {
+		strcpy(st->name, "mpu6050");
+	}
+
+	return 0;
+}
+
+/**
+ *  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)
+{
+	struct inv_reg_map_s *reg;
+	int result;
+	int t_ind;
+
+	if (!strcmp(id->name, "itg3500"))
+		st->chip_type = INV_ITG3500;
+	else if (!strcmp(id->name, "mpu3050"))
+		st->chip_type = INV_MPU3050;
+	else if (!strcmp(id->name, "mpu6050"))
+		st->chip_type = INV_MPU6050;
+	else if (!strcmp(id->name, "mpu9150"))
+		st->chip_type = INV_MPU6050;
+	else if (!strcmp(id->name, "mpu6500"))
+		st->chip_type = INV_MPU6500;
+	else if (!strcmp(id->name, "mpu9250"))
+		st->chip_type = INV_MPU6500;
+	else if (!strcmp(id->name, "mpu6xxx"))
+		st->chip_type = INV_MPU6050;
+	else
+		return -EPERM;
+	inv_setup_func_ptr(st);
+	st->hw  = &hw_info[st->chip_type];
+	st->mpu_slave = NULL;
+	reg = &st->reg;
+	st->setup_reg(reg);
+	st->chip_config.gyro_enable = 1;
+	/* reset to make sure previous state are not there */
+	result = inv_i2c_single_write(st, reg->pwr_mgmt_1, BIT_H_RESET);
+	if (result)
+		return result;
+	msleep(POWER_UP_TIME);
+
+	/* turn off and turn on power to ensure gyro engine is on */
+	result = st->set_power_state(st, false);
+	if (result)
+		return result;
+	result = st->set_power_state(st, true);
+	if (result)
+		return result;
+
+	if (!strcmp(id->name, "mpu6xxx")) {
+		result = inv_detect_6xxx(st);
+		if (result)
+			return result;
+	}
+
+	switch (st->chip_type) {
+	case INV_ITG3500:
+		st->num_channels = INV_CHANNEL_NUM_GYRO;
+		break;
+	case INV_MPU6050:
+	case INV_MPU6500:
+		if (SECONDARY_SLAVE_TYPE_COMPASS ==
+		    st->plat_data.sec_slave_type) {
+			st->chip_config.has_compass = 1;
+			st->num_channels =
+				INV_CHANNEL_NUM_GYRO_ACCL_QUANTERNION_MAGN;
+		} else {
+			st->chip_config.has_compass = 0;
+			st->num_channels =
+				INV_CHANNEL_NUM_GYRO_ACCL_QUANTERNION;
+		}
+		break;
+	case INV_MPU3050:
+		if (SECONDARY_SLAVE_TYPE_ACCEL ==
+		    st->plat_data.sec_slave_type) {
+			if (ACCEL_ID_BMA250 == st->plat_data.sec_slave_id)
+				inv_register_mpu3050_slave(st);
+			st->num_channels = INV_CHANNEL_NUM_GYRO_ACCL;
+		} else {
+			st->num_channels = INV_CHANNEL_NUM_GYRO;
+		}
+		break;
+	default:
+		result = st->set_power_state(st, false);
+		return -ENODEV;
+	}
+
+	switch (st->chip_type) {
+	case INV_MPU6050:
+		result = inv_get_silicon_rev_mpu6050(st);
+		break;
+	case INV_MPU6500:
+		result = inv_get_silicon_rev_mpu6500(st);
+		break;
+	default:
+		result = 0;
+		break;
+	}
+	if (result) {
+		pr_err("read silicon rev error\n");
+		st->set_power_state(st, false);
+		return result;
+	}
+
+//wlg bypass i2c slave
+	result = inv_i2c_single_write(st, REG_INT_PIN_CFG,
+				st->plat_data.int_config | BIT_BYPASS_EN);
+	if (result)
+		return result;
+
+	if (st->chip_config.has_compass) {
+		result = inv_setup_compass(st);
+		if (result) {
+			pr_err("compass setup failed\n");
+			st->set_power_state(st, false);
+			return result;
+		}
+	}
+
+	t_ind = 0;
+	memcpy(&inv_attributes[t_ind], inv_gyro_attributes,
+		sizeof(inv_gyro_attributes));
+	t_ind += ARRAY_SIZE(inv_gyro_attributes);
+
+	if (INV_MPU3050 == st->chip_type && st->mpu_slave != NULL) {
+		memcpy(&inv_attributes[t_ind], inv_mpu3050_attributes,
+		       sizeof(inv_mpu3050_attributes));
+		t_ind += ARRAY_SIZE(inv_mpu3050_attributes);
+		inv_attributes[t_ind] = NULL;
+		return 0;
+	}
+
+	if ((INV_MPU6050 == st->chip_type) || (INV_MPU6500 == st->chip_type)) {
+		memcpy(&inv_attributes[t_ind], inv_mpu6050_attributes,
+		       sizeof(inv_mpu6050_attributes));
+		t_ind += ARRAY_SIZE(inv_mpu6050_attributes);
+	}
+
+	if (st->chip_config.has_compass) {
+		memcpy(&inv_attributes[t_ind], inv_compass_attributes,
+		       sizeof(inv_compass_attributes));
+		t_ind += ARRAY_SIZE(inv_compass_attributes);
+	}
+	inv_attributes[t_ind] = NULL;
+
+	return 0;
+}
+
+/**
+ *  inv_create_dmp_sysfs() - create binary sysfs dmp entry.
+ */
+static const struct bin_attribute dmp_firmware = {
+	.attr = {
+		.name = "dmp_firmware",
+		.mode = S_IRUGO | S_IWUSR
+	},
+	.size = 4096,
+	.read = inv_dmp_firmware_read,
+	.write = inv_dmp_firmware_write,
+};
+
+static int inv_create_dmp_sysfs(struct iio_dev *ind)
+{
+	int result;
+	result = sysfs_create_bin_file(&ind->dev.kobj, &dmp_firmware);
+
+	return result;
+}
+static void inv_mpu_hw_init(struct i2c_client *client)
+{
+	struct regulator *reg_vdd;
+	struct mpu_platform_data *pdata = client->dev.platform_data;
+	reg_vdd = regulator_get(&client->dev, pdata->vdd_name);
+	if (!WARN(IS_ERR(reg_vdd), "inv_mpu regulator: failed to get %s.\n", pdata->vdd_name)) {
+		regulator_set_voltage(reg_vdd, 1800000, 1800000);
+		regulator_enable(reg_vdd);
+	}
+	msleep(100);
+}
+
+/**
+ *  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;
+	}
+	indio_dev = iio_device_alloc(sizeof(*st));
+	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;
+	st->plat_data =
+		*(struct mpu_platform_data *)dev_get_platdata(&client->dev);
+	inv_mpu_hw_init(client);
+
+	/* 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;
+	}
+	result = iio_buffer_register(indio_dev, indio_dev->channels,
+					indio_dev->num_channels);
+	if (result) {
+		pr_err("ring buffer register fail\n");
+		goto out_unreg_ring;
+	}
+	st->irq = gpio_to_irq(client->irq);
+	result = inv_mpu_probe_trigger(indio_dev);
+	if (result) {
+		pr_err("trigger probe fail\n");
+		goto out_remove_ring;
+	}
+
+	/* Tell the i2c counter, we have an IRQ */
+	INV_I2C_SETIRQ(MPU, gpio_to_irq(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->adapter->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_remove_ring:
+	iio_buffer_unregister(indio_dev);
+out_unreg_ring:
+	inv_mpu_unconfigure_ring(indio_dev);
+out_free:
+	iio_device_free(indio_dev);
+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);
+	iio_buffer_unregister(indio_dev);
+	inv_mpu_unconfigure_ring(indio_dev);
+	iio_device_free(indio_dev);
+
+	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 inv_mpu_iio_s *st =
+			iio_priv(i2c_get_clientdata(to_i2c_client(dev)));
+	pr_debug("%s inv_mpu_suspend\n", st->hw->name);
+	return st->set_power_state(st, false);
+}
+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},
+	{}
+};
+
+MODULE_DEVICE_TABLE(i2c, inv_mpu_id);
+
+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,
+	},
+	.address_list = normal_i2c,
+};
+
+static int __init inv_mpu_init(void)
+{
+	int result = i2c_add_driver(&inv_mpu_driver);
+	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");
+
+/**
+ *  @}
+ */