Initial file dump from open source releaseHEADmaster

1 files changed, 1874 insertions, 0 deletions

diff --git a/drivers/iio/imu/inv_mpu/inv_mpu_misc.c b/drivers/iio/imu/inv_mpu/inv_mpu_misc.c
new file mode 100644
index 00000000..22351018
--- /dev/null
+++ b/drivers/iio/imu/inv_mpu/inv_mpu_misc.c
@@ -0,0 +1,1874 @@
+/*
+* 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_misc.c
+ *      @brief   A sysfs device driver for Invensense mpu.
+ *      @details This file is part of invensense mpu driver code
+ */
+
+#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/crc32.h>
+
+#include "inv_mpu_iio.h"
+#include "inv_counters.h"
+
+/* DMP defines */
+#define DMP_ORIENTATION_TIME		500
+#define DMP_ORIENTATION_ANGLE		60
+#define DMP_DEFAULT_FIFO_RATE           200
+#define DMP_TAP_SCALE                   (767603923 / 5)
+#define DMP_MULTI_SHIFT                 30
+#define DMP_MULTI_TAP_TIME              500
+#define DMP_SHAKE_REJECT_THRESH         100
+#define DMP_SHAKE_REJECT_TIME           10
+#define DMP_SHAKE_REJECT_TIMEOUT        10
+#define DMP_ANGLE_SCALE                 15
+#define DMP_PRECISION                   1000
+#define DMP_MAX_DIVIDER                 4
+#define DMP_MAX_MIN_TAPS                4
+#define DMP_IMAGE_CRC_VALUE             0xa87fd63c
+#define DMP_IMAGE_SIZE                  3058
+
+/*--- Test parameters defaults --- */
+#define DEF_OLDEST_SUPP_PROD_REV    8
+#define DEF_OLDEST_SUPP_SW_REV      2
+
+/* sample rate */
+#define DEF_SELFTEST_SAMPLE_RATE             0
+/* LPF parameter */
+#define DEF_SELFTEST_LPF_PARA                1
+/* full scale setting dps */
+#define DEF_SELFTEST_GYRO_FULL_SCALE         (0 << 3)
+#define DEF_SELFTEST_ACCL_FULL_SCALE         (2 << 3)
+#define DEF_SELFTEST_GYRO_SENS            (32768 / 250)
+/* wait time before collecting data */
+#define DEF_GYRO_WAIT_TIME          50
+#define DEF_ST_STABLE_TIME          200
+#define DEF_GYRO_PACKET_THRESH      DEF_GYRO_WAIT_TIME
+#define DEF_GYRO_THRESH             10
+#define DEF_GYRO_SCALE              131
+#define DEF_ST_PRECISION            1000
+#define DEF_ST_ACCL_FULL_SCALE      8000UL
+#define DEF_ST_SCALE                (1L << 15)
+#define DEF_ST_TRY_TIMES            2
+#define DEF_ST_COMPASS_RESULT_SHIFT 2
+#define DEF_ST_ACCEL_RESULT_SHIFT   1
+#define DEF_ST_OTP0_THRESH          60
+#define DEF_ST_ABS_THRESH           20
+#define DEF_ST_TOR                  2
+
+#define DEF_ST_COMPASS_WAIT_MIN     (10 * 1000)
+#define DEF_ST_COMPASS_WAIT_MAX     (15 * 1000)
+#define DEF_ST_COMPASS_TRY_TIMES    10
+#define DEF_ST_COMPASS_8963_SHIFT   2
+
+#define X                           0
+#define Y                           1
+#define Z                           2
+/*---- MPU6050 notable product revisions ----*/
+#define MPU_PRODUCT_KEY_B1_E1_5      105
+#define MPU_PRODUCT_KEY_B2_F1        431
+/* accelerometer Hw self test min and max bias shift (mg) */
+#define DEF_ACCEL_ST_SHIFT_MIN       300
+#define DEF_ACCEL_ST_SHIFT_MAX       950
+
+#define DEF_ACCEL_ST_SHIFT_DELTA     140
+#define DEF_GYRO_CT_SHIFT_DELTA      140
+/* gyroscope Coriolis self test min and max bias shift (dps) */
+#define DEF_GYRO_CT_SHIFT_MIN        10
+#define DEF_GYRO_CT_SHIFT_MAX        105
+
+static struct test_setup_t test_setup = {
+	.gyro_sens     = DEF_SELFTEST_GYRO_SENS,
+	.sample_rate   = DEF_SELFTEST_SAMPLE_RATE,
+	.lpf           = DEF_SELFTEST_LPF_PARA,
+	.fsr           = DEF_SELFTEST_GYRO_FULL_SCALE,
+	.accl_fs      = DEF_SELFTEST_ACCL_FULL_SCALE
+};
+
+/* NOTE: product entries are in chronological order */
+static const struct prod_rev_map_t prod_rev_map[] = {
+	/* prod_ver = 0 */
+	{MPL_PROD_KEY(0,   1), MPU_SILICON_REV_A2, 131, 16384},
+	{MPL_PROD_KEY(0,   2), MPU_SILICON_REV_A2, 131, 16384},
+	{MPL_PROD_KEY(0,   3), MPU_SILICON_REV_A2, 131, 16384},
+	{MPL_PROD_KEY(0,   4), MPU_SILICON_REV_A2, 131, 16384},
+	{MPL_PROD_KEY(0,   5), MPU_SILICON_REV_A2, 131, 16384},
+	{MPL_PROD_KEY(0,   6), MPU_SILICON_REV_A2, 131, 16384},
+	/* prod_ver = 1 */
+	{MPL_PROD_KEY(0,   7), MPU_SILICON_REV_A2, 131, 16384},
+	{MPL_PROD_KEY(0,   8), MPU_SILICON_REV_A2, 131, 16384},
+	{MPL_PROD_KEY(0,   9), MPU_SILICON_REV_A2, 131, 16384},
+	{MPL_PROD_KEY(0,  10), MPU_SILICON_REV_A2, 131, 16384},
+	{MPL_PROD_KEY(0,  11), MPU_SILICON_REV_A2, 131, 16384},
+	{MPL_PROD_KEY(0,  12), MPU_SILICON_REV_A2, 131, 16384},
+	{MPL_PROD_KEY(0,  13), MPU_SILICON_REV_A2, 131, 16384},
+	{MPL_PROD_KEY(0,  14), MPU_SILICON_REV_A2, 131, 16384},
+	{MPL_PROD_KEY(0,  15), MPU_SILICON_REV_A2, 131, 16384},
+	{MPL_PROD_KEY(0,  27), MPU_SILICON_REV_A2, 131, 16384},
+	/* prod_ver = 1 */
+	{MPL_PROD_KEY(1,  16), MPU_SILICON_REV_B1, 131, 16384},
+	{MPL_PROD_KEY(1,  17), MPU_SILICON_REV_B1, 131, 16384},
+	{MPL_PROD_KEY(1,  18), MPU_SILICON_REV_B1, 131, 16384},
+	{MPL_PROD_KEY(1,  19), MPU_SILICON_REV_B1, 131, 16384},
+	{MPL_PROD_KEY(1,  20), MPU_SILICON_REV_B1, 131, 16384},
+	{MPL_PROD_KEY(1,  28), MPU_SILICON_REV_B1, 131, 16384},
+	{MPL_PROD_KEY(1,   1), MPU_SILICON_REV_B1, 131, 16384},
+	{MPL_PROD_KEY(1,   2), MPU_SILICON_REV_B1, 131, 16384},
+	{MPL_PROD_KEY(1,   3), MPU_SILICON_REV_B1, 131, 16384},
+	{MPL_PROD_KEY(1,   4), MPU_SILICON_REV_B1, 131, 16384},
+	{MPL_PROD_KEY(1,   5), MPU_SILICON_REV_B1, 131, 16384},
+	{MPL_PROD_KEY(1,   6), MPU_SILICON_REV_B1, 131, 16384},
+	/* prod_ver = 2 */
+	{MPL_PROD_KEY(2,   7), MPU_SILICON_REV_B1, 131, 16384},
+	{MPL_PROD_KEY(2,   8), MPU_SILICON_REV_B1, 131, 16384},
+	{MPL_PROD_KEY(2,   9), MPU_SILICON_REV_B1, 131, 16384},
+	{MPL_PROD_KEY(2,  10), MPU_SILICON_REV_B1, 131, 16384},
+	{MPL_PROD_KEY(2,  11), MPU_SILICON_REV_B1, 131, 16384},
+	{MPL_PROD_KEY(2,  12), MPU_SILICON_REV_B1, 131, 16384},
+	{MPL_PROD_KEY(2,  29), MPU_SILICON_REV_B1, 131, 16384},
+	/* prod_ver = 3 */
+	{MPL_PROD_KEY(3,  30), MPU_SILICON_REV_B1, 131, 16384},
+	/* prod_ver = 4 */
+	{MPL_PROD_KEY(4,  31), MPU_SILICON_REV_B1, 131,  8192},
+	{MPL_PROD_KEY(4,   1), MPU_SILICON_REV_B1, 131,  8192},
+	{MPL_PROD_KEY(4,   3), MPU_SILICON_REV_B1, 131,  8192},
+	/* prod_ver = 5 */
+	{MPL_PROD_KEY(5,   3), MPU_SILICON_REV_B1, 131, 16384},
+	/* prod_ver = 6 */
+	{MPL_PROD_KEY(6,  19), MPU_SILICON_REV_B1, 131, 16384},
+	/* prod_ver = 7 */
+	{MPL_PROD_KEY(7,  19), MPU_SILICON_REV_B1, 131, 16384},
+	/* prod_ver = 8 */
+	{MPL_PROD_KEY(8,  19), MPU_SILICON_REV_B1, 131, 16384},
+	/* prod_ver = 9 */
+	{MPL_PROD_KEY(9,  19), MPU_SILICON_REV_B1, 131, 16384},
+	/* prod_ver = 10 */
+	{MPL_PROD_KEY(10, 19), MPU_SILICON_REV_B1, 131, 16384}
+};
+
+/*
+*   List of product software revisions
+*
+*   NOTE :
+*   software revision 0 falls back to the old detection method
+*   based off the product version and product revision per the
+*   table above
+*/
+static const struct prod_rev_map_t sw_rev_map[] = {
+	{0,		     0,   0,     0},
+	{1, MPU_SILICON_REV_B1, 131,  8192},	/* rev C */
+	{2, MPU_SILICON_REV_B1, 131, 16384}	/* rev D */
+};
+
+static const int accl_st_tb[31] = {
+	340, 351, 363, 375, 388, 401, 414, 428,
+	443, 458, 473, 489, 506, 523, 541, 559,
+	578, 597, 617, 638, 660, 682, 705, 729,
+	753, 779, 805, 832, 860, 889, 919};
+static const int gyro_6050_st_tb[31] = {
+	3275, 3425, 3583, 3748, 3920, 4100, 4289, 4486,
+	4693, 4909, 5134, 5371, 5618, 5876, 6146, 6429,
+	6725, 7034, 7358, 7696, 8050, 8421, 8808, 9213,
+	9637, 10080, 10544, 11029, 11537, 12067, 12622};
+static const int gyro_3500_st_tb[255] = {
+	2620, 2646, 2672, 2699, 2726, 2753, 2781, 2808,
+	2837, 2865, 2894, 2923, 2952, 2981, 3011, 3041,
+	3072, 3102, 3133, 3165, 3196, 3228, 3261, 3293,
+	3326, 3359, 3393, 3427, 3461, 3496, 3531, 3566,
+	3602, 3638, 3674, 3711, 3748, 3786, 3823, 3862,
+	3900, 3939, 3979, 4019, 4059, 4099, 4140, 4182,
+	4224, 4266, 4308, 4352, 4395, 4439, 4483, 4528,
+	4574, 4619, 4665, 4712, 4759, 4807, 4855, 4903,
+	4953, 5002, 5052, 5103, 5154, 5205, 5257, 5310,
+	5363, 5417, 5471, 5525, 5581, 5636, 5693, 5750,
+	5807, 5865, 5924, 5983, 6043, 6104, 6165, 6226,
+	6289, 6351, 6415, 6479, 6544, 6609, 6675, 6742,
+	6810, 6878, 6946, 7016, 7086, 7157, 7229, 7301,
+	7374, 7448, 7522, 7597, 7673, 7750, 7828, 7906,
+	7985, 8065, 8145, 8227, 8309, 8392, 8476, 8561,
+	8647, 8733, 8820, 8909, 8998, 9088, 9178, 9270,
+	9363, 9457, 9551, 9647, 9743, 9841, 9939, 10038,
+	10139, 10240, 10343, 10446, 10550, 10656, 10763, 10870,
+	10979, 11089, 11200, 11312, 11425, 11539, 11654, 11771,
+	11889, 12008, 12128, 12249, 12371, 12495, 12620, 12746,
+	12874, 13002, 13132, 13264, 13396, 13530, 13666, 13802,
+	13940, 14080, 14221, 14363, 14506, 14652, 14798, 14946,
+	15096, 15247, 15399, 15553, 15709, 15866, 16024, 16184,
+	16346, 16510, 16675, 16842, 17010, 17180, 17352, 17526,
+	17701, 17878, 18057, 18237, 18420, 18604, 18790, 18978,
+	19167, 19359, 19553, 19748, 19946, 20145, 20347, 20550,
+	20756, 20963, 21173, 21385, 21598, 21814, 22033, 22253,
+	22475, 22700, 22927, 23156, 23388, 23622, 23858, 24097,
+	24338, 24581, 24827, 25075, 25326, 25579, 25835, 26093,
+	26354, 26618, 26884, 27153, 27424, 27699, 27976, 28255,
+	28538, 28823, 29112, 29403, 29697, 29994, 30294, 30597,
+	30903, 31212, 31524, 31839, 32157, 32479, 32804};
+
+char *wr_pr_debug_begin(u8 const *data, u32 len, char *string)
+{
+	int ii;
+	string = kmalloc(len * 2 + 1, GFP_KERNEL);
+	for (ii = 0; ii < len; ii++)
+		sprintf(&string[ii * 2], "%02X", data[ii]);
+	string[len * 2] = 0;
+	return string;
+}
+
+char *wr_pr_debug_end(char *string)
+{
+	kfree(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 msgs[3];
+	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 */
+	msgs[0].addr = mpu_addr;
+	msgs[0].flags = 0;
+	msgs[0].buf = bank;
+	msgs[0].len = sizeof(bank);
+
+	msgs[1].addr = mpu_addr;
+	msgs[1].flags = 0;
+	msgs[1].buf = addr;
+	msgs[1].len = sizeof(addr);
+
+	msgs[2].addr = mpu_addr;
+	msgs[2].flags = 0;
+	msgs[2].buf = (u8 *)buf;
+	msgs[2].len = len + 1;
+
+	INV_I2C_INC_MPUWRITE(3 + 3 + (2 + len));
+#if CONFIG_DYNAMIC_DEBUG
+	{
+		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);
+	}
+#endif
+
+	res = i2c_transfer(st->sl_handle, msgs, 3);
+	if (res != 3) {
+		if (res >= 0)
+			res = -EIO;
+		return res;
+	} else {
+		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 msgs[4];
+	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 */
+	msgs[0].addr = mpu_addr;
+	msgs[0].flags = 0;
+	msgs[0].buf = bank;
+	msgs[0].len = sizeof(bank);
+
+	msgs[1].addr = mpu_addr;
+	msgs[1].flags = 0;
+	msgs[1].buf = addr;
+	msgs[1].len = sizeof(addr);
+
+	msgs[2].addr = mpu_addr;
+	msgs[2].flags = 0;
+	msgs[2].buf = &buf;
+	msgs[2].len = 1;
+
+	msgs[3].addr = mpu_addr;
+	msgs[3].flags = I2C_M_RD;
+	msgs[3].buf = data;
+	msgs[3].len = len;
+
+	res = i2c_transfer(st->sl_handle, msgs, 4);
+	if (res != 4) {
+		if (res >= 0)
+			res = -EIO;
+	} else
+		res = 0;
+
+	INV_I2C_INC_MPUWRITE(3 + 3 + 3);
+	INV_I2C_INC_MPUREAD(len);
+#if CONFIG_DYNAMIC_DEBUG
+	{
+		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));
+	}
+#endif
+
+	return res;
+}
+
+int mpu_memory_write_unaligned(struct inv_mpu_iio_s *st, u16 key, int len,
+			       u8 const *d)
+{
+	int addr;
+	int start, end;
+	int len1, len2;
+	int result = 0;
+	if (len > MPU_MEM_BANK_SIZE)
+		return -EINVAL;
+	addr = inv_dmp_get_address(key);
+	start = (addr >> 8);
+	end = ((addr + len - 1) >> 8);
+	if (start == end) {
+		result = mpu_memory_write(st, st->i2c_addr, addr, len, d);
+	} else {
+		end <<= 8;
+		len1 = end - addr;
+		len2 = len - len1;
+		result = mpu_memory_write(st, st->i2c_addr, addr, len1, d);
+		result |= mpu_memory_write(st, st->i2c_addr, end, len2,
+					   d + len1);
+	}
+
+	return result;
+}
+
+/**
+ *  index_of_key()- Inverse lookup of the index of an MPL product key .
+ *  @key: the MPL product indentifier also referred to as 'key'.
+ */
+static short index_of_key(u16 key)
+{
+	int i;
+	for (i = 0; i < NUM_OF_PROD_REVS; i++)
+		if (prod_rev_map[i].mpl_product_key == key)
+			return (short)i;
+	return -EINVAL;
+}
+
+int inv_get_silicon_rev_mpu6500(struct inv_mpu_iio_s *st)
+{
+	struct inv_chip_info_s *chip_info = &st->chip_info;
+	int result;
+	u8 whoami, sw_rev;
+
+	result = inv_i2c_read(st, REG_WHOAMI, 1, &whoami);
+	if (result)
+		return result;
+	if (whoami != MPU6500_ID && whoami != MPU9250_ID)
+		return -EINVAL;
+
+	/*memory read need more time after power up */
+	msleep(POWER_UP_TIME);
+	result = mpu_memory_read(st, st->i2c_addr,
+			MPU6500_MEM_REV_ADDR, 1, &sw_rev);
+	if (sw_rev == 0) {
+		pr_warning("Rev 0 of MPU6500\n");
+		pr_warning("can't sit with other devices in same I2C bus\n");
+	}
+	if (result)
+		return result;
+	if (sw_rev > MPU6500_REV)
+		return -EINVAL;
+
+	/* these values are place holders and not real values */
+	chip_info->product_id = MPU6500_PRODUCT_REVISION;
+	chip_info->product_revision = MPU6500_PRODUCT_REVISION;
+	chip_info->silicon_revision = MPU6500_PRODUCT_REVISION;
+	chip_info->software_revision = sw_rev;
+	chip_info->gyro_sens_trim = DEFAULT_GYRO_TRIM;
+	chip_info->accl_sens_trim = DEFAULT_ACCL_TRIM;
+	chip_info->multi = 1;
+
+	return 0;
+}
+
+int inv_get_silicon_rev_mpu6050(struct inv_mpu_iio_s *st)
+{
+	int result;
+	struct inv_reg_map_s *reg;
+	u8 prod_ver = 0x00, prod_rev = 0x00;
+	struct prod_rev_map_t *p_rev;
+	u8 bank =
+	    (BIT_PRFTCH_EN | BIT_CFG_USER_BANK | MPU_MEM_OTP_BANK_0);
+	u16 mem_addr = ((bank << 8) | MEM_ADDR_PROD_REV);
+	u16 key;
+	u8 regs[5];
+	u16 sw_rev;
+	short index;
+	struct inv_chip_info_s *chip_info = &st->chip_info;
+	reg = &st->reg;
+
+	result = inv_i2c_read(st, REG_PRODUCT_ID, 1, &prod_ver);
+	if (result)
+		return result;
+	prod_ver &= 0xf;
+	/*memory read need more time after power up */
+	msleep(POWER_UP_TIME);
+	result = mpu_memory_read(st, st->i2c_addr, mem_addr,
+			1, &prod_rev);
+	if (result)
+		return result;
+	prod_rev >>= 2;
+	/* clean the prefetch and cfg user bank bits */
+	result = inv_i2c_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,
+				SOFT_PROD_VER_BYTES, regs);
+	if (result)
+		return result;
+
+	sw_rev = (regs[4] & 0x01) << 2 |	/* 0x0b, bit 0 */
+		 (regs[2] & 0x01) << 1 |	/* 0x09, bit 0 */
+		 (regs[0] & 0x01);		/* 0x07, bit 0 */
+	/* if 0, use the product key to determine the type of part */
+	if (sw_rev == 0) {
+		key = MPL_PROD_KEY(prod_ver, prod_rev);
+		if (key == 0)
+			return -EINVAL;
+		index = index_of_key(key);
+		if (index < 0 || index >= NUM_OF_PROD_REVS)
+			return -EINVAL;
+		/* check MPL is compiled for this device */
+		if (prod_rev_map[index].silicon_rev != MPU_SILICON_REV_B1)
+			return -EINVAL;
+		p_rev = (struct prod_rev_map_t *)&prod_rev_map[index];
+	/* if valid, use the software product key */
+	} else if (sw_rev < ARRAY_SIZE(sw_rev_map)) {
+		p_rev = (struct prod_rev_map_t *)&sw_rev_map[sw_rev];
+	} else {
+		return -EINVAL;
+	}
+	chip_info->product_id = prod_ver;
+	chip_info->product_revision = prod_rev;
+	chip_info->silicon_revision = p_rev->silicon_rev;
+	chip_info->software_revision = sw_rev;
+	chip_info->gyro_sens_trim = p_rev->gyro_trim;
+	chip_info->accl_sens_trim = p_rev->accel_trim;
+	if (chip_info->accl_sens_trim == 0)
+		chip_info->accl_sens_trim = DEFAULT_ACCL_TRIM;
+	chip_info->multi = DEFAULT_ACCL_TRIM / chip_info->accl_sens_trim;
+	if (chip_info->multi != 1)
+		pr_info("multi is %d\n", chip_info->multi);
+	return result;
+}
+
+/**
+ *  read_accel_hw_self_test_prod_shift()- read the accelerometer hardware
+ *                                         self-test bias shift calculated
+ *                                         during final production test and
+ *                                         stored in chip non-volatile memory.
+ *  @st:  main data structure.
+ *  @st_prod:   A pointer to an array of 3 elements to hold the values
+ *              for production hardware self-test bias shifts returned to the
+ *              user.
+ */
+static int read_accel_hw_self_test_prod_shift(struct inv_mpu_iio_s *st,
+					int *st_prod)
+{
+	u8 regs[4];
+	u8 shift_code[3];
+	int result, i;
+	st_prod[0] = 0;
+	st_prod[1] = 0;
+	st_prod[2] = 0;
+	result = inv_i2c_read(st, REG_ST_GCT_X, ARRAY_SIZE(regs), regs);
+	if (result)
+		return result;
+	if ((0 == regs[0])  && (0 == regs[1]) &&
+	    (0 == regs[2]) && (0 == regs[3]))
+		return -EINVAL;
+	shift_code[X] = ((regs[0] & 0xE0) >> 3) | ((regs[3] & 0x30) >> 4);
+	shift_code[Y] = ((regs[1] & 0xE0) >> 3) | ((regs[3] & 0x0C) >> 2);
+	shift_code[Z] = ((regs[2] & 0xE0) >> 3) |  (regs[3] & 0x03);
+	for (i = 0; i < 3; i++) {
+		if (shift_code[i] != 0)
+			st_prod[i] = test_setup.accl_sens[i]*
+				accl_st_tb[shift_code[i] - 1];
+	}
+
+	return 0;
+}
+/**
+* inv_check_accl_self_test()- check accel self test. this function returns
+*                              zero as success. A non-zero return value
+*                              indicates failure in self test.
+*  @*st: main data structure.
+*  @*reg_avg: average value of normal test.
+*  @*st_avg:  average value of self test
+*/
+static int inv_check_accl_self_test(struct inv_mpu_iio_s *st,
+	int *reg_avg, int *st_avg){
+	int gravity, reg_z_avg, g_z_sign, fs, j, ret_val;
+	int tmp1;
+	int st_shift_prod[THREE_AXIS], st_shift_cust[THREE_AXIS];
+	int st_shift_ratio[THREE_AXIS];
+	if (st->chip_info.software_revision < DEF_OLDEST_SUPP_SW_REV &&
+	    st->chip_info.product_revision < DEF_OLDEST_SUPP_PROD_REV)
+		return 0;
+	fs = DEF_ST_ACCL_FULL_SCALE;    /* assume +/- 2 mg as typical */
+	g_z_sign = 1;
+	ret_val = 0;
+	test_setup.accl_sens[X] = (u32)(DEF_ST_SCALE *
+						DEF_ST_PRECISION / fs);
+	test_setup.accl_sens[Y] = (u32)(DEF_ST_SCALE *
+						DEF_ST_PRECISION / fs);
+	test_setup.accl_sens[Z] = (u32)(DEF_ST_SCALE *
+						DEF_ST_PRECISION / fs);
+
+	if (MPL_PROD_KEY(st->chip_info.product_id,
+			 st->chip_info.product_revision) ==
+	    MPU_PRODUCT_KEY_B1_E1_5) {
+		/* half sensitivity Z accelerometer parts */
+		test_setup.accl_sens[Z] /= 2;
+	} else {
+		/* half sensitivity X, Y, Z accelerometer parts */
+		test_setup.accl_sens[X] /= st->chip_info.multi;
+		test_setup.accl_sens[Y] /= st->chip_info.multi;
+		test_setup.accl_sens[Z] /= st->chip_info.multi;
+	}
+	gravity = test_setup.accl_sens[Z];
+	reg_z_avg = reg_avg[Z] - g_z_sign * gravity*DEF_ST_PRECISION;
+	read_accel_hw_self_test_prod_shift(st, st_shift_prod);
+	for (j = 0; j < 3; j++) {
+		st_shift_cust[j] = abs(reg_avg[j] - st_avg[j]);
+		if (st_shift_prod[j]) {
+			tmp1 = st_shift_prod[j]/DEF_ST_PRECISION;
+			st_shift_ratio[j] = st_shift_cust[j]/tmp1
+				- DEF_ST_PRECISION;
+			if (st_shift_ratio[j] > DEF_ACCEL_ST_SHIFT_DELTA)
+				ret_val |= 1 << j;
+			if (st_shift_ratio[j] < -DEF_ACCEL_ST_SHIFT_DELTA)
+				ret_val |= 1 << j;
+		} else {
+			if (st_shift_cust[j] <
+				DEF_ACCEL_ST_SHIFT_MIN*gravity)
+				ret_val |= 1 << j;
+			if (st_shift_cust[j] >
+				DEF_ACCEL_ST_SHIFT_MAX*gravity)
+				ret_val |= 1 << j;
+		}
+	}
+
+	return ret_val;
+}
+/**
+* inv_check_3500_gyro_self_test() check gyro self test. this function returns
+*                                 zero as success. A non-zero return value
+*                                 indicates failure in self test.
+*  @*st: main data structure.
+*  @*reg_avg: average value of normal test.
+*  @*st_avg:  average value of self test
+*/
+
+static int inv_check_3500_gyro_self_test(struct inv_mpu_iio_s *st,
+	int *reg_avg, int *st_avg){
+	int result;
+	int gst[3], ret_val;
+	int gst_otp[3], i;
+	u8 st_code[THREE_AXIS];
+	ret_val = 0;
+
+	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);
+	if (result)
+		return result;
+	gst_otp[0] = 0;
+	gst_otp[1] = 0;
+	gst_otp[2] = 0;
+	for (i = 0; i < 3; i++) {
+		if (st_code[i] != 0)
+			gst_otp[i] = gyro_3500_st_tb[st_code[i] - 1];
+	}
+	/* check self test value passing criterion. Using the DEF_ST_TOR
+	 * for certain degree of tolerance */
+	for (i = 0; i < 3; i++) {
+		if (gst_otp[i] == 0) {
+			if (abs(gst[i]) * DEF_ST_TOR < DEF_ST_OTP0_THRESH *
+							DEF_ST_PRECISION *
+							DEF_GYRO_SCALE)
+				ret_val |= (1 << i);
+		} else {
+			if (abs(gst[i]/gst_otp[i] - DEF_ST_PRECISION) >
+					DEF_GYRO_CT_SHIFT_DELTA)
+				ret_val |= (1 << i);
+		}
+	}
+	/* check for absolute value passing criterion. Using DEF_ST_TOR
+	 * for certain degree of tolerance */
+	for (i = 0; i < 3; i++) {
+		if (abs(reg_avg[i]) > DEF_ST_TOR * DEF_ST_ABS_THRESH *
+		    DEF_ST_PRECISION * DEF_GYRO_SCALE)
+			ret_val |= (1 << i);
+	}
+
+	return ret_val;
+}
+
+/**
+* inv_check_6050_gyro_self_test() - check 6050 gyro self test. this function
+*                                   returns zero as success. A non-zero return
+*                                   value indicates failure in self test.
+*  @*st: main data structure.
+*  @*reg_avg: average value of normal test.
+*  @*st_avg:  average value of self test
+*/
+static int inv_check_6050_gyro_self_test(struct inv_mpu_iio_s *st,
+	int *reg_avg, int *st_avg){
+	int result;
+	int ret_val;
+	int ct_shift_prod[3], st_shift_cust[3], st_shift_ratio[3], i;
+	u8 regs[3];
+	if (st->chip_info.software_revision < DEF_OLDEST_SUPP_SW_REV &&
+	    st->chip_info.product_revision < DEF_OLDEST_SUPP_PROD_REV)
+		return 0;
+
+	ret_val = 0;
+	result = inv_i2c_read(st, REG_ST_GCT_X, 3, regs);
+	if (result)
+		return result;
+	regs[X] &= 0x1f;
+	regs[Y] &= 0x1f;
+	regs[Z] &= 0x1f;
+
+	for (i = 0; i < 3; i++) {
+		if (regs[i] != 0)
+			ct_shift_prod[i] = gyro_6050_st_tb[regs[i] - 1];
+		else
+			ct_shift_prod[i] = 0;
+	}
+	for (i = 0; i < 3; i++) {
+		st_shift_cust[i] = abs(reg_avg[i] - st_avg[i]);
+		if (ct_shift_prod[i]) {
+			st_shift_ratio[i] = st_shift_cust[i] /
+				ct_shift_prod[i] - DEF_ST_PRECISION;
+			if (st_shift_ratio[i] > DEF_GYRO_CT_SHIFT_DELTA)
+				ret_val |= 1 << i;
+			if (st_shift_ratio[i] < -DEF_GYRO_CT_SHIFT_DELTA)
+				ret_val |= 1 << i;
+		} else {
+			if (st_shift_cust[i] < DEF_ST_PRECISION *
+				DEF_GYRO_CT_SHIFT_MIN * test_setup.gyro_sens)
+				ret_val |= 1 << i;
+			if (st_shift_cust[i] > DEF_ST_PRECISION *
+				DEF_GYRO_CT_SHIFT_MAX * test_setup.gyro_sens)
+				ret_val |= 1 << i;
+		}
+	}
+	/* check for absolute value passing criterion. Using DEF_ST_TOR
+	 * for certain degree of tolerance */
+	for (i = 0; i < 3; i++) {
+		if (abs(reg_avg[i]) > DEF_ST_TOR * DEF_ST_ABS_THRESH *
+		    DEF_ST_PRECISION * DEF_GYRO_SCALE)
+			ret_val |= (1 << i);
+	}
+
+	return ret_val;
+}
+
+/**
+ *  inv_do_test() - do the actual test of self testing
+ */
+int inv_do_test(struct inv_mpu_iio_s *st, int self_test_flag,
+		int *gyro_result, int *accl_result)
+{
+	struct inv_reg_map_s *reg;
+	int result, i, j, packet_size;
+	u8 data[BYTES_PER_SENSOR * 2], has_accl;
+	int fifo_count, packet_count, ind;
+
+	reg = &st->reg;
+	has_accl = (st->chip_type != INV_ITG3500);
+	packet_size = BYTES_PER_SENSOR*(1 + has_accl);
+
+	result = inv_i2c_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);
+	if (result)
+		return result;
+	/* disable fifo reading */
+	result = inv_i2c_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);
+	if (result)
+		return result;
+	/* setup parameters */
+	result = inv_i2c_single_write(st, reg->lpf, test_setup.lpf);
+	if (result)
+		return result;
+	result = inv_i2c_single_write(st, reg->sample_rate_div,
+		test_setup.sample_rate);
+	if (result)
+		return result;
+	result = inv_i2c_single_write(st, reg->gyro_config,
+		self_test_flag | test_setup.fsr);
+	if (result)
+		return result;
+	if (has_accl) {
+		result = inv_i2c_single_write(st, reg->accl_config,
+			self_test_flag | test_setup.accl_fs);
+		if (result)
+			return result;
+	}
+	/* wait for the output to get stable */
+	if (self_test_flag)
+		msleep(DEF_ST_STABLE_TIME);
+
+	/* enable FIFO reading */
+	result = inv_i2c_single_write(st, reg->user_ctrl, BIT_FIFO_EN);
+	if (result)
+		return result;
+	/* enable sensor output to FIFO */
+	result = inv_i2c_single_write(st, reg->fifo_en, BITS_GYRO_OUT
+		| (has_accl << 3));
+	if (result)
+		return result;
+	mdelay(DEF_GYRO_WAIT_TIME);
+	/* stop sending data to FIFO */
+	result = inv_i2c_single_write(st, reg->fifo_en, 0);
+	if (result)
+		return result;
+	result = inv_i2c_read(st, reg->fifo_count_h, FIFO_COUNT_BYTE, data);
+	if (result)
+		return result;
+	fifo_count = be16_to_cpup((__be16 *)(&data[0]));
+	packet_count = fifo_count / packet_size;
+	for (i = 0; i < 3; i++) {
+		gyro_result[i] = 0;
+		accl_result[i] = 0;
+	}
+	if (abs(packet_count - DEF_GYRO_PACKET_THRESH) > DEF_GYRO_THRESH)
+		return -EAGAIN;
+
+	for (i = 0; i < packet_count; i++) {
+		/* getting FIFO data */
+		result = inv_i2c_read(st, reg->fifo_r_w,
+			packet_size, data);
+		if (result)
+			return result;
+		ind = 0;
+		if (has_accl) {
+			for (j = 0; j < THREE_AXIS; j++)
+				accl_result[j] +=
+					(short)be16_to_cpup(
+						(__be16 *)(&data[ind + 2 * j]));
+				ind += BYTES_PER_SENSOR;
+		}
+		for (j = 0; j < THREE_AXIS; j++)
+			gyro_result[j] +=
+				(short)be16_to_cpup(
+					(__be16 *)(&data[ind + 2 * j]));
+	}
+
+	gyro_result[0] = gyro_result[0] * DEF_ST_PRECISION / packet_count;
+	gyro_result[1] = gyro_result[1] * DEF_ST_PRECISION / packet_count;
+	gyro_result[2] = gyro_result[2] * DEF_ST_PRECISION / packet_count;
+	if (has_accl) {
+		accl_result[0] =
+			accl_result[0] * DEF_ST_PRECISION / packet_count;
+		accl_result[1] =
+			accl_result[1] * DEF_ST_PRECISION / packet_count;
+		accl_result[2] =
+			accl_result[2] * DEF_ST_PRECISION / packet_count;
+	}
+
+	return 0;
+}
+
+/**
+ *  inv_recover_setting() recover the old settings after everything is done
+ */
+
+void inv_recover_setting(struct inv_mpu_iio_s *st)
+{
+	struct inv_reg_map_s *reg;
+	int data;
+	struct iio_dev *indio = iio_priv_to_dev(st);
+
+	reg = &st->reg;
+	set_inv_enable(indio, st->chip_config.enable);
+	inv_i2c_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);
+	data = ONE_K_HZ/st->chip_config.fifo_rate - 1;
+	inv_i2c_single_write(st, reg->sample_rate_div, data);
+	if (INV_ITG3500 != st->chip_type) {
+		inv_i2c_single_write(st, reg->accl_config,
+				     (st->chip_config.accl_fs <<
+				     ACCL_CONFIG_FSR_SHIFT));
+	}
+	st->set_power_state(st, !st->chip_config.is_asleep);
+}
+
+static int inv_check_compass_self_test(struct inv_mpu_iio_s *st)
+{
+	int result;
+	u8 data[6];
+	u8 counter, cntl;
+	short x, y, z;
+	u8 *sens;
+	sens = st->chip_info.compass_sens;
+
+	/* set to bypass mode */
+	result = inv_i2c_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,
+				st->plat_data.int_config);
+		return result;
+	}
+	/* set to power down mode */
+	result = inv_secondary_write(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);
+	if (result)
+		goto AKM_fail;
+	/* set self test mode */
+	result = inv_secondary_write(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);
+		if (result)
+			goto AKM_fail;
+		if ((data[0] & DATA_AKM_DRDY) == 0)
+			counter--;
+		else
+			counter = 0;
+	}
+	if ((data[0] & DATA_AKM_DRDY) == 0) {
+		result = -EINVAL;
+		goto AKM_fail;
+	}
+	result = inv_secondary_read(REG_AKM_MEASURE_DATA,
+					BYTES_PER_SENSOR, data);
+	if (result)
+		goto AKM_fail;
+
+	x = le16_to_cpup((__le16 *)(&data[0]));
+	y = le16_to_cpup((__le16 *)(&data[2]));
+	z = le16_to_cpup((__le16 *)(&data[4]));
+	x = ((x * (sens[0] + 128)) >> 8);
+	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);
+		if (result)
+			goto AKM_fail;
+		if (0 == (cntl & DATA_AKM8963_BIT)) {
+			x <<= DEF_ST_COMPASS_8963_SHIFT;
+			y <<= DEF_ST_COMPASS_8963_SHIFT;
+			z <<= DEF_ST_COMPASS_8963_SHIFT;
+		}
+	}
+	result = -EINVAL;
+	if (x > st->compass_st_upper[X] || x < st->compass_st_lower[X])
+		goto AKM_fail;
+	if (y > st->compass_st_upper[Y] || y < st->compass_st_lower[Y])
+		goto AKM_fail;
+	if (z > st->compass_st_upper[Z] || z < st->compass_st_lower[Z])
+		goto AKM_fail;
+	result = 0;
+AKM_fail:
+	/*write 0 to ASTC register */
+	result |= inv_secondary_write(REG_AKM_ST_CTRL, 0);
+	/*set to power down mode */
+	result |= inv_secondary_write(REG_AKM_MODE, DATA_AKM_MODE_PD);
+	/*restore to non-bypass mode */
+	result |= inv_i2c_single_write(st, REG_INT_PIN_CFG,
+			st->plat_data.int_config);
+	return result;
+}
+
+static int inv_power_up_self_test(struct inv_mpu_iio_s *st)
+{
+	int result;
+	result = inv_i2c_single_write(st, st->reg.pwr_mgmt_1, INV_CLK_PLL);
+	if (result)
+		return result;
+	msleep(POWER_UP_TIME);
+	result = inv_i2c_single_write(st, st->reg.pwr_mgmt_2, 0);
+	if (result)
+		return result;
+	msleep(SENSOR_UP_TIME);
+
+	return 0;
+}
+
+/**
+ *  inv_hw_self_test() - main function to do hardware self test
+ */
+int inv_hw_self_test(struct inv_mpu_iio_s *st)
+{
+	int result;
+	int gyro_bias_st[THREE_AXIS], gyro_bias_regular[THREE_AXIS];
+	int accl_bias_st[THREE_AXIS], accl_bias_regular[THREE_AXIS];
+	int test_times;
+	char compass_result, accel_result, gyro_result;
+	if (st->chip_config.is_asleep      ||
+	    st->chip_config.lpa_mode       ||
+	    (!st->chip_config.gyro_enable) ||
+	    (!st->chip_config.accl_enable)) {
+		result = inv_power_up_self_test(st);
+		if (result)
+			return result;
+	}
+	compass_result = 0;
+	accel_result   = 0;
+	gyro_result    = 0;
+	test_times = DEF_ST_TRY_TIMES;
+	while (test_times > 0) {
+		result = inv_do_test(st, 0, gyro_bias_regular,
+			accl_bias_regular);
+		if (result == -EAGAIN)
+			test_times--;
+		else
+			test_times = 0;
+	}
+	if (result)
+		goto test_fail;
+
+	test_times = DEF_ST_TRY_TIMES;
+	while (test_times > 0) {
+		result = inv_do_test(st, BITS_SELF_TEST_EN, gyro_bias_st,
+					accl_bias_st);
+		if (result == -EAGAIN)
+			test_times--;
+		else
+			break;
+	}
+	if (result)
+		goto test_fail;
+	if (st->chip_type == INV_ITG3500) {
+		gyro_result = !inv_check_3500_gyro_self_test(st,
+			gyro_bias_regular, gyro_bias_st);
+	} else {
+		if (st->chip_config.has_compass)
+			compass_result = !inv_check_compass_self_test(st);
+		accel_result = !inv_check_accl_self_test(st,
+			accl_bias_regular, accl_bias_st);
+		gyro_result = !inv_check_6050_gyro_self_test(st,
+			gyro_bias_regular, gyro_bias_st);
+	}
+test_fail:
+	inv_recover_setting(st);
+
+	return (compass_result << DEF_ST_COMPASS_RESULT_SHIFT) |
+		(accel_result << DEF_ST_ACCEL_RESULT_SHIFT) | gyro_result;
+}
+
+/**
+ *  inv_hw_self_test_6500() - main function to do hardware self test for 6500
+ */
+int inv_hw_self_test_6500(struct inv_mpu_iio_s *st)
+{
+	int compass_result;
+	compass_result = !inv_check_compass_self_test(st);
+	return compass_result << DEF_ST_COMPASS_RESULT_SHIFT;
+}
+
+static int inv_load_firmware(struct inv_mpu_iio_s *st,
+	u8 *data, int size)
+{
+	int bank, write_size;
+	int result;
+	u16 memaddr;
+
+	/* Write and verify memory */
+	for (bank = 0; size > 0; bank++,
+		size -= write_size,
+		data += write_size) {
+		if (size > MPU_MEM_BANK_SIZE)
+			write_size = MPU_MEM_BANK_SIZE;
+		else
+			write_size = size;
+
+		memaddr = ((bank << 8) | 0x00);
+
+		result = mem_w(memaddr, write_size, data);
+		if (result)
+			return result;
+	}
+	return 0;
+}
+
+static int inv_verify_firmware(struct inv_mpu_iio_s *st,
+	u8 *data, int size)
+{
+	int bank, write_size;
+	int result;
+	u16 memaddr;
+	u8 firmware[MPU_MEM_BANK_SIZE];
+
+	/* Write and verify memory */
+	for (bank = 0; size > 0; bank++,
+		size -= write_size,
+		data += write_size) {
+		if (size > MPU_MEM_BANK_SIZE)
+			write_size = MPU_MEM_BANK_SIZE;
+		else
+			write_size = size;
+
+		memaddr = ((bank << 8) | 0x00);
+		result = mpu_memory_read(st,
+			st->i2c_addr, memaddr, write_size, firmware);
+		if (result)
+			return result;
+		if (0 != memcmp(firmware, data, write_size))
+			return -EINVAL;
+	}
+	return 0;
+}
+
+static int inv_set_fifo_div(struct inv_mpu_iio_s *st,
+		u16 fifoRate)
+{
+	u8 regs[2];
+	int result = 0;
+	/*For some reason DINAC4 is defined as 0xb8, but DINBC4 is not*/
+	const u8 regs_end[12] = {DINAFE, DINAF2, DINAAB, 0xc4,
+					DINAAA, DINAF1, DINADF, DINADF,
+					0xbb, 0xaf, DINADF, DINADF};
+
+	regs[0] = (u8)((fifoRate >> 8) & 0xff);
+	regs[1] = (u8)(fifoRate & 0xff);
+	result = mem_w_key(KEY_D_0_22, ARRAY_SIZE(regs), regs);
+	if (result)
+		return result;
+
+	/*Modify the FIFO handler to reset the tap/orient interrupt flags*/
+	/* each time the FIFO handler runs*/
+	result = mem_w_key(KEY_CFG_6, ARRAY_SIZE(regs_end), regs_end);
+
+	return result;
+}
+
+int inv_send_quaternion(struct inv_mpu_iio_s *st, bool on)
+{
+	const u8 regs_on[] = {DINBC0, DINBC2,
+					 DINBC4, DINBC6};
+	const u8 regs_off[] = {DINA80, DINA80,
+					  DINA80, DINA80};
+	const u8 *regs;
+	u8 result;
+	if (on)
+		regs = regs_on;
+	else
+		regs = regs_off;
+	result = mem_w_key(KEY_CFG_LP_QUAT, ARRAY_SIZE(regs_on), regs);
+
+	return result;
+}
+
+int inv_set_display_orient_interrupt_dmp(struct inv_mpu_iio_s *st,
+						bool on)
+{
+	/*Turn on the display orientation interrupt in the DMP*/
+	int result;
+	u8  regs[] = {0xd8};
+
+	if (on)
+		regs[0] = 0xd9;
+	result = mem_w_key(KEY_CFG_DISPLAY_ORIENT_INT, 1, regs);
+	return result;
+}
+
+int inv_set_fifo_rate(struct inv_mpu_iio_s *st, u16 fifo_rate)
+{
+	u8 divider;
+	int result;
+
+	divider = (u8)(ONE_K_HZ / fifo_rate) - 1;
+	if (divider > DMP_MAX_DIVIDER) {
+		st->sample_divider = DMP_MAX_DIVIDER;
+		st->fifo_divider =
+			(u8)(DMP_DEFAULT_FIFO_RATE / fifo_rate) - 1;
+	} else {
+		st->sample_divider = divider;
+		st->fifo_divider = 0;
+	}
+
+	result = inv_set_fifo_div(st, st->fifo_divider);
+	return result;
+}
+
+static int inv_set_tap_interrupt_dmp(struct inv_mpu_iio_s *st,
+	u8 on)
+{
+	int result;
+	u8  regs[] = {0};
+
+	if (on)
+		regs[0] = 0xf8;
+	else
+		regs[0] = DINAD8;
+	result = mem_w_key(KEY_CFG_20, ARRAY_SIZE(regs), regs);
+	if (result)
+		return result;
+	return result;
+}
+
+int inv_set_tap_threshold_dmp(struct inv_mpu_iio_s *st,
+				u32 axis, u16 threshold)
+{
+	/* Sets the tap threshold in the dmp
+	Simultaneously sets secondary tap threshold to help correct the tap
+	direction for soft taps */
+	int result;
+	/* DMP Algorithm */
+	u8 data[2];
+	int sampleDivider;
+	int scaledThreshold;
+	u32 dmpThreshold;
+	u8 sample_div;
+	const u32  accel_sens = (0x20000000 / 0x00010000);
+
+	if ((axis & ~(INV_TAP_AXIS_ALL)) || (threshold > (1 << 15)))
+		return -EINVAL;
+	sample_div = st->sample_divider;
+
+	sampleDivider = (1 + sample_div);
+	/* Scale factor corresponds linearly using
+	* 0  : 0
+	* 25 : 0.0250  g/ms
+	* 50 : 0.0500  g/ms
+	* 100: 1.0000  g/ms
+	* 200: 2.0000  g/ms
+	* 400: 4.0000  g/ms
+	* 800: 8.0000  g/ms
+	*/
+	/*multiply by 1000 to avoid floating point 1000/1000*/
+	scaledThreshold = threshold;
+	/* Convert to per sample */
+	scaledThreshold *= sampleDivider;
+
+	/* Scale to DMP 16 bit value */
+	if (accel_sens != 0)
+		dmpThreshold = (u32)(scaledThreshold * accel_sens);
+	else
+		return -EINVAL;
+	dmpThreshold = dmpThreshold / DMP_PRECISION;
+
+	data[0] = dmpThreshold >> 8;
+	data[1] = dmpThreshold & 0xFF;
+
+	/* MPL algorithm */
+	if (axis & INV_TAP_AXIS_X) {
+		result = mem_w_key(KEY_DMP_TAP_THR_X, ARRAY_SIZE(data), data);
+		if (result)
+			return result;
+
+		/*Also set additional threshold for correcting the direction
+		of taps that were very near the threshold. */
+		data[0] = (dmpThreshold * 3 / 4) >> 8;
+		data[1] = (dmpThreshold * 3 / 4) & 0xFF;
+		result = mem_w_key(KEY_D_1_36, ARRAY_SIZE(data), data);
+		if (result)
+			return result;
+	}
+	if (axis & INV_TAP_AXIS_Y) {
+		result = mem_w_key(KEY_DMP_TAP_THR_Y, 2, data);
+		if (result)
+			return result;
+		data[0] = (dmpThreshold * 3 / 4) >> 8;
+		data[1] = (dmpThreshold * 3 / 4) & 0xFF;
+
+		result = mem_w_key(KEY_D_1_40, ARRAY_SIZE(data), data);
+		if (result)
+			return result;
+	}
+	if (axis & INV_TAP_AXIS_Z) {
+		result = mem_w_key(KEY_DMP_TAP_THR_Z, ARRAY_SIZE(data), data);
+		if (result)
+			return result;
+		data[0] = (dmpThreshold * 3 / 4) >> 8;
+		data[1] = (dmpThreshold * 3 / 4) & 0xFF;
+
+		result = mem_w_key(KEY_D_1_44, ARRAY_SIZE(data), data);
+		if (result)
+			return result;
+	}
+	return 0;
+}
+
+static int inv_set_tap_axes_dmp(struct inv_mpu_iio_s *st,
+				u32 axes)
+{
+	/* Sets a mask in the DMP that indicates what tap events
+	should result in an interrupt */
+	u8 regs[4];
+	u8 result;
+
+	/* check if any spurious bit other the ones expected are set */
+	if (axes & (~(INV_TAP_ALL_DIRECTIONS)))
+		return -EINVAL;
+
+	regs[0] = (u8)axes;
+	result = mem_w_key(KEY_D_1_72, 1, regs);
+
+	return result;
+}
+
+int inv_set_min_taps_dmp(struct inv_mpu_iio_s *st,
+				u16 min_taps) {
+	/*Indicates the minimum number of consecutive taps required
+		before the DMP will generate an interrupt */
+	u8 regs[1];
+	u8 result;
+	/* check if any spurious bit other the ones expected are set */
+	if ((min_taps > DMP_MAX_MIN_TAPS) || (min_taps < 1))
+		return -EINVAL;
+	regs[0] = (u8)(min_taps-1);
+	result = mem_w_key(KEY_D_1_79, ARRAY_SIZE(regs), regs);
+
+	return result;
+}
+
+int  inv_set_tap_time_dmp(struct inv_mpu_iio_s *st, u16 time)
+{
+	/* Determines how long after a tap the DMP requires before
+	  another tap can be registered*/
+	int result;
+	/* DMP Algorithm */
+	u16 dmpTime;
+	u8 data[2];
+	u8 sampleDivider;
+
+	sampleDivider = st->sample_divider;
+	sampleDivider++;
+
+	/* 60 ms minimum time added */
+	dmpTime = ((time) / sampleDivider);
+	data[0] = dmpTime >> 8;
+	data[1] = dmpTime & 0xFF;
+
+	result = mem_w_key(KEY_DMP_TAPW_MIN, ARRAY_SIZE(data), data);
+
+	return result;
+}
+
+static int inv_set_multiple_tap_time_dmp(struct inv_mpu_iio_s *st,
+					u32 time)
+{
+	/*Determines how close together consecutive taps must occur
+	to be considered double/triple taps*/
+	int result;
+	/* DMP Algorithm */
+	u16 dmpTime;
+	u8 data[2];
+	u8 sampleDivider;
+
+	sampleDivider = st->sample_divider;
+	sampleDivider++;
+
+	/* 60 ms minimum time added */
+	dmpTime = ((time) / sampleDivider);
+	data[0] = dmpTime >> 8;
+	data[1] = dmpTime & 0xFF;
+	result = mem_w_key(KEY_D_1_218, ARRAY_SIZE(data), data);
+
+	return result;
+}
+
+int inv_q30_mult(int a, int b)
+{
+	u64 temp;
+	int result;
+	temp = (u64)a * b;
+	result = (int)(temp >> DMP_MULTI_SHIFT);
+
+	return result;
+}
+
+static u16 inv_row_2_scale(const s8 *row)
+{
+	u16 b;
+
+	if (row[0] > 0)
+		b = 0;
+	else if (row[0] < 0)
+		b = 4;
+	else if (row[1] > 0)
+		b = 1;
+	else if (row[1] < 0)
+		b = 5;
+	else if (row[2] > 0)
+		b = 2;
+	else if (row[2] < 0)
+		b = 6;
+	else
+		b = 7;
+
+	return b;
+}
+
+/** Converts an orientation matrix made up of 0,+1,and -1 to a scalar
+*	representation.
+* @param[in] mtx Orientation matrix to convert to a scalar.
+* @return Description of orientation matrix. The lowest 2 bits (0 and 1)
+* represent the column the one is on for the
+* first row, with the bit number 2 being the sign. The next 2 bits
+* (3 and 4) represent
+* the column the one is on for the second row with bit number 5 being
+* the sign.
+* The next 2 bits (6 and 7) represent the column the one is on for the
+* third row with
+* bit number 8 being the sign. In binary the identity matrix would therefor
+* be: 010_001_000 or 0x88 in hex.
+*/
+static u16 inv_orientation_matrix_to_scaler(const signed char *mtx)
+{
+
+	u16 scalar;
+	scalar = inv_row_2_scale(mtx);
+	scalar |= inv_row_2_scale(mtx + 3) << 3;
+	scalar |= inv_row_2_scale(mtx + 6) << 6;
+
+	return scalar;
+}
+
+static int inv_gyro_dmp_cal(struct inv_mpu_iio_s *st)
+{
+	int inv_gyro_orient;
+	u8 regs[3];
+	int result;
+
+	u8 tmpD = DINA4C;
+	u8 tmpE = DINACD;
+	u8 tmpF = DINA6C;
+
+	inv_gyro_orient =
+		inv_orientation_matrix_to_scaler(st->plat_data.orientation);
+
+	if ((inv_gyro_orient & 3) == 0)
+		regs[0] = tmpD;
+	else if ((inv_gyro_orient & 3) == 1)
+		regs[0] = tmpE;
+	else if ((inv_gyro_orient & 3) == 2)
+		regs[0] = tmpF;
+	if ((inv_gyro_orient & 0x18) == 0)
+		regs[1] = tmpD;
+	else if ((inv_gyro_orient & 0x18) == 0x8)
+		regs[1] = tmpE;
+	else if ((inv_gyro_orient & 0x18) == 0x10)
+		regs[1] = tmpF;
+	if ((inv_gyro_orient & 0xc0) == 0)
+		regs[2] = tmpD;
+	else if ((inv_gyro_orient & 0xc0) == 0x40)
+		regs[2] = tmpE;
+	else if ((inv_gyro_orient & 0xc0) == 0x80)
+		regs[2] = tmpF;
+
+	result = mem_w_key(KEY_FCFG_1, 3, regs);
+	if (result)
+		return result;
+
+	if (inv_gyro_orient & 4)
+		regs[0] = DINA36 | 1;
+	else
+		regs[0] = DINA36;
+	if (inv_gyro_orient & 0x20)
+		regs[1] = DINA56 | 1;
+	else
+		regs[1] = DINA56;
+	if (inv_gyro_orient & 0x100)
+		regs[2] = DINA76 | 1;
+	else
+		regs[2] = DINA76;
+
+	result = mem_w_key(KEY_FCFG_3, ARRAY_SIZE(regs), regs);
+
+	return result;
+}
+
+static int inv_accel_dmp_cal(struct inv_mpu_iio_s *st)
+{
+	int inv_accel_orient;
+	int result;
+	u8 regs[3];
+	const u8 tmp[3] = { DINA0C, DINAC9, DINA2C };
+	inv_accel_orient =
+		inv_orientation_matrix_to_scaler(st->plat_data.orientation);
+
+	regs[0] = tmp[inv_accel_orient & 3];
+	regs[1] = tmp[(inv_accel_orient >> 3) & 3];
+	regs[2] = tmp[(inv_accel_orient >> 6) & 3];
+	result = mem_w_key(KEY_FCFG_2, 3, regs);
+	if (result)
+		return result;
+
+	regs[0] = DINA26;
+	regs[1] = DINA46;
+	regs[2] = DINA66;
+	if (inv_accel_orient & 4)
+		regs[0] |= 1;
+	if (inv_accel_orient & 0x20)
+		regs[1] |= 1;
+	if (inv_accel_orient & 0x100)
+		regs[2] |= 1;
+	result = mem_w_key(KEY_FCFG_7, ARRAY_SIZE(regs), regs);
+
+	return result;
+}
+
+static u16 inv_orientation_matrix_to_scalar(const s8 *mtx)
+{
+
+	u16 scalar;
+
+	/*
+       XYZ  010_001_000 Identity Matrix
+       XZY  001_010_000
+       YXZ  010_000_001
+       YZX  000_010_001
+       ZXY  001_000_010
+       ZYX  000_001_010
+	*/
+
+	scalar = inv_row_2_scale(mtx);
+	scalar |= inv_row_2_scale(mtx + 3) << 3;
+	scalar |= inv_row_2_scale(mtx + 6) << 6;
+
+	return scalar;
+}
+
+int inv_set_accel_bias_dmp(struct inv_mpu_iio_s *st)
+{
+	int inv_accel_orient, result, i, accel_bias_body[3], out[3];
+	int tmp[] = {1, 1, 1};
+	int mask[] = {4, 0x20, 0x100};
+	int accel_sf = 0x20000000;/* 536870912 */
+	u8 *regs;
+
+	inv_accel_orient =
+		inv_orientation_matrix_to_scalar(st->plat_data.orientation);
+
+	for (i = 0; i < 3; i++)
+		if (inv_accel_orient & mask[i])
+			tmp[i] = -1;
+
+	for (i = 0; i < 3; i++)
+		accel_bias_body[i] = st->input_accel_bias[(inv_accel_orient >>
+					(i * 3)) & 3] * tmp[i];
+	for (i = 0; i < 3; i++)
+		accel_bias_body[i] = inv_q30_mult(accel_sf,
+					accel_bias_body[i]);
+	for (i = 0; i < 3; i++)
+		out[i] = cpu_to_be32p(&accel_bias_body[i]);
+	regs = (u8 *)out;
+	result = mem_w_key(KEY_D_ACCEL_BIAS, sizeof(out), regs);
+
+	return result;
+}
+
+static int inv_set_gyro_sf_dmp(struct inv_mpu_iio_s *st)
+{
+	/*The gyro threshold, in dps, above which taps will be rejected*/
+	int result;
+	/* DMP Algorithm */
+	u8 sampleDivider;
+	u32 gyro_sf;
+	const u32 gyro_sens = 0x03e80000;
+
+	sampleDivider = st->sample_divider;
+	gyro_sf = inv_q30_mult(gyro_sens,
+			(int)(DMP_TAP_SCALE * (sampleDivider + 1)));
+	result = write_be32_key_to_mem(st, gyro_sf, KEY_D_0_104);
+
+	return result;
+}
+
+static int inv_set_shake_reject_thresh_dmp(struct inv_mpu_iio_s *st,
+						int thresh)
+{	/*THIS FUNCTION FAILS MEM_W*/
+	/*The gyro threshold, in dps, above which taps will be rejected */
+	int result;
+	/* DMP Algorithm */
+	u8 sampleDivider;
+	int thresh_scaled;
+	u32 gyro_sf;
+	const u32 gyro_sens = 0x03e80000;
+	sampleDivider = st->sample_divider;
+	gyro_sf = inv_q30_mult(gyro_sens, (int)(DMP_TAP_SCALE *
+			(sampleDivider + 1)));
+	/* We're in units of DPS, convert it back to chip units*/
+	/*split the operation to aviod overflow of integer*/
+	thresh_scaled = gyro_sens / (1L << 16);
+	thresh_scaled = thresh_scaled / thresh;
+	thresh_scaled = gyro_sf / thresh_scaled;
+	result = write_be32_key_to_mem(st, thresh_scaled, KEY_D_1_92);
+
+	return result;
+}
+
+static int inv_set_shake_reject_time_dmp(struct inv_mpu_iio_s *st,
+						u32 time)
+{
+	/* How long a gyro axis must remain above its threshold
+	before taps are rejected */
+	int result;
+	/* DMP Algorithm */
+	u16 dmpTime;
+	u8 data[2];
+	u8 sampleDivider;
+
+	sampleDivider = st->sample_divider;
+	sampleDivider++;
+
+	/* 60 ms minimum time added */
+	dmpTime = ((time) / sampleDivider);
+	data[0] = dmpTime >> 8;
+	data[1] = dmpTime & 0xFF;
+
+	result = mem_w_key(KEY_D_1_88, ARRAY_SIZE(data), data);
+	return result;
+}
+
+static int inv_set_shake_reject_timeout_dmp(struct inv_mpu_iio_s *st,
+						u32 time)
+{
+	/*How long the gyros must remain below their threshold,
+	after taps have been rejected, before taps can be detected again*/
+	int result;
+	/* DMP Algorithm */
+	u16 dmpTime;
+	u8 data[2];
+	u8 sampleDivider;
+
+	sampleDivider = st->sample_divider;
+	sampleDivider++;
+
+	/* 60 ms minimum time added */
+	dmpTime = ((time) / sampleDivider);
+	data[0] = dmpTime >> 8;
+	data[1] = dmpTime & 0xFF;
+
+	result = mem_w_key(KEY_D_1_90, ARRAY_SIZE(data), data);
+	return result;
+}
+
+int inv_set_interrupt_on_gesture_event(struct inv_mpu_iio_s *st, bool on)
+{
+	u8 result;
+	const u8 regs_on[] = {DINADA, DINADA, DINAB1, DINAB9,
+					 DINAF3, DINA8B, DINAA3, DINA91,
+					 DINAB6, DINADA, DINAB4, DINADA};
+	const u8 regs_off[] = {0xd8, 0xd8, 0xb1, 0xb9, 0xf3, 0x8b,
+					  0xa3, 0x91, 0xb6, 0x09, 0xb4, 0xd9};
+	/*For some reason DINAC4 is defined as 0xb8,
+	but DINBC4 is not defined.*/
+	const u8 regs_end[] = {DINAFE, DINAF2, DINAAB, 0xc4,
+					DINAAA, DINAF1, DINADF, DINADF,
+					0xbb, 0xaf, DINADF, DINADF};
+	const u8 regs[] = {0, 0};
+	/* reset fifo count to zero */
+	result = mem_w_key(KEY_D_1_178, ARRAY_SIZE(regs), regs);
+	if (result)
+		return result;
+
+	if (on)
+		/*Sets the DMP to send an interrupt and put a FIFO packet
+		in the FIFO if and only if a tap/orientation event
+		just occurred*/
+		result = mem_w_key(KEY_CFG_FIFO_ON_EVENT, ARRAY_SIZE(regs_on),
+					regs_on);
+	else
+		/*Sets the DMP to send an interrupt and put a FIFO packet
+		in the FIFO at the rate specified by the FIFO div.
+		see inv_set_fifo_div in hw_setup.c to set the FIFO div.*/
+		result = mem_w_key(KEY_CFG_FIFO_ON_EVENT, ARRAY_SIZE(regs_off),
+					regs_off);
+	if (result)
+		return result;
+
+	result = mem_w_key(KEY_CFG_6, ARRAY_SIZE(regs_end), regs_end);
+
+	return result;
+}
+
+/**
+ * inv_enable_tap_dmp() -  calling this function will enable/disable tap function.
+ */
+int inv_enable_tap_dmp(struct inv_mpu_iio_s *st, bool on)
+{
+	int result;
+	result = inv_set_tap_interrupt_dmp(st, on);
+	if (result)
+		return result;
+	if (on) {
+		result = inv_set_tap_threshold_dmp(st, INV_TAP_AXIS_X,
+						   st->tap.thresh);
+		if (result)
+			return result;
+		result = inv_set_tap_threshold_dmp(st, INV_TAP_AXIS_Y,
+						   st->tap.thresh);
+		if (result)
+			return result;
+		result = inv_set_tap_threshold_dmp(st, INV_TAP_AXIS_Z,
+						   st->tap.thresh);
+		if (result)
+			return result;
+	}
+
+	result = inv_set_tap_axes_dmp(st, INV_TAP_ALL_DIRECTIONS);
+	if (result)
+		return result;
+	result = inv_set_min_taps_dmp(st, st->tap.min_count);
+	if (result)
+		return result;
+
+	result = inv_set_tap_time_dmp(st, st->tap.time);
+	if (result)
+		return result;
+
+	result = inv_set_multiple_tap_time_dmp(st, DMP_MULTI_TAP_TIME);
+	if (result)
+		return result;
+
+	result = inv_set_gyro_sf_dmp(st);
+	if (result)
+		return result;
+
+	result = inv_set_shake_reject_thresh_dmp(st, DMP_SHAKE_REJECT_THRESH);
+	if (result)
+		return result;
+
+	result = inv_set_shake_reject_time_dmp(st, DMP_SHAKE_REJECT_TIME);
+	if (result)
+		return result;
+
+	result = inv_set_shake_reject_timeout_dmp(st,
+						  DMP_SHAKE_REJECT_TIMEOUT);
+	return result;
+}
+
+int inv_send_sensor_data(struct inv_mpu_iio_s *st, u16 elements)
+{
+	int result;
+	u8 regs[] = {DINAA0 + 3, DINAA0 + 3, DINAA0 + 3,
+				DINAA0 + 3, DINAA0 + 3, DINAA0 + 3,
+				DINAA0 + 3, DINAA0 + 3, DINAA0 + 3,
+				DINAA0 + 3};
+
+	if (elements & INV_ELEMENT_1)
+		regs[0] = DINACA;
+	if (elements & INV_ELEMENT_2)
+		regs[4] = DINBC4;
+	if (elements & INV_ELEMENT_3)
+		regs[5] = DINACC;
+	if (elements & INV_ELEMENT_4)
+		regs[6] = DINBC6;
+	if ((elements & INV_ELEMENT_5) || (elements & INV_ELEMENT_6) ||
+	    (elements & INV_ELEMENT_7)) {
+		regs[1] = DINBC0;
+		regs[2] = DINAC8;
+		regs[3] = DINBC2;
+	}
+	result = mem_w_key(KEY_CFG_15, ARRAY_SIZE(regs), regs);
+	return result;
+}
+
+int inv_send_interrupt_word(struct inv_mpu_iio_s *st, bool on)
+{
+	const u8 regs_on[] = { DINA20 };
+	const u8 regs_off[] = { DINAA3 };
+	u8 result;
+
+	if (on)
+		result = mem_w_key(KEY_CFG_27, ARRAY_SIZE(regs_on), regs_on);
+	else
+		result = mem_w_key(KEY_CFG_27, ARRAY_SIZE(regs_off), regs_off);
+
+	return result;
+}
+
+/**
+ * inv_dmp_firmware_write() -  calling this function will load the firmware.
+ *                        This is the write function of file "dmp_firmware".
+ */
+ssize_t inv_dmp_firmware_write(struct file *fp, struct kobject *kobj,
+	struct bin_attribute *attr,
+	char *buf, loff_t pos, size_t size)
+{
+	u8 *firmware;
+	int result;
+	struct inv_reg_map_s *reg;
+	struct iio_dev *indio_dev;
+	struct inv_mpu_iio_s *st;
+
+	indio_dev = dev_get_drvdata(container_of(kobj, struct device, kobj));
+	st = iio_priv(indio_dev);
+
+	if (st->chip_config.is_asleep || st->chip_config.firmware_loaded)
+		return -EINVAL;
+
+	reg = &st->reg;
+	if (DMP_IMAGE_SIZE != size) {
+		pr_err("wrong DMP image size\n");
+		return -EINVAL;
+	}
+
+	firmware = kmalloc(size, GFP_KERNEL);
+	if (!firmware)
+		return -ENOMEM;
+
+	memcpy(firmware, buf, size);
+	result = crc32(CRC_FIRMWARE_SEED, firmware, size);
+	if (DMP_IMAGE_CRC_VALUE != result) {
+		result = -EINVAL;
+		pr_err("firmware CRC error - 0x%08x vs 0x%08x\n",
+			result, DMP_IMAGE_CRC_VALUE);
+		goto firmware_write_fail;
+	}
+
+	result = inv_load_firmware(st, firmware, size);
+	if (result)
+		goto firmware_write_fail;
+
+	result = inv_verify_firmware(st, firmware, size);
+	if (result)
+		goto firmware_write_fail;
+
+	result = inv_i2c_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,
+	st->chip_config.prog_start_addr & 0xff);
+	if (result)
+		goto firmware_write_fail;
+
+	result = inv_set_fifo_rate(st, DMP_DEFAULT_FIFO_RATE);
+	if (result)
+		goto firmware_write_fail;
+	result = inv_gyro_dmp_cal(st);
+	if (result)
+		goto firmware_write_fail;
+	result = inv_accel_dmp_cal(st);
+	if (result)
+		goto firmware_write_fail;
+	st->chip_config.firmware_loaded = 1;
+	result = size;
+firmware_write_fail:
+	kfree(firmware);
+
+	return result;
+}
+
+ssize_t inv_dmp_firmware_read(struct file *filp,
+				struct kobject *kobj,
+				struct bin_attribute *bin_attr,
+				char *buf, loff_t off, size_t count)
+{
+	int bank, write_size, size, data, result;
+	u16 memaddr;
+	struct iio_dev *indio_dev;
+	struct inv_mpu_iio_s *st;
+
+	size = count;
+	indio_dev = dev_get_drvdata(container_of(kobj, struct device, kobj));
+	st = iio_priv(indio_dev);
+
+	data = 0;
+	for (bank = 0; size > 0; bank++, size -= write_size,
+					data += write_size) {
+		if (size > MPU_MEM_BANK_SIZE)
+			write_size = MPU_MEM_BANK_SIZE;
+		else
+			write_size = size;
+
+		memaddr = (bank << 8);
+		result = mpu_memory_read(st,
+			st->i2c_addr, memaddr, write_size, &buf[data]);
+		if (result)
+			return result;
+	}
+
+	return count;
+}
+/**
+ *  @}
+ */