diff options
Diffstat (limited to 'drivers/misc/mpu3050/mpu-dev.c')
| -rwxr-xr-x | drivers/misc/mpu3050/mpu-dev.c | 2280 |
1 files changed, 2280 insertions, 0 deletions
diff --git a/drivers/misc/mpu3050/mpu-dev.c b/drivers/misc/mpu3050/mpu-dev.c new file mode 100755 index 00000000000..ef04ed7a96c --- /dev/null +++ b/drivers/misc/mpu3050/mpu-dev.c @@ -0,0 +1,2280 @@ +/* + mpu-dev.c - mpu3050 char device interface + + Copyright (C) 1995-97 Simon G. Vogl + Copyright (C) 1998-99 Frodo Looijaard <frodol@dds.nl> + Copyright (C) 2003 Greg Kroah-Hartman <greg@kroah.com> + Copyright (C) 2010 InvenSense Corporation, All Rights Reserved. + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + + 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. + + You should have received a copy of the GNU General Public License + along with this program. If not, see <http://www.gnu.org/licenses/>. +*/ +/* Code inside mpudev_ioctl_rdrw is copied from i2c-dev.c + */ + +#include <linux/i2c.h> +#include <linux/i2c-dev.h> +#include <linux/interrupt.h> +#include <linux/module.h> +#include <linux/moduleparam.h> +#include <linux/kernel.h> +#include <linux/init.h> +#include <linux/stat.h> +#include <linux/irq.h> +#include <linux/gpio.h> +#include <linux/signal.h> +#include <linux/miscdevice.h> +#include <linux/slab.h> +#include <linux/version.h> +#include <linux/pm.h> + +#ifdef CONFIG_HAS_EARLYSUSPEND +#include <linux/earlysuspend.h> +#endif + +#include <linux/errno.h> +#include <linux/fs.h> +#include <linux/mm.h> +#include <linux/sched.h> +#include <linux/wait.h> +#include <linux/uaccess.h> +#include <linux/io.h> + +#include "mpuirq.h" +#include "slaveirq.h" +#include "mlsl.h" +#include "mlos.h" +#include "mpu-i2c.h" +#include "mldl_cfg.h" +#include "mpu-accel.h" + +#include "mpu.h" + +#define ACCEL_VENDOR_NAME "KIONIX" +#define ACCEL_CHIP_NAME "KXTF9" + +#define GYRO_VENDOR_NAME "INVENSENSE" +#define GYRO_CHIP_NAME "MPU-3050" + +#define MPU3050_EARLY_SUSPEND_IN_DRIVER 1 + +#define CALIBRATION_FILE_PATH "/efs/calibration_data" +#define CALIBRATION_DATA_AMOUNT 100 + +struct acc_data cal_data; + +/* Platform data for the MPU */ +struct mpu_private_data { + struct mldl_cfg mldl_cfg; + +#ifdef CONFIG_HAS_EARLYSUSPEND + struct early_suspend early_suspend; +#endif +}; + +static int is_lis3dh; + +#define IDEAL_X 0 +#define IDEAL_Y 0 +#define IDEAL_Z 1024 + +static int pid; + +static struct i2c_client *this_client; + +int read_accel_raw_xyz(struct acc_data *acc) +{ + unsigned char acc_data[6]; + s32 temp; + struct mldl_cfg *mldl_cfg; + + struct mpu_private_data *mpu = + (struct mpu_private_data *)i2c_get_clientdata(this_client); + + if (!mpu) { + pr_info("%s : mpu data is NULL, mpu3050 Init error", __func__); + return 0; + } + + mldl_cfg = &mpu->mldl_cfg; + + if (mldl_cfg->accel_is_suspended == 1 || + (mldl_cfg->dmp_is_running == 0 + && mldl_cfg->accel_is_suspended == 0)) { + if (is_lis3dh) { + if (mldl_cfg->accel_is_suspended == 1) { + sensor_i2c_write_register(this_client->adapter, + 0x19, 0x20, 0x67); + MLOSSleep(1); + } + sensor_i2c_read(this_client->adapter, + 0x19, 0x28 | 0x80, 6, acc_data); + if (mldl_cfg->accel_is_suspended == 1) { + sensor_i2c_write_register(this_client->adapter, + 0x19, 0x20, 0x18); + MLOSSleep(1); + } + } else + sensor_i2c_read(this_client->adapter, + 0x0F, 0x06, 6, acc_data); + } else if (mldl_cfg->dmp_is_running && + mldl_cfg->accel_is_suspended == 0) { + if (sensor_i2c_read(this_client->adapter, + DEFAULT_MPU_SLAVEADDR, + 0x23, 6, acc_data) != 0) + return -1; + } else + return -1; + + if (is_lis3dh) { + acc->x = ((acc_data[0] << 8) | acc_data[1]); + acc->x = (acc->x >> 4); + acc->y = ((acc_data[2] << 8) | acc_data[3]); + acc->y = (acc->y >> 4); + acc->z = ((acc_data[4] << 8) | acc_data[5]); + acc->z = (acc->z >> 4); + } else { + temp = ((acc_data[1] << 4) | (acc_data[0] >> 4)); + if (temp < 2048) + acc->x = (s16) (-temp); + else + acc->x = (s16) (4096 - temp); + + temp = ((acc_data[3] << 4) | (acc_data[2] >> 4)); + if (temp < 2048) + acc->y = (s16) (-temp); + else + acc->y = (s16) (4096 - temp); + + temp = ((acc_data[5] << 4) | (acc_data[4] >> 4)); + if (temp < 2048) + acc->z = (s16) (1024 - temp); + else + acc->z = (s16) (3072 - temp); + } + return 0; +} + +static int accel_open_calibration(void) +{ + struct file *cal_filp = NULL; + int err = 0; + mm_segment_t old_fs; + + old_fs = get_fs(); + set_fs(KERNEL_DS); + + cal_filp = filp_open(CALIBRATION_FILE_PATH, O_RDONLY, 0666); + if (IS_ERR(cal_filp)) { + pr_err("%s: Can't open calibration file\n", __func__); + set_fs(old_fs); + err = PTR_ERR(cal_filp); + + cal_data.x = 0; + cal_data.y = 0; + cal_data.z = 0; + + return err; + } + + err = cal_filp->f_op->read(cal_filp, + (char *)&cal_data, 3 * sizeof(s16), + &cal_filp->f_pos); + if (err != 3 * sizeof(s16)) { + pr_err("%s: Can't read the cal data from file\n", __func__); + err = -EIO; + } + + pr_info("%s : (%u,%u,%u)\n", __func__, + cal_data.x, cal_data.y, cal_data.z); + + filp_close(cal_filp, current->files); + set_fs(old_fs); + + return err; +} + +static int accel_do_calibrate(bool do_calib) +{ + struct acc_data data = { 0, }; + struct file *cal_filp = NULL; + int sum[3] = { 0, }; + int err = 0; + int i; + mm_segment_t old_fs; + + if (do_calib) { + for (i = 0; i < CALIBRATION_DATA_AMOUNT; i++) { + err = read_accel_raw_xyz(&data); + if (err < 0) { + pr_err("%s: accel_read_accel_raw_xyz() " + "failed in the %dth loop\n", + __func__, i); + return err; + } + + sum[0] += data.x; + sum[1] += data.y; + sum[2] += data.z; + } + + + if (is_lis3dh) { + cal_data.x = IDEAL_X - cal_data.x; + cal_data.y = IDEAL_Y - cal_data.y; + cal_data.z = IDEAL_Z - cal_data.z; + } else { + cal_data.x = sum[0] / CALIBRATION_DATA_AMOUNT; + cal_data.y = sum[1] / CALIBRATION_DATA_AMOUNT; + cal_data.z = sum[2] / CALIBRATION_DATA_AMOUNT; + } + } else { + cal_data.x = 0; + cal_data.y = 0; + cal_data.z = 0; + } + + pr_info("%s: cal data (%d,%d,%d)\n", __func__, + cal_data.x, cal_data.y, cal_data.z); + + old_fs = get_fs(); + set_fs(KERNEL_DS); + + cal_filp = filp_open(CALIBRATION_FILE_PATH, + O_CREAT | O_TRUNC | O_WRONLY, 0666); + if (IS_ERR(cal_filp)) { + pr_err("%s: Can't open calibration file\n", __func__); + set_fs(old_fs); + err = PTR_ERR(cal_filp); + return err; + } + + err = cal_filp->f_op->write(cal_filp, + (char *)&cal_data, 3 * sizeof(s16), + &cal_filp->f_pos); + if (err != 3 * sizeof(s16)) { + pr_err("%s: Can't write the cal data to file\n", __func__); + err = -EIO; + } + + filp_close(cal_filp, current->files); + set_fs(old_fs); + + return err; +} + +static int mpu_open(struct inode *inode, struct file *file) +{ + struct mpu_private_data *mpu = + (struct mpu_private_data *)i2c_get_clientdata(this_client); + struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg; + + accel_open_calibration(); + + pr_info("%s", __func__); + dev_dbg(&this_client->adapter->dev, "mpu_open\n"); + dev_dbg(&this_client->adapter->dev, "current->pid %d\n", current->pid); + pid = current->pid; + file->private_data = this_client; + + /* we could do some checking on the flags supplied by "open" */ + /* i.e. O_NONBLOCK */ + /* -> set some flag to disable interruptible_sleep_on in mpu_read */ + + /* Reset the sensors to the default */ + mldl_cfg->requested_sensors = ML_THREE_AXIS_GYRO; + if (mldl_cfg->accel && mldl_cfg->accel->resume) + mldl_cfg->requested_sensors |= ML_THREE_AXIS_ACCEL; + + if (mldl_cfg->compass && mldl_cfg->compass->resume) + mldl_cfg->requested_sensors |= ML_THREE_AXIS_COMPASS; + + if (mldl_cfg->pressure && mldl_cfg->pressure->resume) + mldl_cfg->requested_sensors |= ML_THREE_AXIS_PRESSURE; + + return 0; +} + +/* close function - called when the "file" /dev/mpu is closed in userspace */ +static int mpu_release(struct inode *inode, struct file *file) +{ + struct i2c_client *client = (struct i2c_client *)file->private_data; + struct mpu_private_data *mpu = + (struct mpu_private_data *)i2c_get_clientdata(client); + struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg; + struct i2c_adapter *accel_adapter; + struct i2c_adapter *compass_adapter; + struct i2c_adapter *pressure_adapter; + int result = 0; + + pid = 0; + + accel_adapter = i2c_get_adapter(mldl_cfg->pdata->accel.adapt_num); + compass_adapter = i2c_get_adapter(mldl_cfg->pdata->compass.adapt_num); + pressure_adapter = i2c_get_adapter(mldl_cfg->pdata->pressure.adapt_num); + result = mpu3050_suspend(mldl_cfg, client->adapter, + accel_adapter, compass_adapter, + pressure_adapter, TRUE, TRUE, TRUE, TRUE); + pr_info("%s", __func__); + dev_dbg(&this_client->adapter->dev, "mpu_release\n"); + return result; +} + +static noinline int mpudev_ioctl_rdrw(struct i2c_client *client, + unsigned long arg) +{ + struct i2c_rdwr_ioctl_data rdwr_arg; + struct i2c_msg *rdwr_pa; + u8 __user **data_ptrs; + int i, res; + + if (copy_from_user(&rdwr_arg, + (struct i2c_rdwr_ioctl_data __user *)arg, + sizeof(rdwr_arg))) + return -EFAULT; + + /* Put an arbitrary limit on the number of messages that can + * be sent at once */ + if (rdwr_arg.nmsgs > I2C_RDRW_IOCTL_MAX_MSGS) + return -EINVAL; + + rdwr_pa = (struct i2c_msg *) + kmalloc(rdwr_arg.nmsgs * sizeof(struct i2c_msg), GFP_KERNEL); + if (!rdwr_pa) + return -ENOMEM; + + if (copy_from_user(rdwr_pa, rdwr_arg.msgs, + rdwr_arg.nmsgs * sizeof(struct i2c_msg))) { + kfree(rdwr_pa); + return -EFAULT; + } + + data_ptrs = kmalloc(rdwr_arg.nmsgs * sizeof(u8 __user *), GFP_KERNEL); + if (data_ptrs == NULL) { + kfree(rdwr_pa); + return -ENOMEM; + } + + res = 0; + for (i = 0; i < rdwr_arg.nmsgs; i++) { + /* Limit the size of the message to a sane amount; + * and don't let length change either. */ + if ((rdwr_pa[i].len > 8192) || + (rdwr_pa[i].flags & I2C_M_RECV_LEN)) { + res = -EINVAL; + break; + } + data_ptrs[i] = (u8 __user *) rdwr_pa[i].buf; + rdwr_pa[i].buf = kmalloc(rdwr_pa[i].len, GFP_KERNEL); + if (rdwr_pa[i].buf == NULL) { + res = -ENOMEM; + break; + } + if (copy_from_user(rdwr_pa[i].buf, data_ptrs[i], + rdwr_pa[i].len)) { + ++i; /* Needs to be kfreed too */ + res = -EFAULT; + break; + } + } + if (res < 0) { + int j; + for (j = 0; j < i; ++j) + kfree(rdwr_pa[j].buf); + kfree(data_ptrs); + kfree(rdwr_pa); + return res; + } + + res = i2c_transfer(client->adapter, rdwr_pa, rdwr_arg.nmsgs); + while (i-- > 0) { + if (res >= 0 && (rdwr_pa[i].flags & I2C_M_RD)) { + if (copy_to_user(data_ptrs[i], rdwr_pa[i].buf, + rdwr_pa[i].len)) + res = -EFAULT; + } + kfree(rdwr_pa[i].buf); + } + kfree(data_ptrs); + kfree(rdwr_pa); + return res; +} + +/* read function called when from /dev/mpu is read. Read from the FIFO */ +static ssize_t mpu_read(struct file *file, + char __user *buf, size_t count, loff_t *offset) +{ + char *tmp; + int ret; + + struct i2c_client *client = (struct i2c_client *)file->private_data; + + if (count > 8192) + count = 8192; + + tmp = kmalloc(count, GFP_KERNEL); + if (tmp == NULL) + return -ENOMEM; + + pr_info("%s: i2c-dev: i2c-%d reading %zu bytes.\n", __func__, + iminor(file->f_path.dentry->d_inode), count); + +/* @todo fix this to do a i2c trasnfer from the FIFO */ + ret = i2c_master_recv(client, tmp, count); + if (ret >= 0) { + ret = copy_to_user(buf, tmp, count) ? -EFAULT : ret; + if (ret) + ret = -EFAULT; + } + kfree(tmp); + return ret; +} + +static int mpu_ioctl_set_mpu_pdata(struct i2c_client *client, unsigned long arg) +{ + int ii; + struct mpu_private_data *mpu = + (struct mpu_private_data *)i2c_get_clientdata(client); + struct mpu3050_platform_data *pdata = mpu->mldl_cfg.pdata; + struct mpu3050_platform_data local_pdata; + + if (copy_from_user(&local_pdata, (unsigned char __user *)arg, + sizeof(local_pdata))) + return -EFAULT; + + pdata->int_config = local_pdata.int_config; + for (ii = 0; ii < DIM(pdata->orientation); ii++) + pdata->orientation[ii] = local_pdata.orientation[ii]; + pdata->level_shifter = local_pdata.level_shifter; + + pdata->accel.address = local_pdata.accel.address; + for (ii = 0; ii < DIM(pdata->accel.orientation); ii++) + pdata->accel.orientation[ii] = + local_pdata.accel.orientation[ii]; + + pdata->compass.address = local_pdata.compass.address; + for (ii = 0; ii < DIM(pdata->compass.orientation); ii++) + pdata->compass.orientation[ii] = + local_pdata.compass.orientation[ii]; + + pdata->pressure.address = local_pdata.pressure.address; + for (ii = 0; ii < DIM(pdata->pressure.orientation); ii++) + pdata->pressure.orientation[ii] = + local_pdata.pressure.orientation[ii]; + + dev_dbg(&client->adapter->dev, "%s\n", __func__); + + return ML_SUCCESS; +} + +static int +mpu_ioctl_set_mpu_config(struct i2c_client *client, unsigned long arg) +{ + int ii; + int result = ML_SUCCESS; + struct mpu_private_data *mpu = + (struct mpu_private_data *)i2c_get_clientdata(client); + struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg; + struct mldl_cfg *temp_mldl_cfg; + + dev_dbg(&this_client->adapter->dev, "%s\n", __func__); + + temp_mldl_cfg = kzalloc(sizeof(struct mldl_cfg), GFP_KERNEL); + if (NULL == temp_mldl_cfg) + return -ENOMEM; + + /* + * User space is not allowed to modify accel compass pressure or + * pdata structs, as well as silicon_revision product_id or trim + */ + if (copy_from_user(temp_mldl_cfg, (struct mldl_cfg __user *)arg, + offsetof(struct mldl_cfg, silicon_revision))) { + result = -EFAULT; + goto out; + } + + if (mldl_cfg->gyro_is_suspended) { + if (mldl_cfg->addr != temp_mldl_cfg->addr) + mldl_cfg->gyro_needs_reset = TRUE; + + if (mldl_cfg->int_config != temp_mldl_cfg->int_config) + mldl_cfg->gyro_needs_reset = TRUE; + + if (mldl_cfg->ext_sync != temp_mldl_cfg->ext_sync) + mldl_cfg->gyro_needs_reset = TRUE; + + if (mldl_cfg->full_scale != temp_mldl_cfg->full_scale) + mldl_cfg->gyro_needs_reset = TRUE; + + if (mldl_cfg->lpf != temp_mldl_cfg->lpf) + mldl_cfg->gyro_needs_reset = TRUE; + + if (mldl_cfg->clk_src != temp_mldl_cfg->clk_src) + mldl_cfg->gyro_needs_reset = TRUE; + + if (mldl_cfg->divider != temp_mldl_cfg->divider) + mldl_cfg->gyro_needs_reset = TRUE; + + if (mldl_cfg->dmp_enable != temp_mldl_cfg->dmp_enable) + mldl_cfg->gyro_needs_reset = TRUE; + + if (mldl_cfg->fifo_enable != temp_mldl_cfg->fifo_enable) + mldl_cfg->gyro_needs_reset = TRUE; + + if (mldl_cfg->dmp_cfg1 != temp_mldl_cfg->dmp_cfg1) + mldl_cfg->gyro_needs_reset = TRUE; + + if (mldl_cfg->dmp_cfg2 != temp_mldl_cfg->dmp_cfg2) + mldl_cfg->gyro_needs_reset = TRUE; + + if (mldl_cfg->gyro_power != temp_mldl_cfg->gyro_power) + mldl_cfg->gyro_needs_reset = TRUE; + + for (ii = 0; ii < MPU_NUM_AXES; ii++) + if (mldl_cfg->offset_tc[ii] != + temp_mldl_cfg->offset_tc[ii]) + mldl_cfg->gyro_needs_reset = TRUE; + + for (ii = 0; ii < MPU_NUM_AXES; ii++) + if (mldl_cfg->offset[ii] != temp_mldl_cfg->offset[ii]) + mldl_cfg->gyro_needs_reset = TRUE; + + if (memcmp(mldl_cfg->ram, temp_mldl_cfg->ram, + MPU_MEM_NUM_RAM_BANKS * MPU_MEM_BANK_SIZE * + sizeof(unsigned char))) + mldl_cfg->gyro_needs_reset = TRUE; + } + + memcpy(mldl_cfg, temp_mldl_cfg, + offsetof(struct mldl_cfg, silicon_revision)); + +out: + kfree(temp_mldl_cfg); + return result; +} + +static int +mpu_ioctl_get_mpu_config(struct i2c_client *client, unsigned long arg) +{ + /* Have to be careful as there are 3 pointers in the mldl_cfg + * structure */ + struct mpu_private_data *mpu = + (struct mpu_private_data *)i2c_get_clientdata(client); + struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg; + struct mldl_cfg *local_mldl_cfg; + int retval = 0; + + local_mldl_cfg = kzalloc(sizeof(struct mldl_cfg), GFP_KERNEL); + if (NULL == local_mldl_cfg) + return -ENOMEM; + + retval = + copy_from_user(local_mldl_cfg, (struct mldl_cfg __user *)arg, + sizeof(struct mldl_cfg)); + if (retval) { + dev_err(&this_client->adapter->dev, + "%s|%s:%d: EFAULT on arg\n", + __FILE__, __func__, __LINE__); + retval = -EFAULT; + goto out; + } + + /* Fill in the accel, compass, pressure and pdata pointers */ + if (mldl_cfg->accel) { + retval = copy_to_user((void __user *)local_mldl_cfg->accel, + mldl_cfg->accel, + sizeof(*mldl_cfg->accel)); + if (retval) { + dev_err(&this_client->adapter->dev, + "%s|%s:%d: EFAULT on accel\n", + __FILE__, __func__, __LINE__); + retval = -EFAULT; + goto out; + } + } + + if (mldl_cfg->compass) { + retval = copy_to_user((void __user *)local_mldl_cfg->compass, + mldl_cfg->compass, + sizeof(*mldl_cfg->compass)); + if (retval) { + dev_err(&this_client->adapter->dev, + "%s|%s:%d: EFAULT on compass\n", + __FILE__, __func__, __LINE__); + retval = -EFAULT; + goto out; + } + } + + if (mldl_cfg->pressure) { + retval = copy_to_user((void __user *)local_mldl_cfg->pressure, + mldl_cfg->pressure, + sizeof(*mldl_cfg->pressure)); + if (retval) { + dev_err(&this_client->adapter->dev, + "%s|%s:%d: EFAULT on pressure\n", + __FILE__, __func__, __LINE__); + retval = -EFAULT; + goto out; + } + } + + if (mldl_cfg->pdata) { + retval = copy_to_user((void __user *)local_mldl_cfg->pdata, + mldl_cfg->pdata, + sizeof(*mldl_cfg->pdata)); + if (retval) { + dev_err(&this_client->adapter->dev, + "%s|%s:%d: EFAULT on pdata\n", + __FILE__, __func__, __LINE__); + retval = -EFAULT; + goto out; + } + } + + /* Do not modify the accel, compass, pressure and pdata pointers */ + retval = copy_to_user((struct mldl_cfg __user *)arg, + mldl_cfg, offsetof(struct mldl_cfg, accel)); + + if (retval) + retval = -EFAULT; +out: + kfree(local_mldl_cfg); + return retval; +} + +/** + * Pass a requested slave configuration to the slave sensor + * + * @param adapter the adaptor to use to communicate with the slave + * @param mldl_cfg the mldl configuration structuer + * @param slave pointer to the slave descriptor + * @param usr_config The configuration to pass to the slave sensor + * + * @return 0 or non-zero error code + */ +static int slave_config(void *adapter, + struct mldl_cfg *mldl_cfg, + struct ext_slave_descr *slave, + struct ext_slave_config __user *usr_config) +{ + int retval = ML_SUCCESS; + if ((slave) && (slave->config)) { + struct ext_slave_config config; + retval = copy_from_user(&config, usr_config, sizeof(config)); + if (retval) + return -EFAULT; + + if (config.len && config.data) { + int *data; + data = kzalloc(config.len, GFP_KERNEL); + if (!data) + return ML_ERROR_MEMORY_EXAUSTED; + + retval = copy_from_user(data, + (void __user *)config.data, + config.len); + if (retval) { + retval = -EFAULT; + kfree(data); + return retval; + } + config.data = data; + } + retval = slave->config(adapter, + slave, &mldl_cfg->pdata->accel, &config); + kfree(config.data); + } + return retval; +} + +/** + * Get a requested slave configuration from the slave sensor + * + * @param adapter the adaptor to use to communicate with the slave + * @param mldl_cfg the mldl configuration structuer + * @param slave pointer to the slave descriptor + * @param usr_config The configuration for the slave to fill out + * + * @return 0 or non-zero error code + */ +static int slave_get_config(void *adapter, + struct mldl_cfg *mldl_cfg, + struct ext_slave_descr *slave, + struct ext_slave_config __user *usr_config) +{ + int retval = ML_SUCCESS; + if ((slave) && (slave->get_config)) { + struct ext_slave_config config; + void *user_data; + retval = copy_from_user(&config, usr_config, sizeof(config)); + if (retval) + return -EFAULT; + + user_data = config.data; + if (config.len && config.data) { + int *data; + data = kzalloc(config.len, GFP_KERNEL); + if (!data) + return ML_ERROR_MEMORY_EXAUSTED; + + retval = copy_from_user(data, + (void __user *)config.data, + config.len); + if (retval) { + retval = -EFAULT; + kfree(data); + return retval; + } + config.data = data; + } + retval = slave->get_config(adapter, + slave, + &mldl_cfg->pdata->accel, &config); + if (retval) { + kfree(config.data); + return retval; + } + retval = copy_to_user((unsigned char __user *)user_data, + config.data, config.len); + kfree(config.data); + } + return retval; +} + +/* ioctl - I/O control */ +static long mpu_ioctl(struct file *file, unsigned int cmd, unsigned long arg) +{ + struct i2c_client *client = (struct i2c_client *)file->private_data; + struct mpu_private_data *mpu = + (struct mpu_private_data *)i2c_get_clientdata(client); + struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg; + int retval = 0; + struct i2c_adapter *accel_adapter; + struct i2c_adapter *compass_adapter; + struct i2c_adapter *pressure_adapter; + + accel_adapter = i2c_get_adapter(mldl_cfg->pdata->accel.adapt_num); + compass_adapter = i2c_get_adapter(mldl_cfg->pdata->compass.adapt_num); + pressure_adapter = i2c_get_adapter(mldl_cfg->pdata->pressure.adapt_num); + + switch (cmd) { + case I2C_RDWR: + mpudev_ioctl_rdrw(client, arg); + break; + case I2C_SLAVE: + if ((arg & 0x7E) != (client->addr & 0x7E)) { + dev_err(&this_client->adapter->dev, + "%s: Invalid I2C_SLAVE arg %lu\n", + __func__, arg); + } + break; + case MPU_SET_MPU_CONFIG: + retval = mpu_ioctl_set_mpu_config(client, arg); + break; + case MPU_SET_INT_CONFIG: + mldl_cfg->int_config = (unsigned char)arg; + break; + case MPU_SET_EXT_SYNC: + mldl_cfg->ext_sync = (enum mpu_ext_sync)arg; + break; + case MPU_SET_FULL_SCALE: + mldl_cfg->full_scale = (enum mpu_fullscale)arg; + break; + case MPU_SET_LPF: + mldl_cfg->lpf = (enum mpu_filter)arg; + break; + case MPU_SET_CLK_SRC: + mldl_cfg->clk_src = (enum mpu_clock_sel)arg; + break; + case MPU_SET_DIVIDER: + mldl_cfg->divider = (unsigned char)arg; + break; + case MPU_SET_LEVEL_SHIFTER: + mldl_cfg->pdata->level_shifter = (unsigned char)arg; + break; + case MPU_SET_DMP_ENABLE: + mldl_cfg->dmp_enable = (unsigned char)arg; + break; + case MPU_SET_FIFO_ENABLE: + mldl_cfg->fifo_enable = (unsigned char)arg; + break; + case MPU_SET_DMP_CFG1: + mldl_cfg->dmp_cfg1 = (unsigned char)arg; + break; + case MPU_SET_DMP_CFG2: + mldl_cfg->dmp_cfg2 = (unsigned char)arg; + break; + case MPU_SET_OFFSET_TC: + retval = copy_from_user(mldl_cfg->offset_tc, + (unsigned char __user *)arg, + sizeof(mldl_cfg->offset_tc)); + if (retval) + retval = -EFAULT; + break; + case MPU_SET_RAM: + retval = copy_from_user(mldl_cfg->ram, + (unsigned char __user *)arg, + sizeof(mldl_cfg->ram)); + if (retval) + retval = -EFAULT; + break; + case MPU_SET_PLATFORM_DATA: + retval = mpu_ioctl_set_mpu_pdata(client, arg); + break; + case MPU_GET_MPU_CONFIG: + retval = mpu_ioctl_get_mpu_config(client, arg); + break; + case MPU_GET_INT_CONFIG: + retval = put_user(mldl_cfg->int_config, + (unsigned char __user *)arg); + break; + case MPU_GET_EXT_SYNC: + retval = put_user(mldl_cfg->ext_sync, + (unsigned char __user *)arg); + break; + case MPU_GET_FULL_SCALE: + retval = put_user(mldl_cfg->full_scale, + (unsigned char __user *)arg); + break; + case MPU_GET_LPF: + retval = put_user(mldl_cfg->lpf, (unsigned char __user *)arg); + break; + case MPU_GET_CLK_SRC: + retval = put_user(mldl_cfg->clk_src, + (unsigned char __user *)arg); + break; + case MPU_GET_DIVIDER: + retval = put_user(mldl_cfg->divider, + (unsigned char __user *)arg); + break; + case MPU_GET_LEVEL_SHIFTER: + retval = put_user(mldl_cfg->pdata->level_shifter, + (unsigned char __user *)arg); + break; + case MPU_GET_DMP_ENABLE: + retval = put_user(mldl_cfg->dmp_enable, + (unsigned char __user *)arg); + break; + case MPU_GET_FIFO_ENABLE: + retval = put_user(mldl_cfg->fifo_enable, + (unsigned char __user *)arg); + break; + case MPU_GET_DMP_CFG1: + retval = put_user(mldl_cfg->dmp_cfg1, + (unsigned char __user *)arg); + break; + case MPU_GET_DMP_CFG2: + retval = put_user(mldl_cfg->dmp_cfg2, + (unsigned char __user *)arg); + break; + case MPU_GET_OFFSET_TC: + retval = copy_to_user((unsigned char __user *)arg, + mldl_cfg->offset_tc, + sizeof(mldl_cfg->offset_tc)); + if (retval) + retval = -EFAULT; + break; + case MPU_GET_RAM: + retval = copy_to_user((unsigned char __user *)arg, + mldl_cfg->ram, sizeof(mldl_cfg->ram)); + if (retval) + retval = -EFAULT; + break; + case MPU_CONFIG_ACCEL: + retval = slave_config(accel_adapter, mldl_cfg, + mldl_cfg->accel, + (struct ext_slave_config __user *)arg); + break; + case MPU_CONFIG_COMPASS: + retval = slave_config(compass_adapter, mldl_cfg, + mldl_cfg->compass, + (struct ext_slave_config __user *)arg); + break; + case MPU_CONFIG_PRESSURE: + retval = slave_config(pressure_adapter, mldl_cfg, + mldl_cfg->pressure, + (struct ext_slave_config __user *)arg); + break; + case MPU_GET_CONFIG_ACCEL: + retval = slave_get_config(accel_adapter, mldl_cfg, + mldl_cfg->accel, + (struct ext_slave_config __user *) + arg); + break; + case MPU_GET_CONFIG_COMPASS: + retval = slave_get_config(compass_adapter, mldl_cfg, + mldl_cfg->compass, + (struct ext_slave_config __user *) + arg); + break; + case MPU_GET_CONFIG_PRESSURE: + retval = slave_get_config(pressure_adapter, mldl_cfg, + mldl_cfg->pressure, + (struct ext_slave_config __user *) + arg); + break; + case MPU_SUSPEND: + { + unsigned long sensors; + sensors = ~(mldl_cfg->requested_sensors); + retval = mpu3050_suspend(mldl_cfg, + client->adapter, + accel_adapter, + compass_adapter, + pressure_adapter, + ((sensors & ML_THREE_AXIS_GYRO) + == ML_THREE_AXIS_GYRO), + ((sensors & + ML_THREE_AXIS_ACCEL) + == ML_THREE_AXIS_ACCEL), + ((sensors & + ML_THREE_AXIS_COMPASS) + == ML_THREE_AXIS_COMPASS), + ((sensors & + ML_THREE_AXIS_PRESSURE) + == ML_THREE_AXIS_PRESSURE)); + } + break; + case MPU_RESUME: + { + unsigned long sensors; + sensors = mldl_cfg->requested_sensors; + retval = mpu3050_resume(mldl_cfg, + client->adapter, + accel_adapter, + compass_adapter, + pressure_adapter, + sensors & ML_THREE_AXIS_GYRO, + sensors & ML_THREE_AXIS_ACCEL, + sensors & ML_THREE_AXIS_COMPASS, + sensors & + ML_THREE_AXIS_PRESSURE); + } + break; + case MPU_READ_ACCEL: + { + unsigned char data[6]; + retval = mpu3050_read_accel(mldl_cfg, client->adapter, + data); + + if ((ML_SUCCESS == retval) && + (copy_to_user((unsigned char __user *)arg, + data, sizeof(data)))) + retval = -EFAULT; + } + break; + case MPU_READ_COMPASS: + { + unsigned char data[6]; + struct i2c_adapter *compass_adapt = + i2c_get_adapter(mldl_cfg->pdata->compass.adapt_num); + retval = mpu3050_read_compass(mldl_cfg, compass_adapt, + data); + if ((ML_SUCCESS == retval) && + (copy_to_user((unsigned char *)arg, + data, sizeof(data)))) + retval = -EFAULT; + } + break; + case MPU_READ_PRESSURE: + { + unsigned char data[3]; + struct i2c_adapter *pressure_adapt = + i2c_get_adapter(mldl_cfg->pdata->pressure. + adapt_num); + retval = + mpu3050_read_pressure(mldl_cfg, pressure_adapt, + data); + if ((ML_SUCCESS == retval) + && + (copy_to_user + ((unsigned char __user *)arg, data, sizeof(data)))) + retval = -EFAULT; + } + break; + case MPU_READ_MEMORY: + case MPU_WRITE_MEMORY: + default: + dev_err(&this_client->adapter->dev, + "%s: Unknown cmd %d, arg %lu\n", __func__, cmd, arg); + retval = -EINVAL; + } + + return retval; +} + +#ifdef CONFIG_HAS_EARLYSUSPEND +void mpu3050_early_suspend(struct early_suspend *h) +{ + struct mpu_private_data *mpu = container_of(h, + struct mpu_private_data, + early_suspend); + struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg; + struct i2c_adapter *accel_adapter; + struct i2c_adapter *compass_adapter; + struct i2c_adapter *pressure_adapter; + pr_info("%s", __func__); + + accel_adapter = i2c_get_adapter(mldl_cfg->pdata->accel.adapt_num); + compass_adapter = i2c_get_adapter(mldl_cfg->pdata->compass.adapt_num); + pressure_adapter = i2c_get_adapter(mldl_cfg->pdata->pressure.adapt_num); + + dev_dbg(&this_client->adapter->dev, "%s: %d, %d\n", __func__, + h->level, mpu->mldl_cfg.gyro_is_suspended); + if (MPU3050_EARLY_SUSPEND_IN_DRIVER) + (void)mpu3050_suspend(mldl_cfg, this_client->adapter, + accel_adapter, compass_adapter, + pressure_adapter, TRUE, TRUE, TRUE, TRUE); +} + +void mpu3050_early_resume(struct early_suspend *h) +{ + struct mpu_private_data *mpu = container_of(h, + struct mpu_private_data, + early_suspend); + struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg; + struct i2c_adapter *accel_adapter; + struct i2c_adapter *compass_adapter; + struct i2c_adapter *pressure_adapter; + + accel_adapter = i2c_get_adapter(mldl_cfg->pdata->accel.adapt_num); + compass_adapter = i2c_get_adapter(mldl_cfg->pdata->compass.adapt_num); + pressure_adapter = i2c_get_adapter(mldl_cfg->pdata->pressure.adapt_num); + + if (MPU3050_EARLY_SUSPEND_IN_DRIVER) { + if (pid) { + unsigned long sensors = mldl_cfg->requested_sensors; + (void)mpu3050_resume(mldl_cfg, + this_client->adapter, + accel_adapter, + compass_adapter, + pressure_adapter, + sensors & ML_THREE_AXIS_GYRO, + sensors & ML_THREE_AXIS_ACCEL, + sensors & ML_THREE_AXIS_COMPASS, + sensors & ML_THREE_AXIS_PRESSURE); + dev_dbg(&this_client->adapter->dev, + "%s for pid %d\n", __func__, pid); + } + } + dev_dbg(&this_client->adapter->dev, "%s: %d\n", __func__, h->level); + pr_info("%s: h->level = %d\n", __func__, h->level); +} +#endif + +void mpu_shutdown(struct i2c_client *client) +{ + struct mpu_private_data *mpu = + (struct mpu_private_data *)i2c_get_clientdata(client); + struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg; + struct i2c_adapter *accel_adapter; + struct i2c_adapter *compass_adapter; + struct i2c_adapter *pressure_adapter; + pr_info("%s", __func__); + accel_adapter = i2c_get_adapter(mldl_cfg->pdata->accel.adapt_num); + compass_adapter = i2c_get_adapter(mldl_cfg->pdata->compass.adapt_num); + pressure_adapter = i2c_get_adapter(mldl_cfg->pdata->pressure.adapt_num); + + (void)mpu3050_suspend(mldl_cfg, this_client->adapter, + accel_adapter, compass_adapter, pressure_adapter, + TRUE, TRUE, TRUE, TRUE); + dev_dbg(&this_client->adapter->dev, "%s\n", __func__); +} + +int mpu_suspend(struct i2c_client *client, pm_message_t mesg) +{ + struct mpu_private_data *mpu = + (struct mpu_private_data *)i2c_get_clientdata(client); + struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg; + struct i2c_adapter *accel_adapter; + struct i2c_adapter *compass_adapter; + struct i2c_adapter *pressure_adapter; + + accel_adapter = i2c_get_adapter(mldl_cfg->pdata->accel.adapt_num); + compass_adapter = i2c_get_adapter(mldl_cfg->pdata->compass.adapt_num); + pressure_adapter = i2c_get_adapter(mldl_cfg->pdata->pressure.adapt_num); + pr_info("%s", __func__); + if (!mpu->mldl_cfg.gyro_is_suspended) { + dev_dbg(&this_client->adapter->dev, + "%s: suspending on event %d\n", __func__, mesg.event); + (void)mpu3050_suspend(mldl_cfg, this_client->adapter, + accel_adapter, compass_adapter, + pressure_adapter, TRUE, TRUE, TRUE, TRUE); + } else { + dev_dbg(&this_client->adapter->dev, + "%s: Already suspended %d\n", __func__, mesg.event); + } + return 0; +} + +int mpu_resume(struct i2c_client *client) +{ + struct mpu_private_data *mpu = + (struct mpu_private_data *)i2c_get_clientdata(client); + struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg; + struct i2c_adapter *accel_adapter; + struct i2c_adapter *compass_adapter; + struct i2c_adapter *pressure_adapter; + + accel_adapter = i2c_get_adapter(mldl_cfg->pdata->accel.adapt_num); + pr_info("%s: accel_adapter = %p\n", __func__, accel_adapter); + + compass_adapter = i2c_get_adapter(mldl_cfg->pdata->compass.adapt_num); + pressure_adapter = i2c_get_adapter(mldl_cfg->pdata->pressure.adapt_num); + + if (pid) { + unsigned long sensors = mldl_cfg->requested_sensors; + (void)mpu3050_resume(mldl_cfg, this_client->adapter, + accel_adapter, + compass_adapter, + pressure_adapter, + sensors & ML_THREE_AXIS_GYRO, + sensors & ML_THREE_AXIS_ACCEL, + sensors & ML_THREE_AXIS_COMPASS, + sensors & ML_THREE_AXIS_PRESSURE); + dev_dbg(&this_client->adapter->dev, + "%s for pid %d\n", __func__, pid); + } + + pr_info("%s: pid = %d\n", __func__, pid); + return 0; +} + +/* define which file operations are supported */ +static const struct file_operations mpu_fops = { + .owner = THIS_MODULE, + .read = mpu_read, +#if HAVE_COMPAT_IOCTL + .compat_ioctl = mpu_ioctl, +#endif +#if HAVE_UNLOCKED_IOCTL + .unlocked_ioctl = mpu_ioctl, +#endif + .open = mpu_open, + .release = mpu_release, +}; + +static unsigned short normal_i2c[] = { I2C_CLIENT_END }; + +static struct miscdevice i2c_mpu_device = { + .minor = MISC_DYNAMIC_MINOR, + .name = "mpu", /* Same for both 3050 and 6000 */ + .fops = &mpu_fops, +}; + +#define FACTORY_TEST +#ifdef FACTORY_TEST + +static ssize_t mpu3050_power_on(struct device *dev, + struct device_attribute *attr, char *buf) +{ + int count = 0; + + pr_info("%s : this_client = %d\n", __func__, (int)this_client); + count = sprintf(buf, "%d\n", (this_client != NULL ? 1 : 0)); + + return count; +} + +static int mpu3050_factory_on(struct i2c_client *client) +{ + struct mpu_private_data *mpu = i2c_get_clientdata(client); + struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg; + struct i2c_adapter *accel_adapter; + struct i2c_adapter *compass_adapter; + struct i2c_adapter *pressure_adapter; + int prev_gyro_suspended; + pr_info("%s", __func__); + accel_adapter = i2c_get_adapter(mldl_cfg->pdata->accel.adapt_num); + compass_adapter = i2c_get_adapter(mldl_cfg->pdata->compass.adapt_num); + pressure_adapter = i2c_get_adapter(mldl_cfg->pdata->pressure.adapt_num); + + prev_gyro_suspended = mldl_cfg->gyro_is_suspended; + if (prev_gyro_suspended) { + unsigned long sensors = mldl_cfg->requested_sensors; + (void)mpu3050_resume(mldl_cfg, + client->adapter, + accel_adapter, + compass_adapter, + pressure_adapter, + sensors & ML_THREE_AXIS_GYRO, + sensors & ML_THREE_AXIS_ACCEL, + sensors & ML_THREE_AXIS_COMPASS, + sensors & ML_THREE_AXIS_PRESSURE); + } + + return prev_gyro_suspended; +} + +static void mpu3050_factory_off(struct i2c_client *client) +{ + struct mpu_private_data *mpu = i2c_get_clientdata(client); + struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg; + struct i2c_adapter *accel_adapter; + struct i2c_adapter *compass_adapter; + struct i2c_adapter *pressure_adapter; + pr_info("%s", __func__); + accel_adapter = i2c_get_adapter(mldl_cfg->pdata->accel.adapt_num); + compass_adapter = i2c_get_adapter(mldl_cfg->pdata->compass.adapt_num); + pressure_adapter = i2c_get_adapter(mldl_cfg->pdata->pressure.adapt_num); + + (void)mpu3050_suspend(mldl_cfg, + client->adapter, + accel_adapter, + compass_adapter, + pressure_adapter, TRUE, TRUE, TRUE, TRUE); +} + +static ssize_t mpu3050_get_temp(struct device *dev, + struct device_attribute *attr, char *buf) +{ + int count = 0; + short int temperature = 0; + unsigned char data[2]; + int prev_gyro_suspended; + pr_info("%s", __func__); + prev_gyro_suspended = mpu3050_factory_on(this_client); + + /*MPUREG_TEMP_OUT_H, 27 0x1b */ + /*MPUREG_TEMP_OUT_L, 28 0x1c */ + /* TEMP_OUT_H/L: 16-bit temperature data (2's complement data format) */ + sensor_i2c_read(this_client->adapter, DEFAULT_MPU_SLAVEADDR, + MPUREG_TEMP_OUT_H, 2, data); + temperature = (short)(((data[0]) << 8) | data[1]); + temperature = (((temperature + 13200) / 280) + 35); + pr_info("%s :read temperature = %d\n", __func__, temperature); + + count = sprintf(buf, "%d\n", temperature); + + if (prev_gyro_suspended) + mpu3050_factory_off(this_client); + + return count; +} + +/* + Defines +*/ + +#define DEBUG_OUT 1 + +#define DEF_GYRO_FULLSCALE (2000) /* gyro full scale dps */ +#define DEF_GYRO_SENS (32768.f/DEF_GYRO_FULLSCALE) + /* gyro sensitivity LSB/dps */ +#define DEF_PACKET_THRESH (75) /* 600 ms / 8ms / sample */ +#define DEF_TIMING_TOL (.05f) /* 5% */ +#define DEF_BIAS_THRESH (40*DEF_GYRO_SENS) + /* 40 dps in LSBs */ +#define DEF_RMS_THRESH_SQ (0.4f*0.4f*DEF_GYRO_SENS*DEF_GYRO_SENS) + /* (.2 dps in LSBs ) ^ 2 */ +#define DEF_TEST_TIME_PER_AXIS (600) /* ms of time spent collecting + data for each axis, + multiple of 600ms */ + +/* + Macros +*/ + +#define CHECK_TEST_ERROR(x) \ + if (x) { \ + pr_info("error %d @ %s|%d\n", x, __func__, __LINE__); \ + return -1; \ + } + +#define SHORT_TO_TEMP_C(shrt) (((shrt+13200)/280)+35) +#define CHARS_TO_SHORT(d) ((((short)(d)[0])<<8)+(d)[1]) +#define fabs(x) (((x) < 0) ? -(x) : (x)) + +void mpu3050_usleep(unsigned long t) +{ + unsigned long start = MLOSGetTickCount(); + while (MLOSGetTickCount() - start < t / 1000) { + } +} + +#define X (0) +#define Y (1) +#define Z (2) + +static short mpu3050_selftest_gyro_avg[3]; +static int mpu3050_selftest_result; +static int mpu3050_selftest_bias[3]; +static int mpu3050_selftest_rms[3]; + +int mpu3050_test_gyro(struct i2c_client *client, short gyro_biases[3], + short *temp_avg) +{ + void *mlsl_handle = client->adapter; + int retVal = 0; + tMLError result; + + int total_count = 0; + int total_count_axis[3] = { 0, 0, 0 }; + int packet_count; + unsigned char regs[7]; + + char a_name[3][2] = { "X", "Y", "Z" }; + int temperature; + int Avg[3]; + int RMS[3]; + int i, j, tmp; + unsigned char dataout[20]; + + short *x, *y, *z; + + x = kzalloc(sizeof(*x) * (DEF_TEST_TIME_PER_AXIS / 8 * 4), GFP_KERNEL); + y = kzalloc(sizeof(*y) * (DEF_TEST_TIME_PER_AXIS / 8 * 4), GFP_KERNEL); + z = kzalloc(sizeof(*z) * (DEF_TEST_TIME_PER_AXIS / 8 * 4), GFP_KERNEL); + + temperature = 0; + + /* sample rate = 8ms */ + result = MLSLSerialWriteSingle(mlsl_handle, client->addr, + MPUREG_SMPLRT_DIV, 0x07); + + if (result) + goto out_i2c_faild; + + regs[0] = 0x03; /* filter = 42Hz, analog_sample rate = 1 KHz */ + switch (DEF_GYRO_FULLSCALE) { + case 2000: + regs[0] |= 0x18; + break; + case 1000: + regs[0] |= 0x10; + break; + case 500: + regs[0] |= 0x08; + break; + case 250: + default: + regs[0] |= 0x00; + break; + } + result = MLSLSerialWriteSingle(mlsl_handle, client->addr, + MPUREG_DLPF_FS_SYNC, regs[0]); + if (result) + goto out_i2c_faild; + + result = MLSLSerialWriteSingle(mlsl_handle, client->addr, + MPUREG_INT_CFG, 0x00); + + /* 1st, timing test */ + for (j = 0; j < 3; j++) { + + pr_info("%s :Collecting gyro data from %s gyro PLL\n", + __func__, a_name[j]); + + /* turn on all gyros, use gyro X for clocking + Set to Y and Z for 2nd and 3rd iteration */ + result = MLSLSerialWriteSingle(mlsl_handle, client->addr, + MPUREG_PWR_MGM, j + 1); + if (result) + goto out_i2c_faild; + + /* wait for 2 ms after switching clock source */ + mpu3050_usleep(2000); + + /* we will enable XYZ gyro in FIFO and nothing else */ + result = MLSLSerialWriteSingle(mlsl_handle, client->addr, + MPUREG_FIFO_EN2, 0x00); + if (result) + goto out_i2c_faild; + /* enable/reset FIFO */ + result = MLSLSerialWriteSingle(mlsl_handle, client->addr, + MPUREG_USER_CTRL, 0x42); + + tmp = (int)(DEF_TEST_TIME_PER_AXIS / 600); + + while (tmp-- > 0) { + /* enable XYZ gyro in FIFO and nothing else */ + result = + MLSLSerialWriteSingle(mlsl_handle, client->addr, + MPUREG_FIFO_EN1, 0x70); + if (result) + goto out_i2c_faild; + + /* wait for 600 ms for data */ + mpu3050_usleep(600000); + + /* stop storing gyro in the FIFO */ + result = + MLSLSerialWriteSingle(mlsl_handle, client->addr, + MPUREG_FIFO_EN1, 0x00); + if (result) + goto out_i2c_faild; + + /* Getting number of bytes in FIFO */ + result = MLSLSerialRead(mlsl_handle, client->addr, + MPUREG_FIFO_COUNTH, 2, dataout); + if (result) + goto out_i2c_faild; + /* number of 6 B packets in the FIFO */ + packet_count = CHARS_TO_SHORT(dataout) / 6; + pr_info("%s :Packet Count: %d - ", + __func__, packet_count); + + if (abs(packet_count - DEF_PACKET_THRESH) + <= /* Within +-5% range */ + (int)(DEF_TIMING_TOL * DEF_PACKET_THRESH + .5)) { + for (i = 0; i < packet_count; i++) { + /* getting FIFO data */ + result = + MLSLSerialReadFifo(mlsl_handle, + client->addr, 6, + dataout); + if (result) + goto out_i2c_faild; + + x[total_count + i] = + CHARS_TO_SHORT(&dataout[0]); + y[total_count + i] = + CHARS_TO_SHORT(&dataout[2]); + z[total_count + i] = + CHARS_TO_SHORT(&dataout[4]); + if (DEBUG_OUT && 0) { + pr_info("%s :Gyros %-4d " \ + ": %+13d %+13d %+13d\n", + __func__, total_count + i, + x[total_count + i], + y[total_count + i], + z[total_count + i]); + } + } + total_count += packet_count; + total_count_axis[j] += packet_count; + pr_info("%s :OK\n", __func__); + } else { + retVal |= 1 << j; + pr_info("%s :NOK - samples ignored\n", + __func__); + } + } + + /* remove gyros from FIFO */ + result = MLSLSerialWriteSingle(mlsl_handle, client->addr, + MPUREG_FIFO_EN1, 0x00); + if (result) + goto out_i2c_faild; + + /* Read Temperature */ + result = MLSLSerialRead(mlsl_handle, client->addr, + MPUREG_TEMP_OUT_H, 2, dataout); + temperature += (short)CHARS_TO_SHORT(dataout); + } + + pr_info("%s :\nTotal %d samples\n\n", __func__, total_count); + + /* 2nd, check bias from X and Y PLL clock source */ + tmp = total_count != 0 ? total_count : 1; + for (i = 0, Avg[X] = .0f, Avg[Y] = .0f, Avg[Z] = .0f; + i < total_count; i++) { + Avg[X] += x[i]; + Avg[Y] += y[i]; + Avg[Z] += z[i]; + } + + Avg[X] /= tmp; + Avg[Y] /= tmp; + Avg[Z] /= tmp; + + pr_info("%s :bias : %+13d %+13d %+13d (LSB)\n", + __func__, Avg[X], Avg[Y], Avg[Z]); + if (DEBUG_OUT) { + pr_info("%s : : %+13d %+13d %+13d (dps)\n", + __func__, Avg[X] / 131, Avg[Y] / 131, Avg[Z] / 131); + } + for (j = 0; j < 3; j++) { + if (abs(Avg[j]) > (int)DEF_BIAS_THRESH) { + pr_info("%s :%s-Gyro bias (%.0d) exceeded threshold " + "(threshold = %f)\n", __func__, + a_name[j], Avg[j], DEF_BIAS_THRESH); + retVal |= 1 << (3 + j); + } + } + + /* 3rd and finally, check RMS */ + for (i = 0, RMS[X] = 0.f, RMS[Y] = 0.f, RMS[Z] = 0.f; + i < total_count; i++) { + RMS[X] += (x[i] - Avg[X]) * (x[i] - Avg[X]); + RMS[Y] += (y[i] - Avg[Y]) * (y[i] - Avg[Y]); + RMS[Z] += (z[i] - Avg[Z]) * (z[i] - Avg[Z]); + } + + for (j = 0; j < 3; j++) { + if (RMS[j] > (int)DEF_RMS_THRESH_SQ * total_count) { + pr_info + ("%s :%s-Gyro RMS (%d) exceeded threshold (%.4f)\n", + __func__, a_name[j], RMS[j] / total_count, + DEF_RMS_THRESH_SQ); + retVal |= 1 << (6 + j); + } + } + + pr_info("%s :RMS^2 : %+13d %+13d %+13d (LSB-rms)\n", + __func__, + (RMS[X] / total_count), + (RMS[Y] / total_count), (RMS[Z] / total_count)); + if (RMS[X] == 0 || RMS[Y] == 0 || RMS[Z] == 0) { + /*If any of the RMS noise value returns zero, + then we might have dead gyro or FIFO/register failure, + or the part is sleeping */ + retVal |= 1 << 9; + } + + temperature /= 3; + if (DEBUG_OUT) + pr_info("%s :Temperature : %+13d %13s %13s (deg. C)\n", + __func__, SHORT_TO_TEMP_C(temperature), "", ""); + + /* load into final storage */ + *temp_avg = (short)temperature; + gyro_biases[X] = (short)Avg[X]; + gyro_biases[Y] = (short)Avg[Y]; + gyro_biases[Z] = (short)Avg[Z]; + + mpu3050_selftest_bias[X] = (int)Avg[X]; + mpu3050_selftest_bias[Y] = (int)Avg[Y]; + mpu3050_selftest_bias[Z] = (int)Avg[Z]; + + mpu3050_selftest_rms[X] = RMS[X] / total_count; + mpu3050_selftest_rms[Y] = RMS[Y] / total_count; + mpu3050_selftest_rms[Z] = RMS[Z] / total_count; + +out_i2c_faild: + if (result) + pr_info("%s : error %d", __func__, result); + + kfree(x); + kfree(y); + kfree(z); + + return retVal; +} + +int mpu3050_self_test_once(struct i2c_client *client) +{ + void *mlsl_handle = client->adapter; + int result = 0; + + short temp_avg; + + unsigned char regs[5]; + unsigned long testStart = MLOSGetTickCount(); + + pr_info("%s :Collecting %d groups of 600 ms samples for each axis\n\n", + __func__, DEF_TEST_TIME_PER_AXIS / 600); + + result = MLSLSerialRead(mlsl_handle, client->addr, + MPUREG_PWR_MGM, 1, regs); + CHECK_TEST_ERROR(result); + /* reset */ + result = MLSLSerialWriteSingle(mlsl_handle, client->addr, + MPUREG_PWR_MGM, regs[0] | 0x80); + CHECK_TEST_ERROR(result); + MLOSSleep(5); + /* wake up */ + if (regs[0] & 0x40) { + result = MLSLSerialWriteSingle(mlsl_handle, client->addr, + MPUREG_PWR_MGM, 0x00); + CHECK_TEST_ERROR(result); + } + MLOSSleep(60); + + /* collect gyro and temperature data */ + mpu3050_selftest_result = mpu3050_test_gyro(client, + mpu3050_selftest_gyro_avg, + &temp_avg); + + pr_info("%s :\nTest time : %ld ms\n", + __func__, MLOSGetTickCount() - testStart); + + return mpu3050_selftest_result; +} + +static ssize_t mpu3050_self_test(struct device *dev, + struct device_attribute *attr, char *buf) +{ + unsigned char gyro_data[6]; + short int raw[3]; + int count = 0; + int res = 0; + int prev_gyro_suspended; + +/*MPUREG_GYRO_XOUT_H, 29 0x1d */ +/*MPUREG_GYRO_XOUT_L, 30 0x1e */ +/*MPUREG_GYRO_YOUT_H, 31 0x1f */ +/*MPUREG_GYRO_YOUT_L, 32 0x20 */ +/*MPUREG_GYRO_ZOUT_H, 33 0x21 */ +/*MPUREG_GYRO_ZOUT_L, 34 0x22 */ + +/* GYRO_XOUT_H/L: 16-bit X gyro output data (2's complement data format) */ +/* GYRO_YOUT_H/L: 16-bit Y gyro output data (2's complement data format) */ +/* GYRO_ZOUT_H/L: 16-bit Z gyro output data (2's complement data format) */ + + prev_gyro_suspended = mpu3050_factory_on(this_client); + + mpu3050_self_test_once(this_client); + + res = sensor_i2c_read(this_client->adapter, DEFAULT_MPU_SLAVEADDR, + MPUREG_GYRO_XOUT_H, 6, gyro_data); + + if (res) + return 0; + + raw[0] = (short)(((gyro_data[0]) << 8) | gyro_data[1]); + raw[1] = (short)(((gyro_data[2]) << 8) | gyro_data[3]); + raw[2] = (short)(((gyro_data[4]) << 8) | gyro_data[5]); + + pr_info("%s: %s %s, %d, %d, %d, %d, %d, %d\n", __func__, buf, + (!mpu3050_selftest_result ? "OK" : "NG"), + raw[0], raw[1], raw[2], + mpu3050_selftest_gyro_avg[0], + mpu3050_selftest_gyro_avg[1], mpu3050_selftest_gyro_avg[2]); + + count = sprintf(buf, "%s, %d, %d, %d, %d, %d, %d\n", + (!mpu3050_selftest_result ? "OK" : "NG"), + mpu3050_selftest_bias[0], mpu3050_selftest_bias[1], + mpu3050_selftest_bias[2], + mpu3050_selftest_rms[0], + mpu3050_selftest_rms[1], mpu3050_selftest_rms[2]); + + if (prev_gyro_suspended) + mpu3050_factory_off(this_client); + + return count; +} + +static ssize_t mpu3050_acc_read(struct device *dev, + struct device_attribute *attr, char *buf) +{ + unsigned char acc_data[6]; + s16 x, y, z; + s32 temp; + + struct mpu_private_data *mpu = + (struct mpu_private_data *)i2c_get_clientdata(this_client); + struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg; + + if (mldl_cfg->accel_is_suspended == 1 || + (mldl_cfg->dmp_is_running == 0 + && mldl_cfg->accel_is_suspended == 0)) { + if (is_lis3dh) { + if (mldl_cfg->accel_is_suspended == 1) { + sensor_i2c_write_register(this_client->adapter, + 0x19, 0x20, 0x67); + MLOSSleep(1); + } + sensor_i2c_read(this_client->adapter, + 0x19, 0x28 | 0x80, 6, acc_data); + + if (mldl_cfg->accel_is_suspended == 1) { + sensor_i2c_write_register(this_client->adapter, + 0x19, 0x20, 0x18); + MLOSSleep(1); + } + } else + sensor_i2c_read(this_client->adapter, + 0x0F, 0x06, 6, acc_data); + } else if (mldl_cfg->dmp_is_running && + mldl_cfg->accel_is_suspended == 0) { + sensor_i2c_read(this_client->adapter, + DEFAULT_MPU_SLAVEADDR, 0x23, 6, acc_data); + } + + if (is_lis3dh) { + x = ((acc_data[0] << 8) | acc_data[1]); + x = (x >> 4) + cal_data.x; + y = ((acc_data[2] << 8) | acc_data[3]); + y = (y >> 4) + cal_data.y; + z = ((acc_data[4] << 8) | acc_data[5]); + z = (z >> 4) + cal_data.z; + } else { + temp = (s16) ((acc_data[1] << 4) | (acc_data[0] >> 4)) + + cal_data.x; + if (temp < 2048) + x = (s16) (temp); + else + x = (s16) ((4096 - temp)) * (-1); + + temp = (s16) ((acc_data[3] << 4) | (acc_data[2] >> 4)) + + cal_data.y; + if (temp < 2048) + y = (s16) (temp); + else + y = (s16) ((4096 - temp)) * (-1); + + temp = (s16) ((acc_data[5] << 4) | (acc_data[4] >> 4)) + + cal_data.z; + if (temp < 2048) + z = (s16) (temp); + else + z = (s16) ((4096 - temp)) * (-1); + } + +#if defined(CONFIG_MACH_P8) + /* x *= (-1); */ + /* y *= (-1); */ + z *= (-1); + return sprintf(buf, "%d, %d, %d\n", y, x, z); + +#elif defined(CONFIG_MACH_P8LTE) + x *= (-1); + /* y *= (-1); */ + /* z *= (-1); */ + return sprintf(buf, "%d, %d, %d\n", y, x, z); + +#elif defined(CONFIG_MACH_P2) + /* x *= (-1); */ + /* y *= (-1); */ + z *= (-1); + return sprintf(buf, "%d, %d, %d\n", y, x, z); + +#else + x *= (-1); + y *= (-1); + z *= (-1); + return sprintf(buf, "%d, %d, %d\n", y, x, z); + +#endif + +} + +static ssize_t accel_calibration_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + int err; + + err = accel_open_calibration(); + if (err < 0) + pr_err("%s: accel_open_calibration() failed\n", __func__); + + pr_info("accel_calibration_show :%d %d %d\n", + cal_data.x, cal_data.y, cal_data.z); + + if (err < 0) + err = 0; + else + err = 1; + + if (cal_data.x == 0 && cal_data.y == 0 && cal_data.z == 0) + err = 0; + + return sprintf(buf, "%d %d %d %d\n", + err, cal_data.x, cal_data.y, cal_data.z); +} + +static ssize_t accel_calibration_store(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t size) +{ + bool do_calib; + int err; + int count = 0; + char str[11]; + + if (sysfs_streq(buf, "1")) + do_calib = true; + else if (sysfs_streq(buf, "0")) + do_calib = false; + else { + pr_debug("%s: invalid value %d\n", __func__, *buf); + return -EINVAL; + } + + err = accel_do_calibrate(do_calib); + if (err < 0) + pr_err("%s: accel_do_calibrate() failed\n", __func__); + + pr_info("accel_calibration_show :%d %d %d\n", + cal_data.x, cal_data.y, cal_data.z); + if (err > 0) + err = 0; + count = sprintf(str, "%d\n", err); + + strcpy(str, buf); + return count; +} + +static ssize_t get_accel_vendor_name(struct device *dev, + struct device_attribute *attr, char *buf) +{ + return sprintf(buf, "%s\n", ACCEL_VENDOR_NAME); +} + +static ssize_t get_accel_chip_name(struct device *dev, + struct device_attribute *attr, char *buf) +{ + return sprintf(buf, "%s\n", ACCEL_CHIP_NAME); +} + +static ssize_t get_gyro_vendor_name(struct device *dev, + struct device_attribute *attr, char *buf) +{ + return sprintf(buf, "%s\n", GYRO_VENDOR_NAME); +} + +static ssize_t get_gyro_chip_name(struct device *dev, + struct device_attribute *attr, char *buf) +{ + return sprintf(buf, "%s\n", GYRO_CHIP_NAME); +} + +static struct device_attribute dev_attr_accel_vendor = + __ATTR(vendor, S_IRUGO, get_accel_vendor_name, NULL); +static struct device_attribute dev_attr_accel_chip = + __ATTR(name, S_IRUGO, get_accel_chip_name, NULL); + +static struct device_attribute dev_attr_gyro_vendor = + __ATTR(vendor, S_IRUGO, get_gyro_vendor_name, NULL); +static struct device_attribute dev_attr_gyro_chip = + __ATTR(name, S_IRUGO, get_gyro_chip_name, NULL); + +static DEVICE_ATTR(calibration, 0664, + accel_calibration_show, accel_calibration_store); +static DEVICE_ATTR(raw_data, S_IRUGO, mpu3050_acc_read, NULL); + +static DEVICE_ATTR(power_on, S_IRUGO | S_IWUSR, mpu3050_power_on, NULL); +static DEVICE_ATTR(temperature, S_IRUGO | S_IWUSR, mpu3050_get_temp, NULL); +static DEVICE_ATTR(selftest, S_IRUGO | S_IWUSR, mpu3050_self_test, NULL); + +static DEVICE_ATTR(gyro_power_on, S_IRUGO | S_IWUSR, mpu3050_power_on, NULL); +static DEVICE_ATTR(gyro_get_temp, S_IRUGO | S_IWUSR, mpu3050_get_temp, NULL); +static DEVICE_ATTR(gyro_selftest, S_IRUGO | S_IWUSR, mpu3050_self_test, NULL); + + +static struct device_attribute *accel_sensor_attrs[] = { + &dev_attr_raw_data, + &dev_attr_calibration, + &dev_attr_accel_vendor, + &dev_attr_accel_chip, + NULL, +}; + +static struct device_attribute *gyro_sensor_attrs[] = { + &dev_attr_power_on, + &dev_attr_temperature, + &dev_attr_selftest, + &dev_attr_gyro_vendor, + &dev_attr_gyro_chip, + NULL, +}; + +extern struct class *sec_class; +extern struct class *sensors_class; + +static struct device *sec_mpu3050_dev; +static struct device *accel_sensor_device; +static struct device *gyro_sensor_device; + +extern int sensors_register(struct device *dev, void *drvdata, + struct device_attribute *attributes[], char *name); +#endif +#define FEATURE_MPU_AUTO_PROBING + +#if defined(CONFIG_MPU_SENSORS_KXTF9_LIS3DH) + +static int mpu350_auto_probe_accel(struct mpu3050_platform_data *pdata) +{ + int ret = ML_SUCCESS; + unsigned char reg = 0; + struct mpu_private_data *mpu; + struct mldl_cfg *mldl_cfg; + struct i2c_adapter *accel_adapter = NULL; + struct ext_slave_descr *slave_descr = NULL; + accel_adapter = i2c_get_adapter(pdata->accel.adapt_num); + + slave_descr = lis3dh_get_slave_descr(); + ret = MLSLSerialRead(accel_adapter, 0x19, 0x0, 1, ®); + if (ret == 0) { + pdata->accel.get_slave_descr = lis3dh_get_slave_descr; + pdata->accel.address = 0x19; + + mpu = (struct mpu_private_data *) + i2c_get_clientdata(this_client); + mldl_cfg = &mpu->mldl_cfg; + mldl_cfg->accel = slave_descr; +/* + printk("auto probe : found %s\n", + pdata->accel.get_slave_descr()->name); +*/ + is_lis3dh = 1; + } + return ret; +} + +#endif + +int mpu3050_probe(struct i2c_client *client, const struct i2c_device_id *devid) +{ + struct mpu3050_platform_data *pdata; + struct mpu_private_data *mpu; + struct mldl_cfg *mldl_cfg; + int res = 0; + int retry = 5; + + struct i2c_adapter *accel_adapter = NULL; + struct i2c_adapter *compass_adapter = NULL; + struct i2c_adapter *pressure_adapter = NULL; + + pr_info("================%s===============\n", __func__); + + dev_dbg(&client->adapter->dev, "%s\n", __func__); + + if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) { + res = -ENODEV; + goto out_check_functionality_failed; + } +#ifdef FACTORY_TEST + res = sensors_register(accel_sensor_device, NULL, + accel_sensor_attrs, "accelerometer_sensor"); + if (res) + pr_err("%s: cound not register accelerometer "\ + "sensor device(%d).\n", __func__, res); + + res = sensors_register(gyro_sensor_device, NULL, + gyro_sensor_attrs, "gyro_sensor"); + if (res) + pr_err("%s: cound not register gyro "\ + "sensor device(%d).\n", __func__, res); + + sec_mpu3050_dev = device_create(sec_class, NULL, 0, NULL, + "sec_mpu3050"); + if (IS_ERR(sec_mpu3050_dev)) + pr_info("%s :Failed to create device!", __func__); + + if (device_create_file(sec_mpu3050_dev, &dev_attr_gyro_power_on) < 0) { + pr_info("%s :Failed to create device file(%s)!\n", __func__, + dev_attr_gyro_power_on.attr.name); + return -1; + } + if (device_create_file(sec_mpu3050_dev, &dev_attr_gyro_get_temp) < 0) { + pr_info("%s :Failed to create device file(%s)!\n", __func__, + dev_attr_gyro_get_temp.attr.name); + device_remove_file(sec_mpu3050_dev, &dev_attr_gyro_power_on); + return -1; + } + if (device_create_file(sec_mpu3050_dev, &dev_attr_gyro_selftest) < 0) { + pr_info("%s :Failed to create device file(%s)!\n", __func__, + dev_attr_gyro_selftest.attr.name); + device_remove_file(sec_mpu3050_dev, &dev_attr_gyro_power_on); + device_remove_file(sec_mpu3050_dev, &dev_attr_gyro_get_temp); + return -1; + } +#endif + mpu = kzalloc(sizeof(struct mpu_private_data), GFP_KERNEL); + if (!mpu) { + res = -ENOMEM; + goto out_alloc_data_failed; + } + + i2c_set_clientdata(client, mpu); + this_client = client; + mldl_cfg = &mpu->mldl_cfg; + pdata = (struct mpu3050_platform_data *)client->dev.platform_data; + if (!pdata) { + dev_warn(&this_client->adapter->dev, + "Warning no platform data for mpu3050\n"); + } else { + mldl_cfg->pdata = pdata; + + pdata->accel.get_slave_descr = get_accel_slave_descr; + pdata->compass.get_slave_descr = get_compass_slave_descr; + pdata->pressure.get_slave_descr = get_pressure_slave_descr; + + is_lis3dh = 0; +#if defined(CONFIG_MPU_SENSORS_KXTF9_LIS3DH) + mpu350_auto_probe_accel(pdata); + if (pdata->accel.get_slave_descr && !is_lis3dh) { + mldl_cfg->accel = pdata->accel.get_slave_descr(); + dev_info(&this_client->adapter->dev, + "%s: +%s\n", MPU_NAME, mldl_cfg->accel->name); + accel_adapter = i2c_get_adapter(pdata->accel.adapt_num); + + if (!accel_adapter) { + pr_info("%s : accel_adapter i2c get fail", + __func__); + goto out_accel_failed; + } + + if (pdata->accel.irq > 0) { + dev_info(&this_client->adapter->dev, + "Installing Accel irq using %d\n", + pdata->accel.irq); + res = slaveirq_init(accel_adapter, + &pdata->accel, "accelirq"); + if (res) + goto out_accelirq_failed; + } else { + dev_warn(&this_client->adapter->dev, + "WARNING: Accel irq not assigned\n"); + } + } else +#else + if (pdata->accel.get_slave_descr) { + mldl_cfg->accel = pdata->accel.get_slave_descr(); + dev_info(&this_client->adapter->dev, + "%s: +%s\n", MPU_NAME, mldl_cfg->accel->name); + accel_adapter = i2c_get_adapter(pdata->accel.adapt_num); + + if (!accel_adapter) { + pr_info("%s : accel_adapter i2c get fail", + __func__); + goto out_accel_failed; + } + + if (pdata->accel.irq > 0) { + dev_info(&this_client->adapter->dev, + "Installing Accel irq using %d\n", + pdata->accel.irq); + res = slaveirq_init(accel_adapter, + &pdata->accel, "accelirq"); + if (res) + goto out_accelirq_failed; + } else { + dev_warn(&this_client->adapter->dev, + "WARNING: Accel irq not assigned\n"); + } + } else +#endif + { + dev_warn(&this_client->adapter->dev, + "%s: No Accel Present\n", MPU_NAME); + } + + if (pdata->compass.get_slave_descr) { + mldl_cfg->compass = pdata->compass.get_slave_descr(); + dev_info(&this_client->adapter->dev, + "%s: +%s\n", MPU_NAME, + mldl_cfg->compass->name); + compass_adapter = + i2c_get_adapter(pdata->compass.adapt_num); + + if (!compass_adapter) { + pr_info("%s : compass_adapter i2c get fail", + __func__); + goto out_compass_failed; + } + + if (pdata->compass.irq > 0) { + dev_info(&this_client->adapter->dev, + "Installing Compass irq using %d\n", + pdata->compass.irq); + res = slaveirq_init(compass_adapter, + &pdata->compass, + "compassirq"); + if (res) + goto out_compassirq_failed; + } else { + dev_warn(&this_client->adapter->dev, + "WARNING: Compass irq not assigned\n"); + } + } else { + dev_warn(&this_client->adapter->dev, + "%s: No Compass Present\n", MPU_NAME); + } + + if (pdata->pressure.get_slave_descr) { + mldl_cfg->pressure = pdata->pressure.get_slave_descr(); + dev_info(&this_client->adapter->dev, + "%s: +%s\n", MPU_NAME, + mldl_cfg->pressure->name); + pressure_adapter = + i2c_get_adapter(pdata->pressure.adapt_num); + + if (!pressure_adapter) { + pr_info("%s : pressure_adapter i2c get fail", + __func__); + goto out_pressure_failed; + } + + if (pdata->pressure.irq > 0) { + dev_info(&this_client->adapter->dev, + "Installing Pressure irq using %d\n", + pdata->pressure.irq); + res = slaveirq_init(pressure_adapter, + &pdata->pressure, + "pressureirq"); + if (res) + goto out_pressureirq_failed; + } else { + dev_warn(&this_client->adapter->dev, + "WARNING: Pressure irq not assigned\n"); + } + } else { + dev_warn(&this_client->adapter->dev, + "%s: No Pressure Present\n", MPU_NAME); + } + } + + mldl_cfg->addr = client->addr; + + do { + res = mpu3050_open(&mpu->mldl_cfg, client->adapter, + accel_adapter, compass_adapter, + pressure_adapter); + if (res) { + dev_err(&this_client->adapter->dev, + "%s i2c Init Error, ret = %d\n", MPU_NAME, res); + mpu3050_usleep(5000); + } + } while (retry-- && res); + + if (res) { + dev_err(&this_client->adapter->dev, + "Unable to open %s %d\n", MPU_NAME, res); + res = -ENODEV; + goto out_whoami_failed; + } + + res = misc_register(&i2c_mpu_device); + if (res < 0) { + dev_err(&this_client->adapter->dev, + "ERROR: misc_register returned %d\n", res); + goto out_misc_register_failed; + } + + if (this_client->irq > 0) { + dev_info(&this_client->adapter->dev, + "Installing irq using %d\n", this_client->irq); + res = mpuirq_init(this_client); + if (res) + goto out_mpuirq_failed; + } else { + dev_warn(&this_client->adapter->dev, + "WARNING: %s irq not assigned\n", MPU_NAME); + } + + mpu_accel_init(&mpu->mldl_cfg, client->adapter); + +#ifdef CONFIG_HAS_EARLYSUSPEND + mpu->early_suspend.level = EARLY_SUSPEND_LEVEL_BLANK_SCREEN + 1; + mpu->early_suspend.suspend = mpu3050_early_suspend; + mpu->early_suspend.resume = mpu3050_early_resume; + register_early_suspend(&mpu->early_suspend); +#endif + return res; + +out_mpuirq_failed: + misc_deregister(&i2c_mpu_device); +out_misc_register_failed: + mpu3050_close(&mpu->mldl_cfg, client->adapter, + accel_adapter, compass_adapter, pressure_adapter); +out_whoami_failed: + if (pdata && pdata->pressure.get_slave_descr && pdata->pressure.irq) + slaveirq_exit(&pdata->pressure); +out_pressureirq_failed: +out_pressure_failed: + if (pdata && pdata->compass.get_slave_descr && pdata->compass.irq) + slaveirq_exit(&pdata->compass); +out_compassirq_failed: +out_compass_failed: + if (pdata && pdata->accel.get_slave_descr && pdata->accel.irq) + slaveirq_exit(&pdata->accel); +out_accelirq_failed: +out_accel_failed: + kfree(mpu); +out_alloc_data_failed: +out_check_functionality_failed: + dev_err(&this_client->adapter->dev, "%s failed %d\n", __func__, res); + return res; +} + +static int mpu3050_remove(struct i2c_client *client) +{ + struct mpu_private_data *mpu = i2c_get_clientdata(client); + struct i2c_adapter *accel_adapter; + struct i2c_adapter *compass_adapter; + struct i2c_adapter *pressure_adapter; + struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg; + struct mpu3050_platform_data *pdata = mldl_cfg->pdata; + + accel_adapter = i2c_get_adapter(mldl_cfg->pdata->accel.adapt_num); + compass_adapter = i2c_get_adapter(mldl_cfg->pdata->compass.adapt_num); + pressure_adapter = i2c_get_adapter(mldl_cfg->pdata->pressure.adapt_num); + + dev_dbg(&client->adapter->dev, "%s\n", __func__); + +#ifdef CONFIG_HAS_EARLYSUSPEND + unregister_early_suspend(&mpu->early_suspend); +#endif + mpu3050_close(mldl_cfg, client->adapter, + accel_adapter, compass_adapter, pressure_adapter); + + if (client->irq) + mpuirq_exit(); + + if (pdata && pdata->pressure.get_slave_descr && pdata->pressure.irq) + slaveirq_exit(&pdata->pressure); + + if (pdata && pdata->compass.get_slave_descr && pdata->compass.irq) + slaveirq_exit(&pdata->compass); + + if (pdata && pdata->accel.get_slave_descr && pdata->accel.irq) + slaveirq_exit(&pdata->accel); + + misc_deregister(&i2c_mpu_device); + kfree(mpu); + + mpu_accel_exit(mldl_cfg); + + return 0; +} + +static const struct i2c_device_id mpu3050_id[] = { + {MPU_NAME, 0}, + {} +}; + +MODULE_DEVICE_TABLE(i2c, mpu3050_id); + +static struct i2c_driver mpu3050_driver = { + .class = I2C_CLASS_HWMON, + .probe = mpu3050_probe, + .remove = mpu3050_remove, + .id_table = mpu3050_id, + .driver = { + .owner = THIS_MODULE, + .name = MPU_NAME, + }, + .address_list = normal_i2c, + .shutdown = mpu_shutdown, /* optional */ + .suspend = mpu_suspend, /* optional */ + .resume = mpu_resume, /* optional */ +}; + +static int __init mpu_init(void) +{ + int res = i2c_add_driver(&mpu3050_driver); + pid = 0; + + pr_info("%s res=%d\n", __func__, res); + if (res) + dev_err(&this_client->adapter->dev, "%s failed\n", __func__); + return res; +} + +static void __exit mpu_exit(void) +{ + pr_info(KERN_DEBUG "%s\n", __func__); + i2c_del_driver(&mpu3050_driver); +} + +module_init(mpu_init); +module_exit(mpu_exit); + +MODULE_AUTHOR("Invensense Corporation"); +MODULE_DESCRIPTION("User space character device interface for MPU3050"); +MODULE_LICENSE("GPL"); +MODULE_ALIAS(MPU_NAME); |